#include <pic18fregs.h>
#include <stdio.h> 
#include <math.h> 
#include <my_serial.h> 
#include <my_serial2.h> 

extern char stack; 
extern char stack_end;

#pragma stack 0x100 256

void _reset (void) __naked __interrupt 0;
void _startup (void) __naked;

static char counter_2hz;	       
static volatile short counter_10hz;	       
static volatile unsigned char softintrs;
#define INT_10HZ	 (unsigned char)0x01
#define INT_AD		 (unsigned char)0x02
#define INT_RX1		 (unsigned char)0x08
#define INT_RX1OF	 (unsigned char)0x10

static volatile unsigned char status;
#define STAT_WAIT	(unsigned char)0x00
#define STAT_MEASURE	(unsigned char)0x01
#define STAT_EXIT	(unsigned char)0x02
#define STAT_CONSOLE	(unsigned char)0x04

static volatile unsigned char adstatus;
#define ADSTAT_MEASURE	(unsigned char)0x01
static unsigned char ad_resh;
static unsigned char ad_resl;
static unsigned char ad_channel;


#define TIMER0_5MS 192 /* 48 without PLL */

#define TIMER2_10HZ 1000

#define LEDR LATAbits.LATA7
#define LEDG LATAbits.LATA6
#define PWRON LATCbits.LATC2
#define RL1 LATCbits.LATC4
#define RL2 LATCbits.LATC5
#define SENSE PORTAbits.RA5

#define CLRWDT __asm__("clrwdt")      
#define SLEEP __asm__("sleep")      

static void do_measure(void);
static void do_console(void);
static void do_cal_data(void);
static void parse_rx(void);

static char buf[84];

/* address of calibration data in RAM */
#define cal_data 0x01F000


#if 1
#define PRINTHEX(v) \
	{ \
		unsigned char c = (v); \
		if (c < 10) { \
			uart_putchar_hard('0' + c); \
		} else { \
			uart_putchar_hard(('a' - 10) + c ); \
		} \
	}
#else
#define PRINTHEX(v) {}
#endif

