#include "Proximity.h"
#include "mcc_generated_files/mcc.h"
#include "define.h"
#include "display.h"

//Constants for Band Location
#define Range_12    0x00 //0 to 1 ft 
#define Range_24    0x1B //1 to 2 ft
#define Range_36    0x36 //2 to 3 ft
#define Range_48    0x52 //3 to 4 ft
#define Range_60    0x6E //4 to 5 ft
#define Range_72    0x89 //5 to 6 ft
#define Range_84    0xA5 //6 to 7 ft
#define Range_96    0xC1 //7 to 8 ft
#define Range_108   0xDD //8 to 9 ft   

//MAXBAND holds the number of band locations (0-8)
#define MAXBAND     9

//MARGIN is an extra padding added onto the ADC reading while acquiring the 
//reference data, so that slight variations in the returned signal are not
//picked up and displayed as a false detection
#define MARGIN      30

//RangeBuffer[] holds TMR4 periods for each band location
volatile uint16_t RangeBuffer[9] = {Range_12, Range_24, Range_36, 
                                    Range_48, Range_60, Range_72, 
                                    Range_84, Range_96, Range_108};
//ctr holds the current band location
volatile uint8_t ctr;
//DisplayResult flag is set during TMR1 interrupt
//used to initiate comparison between current ADC value and the reference 
//value for that band
volatile bool DisplayResult;

//ADCReferenceMax/Min arrays hold the reference max/min ADC readings
//acquired in getAreaFingerprint() function
int16_t ADCReferenceMax[9], ADCReferenceMin[9];
//ADCResult holds current ADC reading set in TMR1 interrupt
//Used to determine if reading is larger/smaller than ADCReferenceMax/Min 
//in proximityDetection() function
uint16_t ADCResult;

void getAreaFingerprint(void);

void TMR3_ISR_Proximity(void)
{
    //Set TMR4 period depending on the current band location
    TMR4_Period8BitSet(RangeBuffer[ctr]);
    TMR0_Reload();
    TMR4_StartTimer();
}

void TMR4_ISR_Proximity(void)
{
    TMR1_Reload();
    TMR1_StartTimer();
}

void TMR1_ISR_Proximity(void)
{
    DisplayResult = true;
    ADCResult = ADC_GetConversionResult();
    TMR1_StopTimer();
}

inline void disableCCP1_Proximity(void)
{
    CCP1CONbits.CCP1EN = 0;

    // Clear the CCP1 interrupt flag
    PIR1bits.CCP1IF = 0;

    // Disable the CCP1 interrupt
    PIE1bits.CCP1IE = 0;
}

inline void disableCMP1_Proximity(void)
{    
    CM1CON0bits.C1ON = 0;
}

inline void disablePRG1_Proximity(void)
{
    PRG1CON0bits.EN = 0;
}

inline void disableFVR_Proximity(void)
{
    FVRCONbits.FVREN = 0;
}

inline void enableADC_Proximity(void)
{
    ADCON0bits.CHS = 0x5;
    ADCON0bits.ADON = 1;
}

inline void enableCLC3_Proximity(void)
{
    CLC3CONbits.EN = 1;
}

inline void enableTMR6_Proximity(void)
{
    T6CONbits.ON = 1;
}

void initCLC2_Proximity(void)
{
    // LC2EN enabled; INTN disabled; INTP disabled; MODE SR latch; 
    CLC2CON = 0x83;

    // Clear the CLC interrupt flag
    PIR3bits.CLC2IF = 0;
    // Disabling CLC2 interrupt.
    PIE3bits.CLC2IE = 0;
}

void initOPA1_Proximity(void)
{
    // ORS LC2_out; 
    OPA1ORS = 0x0C;
}

void initOPA2_Proximity(void)
{
    // OPA2ORPOL not inverted; OPA2EN enabled; OPA2UG OPAIN-_pin; OPA2ORM tri-stated; 
    OPA2CON = 0x81;
    // ORS LC3_out; 
    OPA2ORS = 0x0D;
    // PCH OPA2IN0+; 
    OPA2PCHS = 0x00;
}

void initTMR0_Proximity(void)
{
    // TMR0 156; 
    TMR0 = 0x9C;

    // Load the TMR value to reload variable
    timer0ReloadVal= 156;
}

void initTMR1_Proximity(void)
{    
    TMR1H = 0xF9;

    TMR1L = 0x0F;

    // Load the TMR value to reload variable
    timer1ReloadVal=(TMR1H << 8) | TMR1L;

    // Clearing IF flag.
    PIR1bits.TMR1IF = 0;
    
    // Enabling TMR1 interrupt.
    PIE1bits.TMR1IE = 1;
    
    // Set Default Interrupt Handler
    TMR1_SetInterruptHandler(TMR1_ISR_Proximity);

    // Start TMR1
    TMR1_StartTimer();
}

