#include <string.h>

#include "mc8051fun/mc8051fun.h"
#include "mc8051fun/lcd.h"
#include "mc8051fun/bcd.h"


#define BUTTONS P2
#define BUTTONS2 P0

#define NBYTES 4
#define BCDBYTES (10)
#define BCDEXPBYTES 2

__xdata uint8_t val0[NBYTES];
__xdata uint8_t val1[NBYTES];
__xdata uint8_t result[NBYTES];
__xdata uint8_t rest[NBYTES];
__xdata uint8_t bcd[BCDBYTES];

#define clearbcdfloat(v) memset(v,0,sizeof(bcdfloat_T))

//uint8_t bcd_val0[BCDBYTES]={0x03,0x00,0x00,0x00};
//uint8_t bcd_val0[BCDBYTES]={0x91,0x99};
//uint8_t bcd_val0[BCDBYTES]={0x17,0x00};
uint8_t bcd_val0[BCDBYTES]={0x31,0x01,0x00,0x00};
uint8_t bcd_val1[BCDBYTES]={0x31,0x01,0x00,0x00};

typedef struct {
	uint8_t m[BCDBYTES];
	int8_t e; //[BCDEXPBYTES];
} bcdfloat_T;

bcdfloat_T vals[3];
uint8_t valp=0;

char ops[3];
uint8_t opsp=0;


uint8_t buffer[BCDBYTES*2];

__idata bcdfloat_T ans;
__idata char input[16+1];
__idata uint8_t input_pos=0;


static void printbcdval(__idata uint8_t * bcd);


void bcd_shl(__idata uint8_t *v,uint8_t l) __reentrant
{
	(void)v;(void)l;

	__asm

	mov	a,_bp
	add	a,#0xfd
	mov	sp,a

	mov	r0,dpl
	pop	ar7

	mov	b,#0x00
001$:
	mov	a,@r0
	swap	a
	push	ACC
	anl	a,#0xf0
	orl	a,b
	mov	@r0,a
	pop	ACC
	anl	a,#0x0f
	mov	b,a
	inc	r0
	djnz	r7,001$

	mov	sp,_bp

	__endasm;
}



void strtobcdfloat(__idata char *str, __idata bcdfloat_T *v)
{
	char sign = ' ';
	uint8_t dp=0;

	clearbcdfloat(v);

	if (*str==0)
		return;


	if (*str=='-' || *str=='+'){
		sign=*str;
		str++;
	}


	while(*str=='0')
		str++;
	
	int p=BCDBYTES*2-2;
	v->e=-1;
	while(*str){
		uint8_t c=*str;

		if (c!='.'){
			c-=0x30;
			if((p&0x01)){
				c=c<<4;
			}
			v->m[(p>>1)]|=c;
			p--;
			if (!dp)
				v->e++;
		}
		else{
			dp=1;
		}
		str++;
	}

	if (sign=='-'){
		bcd_invert(v->m,BCDBYTES);
	}

}

void bcd_div10(__idata uint8_t *v, uint8_t len)
{
	uint8_t b = bcd_getsig(v,len) ? 0x90 : 0x00;
	bcd_shr(v,len,b);
}


uint8_t bcdfloat_is0(__idata bcdfloat_T *v)
{
	for(int8_t i=0; i<BCDBYTES; i++){
		if (v->m[i])
			return 0;
	}
	return 1;
}

void bcdfloat_normalize(__idata bcdfloat_T *v) 
{
	if (bcdfloat_is0(v))
		return;

	uint8_t s = v->m[BCDBYTES-1]&0xf0;
	if (s!=0x90 && s!=0x00){
		bcd_div10(v->m,BCDBYTES);
		v->e++;
		return;
	}

	uint8_t c = s>>4;
	for(int8_t i=0; i<BCDBYTES*2; i++){
		s = v->m[BCDBYTES-1]&0x0f;
		if (s!=c)
			return;
		bcd_shl(v->m,BCDBYTES);
		v->e--;
	}	

}


/*
int8_t bcd_cmp(__idata uint8_t *v1, __idata uint8_t * v2, uint8_t len)
{
	uint8_t s1,s2;
	s1=bcd_getsig(v1,len);
	s2=bcd_getsig(v2,len);
	if (s1 && !s2)
		return -1;
	if (s2 && !s1)
		return 1;


	for(int8_t i=len-1; i>=0; i--){
		if (v1[i]<v2[i]){
			return -1;
		}
		if (v1[i]>v2[i]){
			return 1;
		}
	}
	return 0;
}
*/


void bcdfloat_zero(__idata bcdfloat_T *f)
{
	memset(f,0,sizeof(bcdfloat_T));
}




