#include <16F648A.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)
#use rs232(baud=9600, rcv=PIN_B1, xmit=PIN_B2, parity=N, bits=8)
#use fast_io(A)

//#priority rda,timer1

#define INIT_WIDTH 400

#define PROPO_CHANNEL 6
#define SCHEDULE_SIZE ((PROPO_CHANNEL << 1) + 1)

#define TMR1L 0x0E
#define TMR1H 0x0F
#define T1CON 0x10   

unsigned char schedule_H[SCHEDULE_SIZE];
unsigned char schedule_L[SCHEDULE_SIZE];

void set_schedule(unsigned char target, unsigned long width){
	unsigned long temp_width;
	temp_width = ~((width * 5) >> 1);
	schedule_H[target] = (unsigned char)(temp_width >> 8);
	schedule_L[target] = (unsigned char)(temp_width & 0xFF);
}

void set_width(unsigned char buf){
	static unsigned char byte_count = 0;
	static unsigned char target_channel;
	static unsigned long new_width;

	switch(byte_count){
		case 0:
			if(buf < PROPO_CHANNEL){
				target_channel = buf;
				break;
			}
			putc('$');
			return;
		case 1:
			new_width = buf;
			new_width <<= 8;
			break;
		default:
			new_width |= buf;
			if((new_width >= 400) && (new_width <= 1600)){
				set_schedule((target_channel << 1) + 1, new_width);
				putc('!');
			}
			byte_count = 0;
			return;
	}
	byte_count++;
}

/*
#INT_RDA
void invoked_by_sci_rx(){
	set_width(getc());
}
*/

#INT_TIMER0
void invoked_by_timer0(){
	static unsigned char toggle = 0;
	if(toggle == 0){
		set_timer0(0); // tick=(0.2 * 256)us * 256 = 13.1072 ms
		toggle = 1;
	}else{
		set_timer0(161); // tick=(0.2 * 256)us * 95 = 4.864 ms
		toggle = 0;
		(*TMR1H) = 0xFA;
		(*TMR1L) = 0x00;
		(*T1CON) |= 0x01;
	}
}

#INT_TIMER1
void invoked_by_timer1(){
	static unsigned char target = 0;
	static unsigned char *p_schedule_H = schedule_H;
	static unsigned char *p_schedule_L = schedule_L;

	if(target & 0x01){
		output_high(PIN_A0);
	}else{
		output_low(PIN_A0);
	}
	if(target >= SCHEDULE_SIZE){
		(*T1CON) &= ~(0x01);
		target = 0;
		p_schedule_H = schedule_H;
		p_schedule_L = schedule_L;
		return;
	}
	
	if((*p_schedule_L) > ((*TMR1L) += (*p_schedule_L))){
		(*TMR1H) += ((*p_schedule_H) + 1);
	}else{
		(*TMR1H) += (*p_schedule_H);
	}
	
	target++; 
	p_schedule_H++;
	p_schedule_L++;
}

void main() {
	unsigned char i;

	set_tris_a(0x00);
	setup_timer_0(RTCC_INTERNAL | RTCC_DIV_256);

	(*TMR1L) = (*TMR1H) = 0;
	(*T1CON) = 0x10;


	for(i = 0; i < SCHEDULE_SIZE; i++){set_schedule(i, INIT_WIDTH);}

	// setup interrupts
	enable_interrupts(INT_TIMER0);
	enable_interrupts(INT_TIMER1);
	enable_interrupts(GLOBAL);

	set_timer0(0xA0);

	while(TRUE){set_width(getc());}
}