#include "main.h"

const uint8_t adc_chn[] PROGMEM  = { 2,3,1,0 };     // channel MUXs
const uint8_t adc_divs[] PROGMEM = { 11,11,11,11 }; // channel dividers


register uint8_t oled_x asm("r2");  // X position, pixel 0..127
register uint8_t oled_y asm("r3");  // Y position, row 0..7
register uint8_t oled_sx asm("r4"); // X scale 1..4
register uint8_t oled_sy asm("r5"); // Y scale 1..4
register uint8_t oled_w asm("r6");  // number width 1..5
register uint8_t oled_p asm("r7");  // point position 0..4

#ifdef MODE_RPM
register uint16_t rpm_cnt_h asm("r8");   // RPM high counter
register uint16_t rpm_prev_h asm("r10"); // RPM high previous counter
register uint8_t rpm_prev_l asm("r12");  // RPM low previous counter
register uint8_t rpm_ready asm("r13");   // RPM ready flag
register uint16_t rpm_cur asm("r14");    // RPM current value
#endif

// multiplication 32 * 16
uint32_t mul32x16(uint32_t x, uint16_t y){
	asm volatile(
		"mov r18,r1 \n" // clear result
		"mov r19,r1 \n"
		"mov r26,r1 \n"
		"mov r27,r1 \n"
		
		"ldi r30,16 \n" // bit counter

	"1: \n"
		"sbrs %A1,0 \n" // result += x if y's lsb is set
		"rjmp 2f \n"
		"add r18, %A0 \n"
		"adc r19, %B0 \n"
		"adc r26, %C0 \n"
		"adc r27, %D0 \n"
	"2: \n"

		"lsl %A0 \n"  // x <<= 1
		"rol %B0 \n"
		"rol %C0 \n"
		"rol %D0 \n"

		"lsr %B1 \n"  // y >>= 1
		"ror %A1 \n"

		"dec r30 \n"
		"brne 1b \n"

		"mov %A0,r18 \n"
		"mov %B0,r19 \n"
		"mov %C0,r26 \n"
		"mov %D0,r27 \n"

        :[x]"+r"(x) // output
        :[y]"r"(y) // input
        : "cc" // clobbers
    );
	return x;
}

// division 16 / 10: x/10 = (x * 0x3333 + 0x3333) >> 17
uint16_t div16by10(uint16_t n){
	asm volatile(
		"mov r18,%A0 \n" // n = R18:21
		"mov r19,%B0 \n"
		"mov r20,r1 \n"
		"mov r21,r1 \n"
		"mov r26,r1 \n" // r = R24:27
		"mov r27,r1 \n"

		"ldi r22,1 \n" // n <<= 1; r += n;
		"rcall 1f \n"
		"ldi r22,3 \n" // n <<= 3; r += n;
		"rcall 1f \n"
		"ldi r22,1 \n" // n <<= 1; r += n;
		"rcall 1f \n"
		"ldi r22,3 \n" // n <<= 3; r += n;
		"rcall 1f \n"
		"ldi r22,1 \n" // n <<= 1; r += n;
		"rcall 1f \n"
		"ldi r22,3 \n" // n <<= 3; r += n;
		"rcall 1f \n"
		"ldi r22,1 \n" // n <<= 1; r += n;
		"rcall 1f \n"
		"ldi r18,0x33 \n" // r += 0x3333;
		"mov r19,r18 \n"
		"mov r20,r1 \n"
		"mov r21,r1 \n"
		"rcall 2f \n"
		"rjmp 3f \n"

	"1: lsl r18 \n" // n <<= R22
		"rol r19 \n"
		"rol r20 \n"
		"rol r21 \n"
		"dec r22 \n"
		"brne 1b \n"
	"2: add %A0,r18 \n" // r += n
		"adc %B0,r19 \n"
		"adc r26,r20 \n"
		"adc r27,r21 \n"
		"ret \n"
	"3: mov %A0,r26 \n" // return(r >> 17);
		"mov %B0,r27 \n"
		"lsr %B0 \n"
		"ror %A0 \n"

        :[n]"+r"(n) // output
        : // input
        : "cc" // clobbers
    );
	return n;
}

// show 16-bit number
const uint16_t tens[] PROGMEM = { 1,10,100,1000,10000 };
void oled_u16(uint16_t a){
	for(uint8_t i=oled_w; i;){
		if(i == oled_p) oled_c(10); // put point
		uint16_t ten = pgm_read_word_near(&tens[--i]);
		uint8_t dig = 0; // get next digit
		while(a >= ten){ dig++; a -= ten; }
		oled_c(dig); // show digit
	}
}

