#include <pic.h> 
#include "serial.h" 
#include "globals.h" 

#undef CCPDEBUG 
#define CYCLE_SAMPLES     10 

void capture_init() {
// timer1 stuff

   T1CKPS1  =  0;
   T1CKPS0  =  0;    // 1:1 pre-scaler value
   T1OSCEN  =  0;    // disable external oscillator
   TMR1CS   =  0;    // increment to internal clock -> fosc / 4, set timer mode
                     // for us, fosc / 4 = 1MHz

   TMR1IE   =  0;    // disable Timer1 interrupt on overflow

   TMR1H    =  0;
   TMR1L    =  0;    // clear the timer values

   TMR1ON   =  0;    // turn off Timer1
   TMR1ON   =  1;    // turn Timer1 on
   
   CCPR1H   =  0;    // clear the CCP Timer high byte
   CCPR1L   =  0;    //                     low

   CCP1IE   =  0;    // shut off CCP interrupt for now, if you don't and you're polling,
                     // then you're in a world of hurt.
   CCP1IF   =  0;    // clear the CCP interrupt flag for now

   TRISC2   =  1;    // configure the RC2/CCP1 capture pin to input
   CCP1CON  =  0;    // clear/disable + turn off CCP1
}

/*
 * this function is broken.
 */
/*
void period_avg_one_axis(unsigned int *pp, unsigned char channel) {
   unsigned int time1, time2;
   unsigned char i;

   switch(channel) {
      case 1:
         CCP1CON  =  CCP1CON | 0b00000101;      // capture every rising edge
         break;
      case 2:
         CCP2CON  =  CCP2CON | 0b00000101;      // capture every rising edge
         break;
   }

   for (i = 0; i < CYCLE_SAMPLES; i++) {
      TMR1H    =  0;
      TMR1L    =  0;

      for (;;) {
         if (CCP1IF || CCP2IF) {
            time1  = TMR1H;
            time1  = time1 << 8;
            time1 += TMR1L;
            (channel == 1) ? CCP1IF = 0 : CCP2IF = 0;
            break;
         }
      }
   
      for (;;) {
         if (CCP1IF || CCP2IF) {
            time2  = TMR1H;
            time2  = time2 << 8;
            time2 += TMR1L;
            (channel == 1) ? CCP1IF = 0 : CCP2IF = 0;
            break;
         }
      }

      *pp = *pp + (time2 - time1) / CYCLE_SAMPLES;
   }

   CCP1CON  =  CCP1CON & 0x00;
   CCP2CON  =  CCP2CON & 0x00;
}
*/

void single_reading_two_axis(unsigned int *xpulse, unsigned int *xtotal, unsigned int *ypulse, unsigned int *ytotal) {
   unsigned int timex1 = 0, timex2 = 0, timex3 = 0, timey1 = 0, timey2 = 0, timey3 = 0;

   TMR1H    =  0;
   TMR1L    =  0;

   CCP1CON  =  CCP1CON | 0b00000101;      // capture every rising edge
   CCP2CON  =  CCP2CON | 0b00000101;

   for (;;) {
      if (CCP1IF) {
         timex1  = CCPR1H;
         timex1  = timex1 << 8;
         timex1 += CCPR1L;
         CCP1IF  = 0;
         continue;
      }
      if (CCP2IF) {
         timey1  = CCPR2H;
         timey1  = timey1 << 8;
         timey1 += CCPR2L;
         CCP2IF  = 0;
         continue;
      }
      if ((timex1 != 0) && (timey1 != 0)) {
         break;
      }
   }

   CCP1M0   =  0;
   CCP2M0   =  0;

   for (;;) {
      if (CCP1IF) {
         timex2  = CCPR1H;
         timex2  = timex2 << 8;
         timex2 += CCPR1L;
         CCP1IF  = 0;
         continue;
      }
      if (CCP2IF) {
         timey2  = CCPR2H;
         timey2  = timey2 << 8;
         timey2 += CCPR2L;
         CCP2IF  = 0;
         continue;
      }
      if ((timex2 != 0) && (timey2 != 0)) {
         break;
      }
   }
   
   CCP1M0   =  0;
   CCP2M0   =  0;

   for (;;) {
      if (CCP1IF) {
         timex3  = CCPR1H;
         timex3  = timex3 << 8;
         timex3 += CCPR1L;
         CCP1IF  = 0;
         continue;
      }
      if (CCP2IF) {
         timey3  = CCPR2H;
         timey3  = timey3 << 8;
         timey3 += CCPR2L;
         CCP2IF  = 0;
         continue;
      }
      if ((timex3 != 0) && (timey3 != 0)) {
         break;
      }
   }

   CCP1CON  =  CCP1CON & 0x00;
   CCP2CON  =  CCP2CON & 0x00;
   
   *xpulse = timex2 - timex1;
   *xtotal = timex3 - timex2;
   
   *ypulse = timey2 - timey1;
   *ytotal = timey3 - timey2;
}

