#include "main.h"

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

//struct oled_struct {
//	uint8_t x,y, sx,sy, w,p;
//}oled;

register uint8_t oled_x asm("r2");
register uint8_t oled_y asm("r3");
register uint8_t oled_sx asm("r4");
register uint8_t oled_sy asm("r5");
register uint8_t oled_w asm("r6");
register uint8_t oled_p asm("r7");

inline void oled_xy(uint8_t x,uint8_t y, uint8_t sx,uint8_t sy){ oled_x=x; oled_y=y; oled_sx=sx; oled_sy=sy; }
inline void oled_wp(uint8_t w, uint8_t p){ oled_w = w; oled_p = p; }

//uint8_t adc_chns ;
//const __attribute__((section(".config"))) uint8_t adc_chns = 3;
//volatile const __attribute__((at(0x3FF))) uint8_t adc_chns = 3;

// u32 * u16
uint32_t mul32u(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;
}
// u16 / 10
uint16_t div10(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 u16
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_c1(10);
		uint16_t ten = pgm_read_word_near(&tens[--i]);
		uint8_t dig = 0;
		while(a >= ten){ dig++; a -= ten; }
		oled_c1(dig);
	}
}


const uint16_t u16_tens[] PROGMEM = { 10000, 1000, 100, 10, 1 };
void oled_u16_(uint8_t x,uint8_t y, uint8_t sx,uint8_t sy, uint8_t w,uint8_t p, uint16_t a){
	p = p ? (4 - p) : 10;
	for(uint8_t ten=(5-w); ten<5; ten++){
		uint8_t dig = 0;
		uint16_t tens = pgm_read_word_near(&u16_tens[ten]);
		while(a >= tens){
			dig++;
			a -= tens;
		}
		oled_c(x,y, sx,sy, dig);
		for(uint8_t i=sx; i--;) x += 8;

		if(ten == p){
			oled_c(x,y, sx,sy, 10);
			for(uint8_t i=sx; i--;) x += 8;
		}
	}
}

int main(){
	setup();

	while(1){
		delay_ms(ADC_DELAY / 10);
		adc.raw[adc.chn] = (adc.raw[adc.chn] + ADC) >> 1;

		oled_p = (adc.raw[adc.chn] >= ((10000uL << 10) / ADC_REF / ADC_DIV)) ? 2 : 3;
		uint16_t u = mul32u((uint32_t)((oled_p == 3) ? ADC_REF : (ADC_REF / 10)) * ADC_DIV, adc.raw[adc.chn]) >> 10;

		if(adc.chn < 2){
			oled_x = 0;
			oled_y = (adc.chn == 0) ? 0 : 4;
			oled_u16(u);
		}

		if(++adc.chn >= ADC_CHNS){
			adc.chn = 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();
	oled_w = 4;
	oled_sx = 3;
	oled_sy = 4;
}

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


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 = {
	0xAE, 0x00, 0x10,0x40, 0x81,0xCF,0xA1,0xC8,0xA6, 0xA4,0xA8,0x3F, 0xD3,0x00, 0xD5,0x80,
	0xD9,0xF1, 0xDA,0x12, 0xDB,0x40, 0x20,0x00, 0x40, 0x8D,0x14, 0xA4,0xAF
};
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]));
	//i2c_stop();
	// clear
	i2c_start();
	i2c_wr_u8(OLED_ADDR);
	i2c_wr_u8(0x40);
	for(uint16_t col=(8*132); col--;) i2c_wr_u8(0);
	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 x,uint8_t y, uint8_t sx,uint8_t sy, uint8_t c){
	for(uint8_t row=0; row<sy; row++){
//		oled_goto(x,y+row);
		// goto x,y
		i2c_start();
		i2c_wr_u8(OLED_ADDR);
		i2c_wr_u8(0);
		i2c_wr_u8(0xB0 + y + row);
		i2c_wr_u8(((x & 0xF0) >> 4) | 0x10);
		i2c_wr_u8(x & 0xF);
		i2c_stop();
		// 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[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=sy; j--;){
					ccc >>= 1;
					if(cc & 1) ccc |= 0x80;
				}
			}
			for(uint8_t i=sx; i--; ) i2c_wr_u8(ccc);
		}
		i2c_stop();
	}
}

void oled_c1(uint8_t c){
	for(uint8_t row=0; row<oled_sy; row++){
//		oled_goto(x,y+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 & 0xF0) >> 4) | 0x10);
		i2c_wr_u8(oled_x & 0xF);
		i2c_stop();
		// 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();
	}
	for(uint8_t i=oled_sx; i--;) oled_x += 8;
}