// CW1: FLASH CONFIGURATION WORD 1 (see PIC24 Family Reference Manual 24.1)
#pragma config ICS = PGx1 // Comm Channel Select (Emulator EMUC1/EMUD1 pins are shared with PGC1/PGD1)
#pragma config FWDTEN = OFF // Watchdog Timer Enable (Watchdog Timer is disabled)
#pragma config GWRP = OFF // General Code Segment Write Protect (Writes to program memory are allowed)
#pragma config GCP = OFF // General Code Segment Code Protect (Code protection is disabled)
#pragma config JTAGEN = OFF // JTAG Port Enable (JTAG port is disabled)
// CW2: FLASH CONFIGURATION WORD 2 (see PIC24 Family Reference Manual 24.1)
#pragma config I2C1SEL = PRI // I2C1 Pin Location Select (Use default SCL1/SDA1 pins)
#pragma config IOL1WAY = OFF // IOLOCK Protection (IOLOCK may be changed via unlocking seq)
#pragma config OSCIOFNC = ON // Primary Oscillator I/O Function (CLKO/RC15 functions as I/O pin)
#pragma config FCKSM = CSECME // Clock Switching and Monitor (Clock switching is enabled,
 // Fail-Safe Clock Monitor is enabled)
#pragma config FNOSC = FRCPLL // Oscillator Select (Fast RC Oscillator with PLL module (FRCPLL))

#include <xc.h>
#include <stdlib.h>
#define BUFFSIZE 2


volatile long int overflow = 0;
int high = 0;
int low = 0;
long int duration = 0;   //Once the button is pressed, the TMR3 value is stored here to be compared against 0.25ms and 2s
unsigned long int curBUF = 0;
unsigned long int prevBUF = 0;
unsigned long int curEdge = 0;
unsigned long int prevEdge = 0;
unsigned long int period = 0;
volatile unsigned long globalperiod = 0;

int servoflag = 0;
int trackperiod = 0;
unsigned long int clear = 0;
int idle = 0;

long int circleBuf [BUFFSIZE] = {0};
int write = 0;


void initServo(void)    /*utilize PPS to bind output compare 1 to RP6/RB6 and
                         * standard register writes to initialize Timer 3 to have a period
                         * of 20ms (calculate TCKPS and R3 as well set up the OC1 registers)
                         * Set AD1PCFG and TRIS bits */
{
    __builtin_write_OSCCONL (OSCCON & 0xbf);   //unlock PPS
    RPINR7bits.IC1R = 8; // Use Pin RP8 = "8" for input capture 1 (table 10-2)). Input capture on button
    RPOR3bits.RP6R = 18; //use Pin RP6 for output compare 1 = "18" Table (10-3)). Sends a signal to the servo
    __builtin_write_OSCCONL (OSCCON | 0x40);    //lock PPS

    /*Timer 3: For Output Compare setup*/
    CLKDIVbits.RCDIV = 0;   //Set clock speed to 16Mhz
    T3CON = 0x0010 ;    //PRE 1:8
    TMR3 = 0;           //Initialize to zero
    PR3 = 40000;        //set period to be larger than max external sig duration: PR * 62.5ns = 20ms Period for servo
    T3CONbits.TON = 1;  //start with timer on
    _T3IF = 0;

    /*Input Capture 1 Setup*/
    IFS0bits.IC1IF = 0; //Clear IC1 interrupt flag
    IEC0bits.IC1IE = 1; //IMPORTANT: Enable IC1 interrupts
    IC1CONbits.ICTMR = 1;    //IC1 configured to TMR2. Section 15. Input Capture
    IC1CONbits.ICM = 0b010;  //Capture mode, every falling

    //Output compare
    OC1CON = 0; //turn off OC1 for now
    OC1R = 0;  //servo start position. We won't touch OC1R again. OC1R: Count when output state is changed (inPWM mode pin is forced LOW)
    OC1RS = 3000;   //We will only change this once PWM is turned on. OC1RS: Count when output state is changed (in PWM mode acts as a shadow reg). This value is 30000 for the LED tests
    OC1CONbits.OCTSEL = 1;  //Use timer 3 for compare sources. OCTSEL: 1 = Timer3 is the clock source
    OC1CONbits.OCM = 0b110; //Output compare PWM w/o faults. OCM: 110= PWM mode on OC1, fault pin, OCFx, disabled

    AD1PCFG = 0x9fff;            //sets all pins to digital I/O
    TRISA = 0b0000000100000000; //set port A to inputs, where 1 is input. This means that RP8 is an input (IC1R) which is connected to the button
    TRISB = 0b0000000000000000;  //and port B to outputs
    LATB =  0b0000000000000000;  // where 1 is low
    LATA =  0b0000000000000000;

}

