#include "xc.h"
#include "config.h"
#include "init.h"
#include "extern.h"
#include "CalibrationConstants.h"
#include "../CommonCode/conv.h"
#include "libq.h"
#include "../CommonCode/crc.h"
#include "../CommonCode/pl_interp.h"

fractional abcCoefficient_ChanA[3] __attribute__((section(".xbss")));
fractional controlHistory_ChanA[3] __attribute__((section(".ybss"), far)); //Use this with small data memory model (the default)

fractional abcCoefficient_ChanB[3] __attribute__((section(".xbss")));
fractional controlHistory_ChanB[3] __attribute__((section(".ybss"), far)); //Use this with small data memory model (the default)

void InitIO()
{

    //Setup digital I/O
    //Setup digital outputs
    TRISA = 0x0000;
    TRISAbits.TRISA9 = 1; //Don't make this an output, it's connected to ground as Vref-.
    TRISAbits.TRISA10 = 1; //Don't make this an output, it's connected to 3.3V as Vref+.
    
    TRISB = 0x0000;
    TRISBbits.TRISB11 = 1; //Set Pin 35 as input, required for ADC AN11 to work correctly.
    TRISBbits.TRISB10 = 1; //Set Pin 34 as input, required for ADC AN10 to work correctly.
    TRISBbits.TRISB6 = 1; //Set as input for ICD, not sure if this is required, seems unlikely...
    TRISBbits.TRISB7 = 1; //Set as input for ICD, not sure if this is required, seems unlikely...
    TRISBbits.TRISB2 = 0; //SS1 set as output
    
    //TRISC = 0x0000;  //Need to find out why setting all TRISC=0 disables UART. For now set them one by one..
    TRISCbits.TRISC1 = 0; 
    TRISCbits.TRISC2 = 0;     
    TRISCbits.TRISC3 = 0; 
    TRISCbits.TRISC4 = 0;    
    
    
    TRISD = 0x0000;
               
    TRISE = 0x0000;
    TRISEbits.TRISE5 = 1; //LASER_PWR_SW
    TRISEbits.TRISE6 = 1; //CAMERA_PWR_SW
    TRISEbits.TRISE7 = 1; //SPARE_SW
    TRISEbits.TRISE8 = 1; //SW_CHAGE_ACTIVELOW
    TRISEbits.TRISE1 = 1; //JOYSTCK_REV_SW
    TRISEbits.TRISE2 = 1; //STBD_CAM_SW
    TRISEbits.TRISE3 = 1; //STBD_LIGHT_SW
    TRISEbits.TRISE4 = 1; //EXTENDER_SW
    
    
    
    TRISF = 0x0000;
    TRISFbits.TRISF2 = 1; //U1 Rx
    TRISFbits.TRISF3 = 0; //U1 Tx  
    TRISFbits.TRISF4 = 1; //U2 Rx
    TRISFbits.TRISF5 = 0; //U2 Tx  
    TRISFbits.TRISF6 = 0; //SCK1
    TRISFbits.TRISF7 = 1; //SDI1
    TRISFbits.TRISF8 = 0; //SD01
    TRISFbits.TRISF12 = 1; //U2CTS Set as input
    TRISFbits.TRISF13 = 0; //U2RTS Set as output
    
    
    TRISG = 0x0000;
    TRISGbits.TRISG6 = 1; //SCK2
    TRISGbits.TRISG7 = 1; //SDI2
    TRISGbits.TRISG9 = 1; //SS2
    TRISGbits.TRISG14 = 1; //JOYSTICK_SEN_SW
    TRISGbits.TRISG12 = 1; //PORT_LIGHT_SW
    TRISGbits.TRISG13 = 1; //PORT_CAM_SW
    
    
//Set Outputs to defaults    
    LASER_PWR_LED = 0;
    CAM_PWR_LED = 0;
    SPARE_LED = 0;
    SW_ENABLE_ACTIVELOW = 0;
    EXTENDER_LED = 0;
    HUM_ALARM_LED = 0;
    WATER_ALARM_LED = 0;   
    TEMP_ALARM_LED = 0;
    PORT_CAM_LED = 0;
    PORT_LIGHT_LED = 0;
    JOYSTCK_SEN_LED = 0;
    JOYSTCK_REV_LED = 0;
    STBD_CAM_LED = 0;
    STBD_LIGHT_LED = 0;
    EEPROM_WP = 0;
    
    AUXIO_PIN1 = 0;
    AUXIO_PIN2 = 0;
    AUXIO_PIN3 = 0;
    AUXIO_PIN5 = 0;
    
}


