#include <built_in.h>
#define PULSE_WIDTH 20000       //20ms

const char greeting[] = "PIC is running and awaiting commands.\r\n";
const char servo[]="Servoing to new angle!\r\n";
const char invalid[]="Invalid pulse length!\r\n";
char temp[45];
unsigned long pulseLength=1455;          //middle of robot
unsigned short state=0;
long time;

void strcp_c(char *str1, const char *cstr2) {
  unsigned short i = 0;

  do {
    str1[i] = cstr2[i];
  } while (str1[i++]);
}
void send(char *buffer)
{
    int i;
    for(i=0;i<strlen(buffer);i++)
    {
        USART_Write(buffer[i]);
    }
}
void process(char *buffer)
{
    long t=atoi(buffer);
    if(t>2200 || t<600)
    {
        strcp_c(temp,invalid);
        send(temp);
    }
    else
    {
        pulseLength=t;
        strcp_c(temp,servo);
        send(temp);
    }
}
void setupTimer()
{
    T1CON=((T1CON&11000101)|1);
    INTCON=(INTCON|0b11000000);
    PIE1=(PIE1|1);
}
void interrupt(void)
{
    if(PIR1&1)  //timer1 overflow
    {
        if(state==1)        //it was high
        {
            PORTC=0;        //bring it to low
            state=0;
            time=45535+pulseLength;
            TMR1L=Lo(time);
            TMR1H=Hi(time);
            state=0;
        }
        else
        {
            PORTC=1;
            time=65535-pulseLength;
            TMR1L=Lo(time);
            TMR1H=Hi(time);
            state=1;
        }
        PIR1=(PIR1&0b11111110);
    }
}
void main()
{
    unsigned short ch, buffer[30],ct=0;

    //OSCCON=(OSCCON|0b1110000);          //8MHz
    ANSEL=0;
    ANSELH=0;
    TRISC=0b100000;
    USART_Init(2400);
    //strcp_c(temp,setup);
    //send(temp);
    setupTimer();
    Delay_ms(1000);
    PORTC=0;

    strcp_c(temp,greeting);
    send(temp);
    while(1)
    {
        if(USART_Data_Ready())
        {
            ch=USART_Read();
            if(ct>30) continue;

            USART_Write(ch);            //Echo
            if(ch=='\r')
            {
                USART_Write('\n');
                buffer[ct++]=0;
                process(buffer);
                ct=0;
            }
            else buffer[ct++]=ch;
        }

    }
}