void setServo(int Val)  //set output
{
    int dutycycle = Val;
    OC1RS = dutycycle;
    // _T3IF = 0;
}

void initPushButton(void)
{
    //setup TMR2 to have a period of 1 sec
    AD1PCFG = 0x9FFF;          // set all pins to digital
    TRISB = 0b1111111111001111;  //set input and output pins, where 0 is output.
    CNPU2bits.CN22PUE = 1;
                              /* Enable the pull-up resistor on the input pins so that they default to HIGH.
                               * Important because this makes the button value a logic 1, once the button is pressed
                               * it turns into logic 0.
                               */

    //Timer 2
    T2CON = 0x0010;
    PR2 = 65535;    //Period of about 1 sec for TMR2
    _T2IF = 0;  //Initialize TMR2 interrupt flag to be zero
    TMR2 = 0;
    T2CONbits.TCKPS = 0b11; //Pre scalar to 256
    T2CONbits.TON = 1;  //Turn timer 2 on
    _T2IE = 1;  //Enables TMR2 interrupt

}

void __attribute__((__interrupt__,__auto_psv__)) _T2Interrupt(void)
{
    IFS0bits.T2IF = 0;
    overflow++;                     //increment the overflow every time that Timer2 completes a full loop
}

void storePeriod(unsigned long int localperiod) //This function stores the falling edges
{
    circleBuf [write] = localperiod;
    write++;
    write %= BUFFSIZE; //Once write is incremented to 3, start over at index 0
    trackperiod++;  //for any 'n' amount of clicks
}


 int newPeriod(void)
{
    if (trackperiod > 0)
    {
        return 1;   //a new period has been introduced in the circleBuf array
    }

    else
    {
        return 0;
    }

}



void __attribute__((__interrupt__,__auto_psv__)) _IC1Interrupt(void)
{
    IFS0bits.IC1IF = 0; //Reset the IC1 flag
    curBUF = IC1BUF;    //store value of TMR2 at falling edge
    curEdge = curBUF + (overflow *65535);   //To get the actual value of instruction cycles, must account for the previous cycles (overflow*PR2)
    period = curEdge;

    if(period > 125)    //serves as a de-bounce
    {
        //setServo(2400); //2400, move to the right
        TMR2 = 0;   //By setting TMR2 = 0, I do not need to do curEdge-prevEdge.
        overflow = 0;
        storePeriod(period);
    }

    while(IC1CONbits.ICBNE) //ICBNE = 0 means input capture Buffer is empty.
    {
        clear = IC1BUF;
    }

}


int main(void) {
    initServo();
    initPushButton();

    while(1)
    {
        int i = 0;

        if(newPeriod()) //If a new value is in the array, then check that value. Once this is zero, we have gone through all elements
        {
            while(i< BUFFSIZE)  //Iterate through the entire circleBuf array
            {
                if(circleBuf[i] > 15625)
                {
                     servoflag = 0; //if flag is zero, choose to stay in the left position
                     break; //If any click was long, do not cycle through the rest of the array
                }

                else
                {
                    servoflag = 1;  //If all periods were short then we have a triple click.
                }
                 i++;
            }

            trackperiod = 0;    //This will make the newperiod() false, meaning there no new periods to check
        }

        if (overflow >= 2 || servoflag == 0)    //overflow statement is used if we have been idle for a long time. servoflag also used to determine position
        {
            setServo(3600); //Left
            servoflag = 0;
        }

        else if(servoflag == 1) //If all clicks were short, move to the right
        {
            setServo(2400); //Right

        }
    }
    return (0);
}