#include <dsp.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "../CommonCode/types.h"
#include "../CommonCode/conv.h"
#include "isr.h"
#include "CalibrationConstants.h"
#include "libq.h"
#include "../CommonCode/PIDGainScaling.h"
#include "DAC.h"
#include "../CommonCode/crc.h"
#include "ManageSwitches.h"
#include "ManageLEDs.h"


unsigned int *Buff; //Pointer to appropriate DMA Buffer, switched on PingPong

//This DMA interrupt occurs once at the end of each sampling period in which the specified 
//channels are cycled through the specified number of times.  The channel reading is done
//at a rate specified by timer 3 (1.5uS) and that timer interrupt keeps count of how many
//samples have been made.  Example. If four channels are sampled once each at a rate of 1.5uS
// then this DMA interrupt will occur 6 uS after the sampling was started by the PWM interrupt.

void __attribute__((interrupt, no_auto_psv)) _DMA0Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _DMA0Interrupt(void)
{
    int j;

    static int first_time = 1;

    static fractional ADC_JoyStk_X_Offset;
    static fractional ADC_JoyStk_Y_Offset;


    fractional outSamples[INNERFILTERSAMPLES];

    __asm__ volatile ("push CORCON");

    if (first_time) //Compute analog counts corresponding to zero's of engineering units.
    {
        ADC_JoyStk_X_Offset = 0;//ConvertEngUnitsToInt(0.0, ADC_JoyStk_X_cnt, ADC_JoyStk_X_eng);
        ADC_JoyStk_Y_Offset = 0;//ConvertEngUnitsToInt(0.0, ADC_JoyStk_X_cnt, ADC_JoyStk_X_eng);
        first_time = 0;
    }
    
    if (DMACS1 & 0x0001) //Point to one buffer to fill depending or the other depending on which one caused interrupt
    {
        Buff = BufferB;  //Fill Buffer B, Buffer A is valid
    }
    else
    {
        Buff = BufferA;  //Fill Buffer B, Buffer A is valid
    }

#if defined(DEBUG) && defined(ALLOW_CALIBRATION)
    if (raw) //Remove offset if in calibration mode
    {
        ADC_CameraTemp_Offset = 0;
        first_time = 1;
    }
#endif

    // BufferA/BufferB and Buff are unsigned integers, because the ADC is set up to represent data as unsigned
    // integers with the lowest four bits equal to zero (since it's a 12 bit converter).
    // Below all variables that get set from the ADC values are shifted to the right one bit and then have the counts 
    // representing the zero point subtracted to make signed integers. 
    // This doesn't introduce a lack of precision for variables that are always positive (like VBus) because it's a 12-bit ADC,
    // so representing positive only values in 15 bits instead is OK.
    // The ADC FORMAT setting could be used to produce variables as signed integers, but this isn't used to
    // give some control over the zero-point, which may not fall exactly half-way between 0 and Vref in every case.

    //If the analog inputs used (or their ordering) are changed, the next lines must be changed to match
    //Additionally the analog input pin specification must be changed in init.c

    for (j = 0; j < INNERFILTERSAMPLES; j++) {
        ADC_JoyStk_X[j] = _Q15sub(Buff[0 + NUMAIO * j] >> 1, ADC_JoyStk_X_Offset); //AN10 
        ADC_JoyStk_Y[j] = _Q15sub(Buff[1 + NUMAIO * j] >> 1, ADC_JoyStk_Y_Offset); //AN11
    }

    if (INNERFILTERSAMPLES > 1) {
        asm volatile ("disi #0x3FFF");
        FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & ADC_JoyStk_X[0], &Inner_Filter);
        asm volatile ("disi #0x0000");
        JoyStk_X = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.

        asm volatile ("disi #0x3FFF");
        FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & ADC_JoyStk_Y[0], &Inner_Filter);
        asm volatile ("disi #0x0000");
        JoyStk_Y = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.
    } else {
        JoyStk_X = ADC_JoyStk_X[0];
        JoyStk_Y = ADC_JoyStk_Y[0];      
    }        

 
    
    ManageSwitches();


    
    IFS0bits.DMA0IF = 0; //Clear the DMA interrupt flag bit
    __asm__ volatile ("pop CORCON");

}

//This ISR isn't enabled since the DMA interrupt does all Analog inputs at once
//each time the specified number of conversions takes place.
//It remains here for debugging and could be enabled.

void __attribute__((interrupt, no_auto_psv)) _ADC1Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _ADC1Interrupt(void)
{
    __asm__ volatile ("push CORCON");

    AD1CON1bits.DONE = 0; //clear flag

    _AD1IF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}

//This interrupt occurs when TMR3 Rolls over.  