void initTMR3_Proximity(void)
{
    TMR3H = 0xE7;

    TMR3L = 0x96;

    // Load the TMR value to reload variable
    timer3ReloadVal=(TMR3H << 8) | TMR3L;

    // Clearing IF flag.
    PIR4bits.TMR3IF = 0;
    
    //Disabling TMR3 gate interrupt.
    PIE4bits.TMR3GIE = 0;
    
    // Enabling TMR3 interrupt.
    PIE4bits.TMR3IE = 1;
    
    // Set Default Interrupt Handler
    TMR3_SetInterruptHandler(TMR3_ISR_Proximity);

    // Start TMR3
    TMR3_StartTimer();
}

void initTMR4_Proximity(void)
{
    // T4CKPS 1:64; T4OUTPS 1:1; TMR4ON on; 
    T4CON = 0x60;

    // T4PSYNC Not Synchronized; T4MODE Starts at falling edge and resets on TMR4_ers=1; T4CKPOL Rising Edge; T4CKSYNC Not Synchronized; 
    T4HLT = 0x0F;

    T4PR = 0x00;

    T4TMR = 0x00;

    // Clearing IF flag.
    PIR4bits.TMR4IF = 0;
    
    //Disabling TMR4 interrupt.
    PIE4bits.TMR4IE = 1;
    
    // Set Default Interrupt Handler
    TMR4_SetInterruptHandler(TMR4_ISR_Proximity);

    // Start TMR4
    TMR4_Start();
}

void clearADCRef(void)
{
    int i = 0;
    for (i; i < MAXBAND; i++)
    {
        ADCReferenceMax[i] = 0x00;
        ADCReferenceMin[i] = 0x00;
    }
}

void initializeProximity(void)
{
    INTERRUPT_GlobalInterruptDisable();
    INTERRUPT_PeripheralInterruptDisable();
    
    //Only initialize/enable/disable what is necessary for Proximity Demo
    //Some settings are common between Range and Proximity and were set
    //during the SYSTEM_Initialize() function
    
    enableADC_Proximity();
    enableCLC3_Proximity();
    enableTMR6_Proximity();
    
    disableCCP1_Proximity();
    disableCMP1_Proximity();
    disablePRG1_Proximity();
    disableFVR_Proximity();
    
    initCLC2_Proximity();
    
    initOPA1_Proximity();
    initOPA2_Proximity();
    
    initTMR0_Proximity();
    initTMR1_Proximity();
    initTMR3_Proximity();
    initTMR4_Proximity();
    
    INTERRUPT_GlobalInterruptEnable();
    INTERRUPT_PeripheralInterruptEnable();
    
    getAreaFingerprint();
}

void getAreaFingerprint(void)
{
    //ADCReference holds current ADC value plus/minus MARGIN
        //value gets stored into ADCReferenceMax[]/ADCReferenceMin[] 
        //if its the maximum/minimum value
    uint16_t ADCReference = 0;
    //CaptureRef holds capture area count
        //incremented after each area sweep (from band 0 to MAXBAND - 1)
    uint8_t CaptureRef = 0;
    
    oled_clear();
    oled_putString("Proximity Demo", 0, 0);
    oled_putString("Getting Area", 0, 1);
    oled_putString("Footprint", 0, 2);

    ctr = 0;
    clearADCRef();
    
    //Run area capture 4 times
    while(CaptureRef < 4) 
    {    
        while(ctr < MAXBAND)
        {
            if(DisplayResult)
            {
                //Check if reading is a maximum
                ADCReference = ADCResult  + MARGIN;
                if (ADCReference > ADCReferenceMax[ctr])
                    ADCReferenceMax[ctr] = ADCReference;

                //Check if reading is a minimum
                if (ADCResult <= MARGIN)
                    ADCReference = 0;
                else
                    ADCReference = ADCResult - MARGIN;
                
                if (ADCReference < ADCReferenceMin[ctr])
                    ADCReferenceMin[ctr] = ADCReference;
                
                //Increment band location
                ctr++;
                DisplayResult = false;
            }  
        } 
        
        //Reset band location
        ctr = 0;
        CaptureRef++; 
    }
    
    ctr = 0;
    
    oled_clearRow(1);
    oled_clearRow(2);
}

void proximityDetection(void)
{
    //detecting the presence of a obstacle by comparing ADCResult and ADCReferenceMax/Min
    if(DisplayResult)
    {
        if((ADCResult > ADCReferenceMax[ctr]) || (ADCResult < ADCReferenceMin[ctr]))
        {
            //Obstacle detected, display result
            oled_clearRow(2);
            oled_clearRow(3);
            oled_putString("Detection in", 0, 2);
            char buffer[20]; 
            sprintf(buffer, "Zone %.0d", ctr);
            oled_putString(&buffer, 0, 3);
            
            //Result band location
            ctr = 0;
        }
        else
        {
            //Increment band location
            ctr++;
            
            //If we hit MAXBAND, no detection was found
            if(ctr == MAXBAND)
            {
                //Reset band location
                ctr = 0;
                oled_clearRow(2);
                oled_clearRow(3);
                oled_putString("No Detection", 0, 2);
            }
        } 
        
        DisplayResult = false;
    }
}


