#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

// 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){
		//delay_ms(ADC_DELAY / ADC_CHNS / 10);
		_delay_ms(50);
		//adc.raw[adc.chn] += ADC;//(adc.raw[adc.chn] + ADC) >> 1;
		adc.raw[adc.chn] = (adc.raw[adc.chn] + ADC + (ADCH & 1)) >> 1;

//		if(adc.cnt == (ADC_AVG-1)){
//			adc.cur[adc.chn] = (adc.cur[adc.chn] + adc.raw[adc.chn]) >> 1;
//			uint16_t u = mul32x16(mul32u((uint32_t)(ADC_REF*2), pgm_read_byte_near(&adc_divs[adc.chn])), adc.cur[adc.chn]) >> (11 + ADC_AVG_BITS);
//			uint16_t u = mul32x16(mul32u((uint32_t)(ADC_REF), pgm_read_byte_near(&adc_divs[adc.chn])), adc.raw[adc.chn]) >> 10;
//			adc.raw[adc.chn] = 0;

/*			// 2 rows - 2+3 digits
			if(adc.chn < 2){
				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);
			}
*/
/*			// 2 rows - 2+2 digits
			if(adc.chn < 2){
				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);
			}
*/
			// 3 rows
/*			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);
			}
*/
			// 3 rows, auto-hide
/*			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);
			}
*/
			// 4 rows
/*			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);
*/

		// 128x32, 1 row, 3 digits
/*		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);
		}
*/
		// 128x32, 1 row, 4 digits
		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);
		}


//		}

		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);
	}
	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));

	// 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,
#ifdef OLED_128X32
	0xDA,0x02,
#endif
};
void oled_init(){
	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
//#ifdef OLED_SH1106
	for(uint8_t row=0; row<32; 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();
}
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 };

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 to next char
	for(uint8_t i=oled_sx; i--;) oled_x += 8;
}