void __attribute__((interrupt, no_auto_psv)) _T3Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _T3Interrupt(void)
{
    static int i = 0;
    __asm__ volatile ("push CORCON");

    if (++i == NUMAIO * INNERFILTERSAMPLES) {
        T3CON = 0x0000; //Period match has occurred once for each analog input, so stop timer.  100Hz interupt will restart.
        i = 0;
    }

    _T3IF = 0; // Clear interrupt
    __asm__ volatile ("pop CORCON");
}

void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) //100Hz
////void __attribute__((__interrupt__(__save__(CORCON))))     _T1Interrupt(void)
{
    __asm__ volatile ("push CORCON");

    f_time = f_time + 1.0/100;

    //Start ADC sampling process.
    PR3 = 2200; //Reset counter before turning on Timer.
    T3CON = 0x8000; //Start timer 3, an ADC sample and convert will then happen every ~6.00uSec until INNERFILTERSAMPLES are completed and DMA occurs.
    //The filtering circuitry driving the dsPic ADC requires 1.8uS to settle after the ADC selects that channel (LTC2055 driving the 18pF Sample 
    //and Hold capacitor.  The ADC conversion process is set up to take another 1.8uS (5*14*Tcy), so a total of 3.6uS is required per channel.  
    //6uS selected here gives some margin as it seems that 3.6uS isn't actually quite long enough. 
    //The DMA interupt then stops the timer to keep more from being sampled and converted until next time it's turned on here.

    //Count seconds based on this ISR being at 100Hz
    CentiSecCtr++;  //Rolls over every 65 seconds or so, so useful for saving this value, and subtracting that from current value to mesure time up to a minute.  ((2's complement subtraction)
   
    char *char_ptr;
    int i;
#if 1  //Send out Data Packet to bottom side while ADC is collecting data, 
    SerialDataOut.preamble = 0x5555;
    iTemp2 = Focus_Slider;
    SerialDataOut.FocusTarget = pl_interp(&Focus_interp, Focus_Slider); //Apply linear interp to convert raw counts to -32768->32767  
    iTemp1 = Zoom_Rocker;
    SerialDataOut.ZoomRateTarget = pl_interp(&Zoom_interp, Zoom_Rocker); //Apply linear interp to apply deadband and convert raw counts to -32768->32767  
    SerialDataOut.status = top_status;
    SerialDataOut.crc = calcCRC((const char*) &SerialDataOut, sizeof (TopToBottomDataPacket) - 2); // If this is interrupted and data is changed, crc will be incorrect...

    char_ptr = (char *) &SerialDataOut;
    for (i = 0; i<sizeof (SerialDataOut); i++)  //Note, putU1 blocks until last character is sent, so this will take the time required to send the characters.  Could be made interrupt driven with a FIFO if this takes to long...
        putU1(char_ptr[i]);

#else
    //Fake Packet From Bottomside...
    FakeBottomSidePacket.preamble = 0x5555;
    FakeBottomSidePacket.CameraTemp = 0xdead;
    FakeBottomSidePacket.FocusFollow = (sin(2 * M_PI * f_time / 10));
    FakeBottomSidePacket.ZoomFollow = (sin(2 * M_PI * f_time / 10));
    FakeBottomSidePacket.Humidity = 1.1;
    FakeBottomSidePacket.Pressure = 3.3;

    FakeBottomSidePacket.status.bitfields.AftWaterAlarm = floor(f_time / 7);
    FakeBottomSidePacket.status.bitfields.FwdWaterAlarm = floor(f_time / 8);
    FakeBottomSidePacket.status.bitfields.ZoomExtendState = floor(f_time / 9);
    FakeBottomSidePacket.status.bitfields.CameraPowerState = floor(f_time / 10);
    FakeBottomSidePacket.status.bitfields.LaserPowerState = floor(f_time / 11);
    FakeBottomSidePacket.status.bitfields.OverTemperatureState = floor(f_time / 12);
    FakeBottomSidePacket.crc = calcCRC((const char*) &FakeBottomSidePacket, sizeof (BottomToTopDataPacket) - 2);
    char_ptr = (char *) &FakeBottomSidePacket;
    for (i = 0; i<sizeof (FakeBottomSidePacket); i++)
        putU1(char_ptr[i]);
#endif      


    _T1IF = 0;
    __asm__ volatile ("pop CORCON");

}


