#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 "isr.h"

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


//This DMA interrupt occurs at the rate of the TMR3 interrupt divided by the number of
// analog channels that are scanned.  (i.e TMR3 at 80kHz, 20 conversions of 8 channels = 500kHz
void __attribute__((interrupt, no_auto_psv)) _DMA0Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _DMA0Interrupt(void)
{
    int j;
   __asm__ volatile ("push CORCON");

    if (DMACS1 & 0x0001) //Point to one buffer or the other depending on which one caused interrupt
        Buff = BufferB;
    else
        Buff = BufferA;

    // BuffferA/BufferB and Buff are unsigned integers,
    // the ADC is set up to left shift the 12bit values 4, to become 16 bit unsined integers. because the ADC is set up to represent data as unsigned fractionals (integers),
    
    for (j = 0; j < FILTERSAMPLES; j++) {
        AN0_ADC = Buff[0 + NUMAIO * j]; //AN0
        AN1_ADC = Buff[1 + NUMAIO * j]; //AN1
        AN10_ADC = Buff[2 + NUMAIO * j]; //AN10
        AN11_ADC = Buff[3 + NUMAIO * j]; //AN11
        AN16_ADC = Buff[4 + NUMAIO * j]; //AN16
        AN17_ADC = Buff[5 + NUMAIO * j]; //AN17
        AN18_ADC = Buff[6 + NUMAIO * j]; //AN18
        AN19_ADC = Buff[7 + NUMAIO * j]; //AN19
    }

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

}


//TMR1 Interrupt  25Hz, updates time variable.
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
    static unsigned int j = 0;
    unsigned int seq_num = 0;
    static unsigned int toggle = 0;
    __asm__ volatile ("push CORCON");

    time = time + 0.04;

//    Toggle appropriate outputs
    if(++j == 25)
    {
    toggle = toggle ? 0 : 1;
    j = 0;
    PORTD = 0x100c*toggle;  //Toggle RD2, RD3, and RD12  (Gas Bypass enable, Stepper Enable, and Laser Rangefinder enable)
    PORTE = 0x7f*toggle;  //Toggle RE0-RE6  PWM1L/PWM1H/PWM2L/PWM2H/PWM3L/PWM3H/PWM4L
    PORTG = 0x8000*toggle; //Toggle   //EM cable tester enable
    }
   
    //Send data over CAN
    if (!C1TR01CONbits.TXREQ0) //If previous request for buffer zero isn't still waiting
    {
        AssembleMessageData_TwoFloats(&message[0], time, 0.0);
        message[0].id = Assemble_packetID(CAN_SRCID, 00, seq_num);
        AssembleCAN_Packet(0, &message[0]);
        C1TR01CONbits.TXREQ0 = 1; //Initiates a request
    }
    if (!C1TR01CONbits.TXREQ1) //If previous request for this buffer isn't still waiting
    {
        AssembleMessageData_FourInts(&message[1], AN0_ADC,AN1_ADC,AN10_ADC,AN11_ADC);
        message[1].id = Assemble_packetID(CAN_SRCID, 01, seq_num);
        AssembleCAN_Packet(1, &message[1]);
        C1TR01CONbits.TXREQ1 = 1; //Initiates a request
    }
    if (!C1TR23CONbits.TXREQ2) //If previous request for this buffer isn't still waiting
    {
        AssembleMessageData_FourInts(&message[2], AN16_ADC,AN17_ADC,AN18_ADC,AN19_ADC);
        message[2].id = Assemble_packetID(CAN_SRCID, 02, seq_num);
        AssembleCAN_Packet(2, &message[2]);
        C1TR23CONbits.TXREQ2 = 1; //Initiates a request
    }
    if (!C1TR23CONbits.TXREQ3) //If previous request for this buffer isn't still waiting
    {
        AssembleMessageData_FourInts(&message[3], PORTA, PORTB, PORTC, PORTD);

        message[3].id = Assemble_packetID(CAN_SRCID, 03, seq_num);
        AssembleCAN_Packet(3, &message[3]);
        C1TR23CONbits.TXREQ3 = 1; //Initiates a request
    }
    if (!C1TR45CONbits.TXREQ4) //If previous request for this buffer isn't still waiting
    {
        AssembleMessageData_FourInts(&message[4], PORTE, PORTF, PORTG,0);
        message[4].id = Assemble_packetID(CAN_SRCID, 04, seq_num);
        AssembleCAN_Packet(4, &message[4]);
        C1TR45CONbits.TXREQ4 = 1; //Initiates a request
    }
//    if (!C1TR45CONbits.TXREQ5) //If previous request for this buffer isn't still waiting
//    {
//        AssembleMessageData_TwoFloats(&message[5], 100 * ((float) P1DC1 / 4096), 100 * ((float) P1DC4 / 4096));
//        message[5].id = Assemble_packetID(CAN_SRCID, 05, seq_num);
//        AssembleCAN_Packet(5, &message[5]);
//        C1TR45CONbits.TXREQ5 = 1; //Initiates a request
//    }
//    if (!C1TR67CONbits.TXREQ6) //If previous request for this buffer isn't still waiting
//    {
//        AssembleMessageData_TwoFloats(&message[6], RPMs, Torque);
//        message[6].id = Assemble_packetID(CAN_SRCID, 06, seq_num);
//        AssembleCAN_Packet(6, &message[6]);
//        C1TR67CONbits.TXREQ6 = 1; //Initiates a request
//    }

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

}