static void init (void)
{
	lcd_cmd(LCD_SET_FUNCTION | LCD_FUNCTION_8BIT | LCD_FUNCTION_2LINE);
	lcd_cmd(LCD_SET_DISPLAY | LCD_DISPLAY_ON | LCD_CURSOR_ON | LCD_BLINKING_ON);
	lcd_cmd(LCD_CLEAR_DISPLAY);
	lcd_cmd(LCD_SET_DDADR);
}

static void putchar(char c)
{
	lcd_chr(c);
}


static void lcd_str (char *p)
{
	while(*p){
		putchar(*p++);
	}
}

static void zero_val(__xdata uint8_t * ptr)
{
	for(int i=0; i<NBYTES; i++){
		ptr[i]=0;
	}
}

/*
static void val0tobcd()
{
	__asm
	mov	r7,#NBYTES
	mov	r6,#BCDBYTES
	mov	r0,#_val0
	mov	r1,#_bcd
	acall	long_xtobcd
	__endasm;
}

static void resulttobcd()
{
	__asm
	mov	r7,#NBYTES
	mov	r6,#BCDBYTES
	mov	r0,#_result
	mov	r1,#_bcd
	acall	long_xtobcd
	__endasm;
}

*/

static void printbcdval(__idata uint8_t * bcd)
{
	uint8_t b;
	b = bcd[BCDBYTES-1]&0xf0;
	if (b>=0x50){
		bcd_invert(bcd,BCDBYTES);
		putchar('-');
	}


	for(int i=(BCDBYTES-1); i>=0; i--){
		b=bcd[i]>>4;
		putchar(b+0x30);
		b=bcd[i]&0xf;
		putchar(b+0x30);
	}
}


static void printbcd(__idata uint8_t *v,int8_t len, int8_t dp)
{

	uint8_t s = bcd_getsig(v,len);
	if(s){
		putchar('-');
		bcd_invert(v,len);
	}

	uint8_t b;

//	putchar('0');
//	putchar('.');
//
	uint8_t z=0;

	if (dp<0){
		putchar('0');
		putchar('.');
		for(int8_t i=dp+1; i<0; i++){
			putchar('0');
		}
		z=1;
	}

	int8_t lastz;
	int i;
	for (i=0; i<(len<<1); i++){
		register uint8_t in = i>>1;
		uint8_t b = (i&0x01)==0 ? v[in]&0x0f : (v[in]&0xf0)>>4;
		if (b!=0){
			break;
		}
	}
	lastz=(len<<1)-i;



	for (int8_t i=1; i<(len<<1); i++){
		register uint8_t in = len-1-(i>>1);
		uint8_t b = i&0x01 ? v[in]&0x0f : (v[in]&0xf0)>>4;
		if (b==0){
		       	if (i==lastz && dp<i-1)
				return;
		}

		putchar(b+0x30);
		if(dp==i-1){    
			if ( i < lastz-1)  
				putchar('.');
			else{
				return;
			}
		}
	}
//	if (!z)
//		putchar('0');
}



static void printbcdfloat(__idata bcdfloat_T * v)
{
	bcdfloat_normalize(v);
	if ( v->e<4 && v->e>-4){
		printbcd(v->m,BCDBYTES,v->e);
	}
	else {
		printbcd(v->m,BCDBYTES,0);
		putchar('E');
		if (v->e&0x80){
			v->e=255-v->e+1;
			long_tobcd(&v->e,buffer,1,2);
			bcd_invert(buffer,2);
			v->e=255-v->e+1;
		}else{
			long_tobcd(&v->e,buffer,1,2);
		}
		printbcd(buffer,2,4);


	}

	return;

	if (v->e&0x80){
		v->e=255-v->e+1;
		long_tobcd(&v->e,buffer,1,2);
		bcd_invert(buffer,2);
		v->e=255-v->e+1;
	}else{
		long_tobcd(&v->e,buffer,1,2);
	}

	printbcd(buffer,2,0);
	return;


	printbcd(v->m,BCDBYTES,0);
	return;

	uint8_t s = bcd_getsig(v->m,BCDBYTES);
	uint8_t b;
	if (s){
		bcd_invert(v->m,BCDBYTES);
		putchar('-');
	}
	for(int i=(BCDBYTES-1); i>=0; i--){
		b=v->m[i]>>4;
		putchar(b+0x30);
		b=v->m[i]&0xf;
		putchar(b+0x30);
	}
	s = bcd_getsig(v->e,BCDEXPBYTES);
	putchar('E');
/*	if (s){
		bcd_invert(v->e,BCDEXPBYTES);
		putchar('-');
	}
	for(int i=(BCDEXPBYTES-1); i>=0; i--){
		b=v->e[i]>>4;
		putchar(b+0x30);
		b=v->e[i]&0xf;
		putchar(b+0x30);
	}
*/

}