void __attribute__((interrupt, no_auto_psv)) _SPI1Interrupt(void) {
    static int Meas = 0;
    static int Focus_Measurements[INNERFILTERSAMPLES];
    static int ctr = 0;
    fractional outSamples[INNERFILTERSAMPLES];
    __asm__ volatile ("push CORCON");


    if (MeasStatus & 0x0001) { //First word has already been read so this interrupt is due to the second word being read
        LATBbits.LATB2 = 1;
        Meas |= (SPI1BUF >> 1);
        
        Focus_Measurements[ctr++] = Meas;
        if(ctr == INNERFILTERSAMPLES)
            ctr = 0;
            
        if(INNERFILTERSAMPLES > 1)
        {      
            asm volatile ("disi #0x3FFF");
            FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & Focus_Measurements[0], &Inner_Filter);
            asm volatile ("disi #0x0000");
            Focus_Slider = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.
        }
        else
            Focus_Slider = Focus_Measurements[0]; //Measurement assembled, so copy to global variable. 
        
        
        
        MeasStatus &= ~0x0001; //Clear bit one indicating the second word has been read
        if (MeasStatus == 0x0004) {
            SPI1STATbits.SPIEN = 0;
            SPI2STATbits.SPIEN = 0;
            //SPI_ReadsComplete(); //Both SPI channels have been read
            MeasStatus = 0x0008; //Indicated processing is  done.
        }
    } else {
        Meas = (SPI1BUF << 15);
        MeasStatus |= 0x0001; //Set bit one indicated the first word has been read
        SPI1STATbits.SPIROV = 0;
        if (MeasStatus == 0x03) //Both First words have been read.
        {
            SPI2BUF = *((unsigned int *) &SPI2_DataOut[2]);
            SPI1BUF = *((unsigned int *) &SPI1_DataOut[2]);
            MeasStatus |= 0x04;
        }
    }
    _SPI1IF = 0; // Clear the Interrupt flag
    __asm__ volatile ("pop CORCON");
}

void __attribute__((interrupt, no_auto_psv)) _SPI2Interrupt(void) {
    static int Meas = 0;
    static int Zoom_Measurements[INNERFILTERSAMPLES];
    static int ctr = 0;
    fractional outSamples[INNERFILTERSAMPLES];
    __asm__ volatile ("push CORCON");
    //  LOAD_SWITCH = 1;

    if (MeasStatus & 0x0002) { //First word has already been read so this interrupt is due to the second word being read
        Meas |= (SPI2BUF >> 1);
        
        
        Zoom_Measurements[ctr++] = Meas;
        if(ctr == INNERFILTERSAMPLES)
            ctr = 0;
            
        if(INNERFILTERSAMPLES > 1)
        {      
            asm volatile ("disi #0x3FFF");
            FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & Zoom_Measurements[0], &Inner_Filter);
            asm volatile ("disi #0x0000");
            Zoom_Rocker = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.
        }
        else
            Zoom_Rocker = Zoom_Measurements[0]; //Measurement assembled, so copy to global variable. 
        
        
        
        
        
        MeasStatus &= ~0x0002; //Clear bit two indicating the second word has been read
        if (MeasStatus == 0x0004) {
            SPI1STATbits.SPIEN = 0;
            SPI2STATbits.SPIEN = 0;
            //SPI_ReadsComplete(); //Both SPI channels have been read
            MeasStatus = 0x0008;
        }
    } else {
        Meas = (SPI2BUF << 15);
        MeasStatus |= 0x0002; //Set bit two indicated the first word has been read
        if (MeasStatus == 0x03) //Both First words have been read.
        {
            SPI2BUF = *((unsigned int *) &SPI2_DataOut[2]);
            SPI1BUF = *((unsigned int *) &SPI1_DataOut[2]);
            MeasStatus |= 0x04;
        }
    }

    _SPI2IF = 0; // Clear the Interrupt flag
    __asm__ volatile ("pop CORCON");
}



//This interrupt occurs when TMR5 Rolls over.  ~4012Hz
void __attribute__((interrupt, no_auto_psv)) _T5Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _T3Interrupt(void)
{
 
    __asm__ volatile ("push CORCON");


    //Start SPI Bus process to measure phase currents, SPIinterrupt will occur when done.
        if (MeasStatus == 0x0008) //Only start SPI/ADC collection if last cycles data has been collected and processed
        {
            MeasStatus = 0;
            SPI1STATbits.SPIEN = 1;
            SPI2STATbits.SPIEN = 1;
            SPI1STATbits.SPIROV = 0;
            SPI2STATbits.SPIROV = 0;
            SS1 = 0;
            
            int spi_dataout_DACA = 0;
            int spi_dataout_DACB = 0;

    SPI1_DataOut[1] = 0;
    SPI1_DataOut[0] = (unsigned char) ((spi_dataout_DACA) >> 8);
    SPI1_DataOut[3] = (unsigned char) (spi_dataout_DACA);
    SPI1_DataOut[2] = 0;

    SPI2_DataOut[1] = 0;
    SPI2_DataOut[0] = (unsigned char) ((spi_dataout_DACB) >> 8);
    SPI2_DataOut[3] = (unsigned char) (spi_dataout_DACB);
    SPI2_DataOut[2] = 0;
            
    SPI1BUF = *((unsigned int *) &SPI1_DataOut[0]);
    SPI2BUF = *((unsigned int *) &SPI2_DataOut[0]);
        }

    
    ManageLEDs();
    
    _T5IF = 0; // Clear interrupt
    __asm__ volatile ("pop CORCON");
}