int main(){
	setup();

	while(1){

#ifdef MODE_RPM
		if(rpm_ready){
			//rpm.cur = (60uL * F_CPU / RPM_DIV) / rpm.dif;
			uint32_t a = 60uL * F_CPU / RPM_DIV;
			rpm_cur = 0;
			while(a >= rpm.dif){
				a -= rpm.dif;
				rpm_cur++;
			}
			rpm_ready = 0;
		}
		if(++rpm.ind >= 100){
			rpm.ind = 0;
			if(rpm.res){ // reset value if no pulses
				if(--rpm.res == 0) rpm_cur = 0;
			}
			oled_x = oled_y = oled_p = 0; oled_sy = 4;
#ifdef RPM_5DIGS
			oled_w = 5; oled_sx = 3;
#else
			oled_w = 4; oled_sx = 4;
#endif
			oled_u16(rpm_cur);
		}
		_delay_ms(1);
#else
		// measure next channel
		if(++adc.chn >= ADC_CHNS){
			adc.chn = 0;
	//			if(++adc.cnt >= ADC_AVG) adc.cnt = 0;
		}
		ADMUX = pgm_read_byte_near(&adc_chn[adc.chn]);
		ADCSRA |= (1<<ADSC);
		_delay_ms(ADC_DELAY);
		adc.raw[adc.chn] = (adc.raw[adc.chn] + ADC + (ADCH & 1)) >> 1;
#endif

		// 128x64, 2 rows, 2.3 digits
#ifdef MODE_U2_128X64_2_3
		if(adc.chn < 2){
			uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
			oled_w = 4; oled_p = 3;
			oled_sx = 3; oled_sy = 4;
			oled_x = 0; oled_y = adc.chn << 2;
			if(u > 9999){ // u > 9999mV ? u /= 10
				u = mul32x16(0x3333uL, u+1) >> 17;
				oled_p = 2; // 2 digits
			}
			oled_u16(u);
		}
#endif

		// 128x64, 2 rows, 2.2 digits
#ifdef MODE_U2_128X64_2_2
		if(adc.chn < 2){
			uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
			oled_w = 4; oled_p = 2;
			oled_sx = 3; oled_sy = 4;
			oled_x = 4; oled_y = adc.chn << 2;
			u = mul32x16(0x3333uL, u+1) >> 17; // u /= 10
			if(u < 1000){
				oled_c(11);
				oled_w = 3;
			}
			oled_u16(u);
		}
#endif

		// 128x64, 3 rows, 2.2 digits
#ifdef MODE_U3_128X64_2_2
		uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF / 10), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
		if(adc.chn < 3){
			oled_w = 4; oled_p = 2;
			if(adc.chn){
				oled_sx = oled_sy = 2;
				oled_x = 24;//(adc.chn == 1) ? 0 : 0;
				oled_y = (adc.chn == 1) ? 4 : 6;
			}else{
				oled_sx = 3; oled_sy = 4;
				oled_x = 4;
				oled_y = adc.chn << 2;
			}
			u = mul32x16(0x3333uL, u+1) >> 17; // u /= 10
			if(u < 1000){
				oled_c(11);
				oled_w = 3;
			}
			oled_u16(u);
		}
#endif

		// 128x64, 2/3 rows, auto-hide, 2.2 digits
#ifdef MODE_U23_128X64_2_2
		uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF / 10), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
		if(adc.chn < 3){
			oled_w = 4; oled_p = oled_sx = oled_sy = 2;
			if(adc.chn){
				if(adc.raw[2] > 5){ // 3rd channel > 0
					oled_x = 0; oled_y = (adc.chn == 1) ? 4 : 6;
					for(uint8_t i=3; i; i--) oled_c(11);
					oled_x -= 4;
				}else{
					if(adc.chn == 2) goto SKIP; // hide 3rd
					oled_sx = 3; oled_sy = 4;
					oled_x = 4; oled_y = 4;
				}
			}else{
				oled_sx = 3; oled_sy = 4;
				oled_x = 4; oled_y = 0;
			}
//				u = (uint16_t)(mul32x16(0x3333uL, u+6) >> 16) >> 1; // u /= 10
			if(u < 1000){
				oled_c(11);
				oled_w = 3;
			}
			oled_u16(u);
			SKIP: do{}while(0);
		}
