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

//SpeedOfSound_at_TimerInc = 344m/s speed of sound in inches (13543.3 in/s) * 8us TMR1 increment
#define SpeedOfSound_at_TimerInc 0.0135433 
//HLTPeriod = T4PR * Prescaler = 0x36 * 4
#define HLTPeriod 0xD8 

//flag dictating if no detection was found
//no_detection = true no detection/reset state
//no_detection = false detection found
volatile bool no_detection = false; 

void TMR3_ISR_Range(void)
{
    TMR0_Reload();
    
    TMR4_StartTimer();
    
    //Check if no detection
    if (no_detection)
    {
        oled_clearRow(1);
        oled_putString("No Detection", 0, 1);
    }
    else //Detection occurred, reset no detection flag
        no_detection = true;
}

inline void enableCCP1_Range(void)
{
    CCP1CONbits.CCP1EN = 1;

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

    // Enable the CCP1 interrupt
    PIE1bits.CCP1IE = 1;
    
    CCPCapturedValue = 0;
}

inline void enableCMP1_Range(void)
{    
    CM1CON0bits.C1ON = 1;
}

inline void enablePRG1_Range(void)
{
    PRG1CON0bits.EN = 1;
}

inline void enableFVR_Range(void)
{
    FVRCONbits.FVREN = 1;
}

inline void disableADC_Range(void)
{
    ADCON0bits.ADON = 0;
}

inline void disableCLC3_Range(void)
{
    CLC3CONbits.EN = 0;
}

inline void disableTMR6_Range(void)
{
    T6CONbits.ON = 0;
}

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

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

void initOPA1_Range(void)
{
    // ORS CCP1_out; 
    OPA1ORS = 0x00;
}

void initOPA2_Range(void)
{
    // OPA2ORPOL not inverted; OPA2EN enabled; OPA2UG OPA_Output; OPA2ORM disabled; 
    OPA2CON = 0x90;
    // ORS CCP1_out; 
    OPA2ORS = 0x00;
    // PCH PRG1_OUT; 
    OPA2PCHS = 0x08;
}

void initTMR0_Range(void)
{
    // TMR0 75; 
    TMR0 = 0x4B;

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

void initTMR1_Range(void)
{     
    //TMR 0; 
    TMR1H = 0x00;

    //TMR 0; 
    TMR1L = 0x00;

    // Load the TMR value to reload variable
    timer1ReloadVal=(TMR1H << 8) | TMR1L;
    
    // Disabling TMR1 interrupt.
    PIE1bits.TMR1IE = 0;

    // Start TMR1
    TMR1_StartTimer();
}

void initTMR3_Range(void)
{
    //TMR 0; 
    TMR3H = 0;

    //TMR 0; 
    TMR3L = 0;

    // 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_Range);

    // Start TMR3
    TMR3_StartTimer();
}

void initTMR4_Range(void)
{
    // T4CKPS 1:4; T4OUTPS 1:1; TMR4ON off; 
    T4CON = 0x20;

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

    // T4PR 54; 
    T4PR = 0x36;

    // TMR4 0; 
    T4TMR = 0x00;

    // Clearing IF flag.
    PIR4bits.TMR4IF = 0;
    
    //Disabling TMR4 interrupt.
    PIE4bits.TMR4IE = 0;

    // Start TMR4
    TMR4_Start();
}

void initializeRange(void)
{
    INTERRUPT_GlobalInterruptDisable();
    INTERRUPT_PeripheralInterruptDisable();
    
    //Only initialize/enable/disable what is necessary for Range Demo
    //Some settings are common between Range and Proximity and were set
    //during the SYSTEM_Initialize() function
    
    disableADC_Range();
    disableCLC3_Range();
    disableTMR6_Range();
    
    enableCCP1_Range();
    enableCMP1_Range();
    enablePRG1_Range();
    enableFVR_Range();
    
    initCLC2_Range();
    
    initOPA1_Range();
    initOPA2_Range();
    
    initTMR4_Range();
    initTMR0_Range();
    initTMR1_Range();
    initTMR3_Range();
    
    display_result = false;
    
    INTERRUPT_GlobalInterruptEnable();
    INTERRUPT_PeripheralInterruptEnable();
    
    PRG1_StartRampGeneration();
    
    oled_clear();
    oled_putString("Range Demo", 0, 0);
}

void rangeDetection(void)
{
    unsigned int Distance_in_Counts;
    float Distance;
    
    if (display_result) {
        //calculate the distance of the returned signal
        Distance_in_Counts = (((CCPCapturedValue - timer1ReloadVal)) >> 1) + HLTPeriod;
        Distance = (Distance_in_Counts * SpeedOfSound_at_TimerInc);

        //display reading
        oled_clearRow(1);
        char buffer[20]; 
        sprintf(buffer, "%.0f inches", Distance);
        oled_putString(&buffer, 0, 1);

        display_result = false;
        
        no_detection = false;
    } 
}