void InitVariables()
{
    SerialDataIn = &SerialDataIn_A;  //Start by pointing at A buffer.
    
    SerialDataOut.preamble = 0x5555;
    SerialDataOut.crc =  calcCRC((const char*) &SerialDataOut,sizeof(BottomToTopDataPacket)-2);
            
    
    ComputeScaling();
            
    TopStatus = 0;
    BottomStatus = 0;
   
    
 //   int Zoom_Y[4] = {-32768, 0, 0, 32767};
 //   int Zoom_X[4] = {ZOOM_ROCKER_MIN_CNTS, ZOOM_ROCKER_ZERO_CNTS-ZOOM_ROCKER_DEADBAND_CNTS/2, ZOOM_ROCKER_ZERO_CNTS+ZOOM_ROCKER_DEADBAND_CNTS/2, ZOOM_ROCKER_MAX_CNTS};   //Must be monotonically increasing unique values 
 //   pl_interp_init(&Zoom_interp,4,Zoom_X,Zoom_Y);

    int Zoom_Y[6] = {-32768, -ZOOM_ROCKER_LOWRATE_HEIGHT, 0, 0, ZOOM_ROCKER_LOWRATE_HEIGHT, 32767};
    int Zoom_X[6] = {ZOOM_ROCKER_MIN_CNTS,
                     ZOOM_ROCKER_ZERO_CNTS-ZOOM_ROCKER_DEADBAND_CNTS/2 - ZOOM_ROCKER_LOWRATE_WIDTH,
                     ZOOM_ROCKER_ZERO_CNTS-ZOOM_ROCKER_DEADBAND_CNTS/2, 
                     ZOOM_ROCKER_ZERO_CNTS+ZOOM_ROCKER_DEADBAND_CNTS/2, 
                     ZOOM_ROCKER_ZERO_CNTS+ZOOM_ROCKER_DEADBAND_CNTS/2 + ZOOM_ROCKER_LOWRATE_WIDTH, 
                     ZOOM_ROCKER_MAX_CNTS};   //Must be monotonically increasing unique values 
    pl_interp_init(&Zoom_interp,6,Zoom_X,Zoom_Y);
    
    int Focus_Y[2] = {-32768, 32767};
    int Focus_X[2] = {FOCUS_SLIDER_MIN_CNTS, FOCUS_SLIDER_MAX_CNTS};  //Must be monotonically increasing unique values 
    pl_interp_init(&Focus_interp,2,Focus_X,Focus_Y);
    
}

void InitPIDGains()
{

    //Set up PID structures.
   
    
    ChanA_PID.abcCoefficients = &abcCoefficient_ChanA[0];
    ChanA_PID.controlHistory = &controlHistory_ChanA[0];
    PIDInit(&ChanA_PID);
    kCoeffs_ChanA[0] = Q15(.9); //Tuned 12/10/19 //Prop Gain  These gains must be between 0-1, to get
    kCoeffs_ChanA[1] = Q15(0.012); //Tuned 12/10/19 //Int Gain    higher gain in the system, the output of the PID
    kCoeffs_ChanA[2] = Q15(0.0); //Der Gain    controller must be scaled up.
    PIDCoeffCalc((fractional *) & kCoeffs_ChanA[0], &ChanA_PID);
    
    ChanB_PID.abcCoefficients = &abcCoefficient_ChanB[0];
    ChanB_PID.controlHistory = &controlHistory_ChanB[0];
    PIDInit(&ChanB_PID);
    kCoeffs_ChanB[0] = Q15(.9);  //Tuned 12/10/19 //Prop Gain  These gains must be between 0-1, to get
    kCoeffs_ChanB[1] = Q15(0.012); //Tuned 12/10/19 //Int Gain    higher gain in the system, the output of the PID
    kCoeffs_ChanB[2] = Q15(0.0); //Der Gain    controller must be scaled up.
    PIDCoeffCalc((fractional *) & kCoeffs_ChanB[0], &ChanB_PID);


    
}

void InitDma0()  //DMA channel 0 is used to transfer data at the end of the ADC conversion process
{
DMA0CONbits.AMODE = 0; // Configure DMA for Register indirect with post increment (used with conversion order mode)
DMA0CNT = NUMAIO*INNERFILTERSAMPLES-1;

DMA0CONbits.MODE = 2; // Configure DMA for Continuous mode with ping-pong
DMA0PAD = (int)&ADC1BUF0;
DMA0REQ = 13;  //Select ADC1 convert done
DMA0STA = __builtin_dmaoffset(BufferA);
DMA0STB = __builtin_dmaoffset(BufferB);
_DMA0IP = 5;
IFS0bits.DMA0IF = 0; //Clear the DMA interrupt flag bit
IEC0bits.DMA0IE = 1; //Set the DMA interrupt enable bit
DMA0CONbits.CHEN = 1;

}




