#include "USBdsc.c"
#define MIN_PERIOD        80000.0

typedef union {
    char bytes[8];
    struct {
        unsigned int x_value;
        unsigned int y_value;
        unsigned int rx_value;
        unsigned int ry_value;
    };
} T_USB_Report;

T_USB_Report MyReport;

int debug_toogle = 0;

unsigned long int ic1_start = 0;
unsigned long int value1 = 0;
int toogle1 = 0;

unsigned long int ic2_start = 0;
unsigned long int value2 = 0;
int toogle2 = 0;

unsigned long int ic3_start = 0;
unsigned long int value3 = 0;
int toogle3 = 0;

unsigned long int ic4_start = 0;
unsigned long int value4 = 0;
int toogle4 = 0;

int sync = 0;

char ReadBuffer[64], WriteBuffer[64];

unsigned long int CalculDelta (unsigned long int T2, unsigned long int T1) {
    // Calcul T2-T1, même si le timer déborde et repart à 0
    if (T2<T1) {
       // théoriquement impossible sauf en cas de débordement
       return 4294967295-(T1-T2)+1;
    }
    else return T2-T1;
}

void IC1_ISR() iv IVT_INPUT_CAPTURE_1 ilevel 6 ics ICS_SOFT {
     unsigned long temp = 0;
     LATG.f6 = 1;
     sync = 1;
     TMR1 = 0; //reset timeout comm.
     temp = IC1BUF;
     IC1BUF = 0;
     if (toogle1 == 0) {
         ic1_start = temp;
         toogle1 = 1;
     }
     else {
         // value1 = CalculDelta(temp, ic1_start);
         
         // Calcul T2-T1, même si le timer déborde et repart à 0
         if (temp<ic1_start) {
            // théoriquement impossible sauf en cas de débordement
            value1 = 4294967295-(ic1_start-temp)+1;
         }
         else value1 = temp-ic1_start;
         toogle1 = 0;
     }
     debug_toogle++;
     if (debug_toogle >= 50) {
        debug_toogle = 0;
        LATD.f1 = !PORTD.f1;
     }
     IC1IF_bit = 0;
}

void IC2_ISR() iv IVT_INPUT_CAPTURE_2 ilevel 5 ics ICS_SOFT {
     unsigned long temp = 0;
     temp = IC2BUF;
     IC2BUF = 0;
     if (toogle2 == 0) {
         ic2_start = temp;
         toogle2 = 1;
     }
     else {
         // value2 = CalculDelta(temp, ic2_start);
         
         // Calcul T2-T1, même si le timer déborde et repart à 0
         if (temp<ic2_start) {
            // théoriquement impossible sauf en cas de débordement
            value2 = 4294967295-(ic2_start-temp)+1;
         }
         else value2 = temp-ic2_start;
         toogle2 = 0;
     }
     IC2IF_bit = 0;
}

void IC3_ISR() iv IVT_INPUT_CAPTURE_3 ilevel 4 ics ICS_SOFT {
     unsigned long temp = 0;
     temp = IC3BUF;
     IC3BUF = 0;
     if (toogle3 == 0) {
         ic3_start = temp;
         toogle3 = 1;
     }
     else {
         // value3 = CalculDelta(temp, ic3_start);
         
         // Calcul T2-T1, même si le timer déborde et repart à 0
         if (temp<ic3_start) {
            // théoriquement impossible sauf en cas de débordement
            value3 = 4294967295-(ic3_start-temp)+1;
         }
         else value3 = temp-ic3_start;
         toogle3 = 0;
     }
     IC3IF_bit = 0;
}

void IC4_ISR() iv IVT_INPUT_CAPTURE_4 ilevel 3 ics ICS_SOFT {
     unsigned long temp = 0;
     temp = IC4BUF;
     IC4BUF = 0;
     if (toogle4 == 0) {
         ic4_start = temp;
         toogle4 = 1;
     }
     else {
         // value4 = CalculDelta(temp, ic4_start);
         // Calcul T2-T1, même si le timer déborde et repart à 0
         if (temp<ic4_start) {
            // théoriquement impossible sauf en cas de débordement
            value4 = 4294967295-(ic4_start-temp)+1;
         }
         else value4 = temp-ic4_start;
         toogle4 = 0;
     }
     IC4IF_bit = 0;
}

