#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 "../CommonCode/pl_interp.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_CameraTemp_Offset;

    int outSamples[INNERFILTERSAMPLES];

    __asm__ volatile ("push CORCON");

    if (first_time) //Compute analog counts corresponding to zero's of engineering units.
    {
        ADC_CameraTemp_Offset = 0; //ConvertEngUnitsToInt(0.0, ADC_CameraTemp_cnt, ADC_CameraTemp_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_CameraTemp[j] = _Q15sub(Buff[0 + NUMAIO * j] >> 1, ADC_CameraTemp_Offset); //AN10 
    }

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

    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.  10Hz 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");
    static int ctr = 0;

#ifdef DEBUG
    if (++ctr >= 10) {
        f_time = f_time + 1.0 / 10;
        ctr = 0;
    }
#endif

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




    //Assemble Packet, crc, and send out serial port.
    SerialDataOut.preamble = 0x5555;
    SerialDataOut.CameraTemp = (int) (10 * ConvertIntToEngUnits(CameraTemp, ADC_CameraTemp_cnt, ADC_CameraTemp_eng)); //  CameraTemp in tenths of a degree C;   
 
    SerialDataOut.FocusFollow = ConvertIntToEngUnits(FocusFollow, FocusFollow_cnt, FocusFollow_eng);
    SerialDataOut.ZoomFollow = ConvertIntToEngUnits(ZoomFollow, ZoomFollow_cnt, ZoomFollow_eng);

            

    
    SerialDataOut.status = bottom_status;

    SerialDataOut.crc = calcCRC((const char*) &SerialDataOut, sizeof (BottomToTopDataPacket) - 2); // If this is interrupted and data is changed, crc will be incorrect...

    char *char_ptr;
    int i;
#if 1
    char_ptr = (char *) &SerialDataOut;
    for (i = 0; i<sizeof (BottomToTopDataPacket); i++) {
        putU1(char_ptr[i]);
    }
#else
    //Fake Packet From Topside...
    FakeTopSidePacket.preamble = 0x5555;
    FakeTopSidePacket.FocusTarget = Q15(cos(2 * M_PI * f_time / 50));
    FakeTopSidePacket.ZoomRateTarget = Q15(sin(2 * M_PI * f_time / 50));
    FakeTopSidePacket.status.bitfields.ZoomExtendRequest = floor(f_time / 10);
    FakeTopSidePacket.status.bitfields.CameraPowerRequest = floor(f_time / 11);
    FakeTopSidePacket.status.bitfields.LaserPowerRequest = floor(f_time / 12);
    FakeTopSidePacket.crc = calcCRC((const char*) &FakeTopSidePacket, sizeof (TopToBottomDataPacket) - 2);
    char_ptr = (char *) &FakeTopSidePacket;
    for (i = 0; i<sizeof (FakeTopSidePacket); i++) {
        putU1(char_ptr[i]);
    }
#endif      

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

}

//void SPI_ReadsComplete()  //Both SPI channels have been read
//{

//}

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

    if (MeasStatus & 0x0001) { //First word has already been read so this interrupt is due to the second word being read
        LATBbits.LATB2 = 1;
        LATGbits.LATG9 = 1; //BlueWire
        Meas |= (SPI1BUF >> 1);



        ZoomFollow_Measurements[ctr++] = Meas; //Measurement assembled, so copy to global variable. 
        if (ctr == INNERFILTERSAMPLES)
            ctr = 0;


        if (INNERFILTERSAMPLES > 1) {
            asm volatile ("disi #0x3FFF");
            FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & ZoomFollow_Measurements[0], &Inner_Filter);
            asm volatile ("disi #0x0000");
            ZoomFollow = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.
        } else
            ZoomFollow = ZoomFollow_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) {
    __asm__ volatile ("push CORCON");
    static int Meas = 0;
    static int ctr = 0;
    static int FocusFollow_Measurements[INNERFILTERSAMPLES];

    int outSamples[INNERFILTERSAMPLES];

    if (MeasStatus & 0x0002) { //First word has already been read so this interrupt is due to the second word being read
        Meas |= (SPI2BUF >> 1);


        FocusFollow_Measurements[ctr++] = Meas;
        if (ctr == INNERFILTERSAMPLES)
            ctr = 0;

        if (INNERFILTERSAMPLES > 1) {
            asm volatile ("disi #0x3FFF");
            FIR(INNERFILTERSAMPLES, &outSamples[0], (fractional *) & FocusFollow_Measurements[0], &Inner_Filter);
            asm volatile ("disi #0x0000");
            FocusFollow = outSamples[INNERFILTERSAMPLES - 1]; //Store current value for use later.
        } else
            FocusFollow = FocusFollow_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.  ~1092Hz

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
    //       {
    if (CAMERA_PWR == 1) //SPI Bus Clk won't run when camera is powered down because the DAC reference comes from the camera so that chip powers down with the camera, dragging the SPI clock signal down with it...
    {
        MeasStatus = 0;
        SPI1STATbits.SPIEN = 1;
        SPI2STATbits.SPIEN = 1;
        SPI1STATbits.SPIROV = 0;
        SPI2STATbits.SPIROV = 0;
        LATBbits.LATB2 = 0;
        LATGbits.LATG9 = 0; //BlueWire

        
        iTemp2 = ZoomFollow;
        int ModifiedZoomRate;
        if((ZoomFollow <= WIDEZOOMSTOP) && (SerialDataIn->ZoomRateTarget <= 0))  //At wide zoom limit and desiring to go wider...
           ModifiedZoomRate = -RateZeroAdjust;
        else
           ModifiedZoomRate = _Q15neg(_Q15add(SerialDataIn->ZoomRateTarget, RateZeroAdjust)); 
            
        iTemp1 = ModifiedZoomRate;
            
        int spi_dataout_DACA = ModifiedZoomRate;   

        int spi_dataout_DACB = _Q15neg(pl_interp(&Focus_interp, SerialDataIn->FocusTarget));      
        //int spi_dataout_DACB = _Q15neg(SerialDataIn->FocusTarget);



        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]);
    }


    //Read BottomSide Status bits

    bottom_status.bitfields.FwdWaterAlarm = ~FWD_H20_ALARM_ACTIVELOW;
    bottom_status.bitfields.AftWaterAlarm = ~AFT_H20_ALARM_ACTIVELOW;
    bottom_status.bitfields.CameraPowerState = CAMERA_PWR;
    bottom_status.bitfields.LaserPowerState = ~LASER_PWR_ACTIVELOW;
    bottom_status.bitfields.OverTemperatureState = ~OVERTEMP_ACTIVELOW;
    bottom_status.bitfields.ZoomExtendState = ~EXTENDER_ACTIVELOW;


    //Set Digital Outputs as needed
    EXTENDER_ACTIVELOW = ~(SerialDataIn->status.bitfields.ZoomExtendRequest);
    LASER_PWR_ACTIVELOW = ~SerialDataIn->status.bitfields.LaserPowerRequest;
    
    if(bottom_status.bitfields.OverTemperatureState == 0) 
      CAMERA_PWR = SerialDataIn->status.bitfields.CameraPowerRequest;
    else
      CAMERA_PWR = 0;  





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