void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON))))    _QEIInterrupt(void)
{
    char s[255];
    CANmsg RcvdMessage;
    char TmpMode;
    unsigned int locBridgeDC_OverRide, locLoadDC_OverRide;
    unsigned int locTargetVoltage;
    unsigned int locSpecificPower;
    int err;


    __asm__ volatile ("push CORCON");

    if (C1INTFbits.RBIF) /* check to see if the interrupt is caused by receive */ {
        /*check to see if buffer 8 is full, indicating CmdID was 50, and a mode change is being requested */
        if (C1RXFUL1bits.RXFUL8) {
            DisAssembleCAN_Packet(8, &RcvdMessage);
            sprintf(s, "Buf8 %d     %d %d %d       ", RcvdMessage.data_length, RcvdMessage.data[0], RcvdMessage.data[1], RcvdMessage.data[2]);
            putsU1(s);

            switch (RcvdMessage.data[0]) {

#ifdef BATT_CHARGER_MODE
                case BATT_CHARGER_MODE: //Fixed Scale Factor Battery Charging Mode.
                    //Assign Arguments only if all are within range and message length matches.
                    //if ((RcvdMessage.data_length == 3) &&
                    //        (RcvdMessage.data[1] < MAX_SCALE_FACTOR) &&
                    //        (RcvdMessage.data[1] > MIN_SCALE_FACTOR) &&
                    //        (RcvdMessage.data[2] < MAX_RETRACT_FACTOR) &&
                    //        (RcvdMessage.data[2] > MIN_RETRACT_FACTOR)) {
                    //    scale_factor = RcvdMessage.data[1];
                    if ((RcvdMessage.data_length == 3) &&
                            (RcvdMessage.data[1] <= 100 * MAX_SPECIFIC_POWER) &&
                            (RcvdMessage.data[1] >= 100 * MIN_SPECIFIC_POWER) &&
                            (RcvdMessage.data[2] < MAX_RETRACT_FACTOR) &&
                            (RcvdMessage.data[2] > MIN_RETRACT_FACTOR)) {
                        SpecificPower = ((float) RcvdMessage.data[1] / 100);  //For this packet, specific power is encoded as a byte...
                        retract_factor = RcvdMessage.data[2];
                        Mode = BATT_CHARGER_MODE;
                        sprintf(s, "%lx Mode = %d  Specific Power = %f   Retract Factor = %d\r\n", RcvdMessage.id, Mode, SpecificPower, retract_factor);
                        putsU2(s);
                    }
                    break;
#endif

#ifdef SPECIFIED_VOLTAGE_AND_SPECIFICPOWER_MODE
                case SPECIFIED_VOLTAGE_AND_SPECIFICPOWER_MODE: //Direct Set of Target Voltage and Load DC
                    //Assign Arguments only if all are within range and message length matches.
                    locTargetVoltage = (RcvdMessage.data[2] << 8) + RcvdMessage.data[1]; //Set target voltage here, put it into register after control loop has recognized this is the mode.
                    locSpecificPower = (RcvdMessage.data[4] << 8) + RcvdMessage.data[3]; //Set Load DC Over-ride value here, put it into register after control loop has recognized this is the mode.
                    sprintf(s, "%lx Mode = %d Voltage Target = %d   SpecificPower = %d\r\n", RcvdMessage.id, 10, locTargetVoltage, locSpecificPower);

                    if ((locTargetVoltage <= MAX_TARGET_VOLTAGE) &&
                            (locTargetVoltage >= 10) &&
                            (locSpecificPower <= 4096 * MAX_SPECIFIC_POWER) &&
                            (locSpecificPower >= 4096 * MIN_SPECIFIC_POWER)) {
                        SpecificPower = ((float) locSpecificPower) / 4096;
                        f_TargetVoltage = ConvertEngUnitsToFract((float) locTargetVoltage, VBus_cnt, VBus_eng); //Set Voltage target in same scaling as measured bus voltage (signed fractional)
                        Mode = SPECIFIED_VOLTAGE_AND_SPECIFICPOWER_MODE;
                        putsU2(s);
                    }
                    break;
#endif


#ifdef SPECIFIED_DUTYCYCLE_MODE 
                case SPECIFIED_DUTYCYCLE_MODE: //Bridge and Load duty cycles are specified by user.  Dangerous to set Bridge DC too low.
                    if (RcvdMessage.data_length == 5) {
                        locBridgeDC_OverRide = (RcvdMessage.data[2] << 8) + RcvdMessage.data[1]; //Set Bridge DC Over-ride value here, put it into register after control loop has recognized this is the mode.
                        locLoadDC_OverRide = (RcvdMessage.data[4] << 8) + RcvdMessage.data[3]; //Set Load DC Over-ride value here, put it into register after control loop has recognized this is the mode.

                        if ((locBridgeDC_OverRide <= 4096) &&
                                (locBridgeDC_OverRide >= 4096 * MinDC) &&
                                (locLoadDC_OverRide <= 4096) &&
                                (locLoadDC_OverRide >= 0)) {
                            BridgeDC_OverRide = locBridgeDC_OverRide;
                            LoadDC_OverRide = locLoadDC_OverRide;
                            Mode = SPECIFIED_DUTYCYCLE_MODE;
                            sprintf(s, "%lx Mode = %d  Bridge DC = %d   Load DC = %d\r\n", RcvdMessage.id, 10, locBridgeDC_OverRide, locLoadDC_OverRide);
                            putsU2(s);
                        }
                    }
                    break;
#endif


                    //Fallthrough
                    sprintf(s, "Unknown Mode Type set\r\n");
                    putsU1(s);
                    break;

            }
            C1RXFUL1bits.RXFUL8 = 0;
        }

        /* clear flag */
        C1INTFbits.RBIF = 0;
    }
    else {
        if (C1INTFbits.TBIF) //If interrupt is caused by transmit. 
        {
            /* clear flag */
            C1INTFbits.TBIF = 0;
        }
    }
    /* clear interrupt flag */
    IFS2bits.C1IF = 0;


    __asm__ volatile ("pop CORCON");

}
