/*
char  = 8 bits
short = 16 bits
*/

/* Algorythims:
 *
 * The Rx PWM output is attached to inputs on port B.
 * 
 * A change of value on port B triggers an interrupt.
 * The ISR records (ie. saves in global variables):
 *    The value of the port B input.
 *    The time when the change happened.
 *    A flag signaling the ISR happened.
 * 
 * The program has an infinite main loop.
 * The main loop checks the flag set in the ISR and:
 *    If it was a rising edge, saves the time of the event.
 *    If it was a falling edge, calculates the delta time
 *    since the rising edge.  This is the input pulse width.
 *
 * The pulse width of 2 input channels is measured.
 * The 2 channels are mixed together, ie A+B and A-B,
 * and these values are used to set the 2 PWM outputs.
 *
 */
#define IN_DEAD 55 
#define OUT_DEAD 12

#define MIN_LIMIT 3000
#define MAX_LIMIT 10500

#define MIN_WIDTH 4700
#define MAX_WIDTH 8700

#define DIFF_WIDTH   4000
#define DIFF_WIDTH_2 2000

#define IN_MIN_DEAD 6645 /*MIN_WIDTH+DIFF_WIDTH_2-IN_DEAD*/
#define IN_MAX_DEAD 6755 /*MIN_WIDTH+DIFF_WIDTH_2+IN_DEAD*/

char in;
char inchangeX;
int timeX;
char inX;
char missingX;
char inchangeY;
int timeY;
char inY;
char missingY;
char PIR1_reg;

void interrupt(void){

   /* Port B input change */
   if(INTCON & 0x1){
   
      /* make sure the input has changed */
      in = input_pin_port_b(4);
      if(in != inX && inchangeX == 0){

         /* save the input pin value */
         inX = in;

         /* mark that there was an interrupt */
         inchangeX = 1;

         /* save the timer1 value */
         asm {
            bcf STATUS,RP0
            bcf STATUS,RP1

            ; disable the timer
            bcf T1CON, TMR1ON

            ; lo byte
            movf TMR1L, W
            movwf _timeX

            ; hi byte
            movf TMR1H, W
            movwf _timeX+D'1'

            ; enable the timer
            bsf T1CON, TMR1ON
         }

      }

      /* make sure the input has changed */
      in = input_pin_port_b(6);
      if(in != inY && inchangeY == 0){

         /* save the input pin value */
         inY = in;

         /* mark that there was an interrupt */
         inchangeY = 1;

         /* save the timer1 value */
         asm {
            bcf STATUS,RP0
            bcf STATUS,RP1

            ; disable the timer
            bcf T1CON, TMR1ON

            ; lo byte
            movf TMR1L, W
            movwf _timeY

            ; hi byte
            movf TMR1H, W
            movwf _timeY+D'1'

            ; enable the timer
            bsf T1CON, TMR1ON
         }

      }

      clear_bit(INTCON, RBIF);  // clear int flag
   }

   /* timer 1 overflow */
   asm {
      ; save the PIR1 register
      bcf STATUS,RP0
      bcf STATUS,RP1
      movf PIR1, W
      movwf _PIR1_reg
   }
   if(PIR1_reg & 0x1){
      if(missingX < 5){
         missingX++;
      }
      if(missingY < 5){
         missingY++;
      }

      asm {
         ; clear the interupt on timer1 overflow flag
         bcf STATUS,RP0
         bcf STATUS,RP1
         bcf PIR1, TMR1IF
      }
   }
}