void
main(void) __naked
{
	softintrs = 0;

	ANCON0 = 0xf0; /* an0-an3 analog, an4-an7 digital */
	ANCON1 = 0x3f; /* an8-12 digital */

	RL1 = 0;
	TRISCbits.TRISC4 = 0;
	RL2 = 0;
	TRISCbits.TRISC5 = 0;
	PWRON = 0;
	TRISCbits.TRISC2 = 0;
	LEDR = 1;
	TRISAbits.TRISA7 = 0;
	LEDG = 0;
	TRISAbits.TRISA6 = 0;

	/* switch PLL on */
	OSCTUNEbits.PLLEN = 1;

	/* configure sleep mode: PRI_IDLE */
	OSCCONbits.SCS = 0;
	OSCCONbits.IDLEN = 1;
	/* everything is low priority by default */
	IPR1 = 0;
	IPR2 = 0;
	IPR3 = 0;
	IPR4 = 0;
	IPR5 = 0;
	INTCON = 0;
	INTCON2 = 0;
	INTCON3 = 0;
	INTCON2bits.RBPU = 1;

	RCONbits.IPEN=1; /* enable interrupt priority */

	/* configure timer0 as free-running counter at 46.875Khz */   
	T0CON = 0x07; /* b00000111: internal clock, 1/256 prescaler */  
	INTCONbits.TMR0IF = 0;
	INTCONbits.TMR0IE = 0; /* no interrupt */
	T0CONbits.TMR0ON = 1;

#if 0
	/* configure UART for 57600Bps at 48Mhz */
	SPBRG1 = 12;
#endif
	/* configure UART for 921600Bps at 48Mhz */
	TXSTA1bits.BRGH = 1;
	BAUDCON1bits.BRG16 = 1;
	SPBRGH1 = 0;
	SPBRG1 = 12;

	/* pre-configure UART2 */
	/* turn off PPS write protect */
	__asm
	banksel _PPSCON
	movlw   0x55
	movwf   _EECON2, a
	movlw   0xAA
	movwf   _EECON2, a 
	BCF     _PPSCON, _IOLOCK, b
	__endasm;

	TRISCbits.TRISC0 = 1;
	LATCbits.LATC1 = 1;
	TRISCbits.TRISC1 = 0;
	RPINR16 = 11; /* RC0 = RX */
	RPOR12 = 6; /* RC1 = TX */
	TXSTA2 = 0x24; /* TXEN, BRGH set, others clear */
	RCSTA2 = 0x90; /* UART enable, receiver disabled */
	BAUDCON2 = 0x08; /* BRG16 */

	USART_INIT(0);
	USART2_INIT(0);

	stdout = STREAM_USER; /* Use the macro PUTCHAR with printf */

#if TIMER2_10HZ == 100
	/* configure timer2 for 1Khz interrupt */
	T2CON = 0x22; /* b00100010: postscaller 1/5, prescaler 1/16 */
	PR2 = 150; /* 1khz output */
#elif TIMER2_10HZ == 1000
	/* configure timer2 for 10Khz interrupt */
	T2CON = 0x22; /* b00100010: postscaller 1/5, prescaler 1/16 */  
	PR2 = 15; /* 10khz output */
#elif TIMER2_10HZ == 2000
	/* configure timer2 for 20Khz interrupt */
	T2CON = 0x21; /* b00100001: postscaller 1/5, prescaler 1/4 */ 
	PR2 = 29; /* 20khz output */
#else 
#error "unknown TIMER2_10HZ"
#endif
	counter_10hz = TIMER2_10HZ;
	T2CONbits.TMR2ON = 1;
	PIR1bits.TMR2IF = 0;
	IPR1bits.TMR2IP = 1; /* high priority interrupt */
	PIE1bits.TMR2IE = 1;

	status = STAT_WAIT;
	adstatus = 0;

	INTCONbits.GIE_GIEH=1;  /* enable high-priority interrupts */   
	INTCONbits.PEIE_GIEL=1; /* enable low-priority interrrupts */   

	/*
	 * configure ADC:
	 * AN3: vref+
	 * AN2: I
	 * AN1: vout
	 */

	ADCON0 = 0xc1; /* b11000001 */
	/* clk = fosc/64, tacq = 4tad (5.33us) */
	ADCON1 = 0x96; /* b10010110 */
	/* ANCON already set up */

	/* start calibration */
	ADCON1bits.ADCAL = 1;
	ADCON0bits.GO_NOT_DONE =1;
	while (ADCON0bits.GO_NOT_DONE)
		; /* wait */
	ADCON1bits.ADCAL = 0;
	PIR1bits.ADIF = 0;
	PIE1bits.ADIE = 1;

	printf("hello world\n");
	/* enable watch dog timer */  
	WDTCON = 0x01;
	LEDR = 1;
	LEDG = 1;
	counter_2hz = 5;
	while (counter_2hz > 0) {
		if (softintrs & INT_10HZ) {
			softintrs &= ~INT_10HZ;
			counter_2hz--;
		}
	}
	counter_2hz = 5;
	LEDR = 0;
	LEDG = 0;

	printf("\nready\n");
	ad_channel = 0;
	ADCON0bits.CHS = 0;
	PWRON = 0;

	while ((status & STAT_EXIT) == 0) {
		CLRWDT;
		if (softintrs & INT_10HZ) {
			softintrs &= ~INT_10HZ;
			counter_2hz--;
			if (counter_2hz == 0) {
				counter_2hz = 5;
				LEDG ^= 1;
			}
		}
		if (softintrs & INT_RX1) {
			parse_rx();
		} else {
			SLEEP;
			continue;
		}
		if (status & STAT_MEASURE)
			do_measure();
		else if (status & STAT_CONSOLE)
			do_console();
	}
	printf("returning\n");
	while (PIE1bits.TX1IE)
		; /* wait for transmit to complete */
	INTCONbits.PEIE=0; /* disable peripheral interrupts */
	INTCONbits.GIE=0;  /* disable interrrupts */
}