// This sets up the ADC sample and convert sequence.  This ADC is set up to run in scan mode,
// it cycles throught the specified channels.  After all are converted a DMA transfer
// occurs which transfers all values into memory locations.
// This conversions are started in the PWM interrupt, it cycles through the number of specified channels (NUMIO))
// and samples (INNERFILTERSAMPLES) and after a total of NUMAIO*INNERFILTERSAMPLES samples initiates a DMA interrupt 
// indicating that the conversion is done. 
// With this interrupt set to occur at xxkHz, and x channels to be scanned, new
// analog data is available at xkHz, although the samples were all taken x.xx microseconds apart.

void InitADC_WithIntrpt()
{
    AD1CON1bits.SSRC = 2;  //Clock source select -  010 causes Timer3 compare to end sampling and start conversion

    AD1CON1bits.ASAM = 1; //Auto-sample start bit, this is enabled in PWM interupt.
    AD1CON1bits.AD12B = 1; //12 bit mode, disables simultaneous sampling
    AD1CON1bits.FORM = 2; //Report Results as un-signed Integers.  i.e. dddd dddd dddd 0000 is the twelve bits

    AD1CON2bits.VCFG = 3; //Use VRef+/VRef- as reference, driven by precision reference in our board.
    AD1CON2bits.BUFM = 0;
    AD1CON2bits.CSCNA = 1;  //ScanINput Selections for CH0+ during Sample A bit
    AD1CON2bits.CHPS = 0;  //Converts CH0

    AD1CON3bits.ADCS = 8;  //Conversion clock select - Conversion time is 5*Tcy, see datasheet
    AD1CON3bits.SAMC = 0x1f;  //Auto-sample time bits.  This is only used in SSRC = 7...
       
    AD1PCFGH = AIOMASK_H; // select specified analog input pins
    AD1PCFGL = AIOMASK_L; // select specified analog input pins

    AD1CSSH = ~AIOMASK_H; // Scan specified inputs
    AD1CSSL = ~AIOMASK_L; // Scan specified inputs
//Use Conversion Order mode
    AD1CON1bits.ADDMABM = 1;  //DMA buffers are built in conversion order mode.
    AD1CON2bits.SMPI = NUMAIO-1;

# if INNERFILTERSAMPLES == 1
    AD1CON4bits.DMABL = 0;
# endif
# if INNERFILTERSAMPLES == 2
    AD1CON4bits.DMABL = 1;
# endif
# if INNERFILTERSAMPLES == 4
    AD1CON4bits.DMABL = 2;
# endif
# if INNERFILTERSAMPLES == 8
    AD1CON4bits.DMABL = 3;
# endif
# if INNERFILTERSAMPLES == 16
    AD1CON4bits.DMABL = 4;
# endif
# if INNERFILTERSAMPLES == 32
    AD1CON4bits.DMABL = 5;
# endif
# if INNERFILTERSAMPLES == 64
    AD1CON4bits.DMABL = 6;
# endif
# if INNERFILTERSAMPLES == 128
    AD1CON4bits.DMABL = 7;
# endif


    _AD1IF = 0;  //Clear Interrupt
    _AD1IE = 0;  //Do not Enable ADC Interrupt
    AD1CON1bits.ADON = 0;  //Don't enable ADC yet, wait until everything else is set up and enable in main

}




void InitSPI() {

    SPI1CON1bits.DISSCK = 0;
    SPI1CON1bits.DISSDO = 0;
    SPI1CON1bits.MODE16 = 1;
    SPI1CON1bits.SMP = 0;
    SPI1CON1bits.CKE = 0;
    SPI1CON1bits.SSEN = 0;
    SPI1CON1bits.CKP = 0;
    SPI1CON1bits.MSTEN = 1;
    SPI1CON1bits.SPRE = 4; //4;
    SPI1CON1bits.PPRE = 2;

    _SPI1IP = 6;
    _SPI1IF = 0;          // Clear the Interrupt flag
    _SPI1IE = 1;           // Enable the interrupt



    SPI1STATbits.SPIROV = 0;
    SPI1STATbits.SPIEN = 0;  //Keep Disabled for now

    SPI2CON1bits.DISSCK = 0;
    SPI2CON1bits.DISSDO = 0;
    SPI2CON1bits.MODE16 = 1;
    SPI2CON1bits.SMP = 0;
    SPI2CON1bits.CKE = 0;
    SPI2CON1bits.SSEN = 1; 
    SPI2CON1bits.CKP = 0;
    SPI2CON1bits.MSTEN = 0;


    _SPI2IP = 6;
    _SPI2IF = 0;           // Clear the Interrupt flag
    _SPI2IE = 1;           // Don't Enable the interrupt, not needed


    //SPI2STATbits.SPIEN = 0;  //Keep SPI2 disabled for now


}