unsigned int find_pulse_width(unsigned char channel) {
   unsigned int time1, time2;
   
   switch(channel) {
      case 1:
         CCP1CON  =  CCP1CON | 0b00000101;      // capture every rising edge
         CCP2CON  =  0;
         break;
      case 2:
         CCP2CON  =  CCP1CON | 0b00000101;      // capture every rising edge
         CCP1CON  =  0;
         break;
   }

   TMR1H    =  0;
   TMR1L    =  0;

   for (;;) {
      if (CCP1IF || CCP2IF) {
         time1  = TMR1H;
         time1  = time1 << 8;
         time1 += TMR1L;
         (channel == 1)? CCP1M0 = 0 : CCP2M0 = 0;
         (channel == 1)? CCP1IF = 0 : CCP2IF = 0;
         break;
      }
   }

   for (;;) {
      if (CCP1IF || CCP2IF) {
         time2  = TMR1H;
         time2  = time2 << 8;
         time2 += TMR1L;
         (channel == 1)? CCP1IF = 0 : CCP2IF = 0;
         break;
      }
   }

   CCP1CON  =  CCP1CON & 0x00;
   CCP2CON  =  CCP2CON & 0x00;   
   return (time2 - time1);
}

void find_pw_pp(unsigned int *pulse, unsigned int *total, unsigned char channel) {

   switch(channel) {
      case 1: CCP1CON  =  CCP1CON | 0b00000101;      // capture every rising edge.
              break;
      case 2: CCP2CON  =  CCP2CON | 0b00000101;      // capture every rising edge.
              break;
   }

   for (;;) {
      if (CCP1IF || CCP2IF) {
         TMR1H  = 0;
         TMR1L  = 0;
         (channel == 1)? CCP1M0 = 0 : CCP2M0 = 0;
         (channel == 1)? CCP1IF = 0 : CCP2IF = 0;
         break;
      }
   }

   for (;;) {
      if (CCP1IF || CCP2IF) {
         *pulse  = TMR1H;
         *pulse  = *pulse << 8;
         *pulse += TMR1L;
         (channel == 1)? CCP1M0 = 1 : CCP2M0 = 1;
         (channel == 1)? CCP1IF = 0 : CCP2IF = 0;
         break;
      }
   }
   
   for (;;) {
      if (CCP1IF || CCP2IF) {
         *total  = TMR1H;
         *total  = *total << 8;
         *total += TMR1L;
         break;
      }
   }  
   
   CCP1CON  =  0x00;
   CCP2CON  =  0x00;
   CCP1IF   =  0x00;
   CCP2IF   =  0x00;
}



#ifdef CCPDEBUG 
main() {
   unsigned int   xpw, xpp, ypw, ypp;

// global interrupt disable for now
   setup_interrupts();
   serial_init();
   capture_init();

   for(;;) {
      xpp = 0;
      ypp = 0;
/*
      period_avg_one_axis(&xpp, 1);
      period_avg_one_axis(&ypp, 2);
      putch('c');
      putdec(xpp, 'h');
      putch('|');
      putdec(ypp, 'h');
      putlf();

      find_pw_pp(&xpw, &xpp, 1);
      find_pw_pp(&ypw, &ypp, 2);
      putch('a');
      putdec(xpw, 'h');
      putch('|');
      putdec(xpp, 'h');
      putch('|');
      putdec(ypw, 'h');
      putch('|');
      putdec(ypp, 'h');
      putlf();
*/
      single_reading_two_axis(&xpw, &xpp, &ypw, &ypp);
      putch('b');
      putdec(xpw, 'h');
      putch('|');
      putdec(xpp, 'h');
      putch('|');
      putdec(ypw, 'h');
      putch('|');
      putdec(ypp, 'h');
      putlf();
   }
}
#endif