#endif

		// 128x64, 4 rows, 3.2 digits
#ifdef MODE_U4_128X64_3_2
		uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
		oled_w = 5; oled_p = 3;
		oled_sx = 2; oled_sy = 2;
		oled_x = 16; oled_y = adc.chn << 1;
		if(u <= 9999){ // hide 1st zero
			oled_w = 4;
			oled_c(11);
		}
		oled_u16(u);
#endif

		// 128x32, 1 row, 3 digits
#ifdef MODE_U1_128X32_3_1
		if(adc.chn == 2){
			uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF / 100), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
			oled_w = 3; oled_p = 1;
			oled_sx = 4; oled_sy = 4;
			oled_x = 0; oled_y = 0;
			if(u <= 99){ // hide 1st zero
				oled_w = 2;
				oled_c(11);
			}
			oled_u16(u);
		}
#endif

		// 128x32, 1 row, 4 digits, channel ADC1
#ifdef MODE_U1_128X32_4_2
		if(adc.chn == 2){
			uint16_t u = mul32x16(mul32x16((uint32_t)(ADC_REF / 10), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
			oled_w = 4; oled_p = 2;
			oled_sx = 3; oled_sy = 4;
			oled_x = 0; oled_y = 0;
			if(u <= 999){ // hide 1st zero
				oled_w = 3;
				oled_c(11);
			}
			oled_u16(u);
		}
#endif

	}
	return 0;
}

void setup(){
	// ADC
	DIDR0 = (1<<ADC1D)|(1<<ADC2D)|(1<<ADC3D);
	ADMUX = pgm_read_byte_near(&adc_chn[0]);
	ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);

	// I2C
	DDRB  &= ~((1<<PB0)|(1<<PB1));
	PORTB &= ~((1<<PB0)|(1<<PB1));

#ifdef MODE_RPM
	// RPM input
	DDRB &= ~(1<<PB2);
	PORTB |= (1<<PB2);

	// T0
	TCCR0A = (0<<COM0A1)|(0<<COM0A0)|(0<<WGM01)|(0<<WGM00);
	TCCR0B = (0<<WGM02)|(0<<CS00)|(1<<CS01)|(0<<CS02);
	TIMSK0 = (1<<TOIE0);

	// PCINT
	PCMSK |= (1<<PCINT2);
	GIMSK |= (1<<PCIE);

	sei();
#endif

	// OLED
//	delay_ms(10);
	oled_init();
}

void delay_ms(uint8_t ms){ while(ms--) _delay_ms(5); }


void i2c_start(){
    i2c_scl_1(); i2c_delay();
    i2c_sda_0(); i2c_delay();
    i2c_scl_0(); //i2c_delay();
}
void i2c_stop(){
    i2c_sda_0(); i2c_delay();
    i2c_scl_1(); i2c_delay();
    i2c_sda_1(); //i2c_delay();
}
void i2c_wr_u8(uint8_t b){
    for(uint8_t i=9; i; i--, b<<=1){
		if(b & 0x80) i2c_sda_1(); else i2c_sda_0();
		i2c_delay();
		i2c_scl_1(); i2c_delay();
		i2c_scl_0();
		i2c_sda_1();// i2c_delay();
	}
	return;
}

const uint8_t oled_init_cmds[] PROGMEM = { 0x8D,0x14, 0xAF, 0x81,OLED_CONTR,
#if defined(OLED_128X32) || defined(MODE_U1_128X32_4_2) || defined(MODE_U1_128X32_3_1) || defined(MODE_RPM)
	0xDA,0x02,
#endif
};
void oled_init(){
	// init commands
	i2c_start();
	i2c_wr_u8(OLED_ADDR);
	i2c_wr_u8(0);
	for(uint8_t i=0; i<sizeof(oled_init_cmds); i++)
		i2c_wr_u8(pgm_read_byte_near(&oled_init_cmds[i]));

	// clear rows
//#ifdef OLED_SH1106
	for(uint8_t row=0; row<64; row++){
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0x00);
		i2c_wr_u8(0xB0 | row);
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0x40);
		for(uint8_t col=OLED_WIDTH; col; col--) i2c_wr_u8(0x00);
	}