void USB_ISR() iv IVT_USB_1 ilevel 7 ics ICS_SRS {
     USB_Interrupt_Proc();
     USBIF_bit = 0;
}

void TMR1_ISR() iv IVT_TIMER_1 ilevel 1 ics ICS_SOFT {
     LATG.f6 = 0;
     /*
     value1 = 120000;
     value2 = value1;
     value3 = value1;
     value4 = value1;
     */
     sync = 0;
     TMR1 = 0;
     T1IF_bit = 0;
}

void ClearBuffer (char * buffer, int buffer_len) {
     int i = 0;
     for (i=0; i<buffer_len; i++) *(buffer+i) = 0;
}

void main() {

    toogle1 = 0;
    toogle2 = 0;
    toogle3 = 0;
    toogle4 = 0;
    ic1_start = 0;
    ic2_start = 0;
    ic3_start = 0;
    ic4_start = 0;
    value1 = 0;
    value2 = 0;
    value3 = 0;
    value4 = 0;
    debug_toogle = 0;

    AD1PCFG = 0xFFFF;
    JTAGEN_bit = 0;
    CHECON = 0x32;
    ODCB = 0;
    TRISD = 0xffff;
    TRISD.f1 = 0;
    TRISG.f6 = 0;
    LATG.f6 = 0;
    LATD.f1 = 0;

    IC1CONbits.ICM = 0b110;
    IC1CONbits.FEDGE = 1;
    IC1CONbits.C32 = 1;
    IC1CONbits.ICI = 0;
    IC1CONbits.ICSIDL = 0;
    
    IC2CON = IC1CON;
    IC3CON = IC1CON;
    IC4CON = IC1CON;

    T2CONSET.f1 = 0;
    T2CONSETbits.T32 = 1;
    T2CONSETbits.TGATE = 0;
    T2CONSETbits.TCKPS0 = 0;
    T2CONSETbits.TCKPS1 = 0;
    T2CONSETbits.TCKPS2 = 0;
    TMR2 = 0;
    PR2 = 0xFFFFFFFF;
    T2CONSETbits.ON = 1;
    
    PR1 = 65535;
    TMR1 = 0;
    T1CONSETbits.TCKPS0 = 1;
    T1CONSETbits.TCKPS1 = 1;
    T1CONSETbits.TGATE = 0;
    T1CONSETbits.SIDL = 0;
    T1CONSETbits.TCS = 0;
    T1CONSETbits.ON = 1;

    IC1IP0_bit = 0;
    IC1IP1_bit = 1;
    IC1IP2_bit = 1;

    IC2IP0_bit = 1;
    IC2IP1_bit = 0;
    IC2IP2_bit = 1;

    IC3IP0_bit = 0;
    IC3IP1_bit = 0;
    IC3IP2_bit = 1;

    IC4IP0_bit = 1;
    IC4IP1_bit = 1;
    IC4IP2_bit = 0;
    
    USBIP0_bit = 1;
    USBIP1_bit = 1;
    USBIP2_bit = 1;
    
    T1IP0_bit = 1;
    T1IP1_bit = 0;
    T1IP2_bit = 0;

    
    USBIE_bit = 1;
    IC1IE_bit = 1;
    IC2IE_bit = 1;
    IC3IE_bit = 1;
    IC4IE_bit = 1;
    T1IE_bit = 1;
    
    EnableInterrupts();

    HID_Enable(ReadBuffer, WriteBuffer);
    
    IC1CONbits.ON = 1;
    IC2CONbits.ON = 1;
    IC3CONbits.ON = 1;
    IC4CONbits.ON = 1;

    T4CONSETbits.ON = 1;
    
    while (1) {
          MyReport.x_value = (value1-MIN_PERIOD)/1.22;
          MyReport.y_value = (value2-MIN_PERIOD)/1.22;
          MyReport.rx_value = (value3-MIN_PERIOD)/1.22;
          MyReport.ry_value = (value4-MIN_PERIOD)/1.22;
          HID_Write(MyReport.bytes, sizeof(MyReport));
          delay_ms(10);
    }
}