static void
parse_rx()
{
	char c;
	char c2;
	char ok = 1;
	char err = 0;
	long br;
	short brgreg = 0;

	softintrs &= ~INT_RX1;
	c = uart_getchar();
	switch(c) {
	case 'B':
		c = uart_getchar();
		br = 0;
		while(c != '\n') {
			if (c < '0' || c > '9') {
				err = 1;
				break;
			}
			br = br * 10 + c - '0';
			c = uart_getchar();
		}
		if (br > 0) {
			/*
			 * brg = F / 16 / (n + 1)
			 * brg * (n + 1) = F / 16
			 * n + 1 = F / 16 / brg
			 * n = F / 16 / brg - 1
			 * with F = 48Mhz
			 * n = 3000000 / brg - 1
			 */
			brgreg = (12000000L + br / 2) / br - 1;
			printf("brgreg %d\n", brgreg);
		}
		if (err == 0) {
			if (br > 0) {
				SPBRGH2 = (brgreg >> 8);
				SPBRG2 = (brgreg & 0xff);
			}
			status = STAT_CONSOLE;
		}
		break;
	case 'C':
		do_cal_data();
		c = '\n';
		break;
	case 'E':
		status = STAT_EXIT;
		break;
	case 'M':
		c = uart_getchar();
		if (c == '0') {
			if (status == STAT_MEASURE)
				ok = 0;
			status = STAT_WAIT;
		} else if (c == '1') {
			status = STAT_MEASURE;
		} else {
			err = 1;
		}
		break;
	case 'P':
		c = uart_getchar();
		if (c == '0')
			PWRON = 0;
		else if (c == '1')
			PWRON = 1;
		else
			err = 1;
		break;
	case 'R':
		c = uart_getchar();
		if (c != '\n') {
			c2 = uart_getchar();
			if (c == '1') {
				if (c2 == '0')
					RL1 = 0;
				else if (c2 == '1')
					RL1 = 1;
				else
					err = 1;
			} else if (c == '2') {
				if (c2 == '0')
					RL2 = 0;
				else if (c2 == '1')
					RL2 = 1;
				else
					err = 1;
			} else {
				err = 1;
			}
			c = c2;
		}
		break;
	case 'S':
		printf("power %s RL %s %s SENSE %s GPIO 0x%x\n",
		    PWRON ? "on" : "off",
		    RL1 ? "on" : "off",
		    RL2 ? "on" : "off",
		    SENSE ? "off" : "on",
		    PORTB);
		break;
	default:
		err = 1;
		break;
	}
	while (c != '\n')
		c = uart_getchar();
	if (err)
		printf("\nERROR\n");
	else if (ok)
		printf("\nOK\n");

	if (softintrs & INT_RX1OF) {
		PIE1bits.RC1IE = 0;
		softintrs &= ~INT_RX1OF;
		softintrs &= ~INT_RX1;
		uart_rxbuf_prod = uart_rxbuf_cons = 0;
		PIE1bits.RC1IE = 1;
	}
}

static void
printhex(unsigned char c) __wparam __naked
{
	(void)c;
	__asm
	andlw 0x0f;
	sublw 9;
	bc decimal;
	sublw '@';
	goto _uart_putchar_hard;
decimal:
	sublw '9';
	goto _uart_putchar_hard;
	__endasm;
}

static void
do_measure()
{
	softintrs &= ~INT_AD;
	adstatus = ADSTAT_MEASURE;
	ADCON0bits.CHS = 0;

	LEDR = 0;
	LEDG = 1;
	UART_FLUSH();
	uart_putchar_hard('X');
	while (status & STAT_MEASURE) {
		CLRWDT;
		if (softintrs & INT_AD) {
			if (ad_channel == 0) {
				printhex(PORTB >> 4);
				printhex(PORTB);
				printhex(ad_resh);
				printhex(ad_resl >> 4);
				printhex(ad_resl);
			} else {
				printhex(ad_resh);
				printhex(ad_resl >> 4);
				printhex(ad_resl);
				uart_putchar_hard('X');
			}
			softintrs &= ~INT_AD;
		}
		if (softintrs & INT_RX1) {
			parse_rx();
		} else {
			SLEEP;
		}
	}
	adstatus = 0;
	printf("\nOK\n");
}

static void
do_console()
{
	char previous_rx1 = 0;
	char c;
	char more_work = 0;

	printf("connecting to console - exit with #.\n");
	/* clear buffer */
	PIE3bits.RC2IE = 0;
	uart2_rxbuf_prod = uart2_rxbuf_cons = 0;
	PIE3bits.RC2IE = 1;
	RCSTA2bits.SPEN = 1;
	while ((status & STAT_CONSOLE) || more_work) {
		CLRWDT;
		more_work = 0;
		if (softintrs & INT_10HZ) {
			softintrs &= ~INT_10HZ;
			LEDG ^= 1;
		}
		if (uart_rxbuf_cons != uart_rxbuf_prod) {
			c = uart_getchar();
			/*
			 * #. exits console mode
			 * ## sends #
			 * anything else send both char unmodified
			 */
			if (previous_rx1 == '#') {
				if (c == '.') {
					status = STAT_WAIT;
				} else if (c == '#') {
					uart2_putchar_raw(c);
				} else {
					uart2_putchar_raw(previous_rx1);
					uart2_putchar_raw(c);
				}
			} else {
				uart2_putchar_raw(c);
			}
			previous_rx1 = c;
			more_work = 1;
		}
		if (uart2_rxbuf_cons != uart2_rxbuf_prod) {
			c = uart2_getchar();
			uart_putchar_raw(c);
			more_work = 1;
		}
		if (more_work == 0 && (status & STAT_CONSOLE))
			SLEEP;
	}
	RCSTA2bits.SPEN = 0;
	printf("exit from console\n");
}

static void
do_write(void)
{
	EECON2 = 0x55;
	EECON2 = 0xaa;
	EECON1bits.WR = 1;
	while (EECON1bits.WR)
		; /* wait */
}