//#else
//	i2c_start();
//	i2c_wr_u8(OLED_ADDR);
//	i2c_wr_u8(0x40);
//	for(uint16_t col=(8*132); col; col--) i2c_wr_u8(0);
//#endif
	i2c_stop();
}
void oled_clr(){
	for(uint8_t row=0; row<8; row++){
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0);
		i2c_wr_u8(0xB0 + row);
		i2c_wr_u8(((OLED_SHIFT & 0xF0) >> 4) | 0x10);
		i2c_wr_u8(OLED_SHIFT & 0xF);

		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0x40);
		for(uint8_t col=0; col<128; col++) i2c_wr_u8(0);
		i2c_stop();
	}
}
void oled_goto(uint8_t x, uint8_t y){
	i2c_start();
	i2c_wr_u8(OLED_ADDR);
	i2c_wr_u8(0);
	i2c_wr_u8(0xB0 + y);
	i2c_wr_u8(((x & 0xF0) >> 4) | 0x10);
	i2c_wr_u8(x & 0xF);
	i2c_stop();
}

// font 8x8 0..9.space
const uint8_t font_8x8[][8] PROGMEM = {
	{ 0x3e, 0x7f, 0x71, 0x59, 0x4d, 0x7f, 0x3e, 0x0 }, // 0
	{ 0x40, 0x42, 0x7f, 0x7f, 0x40, 0x40, 0x0, 0x0 },  // 1
	{ 0x62, 0x73, 0x59, 0x49, 0x6f, 0x66, 0x0, 0x0 },  // 2
	{ 0x22, 0x63, 0x49, 0x49, 0x7f, 0x36, 0x0, 0x0 },  // 3
	{ 0x18, 0x1c, 0x16, 0x53, 0x7f, 0x7f, 0x50, 0x0 }, // 4
	{ 0x27, 0x67, 0x45, 0x45, 0x7d, 0x39, 0x0, 0x0 },  // 5
	{ 0x3c, 0x7e, 0x4b, 0x49, 0x79, 0x30, 0x0, 0x0 },  // 6
	{ 0x3, 0x3, 0x71, 0x79, 0xf, 0x7, 0x0, 0x0 },      // 7
	{ 0x36, 0x7f, 0x49, 0x49, 0x7f, 0x36, 0x0, 0x0 },  // 8
	{ 0x6, 0x4f, 0x49, 0x69, 0x3f, 0x1e, 0x0, 0x0 },   // 9
	{ 0x0, 0x0, 0x60, 0x60, 0x0, 0x0, 0x0, 0x0 },      // .
	{ 0x0, 0x0, 0x00, 0x00, 0x0, 0x0, 0x0, 0x0 },      // space
};
const uint8_t col_bits[] PROGMEM = { 8,4,3,2 }; // column bits

// draw scaled char at current position
void oled_c(uint8_t c){
	for(uint8_t row=0; row<oled_sy; row++){
		// goto x,y
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0);
		i2c_wr_u8(0xB0 | (oled_y + row));
		i2c_wr_u8((oled_x >> 4) | 0x10);
		i2c_wr_u8(oled_x & 0xF);
		// draw char
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0x40);
		for(uint8_t col=0; col<8; col++){
			uint8_t cc = pgm_read_byte_near(&font_8x8[c][col]); // digit column
			uint8_t nbits = pgm_read_byte_near(&col_bits[oled_sy-1]); // bits per OLED row
			for(uint8_t i=row; i--;) for(uint8_t j=nbits; j--;) cc >>= 1; // shift to current row
			uint8_t ccc = 0;
			for(uint8_t i=nbits; i--; cc >>= 1){ // multiply bits
				for(uint8_t j=oled_sy; j--;){
					ccc >>= 1;
					if(cc & 1) ccc |= 0x80;
				}
			}
			for(uint8_t i=oled_sx; i--; ) i2c_wr_u8(ccc);
		}
		i2c_stop();
	}
	// move X to next char position
	for(uint8_t i=oled_sx; i--;) oled_x += 8;
}

#ifdef MODE_RPM

ISR(PCINT0_vect){
	if(!rpm_in()){
		uint8_t rpm_l = TCNT0;
		uint16_t rpm_h = rpm_cnt_h;
		if(!rpm_ready){
			rpm_ready = 1;
			rpm.dif = (((uint32_t)rpm_h << 8) + rpm_l) - (((uint32_t)rpm_prev_h << 8) + rpm_prev_l);
		}
		rpm_prev_l = rpm_l;
		rpm_prev_h = rpm_h;
		rpm.res = 50;
	}
}
ISR(TIM0_OVF_vect){
	rpm_cnt_h++;
}

#endif