main() {
   char instateX;
   char instateY;
   char oklo;
   char okhi;
   char okdiff;
   char change;
   int tmp1;
   int tmp2;
   int tmp3;
   int pstartX;
   int pstopX;
   int pstartY;
   int pstopY;
   int valX;
   int valY;
   char dead_input;
   char pwm1;
   char pwm2;

   /* setup the PWMs */
   asm {

      ; reset CCP module
      bcf STATUS, RP0
      bcf STATUS, RP1
      clrf CCP1CON
      clrf CCP2CON

      ; set PWM period 20kHz by writing to PR2
      bsf STATUS, RP0
      bcf STATUS, RP1
      movlw d'251'
      movwf PR2

      ; set PWM duty cycle by writing to CCPR1l and CCP1CON<5:4>
      bcf STATUS, RP0
      bcf STATUS, RP1
      movlw d'0'           ; 25=5uSon 125=25uSon 248=400nSoff
      movwf CCPR1L         ; 8 MSB's
      movwf CCPR2L         ; 8 MSB's
      bcf CCP1CON, CCP1X   ; LSB's
      bcf CCP1CON, CCP1Y   ; LSB's
      bcf CCP2CON, CCP1X   ; LSB's
      bcf CCP2CON, CCP1Y   ; LSB's

      ; make RC<2>/CCP1 RC<1>/CCP2 outputs by writing to TRISC
      bsf STATUS, RP0
      bcf STATUS, RP1
      bcf TRISC, 2
      bcf TRISC, 1

      ; reset the Timer 2
      bcf STATUS, RP0
      bcf STATUS, RP1
      clrf T2CON
      clrf TMR2

      ; enable TRM2 by writing to T2CON
      bcf STATUS, RP0
      bcf STATUS, RP1
      bsf T2CON, TMR2ON   ; enable

      ; configure CCP1 CCP2 module for PWM
      bcf STATUS, RP0
      bcf STATUS, RP1
      bsf CCP1CON, CCP1M3
      bsf CCP1CON, CCP1M2
      bsf CCP2CON, CCP2M3
      bsf CCP2CON, CCP2M2

   }

   /* setup Timer 1 */
   asm {

      ; initialize timer 1
      bcf STATUS,RP0
      bcf STATUS,RP1
      clrf T1CON
      bsf T1CON, TMR1ON   ; enable the timer

      ; clear the interupt on timer1 overflow flag
      bcf STATUS,RP0
      bcf STATUS,RP1
      bcf PIR1, TMR1IF

      ; enable interupt on timer1 overflow
      bsf STATUS,RP0
      bcf STATUS,RP1
      bsf PIE1, TMR1IE
   }

   /* setup port A */
   asm {

      ; initialize port A
      bcf STATUS,RP0
      bcf STATUS,RP1
      clrf PORTA

      ; configure ADC such that bits are digital inputs
      bsf STATUS,RP0
      bcf STATUS,RP1
      movlw 0x06
      movwf ADCON1

      ; set RA0, RA1, RA2, RA3, RA5 to be an outputs 
      ; bit = 0 -> output
      ; bit = 1 -> input
      bsf STATUS,RP0
      bcf STATUS,RP1
      bcf TRISA, 0
      bcf TRISA, 1
      bcf TRISA, 2
      bcf TRISA, 3
      bcf TRISA, 5
   }

   /* setup port B */
   asm {

      ; initialize port B
      bcf STATUS,RP0
      bcf STATUS,RP1
      clrf PORTB

      ; set RB4, RB6 to be an input 
      ; bit = 0 -> output
      ; bit = 1 -> input
      bsf STATUS,RP0
      bcf STATUS,RP1
      bsf TRISB, 4
      bsf TRISB, 6

      ; enable the weak pull up on the inputs
      ; bsf STATUS,RP0
      ; bcf STATUS,RP1
      ; bcf OPTION_REG, NOT_RBPU

      ; enable interupts on port B change
      bcf INTCON, RBIF
      bsf INTCON, RBIE
   }

   enable_interrupt( PEIE );
   enable_interrupt( GIE );

   change = 0;
   missingX = 0;
   missingY = 0;
   instateX = 0;
   instateY = 0;
   inchangeX = 0;
   inchangeY = 0;

   while(1){

      if(inchangeX){

         /* high to low transtiion */
         if(instateX == 1 && inX == 0){
            instateX = 0;

            /* save the timer1 value */
            pstopX = timeX;

            /* correct for clock overflow */
            okdiff = pstartX < pstopX;
            if(okdiff){
               tmp1 = pstopX - pstartX;
            } else {
               tmp2 = pstopX - pstartX;
               tmp1 = tmp2 + 65535;
            }

            /* offset adjustment */
            tmp2 = tmp1 - 400; 
            tmp1 = tmp2;

            /* check to make sure the pulse is in range */
            oklo = tmp1 > MIN_LIMIT;
            okhi = tmp1 < MAX_LIMIT;
            if(oklo && okhi){

               /* no missing pulses anymore... */
               missingX = 0;

               if(tmp1 < MIN_WIDTH) tmp1 = MIN_WIDTH;
               if(tmp1 > MAX_WIDTH) tmp1 = MAX_WIDTH;
               valX = tmp1;

               /* set a flag that we need to update the PWM */
               change = 1;
            }

         /* low to high transtiion */
         } else if(instateX == 0 && inX == 1){

            instateX = 1;

            /* save the timer1 value */
            pstartX = timeX;
         }

         inchangeX = 0;
      }
     
      if(inchangeY){

         /* high to low transtiion */
         if(instateY == 1 && inY == 0){
            instateY = 0;

            /* save the timer1 value */
            pstopY = timeY;

            okdiff = pstartY < pstopY;
            if(okdiff){
               tmp1 = pstopY - pstartY;
            } else {
               tmp2 = pstopY - pstartY;
               tmp1 = tmp2 + 65535;
            }

            /* check to make sure the pulse is in range */
            oklo = tmp1 > MIN_LIMIT;
            okhi = tmp1 < MAX_LIMIT;
            if(oklo && okhi){

               /* no missing pulses anymore... */
               missingY = 0;

               if(tmp1 < MIN_WIDTH) tmp1 = MIN_WIDTH;
               if(tmp1 > MAX_WIDTH) tmp1 = MAX_WIDTH;
               valY = tmp1;

               /* set a flag that we need to update the PWM */
               change = 1;
            }

         /* low to high transtiion */
         } else if(instateY == 0 && inY == 1){

            instateY = 1;

            /* save the timer1 value */
            pstartY = timeY;
         }

         inchangeY = 0;
      }

      /* check if both inputs are in the dead band */
      dead_input = 0;
      if(valX > IN_MIN_DEAD &&
         valX < IN_MAX_DEAD &&
         valY > IN_MIN_DEAD &&
         valY < IN_MAX_DEAD){
         dead_input = 1;
      }
     
      /* if we have not received valid input for a while */
      if(missingX >= 5 || missingY >= 5 || dead_input == 1){

         /* bad input */
         output_low_port_a(0); 

         /* turn off direction flags */
         output_low_port_a(1);
         output_low_port_a(2);
         output_low_port_a(3);
         output_low_port_a(5);

         /* turn off the PWM */
         asm {
            ; set PWM duty cycle
            bcf STATUS,RP0
            bcf STATUS,RP1
            clrf CCPR1L         ; 8 MSB's
            clrf CCPR2L         ; 8 MSB's
         }

      } else {

         /* good input */
         output_high_port_a(0); 

         if(change){
            change = 0;

            /* mix throttle and steering to get left side */
            tmp2 = valX - MIN_WIDTH;
            tmp1 = valY - MIN_WIDTH;
            tmp3 = tmp2 + tmp1;
            if(tmp3 < DIFF_WIDTH-DIFF_WIDTH_2) tmp3 = DIFF_WIDTH-DIFF_WIDTH_2;
            if(tmp3 > DIFF_WIDTH+DIFF_WIDTH_2) tmp3 = DIFF_WIDTH+DIFF_WIDTH_2;
            tmp2 = tmp3 - DIFF_WIDTH_2;

            if(tmp2 > DIFF_WIDTH_2+OUT_DEAD){
               /* foreward */
               output_low_port_a(5);
               output_high_port_a(3);
               tmp1 = tmp2 - (DIFF_WIDTH_2+OUT_DEAD);
            } else if(tmp2 < DIFF_WIDTH_2-OUT_DEAD){
               /* reverse */
               output_low_port_a(3);
               output_high_port_a(5);
               tmp1 = (DIFF_WIDTH_2-OUT_DEAD) - tmp2;
            } else {
               /* stop */
               output_low_port_a(3);
               output_low_port_a(5);
               tmp1 = 0;
            }

            /* Is this faster than / ? */
            /* Note: if OUT_DEAD is changed, then this stuff must be
             * changed.  (the "65" will change) */
            tmp2 = tmp1;
            tmp1 = tmp2 >> 2;
            tmp2 = tmp1 * 65;
            tmp1 = tmp2 >> 7;

            asm {
               movf _tmp1_main, W
               movwf _pwm1_main
            }
            if(pwm1 > 251) pwm1 = 251;
            pwm2 = 252 - pwm1;

            /* change the duty cycle */
            asm {
               ; set PWM duty cycle by writing to CCPR2l
               bcf STATUS,RP0
               bcf STATUS,RP1
               movf _pwm2_main, W
               movwf CCPR2L         ; 8 MSB's
            }

            /* mix throttle and steering to get right side */
            tmp1 = valX - MIN_WIDTH;
            tmp2 = tmp1 + DIFF_WIDTH;
            tmp1 = valY - MIN_WIDTH;
            tmp3 = tmp2 + tmp1;
            tmp1 = tmp3 - DIFF_WIDTH;
            if(tmp1 < DIFF_WIDTH-DIFF_WIDTH_2) tmp1 = DIFF_WIDTH-DIFF_WIDTH_2;
            if(tmp1 > DIFF_WIDTH+DIFF_WIDTH_2) tmp1 = DIFF_WIDTH+DIFF_WIDTH_2;
            tmp2 = tmp1 - DIFF_WIDTH_2;

            tmp2 = valX + DIFF_WIDTH;
            tmp1 = tmp2 - valY;
            if(tmp1 < DIFF_WIDTH-DIFF_WIDTH_2) tmp1 = DIFF_WIDTH-DIFF_WIDTH_2;
            if(tmp1 > DIFF_WIDTH+DIFF_WIDTH_2) tmp1 = DIFF_WIDTH+DIFF_WIDTH_2;
            tmp2 = tmp1 - DIFF_WIDTH_2;

            if(tmp2 > DIFF_WIDTH_2+OUT_DEAD){
               /* foreward */
               output_low_port_a(2);
               output_high_port_a(1);
               tmp1 = tmp2 - (DIFF_WIDTH_2+OUT_DEAD);
            } else if(tmp2 < DIFF_WIDTH_2-OUT_DEAD){
               /* reverse */
               output_low_port_a(1);
               output_high_port_a(2);
               tmp1 = (DIFF_WIDTH_2-OUT_DEAD) - tmp2;
            } else {
               /* stop */
               output_low_port_a(1);
               output_low_port_a(2);
               tmp1 = 0;
            }

            /* Is this faster than / ? */
            tmp2 = tmp1;
            tmp1 = tmp2 >> 2;
            tmp2 = tmp1 * 65;
            tmp1 = tmp2 >> 7;

            asm {
               movf _tmp1_main, W
               movwf _pwm1_main
            }
            if(pwm1 > 251) pwm1 = 251;
            pwm2 = 252 - pwm1;

            /* change the duty cycle */
            asm {
               ; set PWM duty cycle by writing to CCPR1l
               bcf STATUS,RP0
               bcf STATUS,RP1
               movf _pwm2_main, W
               movwf CCPR1L         ; 8 MSB's
            }
         }
      }

      /* heartbeat */
      clear_wdt();

   }

}