static void
do_cal_data()
{
	char i = 0;
	char err = 0;
	char c;

	c = uart_getchar();
	while (c != '\n') {
		if ((c < '0' || c > '9') && c != ' ' && c != '.') {
			printf("cal error at %c (%d)\n", c, i);
			err = 1;
		}
		if (i > 82) {
			printf("cal error at %c (%d)\n", c, i);
			err = 1;
		} else {
			buf[i] = c;
			i++;
		}
		c = uart_getchar();
	}
	if (err == 0 && i != 0 && i != 83) {
		printf("cal error: %d\n", i);
	} else if (err == 0 && i != 0) {
		/* erase 1k block */
		INTCONbits.GIE_GIEH=0; /* disable interrupts */
		INTCONbits.PEIE_GIEL=0;
		TBLPTRU = ((long)cal_data >> 16) & 0xff;
		TBLPTRH = ((long)cal_data >> 8) & 0xff;
		TBLPTRL = (long)cal_data & 0xff;
		EECON1 = 0x14; /* enable write+erase */
		do_write();
		EECON1 = 0x00; /* disable write */
		for (i = 0; i < 83; i++) {
			TABLAT = buf[i];
			__asm__("tblwt*+");
			if (i == 63) {
				__asm__("tblrd*-");
				EECON1 = 0x04; /* enable write */
				do_write();
				EECON1 = 0x00; /* disable write */
				__asm__("tblrd*+");
			}
		}
		__asm__("tblrd*-");
		EECON1 = 0x04; /* enable write */
		do_write();
		EECON1 = 0x00; /* disable write */
		INTCONbits.PEIE_GIEL=1;
		INTCONbits.GIE_GIEH=1; /* enable interrupts */
	}
	printf("cal_data ");
	TBLPTRU = ((long)cal_data >> 16) & 0xff;
	TBLPTRH = ((long)cal_data >> 8) & 0xff;
	TBLPTRL = (long)cal_data & 0xff;
	EECON1 = 0x00; /* disable writes */
	for (i = 0; i < 83; i++) {
		__asm__("tblrd*+");
		putchar(TABLAT);
	}
	printf("\n");
}

unsigned short
timer0_read() __naked   
{
	/* return TMR0L | (TMR0H << 8), reading TMR0L first */
	__asm   
	movf    _TMR0L, w
	movff   _TMR0H, _PRODL
	return
	__endasm;       
}

/* Vectors */
void _reset (void) __naked __interrupt 0
{
	__asm__("goto __startup");
}


void _startup (void) __naked
{

  __asm
    // Initialize the stack pointer
    lfsr 1, _stack_end
    lfsr 2, _stack_end
    clrf _TBLPTRU, 0    // 1st silicon doesn't do this on POR
    
    // initialize the flash memory access configuration. this is harmless
    // for non-flash devices, so we do it on all parts.
    bsf _EECON1, 7, 0
    bcf _EECON1, 6, 0
    __endasm ;

  /* Call the user's main routine */
  main();
  __asm__("reset");
}

/*
 * high priority interrupt. Split in 2 parts; one for the entry point
 * where we'll deal with timer0, then jump to another address
 * as we don't have enough space before the low priority vector
 */
void _irqh (void) __naked __shadowregs __interrupt 1
{
	__asm
	bcf   _PIR1, 1
	goto _irqh_timer2
	__endasm ;

}

void irqh_timer2(void) __naked
{
	/*
	 * no sdcc registers are automatically saved,
	 * so we have to be carefull with C code !
	 */
	counter_10hz--;
	if (counter_10hz == 0) {
		counter_10hz = TIMER2_10HZ;
		softintrs |= INT_10HZ;
	}			     
	if (adstatus & ADSTAT_MEASURE) {
		if (PIR1bits.ADIF) {
			LEDR = 1;
		}
		ADCON0bits.GO_NOT_DONE = 1;
	}
	__asm
	retfie 1
	nop
	__endasm;
}

void _irq (void) __interrupt 2 /* low priority */
{
	USART_INTR;
	USART2_INTR;
	if (PIE1bits.ADIE && PIR1bits.ADIF) {
		if (softintrs & INT_AD) {
			LEDR = 1;
		}
		ad_channel = ADCON0bits.CHS;
		ad_resl = ADRESL;
		ad_resh = ADRESH;
		/*
		 * needs 2Tac, or 32 instrutions
		 * before next sample. assume we'll
		 * have them at timer2 interrupt
		 */
		if (ad_channel == 0)
			ADCON0bits.CHS = 1;
		else
			ADCON0bits.CHS = 0;
		PIR1bits.ADIF = 0;
		softintrs |= INT_AD;
	}
}