/*
static void val0val1_add()
{
	__asm
	mov	r0,#_val0
	mov	r1,#_val1
	mov	r7,#NBYTES
	lcall	long_xadd
	__endasm;
}*/

/*
static int8_t muxx(__idata uint8_t *v1, __idata uint8_t *v2, uint8_t len) __using 2
{
	ACC = (uint8_t)v1;
	P2 = (uint8_t)v2;

	return 1;
}*/
/*
uint8_t bsig(__idata uint8_t*v, uint8_t len)
{
	return v[len-1]>=0x50;
}
*/

uint8_t bcd_add_signed(__idata uint8_t *v1,__idata uint8_t *v2, uint8_t len) 
{
	uint8_t s1,s2;
	s1 = bcd_getsig(v1,len);
	s2 = bcd_getsig(v2,len);
	(void)bcd_add(v1,v2,len);
	if (s1^s2){
		return 0;
	}
	return s1 != bcd_getsig(v1,len);
}


void bcdfloat_add(__idata bcdfloat_T *v1,__idata bcdfloat_T *v2)
{

	bcdfloat_normalize(v1);
	bcdfloat_normalize(v2);

//	lcd_cmd(LCD_GOTO_LINE1);
//	printbcdval(v2->m);
//	while(1);

	__idata bcdfloat_T *va,*vb;
	if (v1->e < v2->e){
		va=v1;vb=v2;
	} 
	else {
		vb=v1;va=v2;
	} 

	for(int8_t i=0; i<BCDBYTES*2; i++){
		if(va->e==vb->e)
			break;
		bcd_div10(va->m,BCDBYTES);
		va->e++;
	}

	bcd_add(v1->m,v2->m,BCDBYTES);
	bcdfloat_normalize(v1);


//	int8_t s = bcd_cmp(v1->e,v2->e,BCDEXPBYTES);
	
}


static const char bm[4][4] = {
	{'7','8','9','/'},
	{'4','5','6','*'},
	{'1','2','3','-'},
	{'0','.','=','+'}
};

uint8_t mtoint(uint8_t b){
	uint8_t r=0;
	for(int8_t i=0; i<4; i++){
		if (!(b&(8>>i))){
			if (r!=0)
				return 0xff;
			r=i+1;
		}	
	}
	return r;
}

static uint8_t v1=0;
static uint8_t ctr=0;

static uint8_t debounce(uint8_t v,uint8_t n)
{
	if (v==v1){
		if(ctr>n)
			return 1;
		ctr++;
		return 0;
	}	
	ctr=0;
	v1=v;
	return 0;
}

static char read_button()
{
	uint8_t b;
	uint8_t x,y;

	BUTTONS=0x0f;
	b = BUTTONS;
	x = mtoint(b&0x0f); 

	if (x==0)
		return 0;

	BUTTONS=0xf0;
	b |=BUTTONS;
	y = mtoint(b>>4);

	if (y==0)
		return 0;
	
	return (bm[x-1][y-1]);
}

static char read_button2()
{
	uint8_t b=P0;
	P3_0=1;
	P3_1=1;
	P3_2=1;
	P3_3=1;
	b =0;
	if(!P3_0)
		b= 1;
	if(!P3_1)
		b= 2;
	if(!P3_2)
		b= 3;
	if(!P3_3)
		b= 4;

	return b;



	P0_3=0;
	b = P0_3;
	return 1-b;

	if (b) return 1;
	return 0;
//	return b>>4;
}

void inputchar(char c)
{
	input[input_pos]=c;
	input_pos++;

	input[input_pos]=0;
	putchar(c);
}

void inputreset()
{
	input_pos=0;
	input[0]=0;
	lcd_cmd(LCD_GOTO_LINE1);
	for(int8_t i=0;i<16;i++)
		putchar(' ');
	lcd_cmd(LCD_GOTO_LINE1);

}

void update_ans(__idata bcdfloat_T *v)
{
	lcd_cmd(LCD_GOTO_LINE2);
	for(int8_t i=0;i<16;i++)
		putchar(' ');
	lcd_cmd(LCD_GOTO_LINE2);
	printbcdfloat(v);
}


void bcd_mulab__ () __naked
{
	__asm
	.globl	bcd_mulab
bcd_mulab:
	push	AR7
	mov	r7,a
	mov	a,#0x00
001$:
	add	a,b
	da 	a
	djnz	r7,001$
	pop	AR7

	ret

	__endasm;

}


//void doop(__idata bcdfloat_T *v1,__idata bcdfloat_T *v2, char op)
void doop(uint8_t n)
{
	__idata bcdfloat_T *v1;
	__idata bcdfloat_T *v2;

	char op;

	v1=&vals[n-1];
	v2=&vals[n];
	op = ops[n-1];

	ops[n-1]=ops[n];

//	P1=op;

	
	switch(op){
	case '+':
		P1=0x01^0xff;
		bcdfloat_add(v1,v2);
		break;
	case '-':
		P1=0x02^0xff;
		bcd_invert(v2->m,BCDBYTES);
		bcdfloat_add(v1,v2);
		break;

	default:
		break;

	}
}
void flushops()
{
	if(valp<2)
		return;
	doop(valp-1);
	valp--;

			
	
		
}


void bcd_mul_byte__ () __naked{
	__asm
	mov	r4,#0x00

	mov	a,@r0
	anl	a,#0x0f
	lcall	bcd_mulab
	add	a,r4
	da	a
	push	ACC
	swap	a
	anl	a,#0x0f
	mov	r4,a
	pop	ACC

	mov	r3,a
	mov	a,@r0
	swap	a
	anl	a,#0x0f
	lcall	bcd_mulab



	__endasm;
}

/*
r0 = bcd
r1 = bin
r7 = len of bcd
r6 = len of bin
*/
 
void bcd_tolong__() __naked
{

	__asm

	.globl	bcd_tolong
bcd_tolong:
	mov	a,AR6		; use r4 as counter
	mov	b,#0x08
	mul	ab
	mov	r4,a


000$:
	push	AR0
	push	AR7
	clr	c

	lcall	long_rrc
	mov	ar0,AR1
	mov	ar7,AR6

	lcall	long_rrc
	pop	AR7
	pop	AR0
	push	AR0
	push 	AR7
	
001$:
	mov	a,@r0
	acall	003$
	acall	003$
	mov	@r0,a

	inc 	r0
	djnz	r7,001$
	pop	AR7
	pop	AR0

	djnz	ar4,000$

	
	ret				


003$:
	swap	a
	clr	c
	subb	a,#0x80
	jc	002$
	subb	a,#0x30
002$:
	add	a,#0x80
	ret
	
	__endasm;

}


/*
 * C-Wrapper for long_tobcd
 */

void bcd_tolong(__idata uint8_t *bcdval, __idata uint8_t *binval, uint8_t bcdlen, uint8_t binlen) __reentrant
{
	(void)binval;
	(void)bcdval;
	(void)binlen;
	(void)bcdlen;

	__asm
	mov	a,_bp
	add	a,#0xfd
	mov	r0,a
	mov	a,@r0		; binval
	mov	r1,a		
	dec	r0
	mov	a,@r0		; bcdlen
	mov	r7,a
	dec	r0		
	mov	a,@r0		; binlen
	mov	r6,a
	mov	r0,dpl		; bcdval
	lcall	bcd_tolong
			
	__endasm;
}


uint8_t tbcd[3]={0x54,0x02,0x00};
uint8_t	tbin[1]={0x00};

void main()
{
	init();

	memset(&vals[0],0,sizeof(bcdfloat_T));
	memset(&vals[1],0,sizeof(bcdfloat_T));
	memset(&vals[1],0,sizeof(bcdfloat_T));

	clearbcdfloat(&ans);

/*	
	__asm
	mov	a,#9
	mov	b,#4
	lcall	bcd_mulab
	xrl	a,#0xff
	mov	_P1,a
	__endasm;


	while(1);
*/
	lcd_cmd(LCD_GOTO_LINE2);
	printbcdfloat(&ans);
	lcd_cmd(LCD_GOTO_LINE1);


	bcd_tolong(tbcd,tbin,3,1);
	P1=tbin[0]^0xff;
	while(1);
	

	char lastc = 0;

#define STAT_1 1

	uint8_t stat=0;
	while(1){
		P0=0xf0;
	//	P1=P0;
		char c = read_button();
		if (!c)
			c=read_button2();
		if (!debounce(c,100))
			continue;
		
		if (lastc==c)
			continue;
		lastc=c;
		if (!c)
			continue;
		//P1 = c^0xff;

		if (c=='-' || c=='+' ){
			//P1=input_pos^0xff;
			if(input_pos==0){
				inputchar(c);
				continue;
			}
		}
	
		if (c=='='){
			strtobcdfloat(input,&vals[0]);
			lcd_cmd(LCD_GOTO_LINE2);
			bcdfloat_add(&ans,&vals[0]);
			update_ans(&ans);
			inputreset();
			continue;
		}

		if (c=='+' || c=='-'){
			strtobcdfloat(input,&vals[valp]);
			ops[valp]=c;
			valp++;
			//P1=valp^0xff;
			flushops();
			update_ans(&vals[0]);
			inputreset();
			continue;
		}


		inputchar(c);
		continue;


		if (c<0x10){
			if (c==1){
				lcd_cmd(LCD_SET_SHIFT);
				putchar(' ');
				lcd_cmd(LCD_SET_SHIFT);

			}
			continue;
		}
		putchar(c);

	}


	while(1);

}