//###########################################################################
// Edited: WW   02/27/2020
// FILE:    Long_max_DCDC_28388D_cpu01.c
//
// TITLE:   DC/DC CCCV charging control with SPI
//
//***** using relaxed float point operation *** if needed, change it to restrict in the build option
//!
//###############################################################################
#include "F28x_project.h"
#include "math.h"
//#include "F2837xD_usDelay.asm"

#include "shared_var_longmax_28379D.h"              // defined shared variable with CPU2
#include "local_var_longmax_28379D_cpu01.h"         // defined local variable for CPU1
#include "Control_function_variable_declare.h"

//new
extern volatile struct DAC_REGS* DAC_PTR[4];
//extern volatile struct EPWM_REGS* ePWM[11];
extern volatile struct SPI_REGS* SPIs[3];
//typedef     unsigned char   uint8_t;                            // 1 byte 0 to 255
#include "SFO_V8.h"

//####################################################################################
//.................................... Function Prototypes...................................

float CVController(float Vbatt_error, float Vbatt_error_I_k, float Ibatt_ref_uppder,float Ibatt_ref_lower); // CV controller declaration
float CCController(float I_ref_CV, float duty_fwd, float duty_max, float duty_min, float Ibatt_error, float Ibatt_error_I_k);   //  CC controller declaration
void reset_controller();                                        // reset the variables for controller after EN_AC_controller = 0
void reset_PIerror();                                   // reset the PI errors and make the duty cycle = duty_fwd
//####################################################################################
//....................................... Interruption PIE.........................
//__interrupt void epwmA_isr(void);
__interrupt void epwmADC_isr(void);
__interrupt void epwmcontrol_isr(void);
//####################################################################################
//............................................. Main.............................
void main(void)
{

//* Step 1. Initialize System Control:
//* PLL, WatchDog, enable Peripheral Clocks
//* This example function is found in the F2837xS_SysCtrl.c file.


    EALLOW;
        InitSysCtrl();
//        InitIpc();
    EDIS;

    //Give control of SPI-A to CPU2
    //
    EALLOW;
        DevCfgRegs.CPUSEL6.bit.SPI_A = 1;
        MemCfgRegs.GSxMSEL.bit.MSEL_GS1 = 1;      // Assign the ownership to CPU2 to write
//    CpuSysRegs.SECMSEL.bit.PF1SEL = 1;      // Set EPWM Secondary Master to DMA
//    MemCfgRegs.GSxMSEL.bit.MSEL_GS2 = 1;    // Give CPU2 control of GS2
    EDIS;

//    EALLOW;                                                         //  Allow CPU to write to protected control regs
//        CpuSysRegs.PCLKCR0.bit.TBCLKSYNC            = 0;            //  ePWM clock sync
//    EDIS;

    //comment this back in
//    #ifdef _STANDALONE
//    #ifdef _FLASH
//
//         Send boot command to allow the CPU02 application to begin execution
//
//       IPCBootCPU2(C1C2_BROM_BOOTMODE_BOOT_FROM_FLASH);
//    #else
//
//         Send boot command to allow the CPU02 application to begin execution
//
//       IPCBootCPU2(C1C2_BROM_BOOTMODE_BOOT_FROM_RAM);
//    #endif
//    #endif

//* Step 2. Initialize GPIO:
//* This example function is found in the F2837xS_Gpio.c file and
//* illustrates how to set the GPIO to its default state.


    //initializations
    InitGpio();
    GPIO_relay_longmax();                                           // relay and signal GPIOs configuration.  Relays initialize as open
    open_main_grid_relay();                                         // manually open all relays just in case
    open_precharge_grid_relay();
    open_main_batt_relay();
    open_precharge_batt_relay();
    gate_driver_DESAT_init(); //initialize gate driver DESAT GPIO
    gate_driver_EN_init(); //initialize gate driver EN GPIO
    gate_driver_RST_init(); //initialize gate driver RST GPIO
    gate_driver_RDY_init(); //initialize gate driver RDY GPIO
    gate_driver_disable(); //we will enable when all protection checks are passed
    GPIO_temp_meas_select_init();
    InitspiaGpio();

    //******************* Modify this accordingly for different EPWM source*******************//
    //* Configure the ADC and DAC, then power it up
    //ConfigureADC(adc_prescale, ADC_SIGNALMODE_SINGLE, acq_window, adc_resolution, 0x05);     //**** 0x05should change the last entry (trigger source) accordingly when change EPWM (Table 20-32 28388D reference manual)
    ConfigureADC(adc_prescale, ADC_SIGNALMODE_SINGLE, acq_window, adc_resolution, 0x15);     //**** should change the last entry (trigger source) accordingly when change EPWM (Table 20-32 28388D reference manual)
    //***************************************************************************************//


    InitEPWMsGpio(EPWM_A);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_B);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_C);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_D);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_E);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_F);                                               // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_fan_a_b);                                           // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_fan_c_d);                                           // Configure the GPIOs' pull-resistors on ePWMs
    InitEPWMsGpio(EPWM_fan_e_f);
    //just for testing!!!
//    InitEPWMsGpio(EPWM_adc_interrupt);                                           // Configure the GPIOs' pull-resistors on ePWMs
//    InitEPWMsGpio(EPWM_control_interrupt);                                           // Configure the GPIOs' pull-resistors on ePWMs

    InitspiaGpio();

//* Step 3. Clear all interrupts and initialize PIE vector table:
//* Disable CPU interrupts

    DINT;
    IER = 0x0000;   //* Disable CPU interrupts and clear all CPU interrupt flags:
    IFR = 0x0000;

//* Initialize the PIE control registers to their default state.
//* The default state is all PIE interrupts disabled and flags
//* are cleared.
//* This function is found in the F2837xS_PieCtrl.c file.

    InitPieCtrl();

//* Initialize the PIE vector table with pointers to the shell Interrupt
//* Service Routines (ISR).
//* This will populate the entire table, even if the interrupt
//* is not used in this example.  This is useful for debug purposes.
//* The shell ISR routines are found in F2837xS_DefaultIsr.c.
//* This function is found in F2837xS_PieVect.c.

    InitPieVectTable();


//* Interrupts that are used in this example are re-mapped to
//* ISR functions found within this file.

    //******************* Modify this accordingly for different EPWM source*******************//
    EALLOW;                                                         // This is needed to write to EALLOW protected registers
            //old and good
            //PieVectTable.EPWM9_INT   = &epwmADC_isr;                      // Interrupt service routine for ePWM9  (THIS IS PRIORATY!! BECAUSE THE CONTROL IS IN THE FUNCTION. PIE3 > PIE7 inherently, so epwm will be served first)

            //new and questionable
            PieVectTable.ADCA1_INT   = &epwmADC_isr;
    //        PieVectTable.ADCD1_INT   = &epwmADC_isr;
            PieVectTable.EPWM10_INT   = &epwmcontrol_isr;                      // Interrupt service routine for ePWM10  (THIS IS PRIORATY!! BECAUSE THE CONTROL IS IN THE FUNCTION. PIE3 > PIE7 inherently, so epwm will be served first)
        EDIS;                                                              // This is needed to disable write to EALLOW protected registers
    //***************************************************************************************//

//* calculate the execution time
    configTimer();

    //this is fine
//* Step 4. Initialize the Device Peripherals:
    if (EN_SPI_DMA == 1)
    {

    }
         //DMA not working yet

    else
    {
        spi_fifo_init(SPI_use,fifo_rxlvl, fifo_txlvl, SPI_delay);                        // Initialize the SPI FIFO, FIFO is a buffer between TX and DTA
        Initspi(SPI_use,SLAVE,DIS_LOOPBACK,SPI_CPOL,SPI_CPHA,WORD_BIT);                  // Configure SPIA properties on 28389D
//        Initspi(SPI_use,MASTER,EN_LOOPBACK,SPI_CPOL,SPI_CPHA,WORD_BIT);                // Self-test setting (loop-back)
    }
    //to here

    EALLOW;
        CpuSysRegs.PCLKCR0.bit.TBCLKSYNC = 0;       // Disable the clock sync when initialize the epwms
    EDIS;
    // Initialize the EPWMs
    InitUPDOWNEPWM(EPWM_A,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[0],0);    // Phase A
    InitUPDOWNEPWM(EPWM_B,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[1],0);    // Phase B
    InitUPDOWNEPWM(EPWM_C,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[2],0);    // Phase C
    InitUPDOWNEPWM(EPWM_D,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[3],0);    // Phase D
    InitUPDOWNEPWM(EPWM_E,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[4],0);    // Phase E
    InitUPDOWNEPWM(EPWM_F,EPWM_PRD7,EPWM_DBRED,EPWM_DBFED,0,duty_phase[5],0);    // Phase F
/*
    InitUPDOWNEPWM(EPWM_fan_grid,EPWM_PRD_fan,0,0,0,duty_fan_grid,0); // Phase A B C fans
    InitUPDOWNEPWM(EPWM_fan_batt,EPWM_PRD_fan,0,0,0,duty_fan_batt,0); // Phase D E F fans
*/

    InitfanEPWM(EPWM_fan_a_b,EPWM_PRD_fan,0,duty_fan[0], duty_fan[1]); // Phase A B fans
    InitfanEPWM(EPWM_fan_c_d,EPWM_PRD_fan,0,duty_fan[2], duty_fan[3]); // Phase C D fans
    InitfanEPWM(EPWM_fan_e_f,EPWM_PRD_fan,0,duty_fan[4], duty_fan[5]); // Phase E F fans

//    InitfanEPWM(EPWM_fan_AB,EPWM_PRD_fan,0,duty_fan[0], duty_fan[1]); // Phase A B  fans
//    InitfanEPWM(EPWM_fan_CD,EPWM_PRD_fan,0,duty_fan[2], duty_fan[3]); // Phase C D  fans
//    InitfanEPWM(EPWM_fan_EF,EPWM_PRD_fan,0,duty_fan[4], duty_fan[5]); // Phase E F  fans

    InitUPDOWNEPWM(EPWM_adc_interrupt,EPWM_PRD_ADC,0,0,0,.5,1); // ADC trigger pwm9
    InitUPDOWNEPWM(EPWM_control_interrupt,EPWM_PRD_control,0,0,0,.5,2); // control update pwm10

    EALLOW;
        CpuSysRegs.PCLKCR0.bit.TBCLKSYNC = 1;       // enable the clock sync after we initialize the epwms
    EDIS;

    //is this the right way to set up the epwm??



    (*ePWM[EPWM_A]).TBCTL.bit.SYNCOSEL = TB_CTR_ZERO;                      // Manually overwrite the sync down-stream module from phase A (epwm7)
    (*ePWM[EPWM_B]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase B (epwm8)
    (*ePWM[EPWM_C]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase C (epwm9)
    (*ePWM[EPWM_D]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN; //TB_CTR_ZERO;                      // Manually overwrite the sync down-stream module from phase A (epwm7)
    (*ePWM[EPWM_E]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase B (epwm8)
    (*ePWM[EPWM_F]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase C (epwm9)

//    (*ePWM[EPWM_fan_grid]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase C (epwm9)
//    (*ePWM[EPWM_fan_batt]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase C (epwm9)

    (*ePWM[EPWM_fan_a_b]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase
    (*ePWM[EPWM_fan_c_d]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase
    (*ePWM[EPWM_fan_e_f]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase

    (*ePWM[EPWM_adc_interrupt]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;                                // Manually overwrite the sync down-stream module from phase C (epwm9)
    (*ePWM[EPWM_control_interrupt]).TBCTL.bit.SYNCOSEL = TB_SYNC_IN;

    EALLOW;
        SyncSocRegs.SYNCSELECT.bit.SYNCOUT = 00;
        SyncSocRegs.SYNCSELECT.bit.EPWM4SYNCIN = 000;
        SyncSocRegs.SYNCSELECT.bit.EPWM7SYNCIN = 000;
        SyncSocRegs.SYNCSELECT.bit.EPWM10SYNCIN = 000;
    EDIS;

/*
//Interleaving (phase shift)
        (*ePWM[EPWM_B]).TBCTL.bit.PHSEN = TB_ENABLE;                     // (Overwrite) Enable phase loading
        (*ePWM[EPWM_C]).TBCTL.bit.PHSEN = TB_ENABLE;                     // (Overwrite) Enable phase loading
        EPWM_phaseshiftupdate(EPWM_B,phase_shift_B);                     // update the phase shift
        EPWM_phaseshiftupdate(EPWM_C,phase_shift_C);                     // update the phase shift
*/

//    EALLOW;                                                         //  Allow CPU to write to protected control regs
//        if (EN_SPI_DMA == 1)
//            CpuSysRegs.SECMSEL.bit.PF2SEL               = 1;            //  ensure DMA is connected to Peripheral Frame 2 bridge (EALLOW protected) <Not working for 28388D>
//
//          // adding this in causes ADC c to not read
//        CpuSysRegs.PCLKCR0.bit.TBCLKSYNC            = 1;            //  ePWM clock sync
//        ClkCfgRegs.SYSCLKDIVSEL.bit.PLLSYSCLKDIV    = 0;            //  PLLSCLK = SYSCLK
//        ClkCfgRegs.PERCLKDIVSEL.bit.EPWMCLKDIV      = 0;            //  No clock division--EPWMCLK = PLLSYSCLK
//    EDIS;                                                           //  Protect the control regs again



    // Transmitting data initialization
   //
    for (i=0;i<ADC_trans_num;i++)
    {
        sData_32[i] = 0x0000;
        rData_32_local[i] = 0.0;
    }

    /*
     * Separate higher and lower 8bits from 16-bit data
     */

//    for (i = 0;i<7;i++)
//    {
//            // 16 bit data to 2 8-bit data
//            sData_L8[i] = sData[i] & (0x00FF);                          // extract lower 8 bits from 16 bit data (NEED TO SHIFT TO THE LEFT 8 BITS due to the DTA is left justified)
//            sData_H8[i] = sData[i] & (0xFF00);                          // higher 8bits from 16 bit data
//
//    }

//* Step 5. User specific code, enable interrupts:
//* Initialize counters:
//* enable CPU INTx which is connected to EPWM 7-8 INT:



    //Configuring interrupts
        IER |= M_INT1; // enable group 1 interrupts (ADC)
        IER |= M_INT3; // enable group 3 interrupts (ePWM)
        EINT;                                                                 // enable Global interrupt INTM
        ERTM;                                                                 // enable Global realtime interrupt DBGM
        PieCtrlRegs.PIECTRL.bit.ENPIE = 1;                                //  enable the PIE block
        PieCtrlRegs.PIEIER1.bit.INTx1 = 1; //Enable ADCINTA1 in PIE table (for ADC interrupt)
    //    PieCtrlRegs.PIEIER1.bit.INTx6 = 1; //Enable ADCINTA1 in PIE table (for ADC interrupt)
        PieCtrlRegs.PIEIER3.bit.INTx10 = 1; //Enable EPWM10INT in PIE table (for control interrupt)



    if (EN_SPI_DMA == 0)                                                  // Using SPI only
    {
        PieCtrlRegs.PIEIER6.bit.INTx1     = 1;                            //  enable PIE Group 6, INT 1 SPIA RX
        PieCtrlRegs.PIEIER6.bit.INTx2     = 1;                            //  enable PIE Group 6, INT 2 SPIA TX
    }
    else                                                                  // enable DMA for SPI
    {
        PieCtrlRegs.PIEIER7.bit.INTx5     = 1;                            //  enable PIE Group 7, INT 5 (DMA CH5)
        PieCtrlRegs.PIEIER7.bit.INTx6     = 1;                            //  enable PIE Group 7, INT 6 (DMA CH6)
    }

//* enable global Interrupts and higher priority real-time debug events:
    EINT;                                                                 // enable Global interrupt INTM
    ERTM;                                                                 // enable Global realtime interrupt DBGM

    if (EN_SPI_DMA == 1)                                                  // enable DMA for SPI
    {
        StartDMACH6();                                                    // Start SPI RX DMA channel
        StartDMACH5();                                                    // Start SPI TX DMA channel
    }


//* Step 6. IDLE loop. Just sit and loop forever:
    StopEPWM(EPWM_A);
    StopEPWM(EPWM_B);
    StopEPWM(EPWM_C);
    StopEPWM(EPWM_D);
    StopEPWM(EPWM_E);
    StopEPWM(EPWM_F);
    StopEPWM(EPWM_fan_a_b);
    StopEPWM(EPWM_fan_c_d);
    StopEPWM(EPWM_fan_e_f);

//    StopEPWM(EPWM_adc_interrupt); // epwm9
//    StopEPWM(EPWM_control_interrupt); // epwm10

    (*ePWM[EPWM_A]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_B]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_C]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_D]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_E]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_F]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
//    (*ePWM[EPWM_fan_grid]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
//    (*ePWM[EPWM_fan_batt]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode

    (*ePWM[EPWM_fan_a_b]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_fan_c_d]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_fan_e_f]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode

    (*ePWM[EPWM_adc_interrupt]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode
    (*ePWM[EPWM_control_interrupt]).TBCTL.bit.CTRMODE = TB_COUNT_UPDOWN;              //Enable up-down PWM counter mode


// initialize oversample buffers to zero
    for (i=0; i<oversample_factor; i++)
    {
        IindA_oversample_buffer[i] = 0.0;
        IindA_oversample_buffer[i] = 0.0;
        IindA_oversample_buffer[i] = 0.0;
        IindA_oversample_buffer[i] = 0.0;
        IindA_oversample_buffer[i] = 0.0;
        IindA_oversample_buffer[i] = 0.0;

        VphaseA_oversample_buffer[i] = 0.0;
        VphaseA_oversample_buffer[i] = 0.0;
        VphaseA_oversample_buffer[i] = 0.0;
        VphaseA_oversample_buffer[i] = 0.0;
        VphaseA_oversample_buffer[i] = 0.0;
        VphaseA_oversample_buffer[i] = 0.0;

        IgridA_oversample_buffer[i] = 0.0;
        IgridA_oversample_buffer[i] = 0.0;
        IgridA_oversample_buffer[i] = 0.0;
        IgridA_oversample_buffer[i] = 0.0;
        IgridA_oversample_buffer[i] = 0.0;
        IgridA_oversample_buffer[i] = 0.0;

        Vbus_oversample_buffer[i] = 0.0;
    }

//    oversample_factor_math = oversample_factor;
//    oversample_period = 1.0 / oversample_factor_math;



    // Main loop
    while(true)
    {



    }
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// ISR - all the controller updates happen here ///////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//

// testing for oversampling
// epwm9 will trigger the ADC at the oversampling frequency
// epwm10 will have the control code
// epwm 1-6 are the gate drive signals
// epsm 7,8, 11, 12 are for the fans



//interrupt where we read the ADCs at oversample_factor * switching frequency
__interrupt void epwmADC_isr(void)
{
    //timer start here
    //    StartTimer();

//    open_main_grid_relay();

    //wait until the for ADC modules finish
    while ((AdcaRegs.ADCINTFLG.bit.ADCINT4 == 0)  | \
            (AdcbRegs.ADCINTFLG.bit.ADCINT4 == 0) | \
            (AdccRegs.ADCINTFLG.bit.ADCINT4 == 0) | \
            (AdcdRegs.ADCINTFLG.bit.ADCINT4 == 0));

    //    float cpuTime_us= StopTimer();
    //    running_time    = cpuTime_us/1000000;

        //time to here is 1.61us

        VphaseA_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT2; // Phase A grid voltage
        VphaseB_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT2; // Phase B grid voltage
        VphaseC_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT2; // Phase C grid voltage
        VphaseD_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT3; // Phase D batt voltage
        VphaseE_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT3; // Phase E batt voltage
        VphaseF_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT3; // Phase F batt voltage

        // time to here .42us

        IindA_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT0; // phase A inductor current
        IindB_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT0; // phase B inductor current
        IindC_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT0; // phase C inductor current
        IindD_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT1; // phase A inductor current
        IindE_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT1; // phase B inductor current
        IindF_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT1; // phase C inductor current

        // time to here .75us

        IgridA_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT4; // phase A output current
        IgridB_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT4; // phase B output current
        IgridC_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT4; // phase C output current
        IgridD_oversample_buffer[oversample_counter] = AdccResultRegs.ADCRESULT5; // phase D output current
        IgridE_oversample_buffer[oversample_counter] = AdcaResultRegs.ADCRESULT5; // phase E output current
        IgridF_oversample_buffer[oversample_counter] = AdcbResultRegs.ADCRESULT5; // phase F output current

        Vbus_oversample_buffer[oversample_counter] = AdcdResultRegs.ADCRESULT0; // DC bus voltage
/*
        Temp1_oversample_buffer[oversample_counter] = AdcdResultRegs.ADCRESULT2;
        Temp2_oversample_buffer[oversample_counter] = AdcdResultRegs.ADCRESULT3;
        Temp3_oversample_buffer[oversample_counter] = AdcdResultRegs.ADCRESULT1;
*/
        // time to here 1.14us

        oversample_counter ++;

        if (oversample_counter >= oversample_factor)
        {
            oversample_counter = 0;
        }

        AdcaRegs.ADCINTFLGCLR.bit.ADCINT4 = 1; //clear ADCAINT4 flag
        AdcbRegs.ADCINTFLGCLR.bit.ADCINT4 = 1; //clear ADCBINT4 flag
        AdccRegs.ADCINTFLGCLR.bit.ADCINT4 = 1; //clear ADCCINT4 flag
        AdcdRegs.ADCINTFLGCLR.bit.ADCINT4 = 1; //clear ADCAINT4 flag


//        close_main_grid_relay();
    //

        // old an functional
    //    EPwm9Regs.ETCLR.bit.INT =1; // Clear ePWM9 INT flag
    //    PieCtrlRegs.PIEACK.all = PIEACK_GROUP3; // Acknowledge the interruption

        // time to here 1.43us



        // new and questionable
    //    EPwm9Regs.ETCLR.bit.INT =1; // Clear ePWM9 INT flag
        AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //Clear ADCINTA1 flag
    //    AdcdRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //Clear ADCINTA1 flag
        PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; // Acknowledge the interruption
}

__interrupt void epwmcontrol_isr(void)
{
    //CpuTimer1Regs.TIM.bit.LSW = 0xFFFF; //Max out timer counter (for determining number of CPU cycles to execute)

//    EALLOW;
//    GpioDataRegs.GPCTOGGLE.bit.GPIO67 = 1;
//    EDIS;

//new and questionable
    TempPIEIER = PieCtrlRegs.PIEIER1.all; // Save PIEIER register for later
    IER |= 0x001; //0x020                        // Set global priority by adjusting IER
    IER &= 0x001; //0x020
//    IER |= 0x020; //0x020                        // Set global priority by adjusting IER
//    IER &= 0x020; //0x020
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;    // Set group priority by adjusting PIEIER2 to allow INT2.9 to interrupt current ISR
//    PieCtrlRegs.PIEIER1.bit.INTx6 = 1;    // Set group priority by adjusting PIEIER2 to allow INT2.9 to interrupt current ISR
    PieCtrlRegs.PIEACK.all = 0xFFFF;      // Enable PIE interrupts
    asm("       NOP");                    // Wait one cycle
    EINT;

//    open_main_batt_relay();

    StartTimer(); //start time to count the elapsed time

// Startup procedure

/*
    if (EN_startup == 1)
    {
        // precharge the DC bus voltage to Vdc_precharge
        precharge_counter = precharge_counter+1;

        if (Flag_precharge == 0)
        {
            //make sure all the main relays are open
            open_main_grid_relay();
            open_main_batt_relay();
            relay_grid = 0;
            relay_batt = 0;

            //close and keep closed the precharge relays
            close_precharge_grid_relay();
            close_precharge_batt_relay();
            relay_precharge = 1;
        }

//        #define precharge_counter_complete 60000
//        #define gridvoltage_check 60000
//        #define PLLcheck1 120000
//        #define PLLcheck2 60000
//        #define DCbusControl_check 120000
        #define precharge_DCbus_resistive_min 0.5*2*Vmgrid_rated
        #define precharge_DCbus_resistive_max 10.0*2*Vmgrid_rated
        #define precharge_DCbus_DCcontrol_min 0.5*Vdc_ref
        #define precharge_DCbus_DCcontrol_max 10.0*Vdc_ref

        if (Vdc > precharge_DCbus_resistive_min && precharge_counter > precharge_counter_complete) // precharge successful process
        {
            Flag_precharge = 1;
            Flag_inrush    = 1;

            //close the main relays
            close_main_grid_relay();
            close_main_batt_relay();
            relay_grid = 1;
            relay_batt = 1;

            //open the precharge relays
            open_precharge_grid_relay();
            open_precharge_batt_relay();
            relay_precharge = 0;

            Vgrid_counter ++;

            if (Vcap_meas[0]<1.2*2*Vmgrid_rated && Vcap_meas[1]<1.2*2*Vmgrid_rated && Vcap_meas[2]<1.2*2*Vmgrid_rated && Vgrid_counter > gridvoltage_check)
            {
                PLL_counter1 ++;

                if (350 < omega_PLL && omega_PLL < 400 && PLL_counter1 > PLLcheck1)
                {
                    PLL_counter2 ++;

                    Kp_PLL = -0.1;

                    if (370 < omega_PLL && omega_PLL < 385 && PLL_counter2 > PLLcheck2)
                    {
                        EN_AC_controller = 1;

                        DCbus_counter ++;

                        if (precharge_DCbus_DCcontrol_min < Vdc && Vdc < precharge_DCbus_DCcontrol_max && DCbus_counter > DCbusControl_check)
                        {
//                            EN_DC_controller = 1;
                            EN_startup = 0;

                            precharge_counter = 0;
                            Vgrid_counter = 0;
                            PLL_counter1 = 0;
                            PLL_counter2 = 0;
                            DCbus_counter = 0;

                            Flag_precharge = 0;

                        }

                        else if ((precharge_DCbus_DCcontrol_min > Vdc || Vdc > precharge_DCbus_DCcontrol_max) && DCbus_counter > DCbusControl_check)
                        {
                            //make sure all the main relays are open
                            open_main_grid_relay();
                            open_main_batt_relay();
                            relay_grid = 0;
                            relay_batt = 0;

                            //open the precharge relays
                            open_precharge_grid_relay();
                            open_precharge_batt_relay();
                            relay_precharge = 0;

                            EN_startup = 0;
                        }

                    }

                    else if ((370 > omega_PLL || omega_PLL > 385) && PLL_counter2 > PLLcheck2)
                    {
                        //make sure all the main relays are open
                        open_main_grid_relay();
                        open_main_batt_relay();
                        relay_grid = 0;
                        relay_batt = 0;

                        //open the precharge relays
                        open_precharge_grid_relay();
                        open_precharge_batt_relay();
                        relay_precharge = 0;

                        EN_startup = 0;
                    }

                }

                else if ((350 > omega_PLL || omega_PLL > 400) && PLL_counter1 > PLLcheck1)
                {
                    //make sure all the main relays are open
                    open_main_grid_relay();
                    open_main_batt_relay();
                    relay_grid = 0;
                    relay_batt = 0;

                    //open the precharge relays
                    open_precharge_grid_relay();
                    open_precharge_batt_relay();
                    relay_precharge = 0;

                    EN_startup = 0;
                }

            }

            else if ((Vcap_meas[0]>1.2*2*Vmgrid_rated || Vcap_meas[1]>1.2*2*Vmgrid_rated || Vcap_meas[2]>1.2*2*Vmgrid_rated) && DCbus_counter==0 && Vgrid_counter > gridvoltage_check)
            {
                //make sure all the main relays are open
                open_main_grid_relay();
                open_main_batt_relay();
                relay_grid = 0;
                relay_batt = 0;

                //open the precharge relays
                open_precharge_grid_relay();
                open_precharge_batt_relay();
                relay_precharge = 0;

                EN_startup = 0;
            }

        }

        else if ((Vdc < precharge_DCbus_resistive_min) && DCbus_counter==0 && precharge_counter > precharge_counter_complete) // precharge fail process
        {
            //make sure all the main relays are open
            open_main_grid_relay();
            open_main_batt_relay();
            relay_grid = 0;
            relay_batt = 0;

            //open the precharge relays
            open_precharge_grid_relay();
            open_precharge_batt_relay();
            relay_precharge = 0;

            EN_startup = 0;
        }

    }
*/

/*
//precharge ------------------------------------------------------------------------------------------------------------------------------
    if (EN_precharge == 1)
    {
        precharge_counter1 ++; //increment precharge counter 1

        //make sure control is off
        EN_AC_controller == 0;
        EN_AC_controller_old == 0;

        //make sure all the main relays are open
        open_main_grid_relay();
        open_main_batt_relay();
        relay_grid = 0;
        relay_batt = 0;

        //close and keep closed the precharge relays
        close_precharge_grid_relay();
        close_precharge_batt_relay();
        relay_precharge = 1;

        //increment overflow counter
        if (precharge_counter1 > 20000)
        {
            precharge_counter2 ++;
            precharge_counter1 = 0;
        }

        //once counter2 is greater than 8 the precharge time is finished
        if (precharge_counter2 > 8)
        {
            EN_precharge = 0; //precharge is done, turn precharge off

            //close the main relays
            close_main_grid_relay();
            close_main_batt_relay();
            relay_grid = 1;
            relay_batt = 1;

            //open the precharge relays
            open_precharge_grid_relay();
            open_precharge_batt_relay();
            relay_precharge = 0;

            //reset the counters
            precharge_counter1 = 0;
            precharge_counter2 = 0;
        }
    }
*/
    //enabling controller ---------------------------------------------------------------------------------------------------------------------
    if (EN_AC_controller == 1 && EN_AC_controller_old == 0)
    {
        //reset measurements and control variables
        reset_PIerror();
        reset_controller();

        //only enabling the PWM signals for phases that we want

        if (StopA == 0)
        {
            StartEPWM(EPWM_A);
        }
        if (StopB == 0)
        {
            StartEPWM(EPWM_B);
        }
        if (StopC == 0)
        {
            StartEPWM(EPWM_C);
        }

//        if (StopD == 0)
//        {
//            StartEPWM(EPWM_D);
//        }
//        if (StopE == 0)
//        {
//            StartEPWM(EPWM_E);
//        }
//        if (StopF == 0)
//        {
//            StartEPWM(EPWM_F);
//        }

        //enable the gate drivers
        gate_driver_enable();

        EN_AC_controller_old = 1;
    }

    if (EN_DC_controller == 1 && EN_DC_controller_old == 0)
    {
        //reset measurements and control variables
        reset_PIerror();
        reset_controller();

        //only enabling the PWM signals for phases that we want

//        if (StopA == 0)
//        {
//            StartEPWM(EPWM_A);
//        }
//        if (StopB == 0)
//        {
//            StartEPWM(EPWM_B);
//        }
//        if (StopC == 0)
//        {
//            StartEPWM(EPWM_C);
//        }
        if (Phase_num_DC == 1)
        {
            StartEPWM(EPWM_D);
            StopEPWM(EPWM_E);
            StopEPWM(EPWM_F);
        }
        if (Phase_num_DC == 2)
        {
            StartEPWM(EPWM_D);
            StartEPWM(EPWM_E);
            StopEPWM(EPWM_F);
        }
        if (Phase_num_DC == 3)
        {
            StartEPWM(EPWM_D);
            StartEPWM(EPWM_E);
            StartEPWM(EPWM_F);
        }
//
//        if (StopD == 0)
//        {
//            StartEPWM(EPWM_D);
//        }
//        if (StopE == 0)
//        {
//            StartEPWM(EPWM_E);
//        }
//        if (StopF == 0)
//        {
//            StartEPWM(EPWM_F);
//        }

        //enable the gate drivers
        gate_driver_enable();

        EN_DC_controller_old = 1;
    }

    //enabling the fans -------------------------------------------------------------------------------------------------------------------------
    if (EN_fans == 1 && EN_fans_old == 0)
    {
        StartEPWM(EPWM_fan_a_b);
        StartEPWM(EPWM_fan_c_d);
        StartEPWM(EPWM_fan_e_f);
        EN_fans_old = 1;
    }


    //relay control -----------------------------------------------------------------------------------------------------------------------------
    if (relay_batt == 1) //close battery side main relays
    {
        close_main_batt_relay();
    }
    else //open battery side main relays
    {
        open_main_batt_relay();
    }

    if (relay_grid == 1) //close grid side main relays
    {
        close_main_grid_relay();
    }
    else //open grid side main relays
    {
        open_main_grid_relay();
    }

    if (relay_precharge == 1) //close the grid and battery side precharge relays
    {
        close_precharge_grid_relay();
        close_precharge_batt_relay();
    }
    else //open the grid and battery side precharge relays
    {
        open_precharge_grid_relay();
        open_precharge_batt_relay();
    }


/*
//calibrating the sensors -------------------------------------------------------------------------------------------------------------------
    //..................linear calibration for cap voltage..................//
    Vphase_meas_cal[0] = Vphase_meas_uncal[0] * 0.3385-3.2418;//0.33847-3.2418;//0.28525+0.405;//cal_vphase[0] + Vphase_meas_offset[0]; //Phase A grid voltage
    Vphase_meas_cal[1] = Vphase_meas_uncal[1] * 0.3406-5.4019;//0.33847-3.2418;//0.3429+1.8358;//cal_vphase[1] + Vphase_meas_offset[1]; //Phase B grid voltage
    Vphase_meas_cal[2] = Vphase_meas_uncal[2] * 0.2807-2.7374;//0.284638-2.2897;//0.282646+0.6223;//cal_vphase[2] + Vphase_meas_offset[2]; //Phase C grid voltage
    Vphase_meas_cal[3] = Vphase_meas_uncal[3] * 0.2846-0.5706;//0.28466-0.875;//0.285711+2.6556;//0.28624+1.83215;//cal_vphase[3] + Vphase_meas_offset[3]; //Phase D grid voltage
    Vphase_meas_cal[4] = Vphase_meas_uncal[4] * 0.2833-1.8038;//0.27732+0.622;//0.28624+1.83215;//cal_vphase[4] + Vphase_meas_offset[4]; //Phase E grid voltage
    Vphase_meas_cal[5] = Vphase_meas_uncal[5] * 0.2773+0.622;//0.2773+0.622;//0.27732+0.622;//0.0314;//0.28624+1.83215;//cal_vphase[5] + Vphase_meas_offset[5]; //Phase F grid voltage

    Iind_meas_cal[0] = -Iind_meas_uncal[0] * 0.05570+124.369;//0.055896+124.5678;//cal_Iind[0] + Iind_meas_offset[0]; //Phase A inductor current
    Iind_meas_cal[1] = -Iind_meas_uncal[1] * 0.05622+125.192;//0.055896+124.5678;//cal_Iind[1] + Iind_meas_offset[1]; //Phase B inductor current
    Iind_meas_cal[2] = -Iind_meas_uncal[2] * 0.05606+124.863;//0.055896+124.5678;//cal_Iind[2] + Iind_meas_offset[2]; //Phase C inductor current
    Iind_meas_cal[3] = -Iind_meas_uncal[3] * 0.056+124.808;//0.055896+124.5678;//cal_Iind[3] + Iind_meas_offset[3]; //Phase D inductor current
    Iind_meas_cal[4] = -Iind_meas_uncal[4] * 0.056+124.808;//0.055896+124.5678;//cal_Iind[4] + Iind_meas_offset[4]; //Phase E inductor current
    Iind_meas_cal[5] = -Iind_meas_uncal[5] * 0.056+124.808;//0.055896+124.5678;//cal_Iind[5] + Iind_meas_offset[5]; //Phase F inductor current

//    Igrid_meas_cal[0] = Igrid_meas_uncal[0] * cal_Igrid[0] - Igrid_meas_offset[0]; //Phase A grid current
//    Igrid_meas_cal[1] = Igrid_meas_uncal[1] * cal_Igrid[1] - Igrid_meas_offset[1]; //Phase B grid current
//    Igrid_meas_cal[2] = Igrid_meas_uncal[2] * cal_Igrid[2] - Igrid_meas_offset[2]; //Phase C grid current
    Igrid_meas_cal[3] = Igrid_meas_uncal[3] * 0.056+124.808;//cal_Igrid[3] - Igrid_meas_offset[3]; //Phase D grid current
//    Igrid_meas_cal[4] = Igrid_meas_uncal[4] * cal_Igrid[4] - Igrid_meas_offset[4]; //Phase E grid current
//    Igrid_meas_cal[5] = Igrid_meas_uncal[5] * cal_Igrid[5] - Igrid_meas_offset[5]; //Phase F grid current


    //..................linear calibration for DC bus voltage..................//
    Vbus_meas = Vbus_meas_uncal * 0.2853323+0.9407;//0.3429+1.8358 + 2;//cal_Vbus + Vbus_meas_offset; //DC bus voltage
*/





//zeroing out the running totals
    Iind_oversample_running_total[0] = 0;
    Iind_oversample_running_total[1] = 0;
    Iind_oversample_running_total[2] = 0;
    Iind_oversample_running_total[3] = 0;
    Iind_oversample_running_total[4] = 0;
    Iind_oversample_running_total[5] = 0;

    Vphase_oversample_running_total[0] = 0;
    Vphase_oversample_running_total[1] = 0;
    Vphase_oversample_running_total[2] = 0;
    Vphase_oversample_running_total[3] = 0;
    Vphase_oversample_running_total[4] = 0;
    Vphase_oversample_running_total[5] = 0;

    Igrid_oversample_running_total[0] = 0;
    Igrid_oversample_running_total[1] = 0;
    Igrid_oversample_running_total[2] = 0;
    Igrid_oversample_running_total[3] = 0;
    Igrid_oversample_running_total[4] = 0;
    Igrid_oversample_running_total[5] = 0;

    Vbus_oversample_running_total = 0;
/*
    Temp_oversample_running_total[0] = 0;
    Temp_oversample_running_total[1] = 0;
    Temp_oversample_running_total[2] = 0;
*/
    //time to here is .2us (8x)
    //time to here is .2us (10x)

    //need to "pluck" buffer here so it doesn't get updated while this code is running?

    for (i=0; i<oversample_factor; i++) //adding all values in the buffer together
    {
        Iind_oversample_running_total[0] += IindA_oversample_buffer[i];
        Iind_oversample_running_total[1] += IindB_oversample_buffer[i];
        Iind_oversample_running_total[2] += IindC_oversample_buffer[i];
        Iind_oversample_running_total[3] += IindD_oversample_buffer[i];
        Iind_oversample_running_total[4] += IindE_oversample_buffer[i];
        Iind_oversample_running_total[5] += IindF_oversample_buffer[i];

        Vphase_oversample_running_total[0] += VphaseA_oversample_buffer[i];
        Vphase_oversample_running_total[1] += VphaseB_oversample_buffer[i];
        Vphase_oversample_running_total[2] += VphaseC_oversample_buffer[i];
        Vphase_oversample_running_total[3] += VphaseD_oversample_buffer[i];
        Vphase_oversample_running_total[4] += VphaseE_oversample_buffer[i];
        Vphase_oversample_running_total[5] += VphaseF_oversample_buffer[i];

        Igrid_oversample_running_total[0] += IgridA_oversample_buffer[i];
        Igrid_oversample_running_total[1] += IgridB_oversample_buffer[i];
        Igrid_oversample_running_total[2] += IgridC_oversample_buffer[i];
        Igrid_oversample_running_total[3] += IgridD_oversample_buffer[i];
        Igrid_oversample_running_total[4] += IgridE_oversample_buffer[i];
        Igrid_oversample_running_total[5] += IgridF_oversample_buffer[i];

        Vbus_oversample_running_total += Vbus_oversample_buffer[i];
/*
        Temp_oversample_running_total[0] += Temp1_oversample_buffer[i];
        Temp_oversample_running_total[1] += Temp2_oversample_buffer[i];
        Temp_oversample_running_total[2] += Temp3_oversample_buffer[i];
*/
    }


    //time to here is 12.6us (8x)
    //time to here is 16.6us (10x)

    oversample_period = 1.0 / (float)oversample_factor;

    // dividing running total by oversampling factor to get the average, then calibrating
    Vphase_meas_cal[0] = ((float)Vphase_oversample_running_total[0] * oversample_period) * cal_vphase[0] + Vphase_meas_offset[0]; //Phase A grid voltage
    Vphase_meas_cal[1] = ((float)Vphase_oversample_running_total[1] * oversample_period) * cal_vphase[1] + Vphase_meas_offset[1]; //Phase B grid voltage
    Vphase_meas_cal[2] = ((float)Vphase_oversample_running_total[2] * oversample_period) * cal_vphase[2] + Vphase_meas_offset[2]; //Phase C grid voltage
    Vphase_meas_cal[3] = ((float)Vphase_oversample_running_total[3] * oversample_period) * cal_vphase[3] + Vphase_meas_offset[3]; //Phase D grid voltage
    Vphase_meas_cal[4] = ((float)Vphase_oversample_running_total[4] * oversample_period) * cal_vphase[4] + Vphase_meas_offset[4]; //Phase E grid voltage
    Vphase_meas_cal[5] = ((float)Vphase_oversample_running_total[5] * oversample_period) * cal_vphase[5] + Vphase_meas_offset[5]; //Phase F grid voltage

    Iind_meas_cal[0] = -((float)Iind_oversample_running_total[0] * oversample_period) * cal_Iind[0] + Iind_meas_offset[0]; //Phase A inductor current
    Iind_meas_cal[1] = -((float)Iind_oversample_running_total[1] * oversample_period) * cal_Iind[1] + Iind_meas_offset[1]; //Phase B inductor current
    Iind_meas_cal[2] = -((float)Iind_oversample_running_total[2] * oversample_period) * cal_Iind[2] + Iind_meas_offset[2]; //Phase C inductor current
    Iind_meas_cal[3] = -((float)Iind_oversample_running_total[3] * oversample_period) * cal_Iind[3] + Iind_meas_offset[3]; //Phase D inductor current
    Iind_meas_cal[4] = -((float)Iind_oversample_running_total[4] * oversample_period) * cal_Iind[4] + Iind_meas_offset[4]; //Phase E inductor current
    Iind_meas_cal[5] = -((float)Iind_oversample_running_total[5] * oversample_period) * cal_Iind[5] + Iind_meas_offset[5]; //Phase F inductor current



    Igrid_meas_cal[0] = (Igrid_oversample_running_total[0] * oversample_period) * cal_Igrid[0] - Igrid_meas_offset[0]; //Phase A grid current
    Igrid_meas_cal[1] = (Igrid_oversample_running_total[1] * oversample_period) * cal_Igrid[1] - Igrid_meas_offset[1]; //Phase B grid current
    Igrid_meas_cal[2] = (Igrid_oversample_running_total[2] * oversample_period) * cal_Igrid[2] - Igrid_meas_offset[2]; //Phase C grid current
    Igrid_meas_cal[3] = (Igrid_oversample_running_total[3] * oversample_period) * cal_Igrid[3] - Igrid_meas_offset[3]; //Phase D grid current
    Igrid_meas_cal[4] = (Igrid_oversample_running_total[4] * oversample_period) * cal_Igrid[4] - Igrid_meas_offset[4]; //Phase E grid current
    Igrid_meas_cal[5] = (Igrid_oversample_running_total[5] * oversample_period) * cal_Igrid[5] - Igrid_meas_offset[5]; //Phase F grid current

    Vbus_meas_cal = ((float)Vbus_oversample_running_total * oversample_period) * cal_Vbus + Vbus_meas_offset; //DC bus voltage

/*
    Temp_meas_cal[0] = (Temp_oversample_running_total[0] * oversample_period) * cal_Temp + Temp_meas_offset; //Temperature 1
    Temp_meas_cal[1] = (Temp_oversample_running_total[1] * oversample_period) * cal_Temp + Temp_meas_offset; //Temperature 1
    Temp_meas_cal[2] = (Temp_oversample_running_total[2] * oversample_period) * cal_Temp + Temp_meas_offset; //Temperature 1
*/

//Liwei's control code ----------------------------------------------------------------------------------------------------------------------

//.............. Control function implementation ............................//


// get the DC/AC side samples from the oversampled ADC averaging
    Iabc_ind[0] = Iind_meas_cal[0]; //Igrid_meas_cal[0];//three phase inductor current measurement for DC/AC side
    Iabc_ind[1] = Iind_meas_cal[1]; //Igrid_meas_cal[1];//three phase inductor current measurement for DC/AC side
    Iabc_ind[2] = Iind_meas_cal[2]; //Igrid_meas_cal[2];//three phase inductor current measurement for DC/AC side

    Vcap_meas[0] = Vphase_meas_cal[0]; //Vphase_meas_cal[3]; //three phase capacitor voltage measurement for DC/AC side
    Vcap_meas[1] = Vphase_meas_cal[1]; //Vphase_meas_cal[4]; //three phase capacitor voltage measurement for DC/AC side
    Vcap_meas[2] = Vphase_meas_cal[4]; //Vphase_meas_cal[5]; //three phase capacitor voltage measurement for DC/AC side

    Vdc          = Vphase_meas_cal[5]; //Vphase_meas_cal[2]; //Vbus_meas;//Vphase_meas_cal[2]; //DC bus voltage measurement

    Vbus_meas    = Vphase_meas_cal[5];

    Vdc_duty = Vphase_meas_cal[5];//Vbus_meas; //Vbus_meas_avg; //DC bus voltage measurement


// get the DC/DC side samples from the oversampled ADC averaging
    Ibus_meas[0] = Iind_meas_cal[3];
    Ibus_meas[1] = Iind_meas_cal[4];
    Ibus_meas[2] = Iind_meas_cal[5];

//    Vdc          = Vphase_meas_cal[5];
    Vbatt_meas   = Vphase_meas_cal[3];


/*
// get the samples from the control interrupt moving average
    Iabc_ind[0] = Iind_meas_avg[0]; //three phase inductor current measurement for DC/AC side
    Iabc_ind[1] = Iind_meas_avg[1]; //three phase inductor current measurement for DC/AC side
    Iabc_ind[2] = Iind_meas_avg[2]; //three phase inductor current measurement for DC/AC side

    Vcap_meas[0] = Vout_meas_avg[0]; //three phase capacitor voltage measurement for DC/AC side
    Vcap_meas[1] = Vout_meas_avg[1]; //three phase capacitor voltage measurement for DC/AC side
    Vcap_meas[2] = Vout_meas_avg[2]; //three phase capacitor voltage measurement for DC/AC side

    Vdc = Vbus_meas_avg; //DC bus voltage measurement
*/

// get the samples from the control interrupt average
//    Iabc_ind[0] = Iind_meas_uncal_inCTRL[0]; //three phase inductor current measurement for DC/AC side
//    Iabc_ind[1] = Iind_meas_uncal_inCTRL[1]; //three phase inductor current measurement for DC/AC side
//    Iabc_ind[2] = Iind_meas_uncal_inCTRL[2]; //three phase inductor current measurement for DC/AC side
//
//    Vcap_meas[0] = Vphase_meas_uncal_inCTRL[0]; //three phase capacitor voltage measurement for DC/AC side
//    Vcap_meas[1] = Vphase_meas_uncal_inCTRL[1]; //three phase capacitor voltage measurement for DC/AC side
//    Vcap_meas[2] = Vphase_meas_uncal_inCTRL[2]; //three phase capacitor voltage measurement for DC/AC side


//............... AC side voltage protection triggered if measured voltage squares exceed the protection threshold...........//

//    Vgrid_square = Vcap_meas[0]*Vcap_meas[0]+Vcap_meas[1]*Vcap_meas[1]+Vcap_meas[2]*Vcap_meas[2];

    if(abs(Vcap_meas[0])>=Vgrid_over || abs(Vcap_meas[1])>=Vgrid_over || abs(Vcap_meas[2])>=Vgrid_over || Vdc>=1000)
    {
        StopEPWM(EPWM_A);
        StopEPWM(EPWM_B);
        StopEPWM(EPWM_C);
        StopEPWM(EPWM_D);
        StopEPWM(EPWM_E);
        StopEPWM(EPWM_F);
        StopEPWM(EPWM_fan_a_b);
        StopEPWM(EPWM_fan_c_d);
        StopEPWM(EPWM_fan_e_f);

        //Reset conditions
        Vdq0[0] = 0.0;
        Vdq0[1] = 0.0;
        Vdq0[2] = 0.0;
        Idq0_ind_ref[0] = 0.0; //Reset d-axis reference current
        Idq0_ind_ref[1] = 0.0; //Reset q-axis reference current
        Idq0_ind_ref[2] = 0.0; //Reset zero-axis reference current
        Idq0_grid_ref[0] = 0.0; //Reset d-axis reference current
        Idq0_grid_ref[1] = 0.0; //Reset q-axis reference current
        Idq0_grid_ref[2] = 0.0; //Reset zero-axis reference current
        PQ_ref[0] = 0.0;
        PQ_ref[1] = 0.0;
        Idq0_grid[0] = 0.0; //Reset d-axis current
        Idq0_grid[1] = 0.0; //Reset q-axis current
        Idq0_grid[2] = 0.0; //Reset 0-axis current
        Idq0_ind[0] = 0.0; //Reset d-axis current
        Idq0_ind[1] = 0.0; //Reset q-axis current
        Idq0_ind[2] = 0.0; //Reset 0-axis current
        Intdq0_ind[0] = 0.0; //Reset integrator
        Intdq0_ind[1] = 0.0; //Reset integrator
        Intdq0_ind[2] = 0.0; //Reset integrator
        Intdq0_grid[0] = 0.0; //Reset integrator
        Intdq0_grid[1] = 0.0; //Reset integrator
        Intdq0_grid[2] = 0.0; //Reset integrator
        edq_ind_star[0] = 0.0;
        edq_ind_star[1] = 0.0;
        edq_ind_star[2] = 0.0;
        edq_grid_star[0] = 0.0;
        edq_grid_star[1] = 0.0;
        edq_grid_star[2] = 0.0;
        V_PI[0] = 0.0;
        V_PI[1] = 0.0;
        V_PI[2] = 0.0;
        Int_zs = 0.0;
        Int_Vdc = 0.0;
        Iabc_grid[0] = 0.0;
        Iabc_grid[1] = 0.0;
        Iabc_grid[2] = 0.0;
        Iabc_ind[0] = 0.0;
        Iabc_ind[1] = 0.0;
        Iabc_ind[2] = 0.0;

        relay_grid = 0;
        relay_batt = 0;
        open_main_grid_relay(); //make sure main grid relays are open
        open_main_batt_relay(); //make sure main batt relays are open
        //EN_AC_controller = 0; //Disable control
        //EN_AC_controller_old = 0; //Disable control
//        overCurrent  = 0; //Reset overcurrent signal
        protection_on = 1;
        protection_AC_volt = 1;
    }

//............... AC side current protection triggered if measured currents exceed the protection threshold...........//
    if(abs(Iabc_ind[0])>=Isafety || abs(Iabc_ind[1])>=Isafety || abs(Iabc_ind[2])>=Isafety) //If currents exceed specified limit, activate PWM tripzone
    {
        overCurrent++;

        if(overCurrent>=consecutive || ( abs(Iabc_ind[0])>=Icritical || abs(Iabc_ind[1])>=Icritical || abs(Iabc_ind[2])>=Icritical ))
        {
            StopEPWM(EPWM_A);
            StopEPWM(EPWM_B);
            StopEPWM(EPWM_C);
            StopEPWM(EPWM_D);
            StopEPWM(EPWM_E);
            StopEPWM(EPWM_F);
            StopEPWM(EPWM_fan_a_b);
            StopEPWM(EPWM_fan_c_d);
            StopEPWM(EPWM_fan_e_f);

            //Reset conditions
            Vdq0[0] = 0.0;
            Vdq0[1] = 0.0;
            Vdq0[2] = 0.0;
            Idq0_ind_ref[0] = 0.0; //Reset d-axis reference current
            Idq0_ind_ref[1] = 0.0; //Reset q-axis reference current
            Idq0_ind_ref[2] = 0.0; //Reset zero-axis reference current
            Idq0_grid_ref[0] = 0.0; //Reset d-axis reference current
            Idq0_grid_ref[1] = 0.0; //Reset q-axis reference current
            Idq0_grid_ref[2] = 0.0; //Reset zero-axis reference current
            PQ_ref[0] = 0.0;
            PQ_ref[1] = 0.0;
            Idq0_grid[0] = 0.0; //Reset d-axis current
            Idq0_grid[1] = 0.0; //Reset q-axis current
            Idq0_grid[2] = 0.0; //Reset 0-axis current
            Idq0_ind[0] = 0.0; //Reset d-axis current
            Idq0_ind[1] = 0.0; //Reset q-axis current
            Idq0_ind[2] = 0.0; //Reset 0-axis current
            Intdq0_ind[0] = 0.0; //Reset integrator
            Intdq0_ind[1] = 0.0; //Reset integrator
            Intdq0_ind[2] = 0.0; //Reset integrator
            Intdq0_grid[0] = 0.0; //Reset integrator
            Intdq0_grid[1] = 0.0; //Reset integrator
            Intdq0_grid[2] = 0.0; //Reset integrator
            edq_ind_star[0] = 0.0;
            edq_ind_star[1] = 0.0;
            edq_ind_star[2] = 0.0;
            edq_grid_star[0] = 0.0;
            edq_grid_star[1] = 0.0;
            edq_grid_star[2] = 0.0;
            V_PI[0] = 0.0;
            V_PI[1] = 0.0;
            V_PI[2] = 0.0;
            Int_zs = 0.0;
            Int_Vdc = 0.0;
            Iabc_grid[0] = 0.0;
            Iabc_grid[1] = 0.0;
            Iabc_grid[2] = 0.0;
            Iabc_ind[0] = 0.0;
            Iabc_ind[1] = 0.0;
            Iabc_ind[2] = 0.0;

            relay_grid = 0;
            relay_batt = 0;
            open_main_grid_relay(); //make sure main grid relays are open
            open_main_batt_relay(); //make sure main batt relays are open
            //EN_AC_controller = 0; //Disable control
            //EN_AC_controller_old = 0; //Disable control
            overCurrent  = 0; //Reset overcurrent signal
            protection_on = 1;
            protection_AC_curr = 1;
        }
    }

    else
    {
        overCurrent = 0; //Reset counter
    }


//..........................DC/DC side protection (from over current) ............................//
    if (/*(fabs(Ibatt_meas[0]) >= I_over)|(fabs(Ibatt_meas[1]) >= I_over)|(fabs(Ibatt_meas[2]) >= I_over)| */\
            (fabs(Ibus_meas[0]) >= I_over)|(fabs(Ibus_meas[1]) >= I_over)|(fabs(Ibus_meas[2]) >= I_over)|\
            (Vbatt_meas >= Vbatt_over)|(Vdc >= Vbus_over_hi) & (EN_protection == 1))
    {

        if ((Count_ADCint - protection_triggernum == 1) | (Count_ADCint - protection_triggernum == 50000))   // did the Over currEN_controllert happen consecutively?
                OCconsec ++;                               // Consecutive over current event number
        if (OCconsec >= OCallow)              // allow a few times over current
        {
            StopEPWM(EPWM_A);
            StopEPWM(EPWM_B);
            StopEPWM(EPWM_C);
            StopEPWM(EPWM_D);
            StopEPWM(EPWM_E);
            StopEPWM(EPWM_F);
            StopEPWM(EPWM_fan_a_b);
            StopEPWM(EPWM_fan_c_d);
            StopEPWM(EPWM_fan_e_f);
            //open_relay();
            relay_grid = 0;
            relay_batt = 0;
            open_main_grid_relay(); //make sure main grid relays are open
            open_main_batt_relay(); //make sure main batt relays are open

            protection_on           = 1;                                    // protection on indicator
            protection_DC = 1;
            for (i=0;i<3;i++)
            {
                if (fabs(Ibatt_meas[i]) >= I_over)
                {
                    error_batt_oc[i]          = 1;
                    error_batt_oc_value[i]    = Ibatt_meas[i];
                }

                if (fabs(Ibus_meas[i]) >= I_over)
                {
                    error_bus_oc[i]           = 1;
                    error_bus_oc_value[i]     = Ibus_meas[i];
                }


            }

            if (fabs(Vbatt_meas) >= Vbatt_over)
                error_batt_ov             = 1;
//            if (fabs(Vbus_meas) <= Vbus_over_lo)
//                error_batt_lv             = 1;
            if (fabs(Vbus_meas) >= Vbus_over_hi)
                error_bus_ov              = 1;

        }
        protection_triggernum   = Count_ADCint;
    }

//........................................DC/AC side control algorithms........................................//

//Phase-locked loop (PLL): Acquire grid angle (theta) from Vcap measurements (where Vcap ~ Vgrid)
    PLL();

    //Precompute sine/cosine to save operations (trig is expensive!)
    float sine = sin(theta);
    float cosine =  cos(theta);

    //Clarke transform--OUTSIDE BECAUSE I ALWAYS NEED TO UPDATE THIS FOR OUTPUT
    Ialpha_ind = (1.0/3.0)*(2.0*Iabc_ind[0] - Iabc_ind[1] - Iabc_ind[2]);
    Ibeta_ind = (1.0/sqrt3)*(Iabc_ind[1] - Iabc_ind[2]);
    float Igamma_ind = (1.0/3.0)*(Iabc_ind[0] + Iabc_ind[1] + Iabc_ind[2]);

    Valpha_cap = (1.0/3.0)*(2.0*Vcap_meas[0] - Vcap_meas[1] - Vcap_meas[2]);
    Vbeta_cap = (1.0/sqrt3)*(Vcap_meas[1] - Vcap_meas[2]);
    float Vgamma_cap = (1.0/3.0)*(Vcap_meas[0] + Vcap_meas[1] + Vcap_meas[2]);

    //Park transform
    Idq0_ind[0] = Ialpha_ind*cosine + Ibeta_ind*sine;
    Idq0_ind[1] = -Ialpha_ind*sine + Ibeta_ind*cosine;
    Idq0_ind[2] = Igamma_ind;

    Vcapdq0[0] = Valpha_cap*cosine + Vbeta_cap*sine;
    Vcapdq0[1] = -Valpha_cap*sine + Vbeta_cap*cosine;
    Vcapdq0[2] = Vgamma_cap;

    if (EN_AC_controller == 0)
    {
        Vcapdq0_initial[0] = Vcapdq0[0];
        Vcapdq0_initial[1] = Vcapdq0[1];
        Vcapdq0_initial[2] = Vmgrid_rated;//Vdc_control;//Vcapdq0[2];
    }

    if(EN_AC_controller==1) //Start control when EN_AC_controller==1, else bypass
    {
/*
        EN_voltageCTRL_counter ++;
        if (EN_voltageCTRL_counter > 100)
        {
            zeroVoltage();
            dcVoltage();
            EN_voltageCTRL_counter = 200;
        }
*/
        zeroVoltage(); //Zero sequence voltage control algorithm: generate i0_ref to drive a zero sequence voltage (DC offset on the capacitor)

        //DC bus control: Generate Id_ref for maintaining DC bus voltage
        dcVoltage(); //Tries to push Vdc to Vdc_ref by giving a current reference (power reference if Kp & Ki are large)

        //Convert reference active and reactive power to a dq current references. d = active (P); q = reactive (Q)
        //powerCalc(); //Power transforms: Use Vdq_cap to go from PQ_ref to idq_ref. Vdq_cap MUST be initialized to a non-zero value

        //Grid service working modes
        /*
        VoltVarmode();

        PQmode();

        constPFmode();

        VoltWattmode();

        FreqWattmode();

        */

        reactivepower(); //Reactive power control to give grid current q reference

        indCurrent(); //Inductor current control algorithm

     }

     duty_AC();

//........................................DC/DC side control algorithms........................................//



    // ...... Error update for CV controller...........
    Vbatt_error         =   Vbatt_ref       - Vbatt_meas;             // battery voltage error update
    // anti-windup
    if (((Ibatt_ref_CV >= Ibatt_ref_upper/3) && (Vbatt_error > 0)) | ((Ibatt_ref_CV <= Ibatt_ref_lower/3) && (Vbatt_error <0)))    // saturated and keep saturating then make the integral error constant
            Vbatt_error_I_k     =   Vbatt_error_I_k_1;                                      // reset the integral term
    else
    {
        Vbatt_error_I_k_1   =   Vbatt_error_I_k;
        Vbatt_error_I_k     =   Vbatt_error_I_k_1 + Ts * Vbatt_error;      //  battery voltage integrated error update
    }




    if (relay_on_num == 0)                                                 // add feedforward control only when start up
    {
        duty_fwd            =   Vbatt_meas / Vbus_meas;             // feed forward control to be added to the duty cycle
        Vbatt_fake          =   Vbatt_meas;
        Vbus_fake           =   Vbus_meas;
    }

    else
        duty_fwd            =   Vbatt_fake / Vbus_meas;             // feed forward control to be added to the duty cycle

    // feedforward control saturation
    if (duty_fwd<=duty_min)
        duty_fwd = duty_min;
    if (duty_fwd>=duty_max)
        duty_fwd = duty_max;


    if (EN_DC_controller == 1) // enable error update when controller is enabled
    {
        for (i=0;i<3;i++)
        {
            // ...... CC reference update and error update (using battery current measurements)......
            Ibatt_error[i]     =   Ibatt_ref_CV - Ibatt_meas[i];                    // battery current error update
            Ibatt_error_I_k_1[i] = Ibatt_error_I_k[i];
            Ibatt_error_I_k[i] =   Ibatt_error_I_k_1[i] + Ts * Ibatt_error[i];          //  battery current intergrated error update

            // ...... CC reference update and error update (using inductor current measurements)......
            Ibus_error[i]     =   Ibatt_ref_CV - Ibus_meas[i];                    // bus current error update
            Ibus_error_I_k_1[i] = Ibus_error_I_k[i];
            Ibus_error_I_k[i] =   Ibus_error_I_k_1[i] + Ts * Ibus_error[i];          //  bus current intergrated error update

        }
    }


    // ...... CV reference update for CC controller....
    if (EN_CV==1)
        Ibatt_ref_CV    =   -CVController(Vbatt_error,Vbatt_error_I_k,Ibatt_ref_upper,Ibatt_ref_lower)/Phase_num_DC;              // Run PI controller to update the parameters (comment when open loop)
    else
        Ibatt_ref_CV    =   -Ibatt_ref_const/Phase_num_DC;//2;//3;                                                                        // default CC current if CV is disabled


//////....... CC controller ................///////////////////////////////////////////


    if (EN_duty_manual == 0)
    {
        for (i=3;i<6;i++)
        {
            duty_phase[i]    =    CCController(Ibatt_ref_CV, duty_fwd, duty_max, duty_min, Ibus_error[i], Ibus_error_I_k[i]); // Run PI controller to update the parameters (comment when open loop)
        }

//        duty_phaseB      =    CCController(Ibatt_ref_CV, duty_fwd, duty_max, duty_min, Ibus_error[1], Ibus_error_I_k[1]); // Run PI controller to update the parameters (comment when open loop)
//        duty_phaseC      =    CCController(Ibatt_ref_CV, duty_fwd, duty_max, duty_min, Ibus_error[2], Ibus_error_I_k[2]); // Run PI controller to update the parameters (comment when open loop)

    }


    // anti-windup
    for (i=3;i<6;i++)
    {
        if ( ( (duty_phase[i] >= duty_max) && (Ibus_error[i]>0) ) | ( (duty_phase[i] <= duty_min) && (Ibus_error[i] <0) ))
                Ibus_error_I_k[i]     =   Ibus_error_I_k_1[i];
    }


//    if ( ( (duty_phaseA >= duty_max) && (Ibus_error[0]>0) ) | ( (duty_phaseA <= duty_min) && (Ibus_error[0] <0) ))
//        Ibus_error_I_k[0]     =   Ibus_error_I_k_1[0];
//    if ( ( (duty_phaseB >= duty_max) && (Ibus_error[1]>0) ) | ( (duty_phaseB <= duty_min) && (Ibus_error[1] <0) ))
//        Ibus_error_I_k[1]     =   Ibus_error_I_k_1[1];
//    if ( ( (duty_phaseC >= duty_max) && (Ibus_error[2]>0) ) | ( (duty_phaseC <= duty_min) && (Ibus_error[2] <0) ))
//        Ibus_error_I_k[2]     =   Ibus_error_I_k_1[2];



    // Post processing

//    Vbatt_error_I_k_1 = Vbatt_error_I_k;                                    // assign kth value to k-1th for next sampling
//    for (i=0;i<3;i++)
//    {
//        Ibus_error_I_k_1[i] = Ibus_error_I_k[i];
//    }


    // reset the PI controller after the relays are closed due to the initial accumulated integral errors
/*
    if ((relay_on_num == 0) && (EN_duty_manual == 0))                          // first time relay is on, reset the duty cycle and error terms in order to eliminate the PI and duty cycle history
    {
//            reset_PIerror();
        for (i=0;i<3;i++)
        {
            duty_phase[i] = duty_fwd;
        }

        if ((relay == 1) && (EN_controller == 1))
            relay_on_num ++;
    }
*/




    //update duty cycles ------------------------------------------------------------------------------------------------------------------------
    EPWM_dutyupdate(EPWM_A, duty_phase[0]);
    EPWM_dutyupdate(EPWM_B, duty_phase[1]);
    EPWM_dutyupdate(EPWM_C, duty_phase[2]);
    EPWM_dutyupdate(EPWM_D, duty_phase[3]);
    EPWM_dutyupdate(EPWM_E, duty_phase[4]);
    EPWM_dutyupdate(EPWM_F, duty_phase[5]);
    EPWM_fan_duty_update(EPWM_fan_a_b, duty_fan[0], duty_fan[1]);
    EPWM_fan_duty_update(EPWM_fan_c_d, duty_fan[2], duty_fan[3]);
    EPWM_fan_duty_update(EPWM_fan_e_f, duty_fan[4], duty_fan[5]);

//    EPWM_fan_duty_update(EPWM_fan_AB, duty_fan[0], duty_fan[1]);
//    EPWM_fan_duty_update(EPWM_fan_CD, duty_fan[2], duty_fan[3]);
//    EPWM_fan_duty_update(EPWM_fan_EF, duty_fan[4], duty_fan[5]);



    if (EN_AC_controller == 1 && EN_AC_controller_old == 0)                           // Control was disabled, now we want to enable the control
    {
//        capture = 1;
        counter_delay++;
        if (counter_delay>=20)
            {
                reset_PIerror();
                reset_controller();                                             // reset the measurements, control variables and other states into th

                if (StopA == 0)
                    StartEPWM(EPWM_A);                                                         // Start the counter, clear TZ, and configure counter mode

                if (StopB == 0)
                    StartEPWM(EPWM_B);

                if (StopC == 0)
                    StartEPWM(EPWM_C);


//                if (StopD == 0)
//                    StartEPWM(EPWM_D);                                                         // Start the counter, clear TZ, and configure counter mode
//
//                if (StopE == 0)
//                    StartEPWM(EPWM_E);
//
//                if (StopF == 0)
//                    StartEPWM(EPWM_F);

                EN_AC_controller_old = 1;

                gate_driver_enable();
            }
    }


    // ...... data export ....... //


    if (capture == 1)
    {
        Export_data[0][sample]          =   omega_PLL;//Vcap_meas[0];//Idq0_ind[0];//AdcaResultRegs.ADCRESULT4+AdcbResultRegs.ADCRESULT4+AdccResultRegs.ADCRESULT4;//Vcapdq0[0];//Iind_meas_cal[0];
        Export_data[1][sample]          =   Vcapdq0[0];//Vcap_meas[1];//Idq0_ind[1];//AdcaResultRegs.ADCRESULT4;//AdcaResultRegs.ADCRESULT0;//Iind_meas_cal[1];//Vcap_meas[1];//Vdc;//Vdc;//PQ[1];//Vphase_meas_cal[1];//Vdq0[2];//Vcapdq0[2];//duty_phase[0];
//        Export_data[2][sample]          =   Vcap_meas[2];//Vcap_meas[2];//omega_PLL;//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[3][sample]          =   Iabc_ind[0];//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[4][sample]          =   dutyABC[0];//omega_PLL;//Vphase_meas_cal[2];

        if (sample >= sample_max)
        {
            capture =   0;                                                  // turn off capture the data
            sample  =   0;                                                  // reset sample index, make it easier to restart the sample
        }

        sample ++;                                                          // increase the sample index
    }

    if (capture == 2)
    {
        Export_data[0][sample]          =   Vcap_meas[2];//Idq0_ind[2];//AdcbResultRegs.ADCRESULT4;//Iind_meas_cal[1];//Vcapdq0[0];//PQ_ref[1];//Vdc;//Vcap_meas[0];
        Export_data[1][sample]          =   Vcapdq0[0];//Vdc;//AdccResultRegs.ADCRESULT4;//AdcbResultRegs.ADCRESULT0;//Vcapdq0[1];//Vdc;//Vdc;//PQ[1];//Vphase_meas_cal[1];//Vdq0[2];//Vcapdq0[2];//duty_phase[0];
//        Export_data[2][sample]          =   Vcapdq0[2];//Vcap_meas[2];//omega_PLL;//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[3][sample]          =   Iabc_ind[0];//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[4][sample]          =   dutyABC[0];//omega_PLL;//Vphase_meas_cal[2];

        if (sample >= sample_max)
        {
            capture =   0;                                                  // turn off capture the data
            sample  =   0;                                                  // reset sample index, make it easier to restart the sample
        }

        sample ++;                                                          // increase the sample index
    }

    if (capture == 3)
    {
        Export_data[0][sample]          =   Vcapdq0[1];// phase A cap voltage ADC readings
        Export_data[1][sample]          =   Vcapdq0[2];// phase B cap voltage ADC readings
//        Export_data[3][sample]          =   Iabc_ind[0];//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[4][sample]          =   dutyABC[0];//omega_PLL;//Vphase_meas_cal[2];

        if (sample >= sample_max)
        {
            capture =   0;                                                  // turn off capture the data
            sample  =   0;                                                  // reset sample index, make it easier to restart the sample
        }

        sample ++;                                                          // increase the sample index
    }
/*
    if (capture == 4)
    {
        Export_data[0][sample]          =   duty_phase[0];// phase A grid current ADC readings
        Export_data[1][sample]          =   Vdc;//duty_phase[1];// phase B grid current ADC readings
//      Export_data[2][sample]          =   duty_phase[2];// phase C grid current ADC readings
//        Export_data[3][sample]          =   Iabc_ind[0];//omega_PLL;//Vphase_meas_cal[2];
//        Export_data[4][sample]          =   dutyABC[0];//omega_PLL;//Vphase_meas_cal[2];

        if (sample >= sample_max)
        {
            capture =   0;                                                  // turn off capture the data
            sample  =   0;                                                  // reset sample index, make it easier to restart the sample
        }

        sample ++;                                                          // increase the sample index
    }
*/

    //gate driver reset (to reset DESAT protection) ---------------------------------------------------------------------------------------------
    if (RST_gate_driver == 1)
    {
        count_RST++;
        if (count_RST == 1)
        {
            gate_driver_RST_low();
        }
        else if(count_RST == 11)
        {
            gate_driver_RST_high();
            RST_gate_driver = 0;
            count_RST = 0;
        }
    }

    //disabling controller ------------------------------------------------------------------------------------------------------------------------
    if (EN_AC_controller == 0 && EN_AC_controller_old == 1) // Control was enabled, now we want to disable the control
    {
        //disable gate drivers
        gate_driver_disable();

        //stop the EPWMs
        StopEPWM(EPWM_A);
        StopEPWM(EPWM_B);
        StopEPWM(EPWM_C);
        StopEPWM(EPWM_D);
        StopEPWM(EPWM_E);
        StopEPWM(EPWM_F);

        //reset the controller
        reset_controller();

        Int_zs        = 0.0;
        Intdq0_ind[0] = 0.0;
        Intdq0_ind[1] = 0.0;
        Intdq0_ind[2] = 0.0;
        Q_error_int   = 0.0;
        Int_Vdc       = 0.0;

        //open all the relays
        open_precharge_grid_relay();
        open_main_grid_relay();
        open_precharge_batt_relay();
        open_main_batt_relay();
        relay_batt = 0;
        relay_grid = 0;
        relay_precharge = 0;

        EN_AC_controller_old = 0;
    }

    //disabling fans ------------------------------------------------------------------------------------------------------------------------------------
    if (EN_fans == 0 && EN_fans_old ==1)
    {
        StopEPWM(EPWM_fan_a_b);
        StopEPWM(EPWM_fan_c_d);
        StopEPWM(EPWM_fan_e_f);
        EN_fans_old = 0;
    }

    float cpuTime_us= StopTimer();
    running_time    = cpuTime_us/1000000.0;

//    close_main_batt_relay();

// new and questionable
    DINT;
    PieCtrlRegs.PIEIER1.all = TempPIEIER;

    EPwm10Regs.ETCLR.bit.INT = 1; // Clear ePWM9 INT flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP3; // Acknowledge the interruption



    //time = CpuTimer1Regs.PRD.bit.LSW - CpuTimer1Regs.TIM.bit.LSW; //Calculate execution time (in CPU cycles)
}


//####################################################################################
//####################################################################################
//################################## Defined Functions ###############################
//####################################################################################
//####################################################################################

void linObs(unsigned short i)
{
//Pre-calculate repeated variables
float TsC = Ts/C_obs;
float TsL = Ts/L_obs;

//Capacitor voltage estimate (relies on feedback from sensor; hence, calculate it first)
Vcap_obs[i] = TsC*(Iind_obs[i] - Iabc_grid[i]) + Vcap_obs[i] + Lp*(Vcap_meas[i] - Vcap_obs[i]);

//Inductor current estimate
Iind_obs[i] = TsL*(Vabc[i] - Vcap_obs[i]) + Iind_obs[i];
//unsigned short x = 0;
}

//* Feed forward theta to compromize the overshoot at the beginning


//* PI controller code: input the errors and change the duty cycle


float CVController(float Vbatt_error, float Vbatt_error_I_k, float Ibatt_ref_upper,float Ibatt_ref_lower)
{
    float I_ref_CV;


    // ............. CV PI.................
    // -- measurement update --
    I_ref_CV            =   Kp1 * Vbatt_error + Ki1 * Vbatt_error_I_k;      // the output from the first CV PI, which is going to the current saturation block

    // -- current saturation--
    if (I_ref_CV >= Ibatt_ref_upper)                                        //  going into this statement if the current is larger than the maximum charging current
    {
        I_ref_CV = Ibatt_ref_upper;                                         //  set the output of the PI to the maximum CHARGING current
    }
    else if (I_ref_CV <= Ibatt_ref_lower)
    {
        I_ref_CV = Ibatt_ref_lower;                                         //  set the output of the PI to the maximum DISCHARGING current
    }

    //.............. DC bus stabilization....
//  if (Vbus_upper < Vbus_meas)             // Vbus out of upper boundary
//  {
//      I_ref_CV_DCstab = I_ref_CV + IDC_adj;
//  }
//  else if (Vbus_lower > Vbus_meas)            // Vbus out of lower boundary
//  {
//      I_ref_CV_DCstab = I_ref_CV - IDC_adj;
//  }
//  else                                        // Acting normally
//  {
//      I_ref_CV_DCstab = I_ref_CV;
//  }

        return I_ref_CV;
}


float CCController(float I_ref_CV, float duty_fwd, float duty_max, float duty_min, float Ibatt_error, float Ibatt_error_I_k)
{
    // -- PI update --

//    float duty        =   Kp2 * Ibatt_error + Ki2 * Ibatt_error_I_k + duty_fwd
    float duty        =   (Kp2 * Ibatt_error + Ki2 * Ibatt_error_I_k)/Vdc + duty_initial_DC;   // duty cycle update

    if (EN_DC_controller == 0)
    {
        duty_initial_DC = Vbatt_meas/Vdc;
    }

    // -- saturation block --
    if  (duty >= duty_max)
    {
        duty = duty_max;                                                    // set the duty cycle to the maximum
    }
    else if (duty <=duty_min)
    {
        duty = duty_min;                                                    // set the  duty cycle to the minimum
    }

    return duty;
}


/*
 * reset the controller after en is set to 0
 */

void reset_controller()
{
//    Vbus_meas     = 0.0;                      // Define measured DC bus voltage
//
//    for (i=0;i<3;i++)
//    {
//        Vbatt_meas[i]    = 0.0;
//        Ibatt_meas[i]    = 0.0;                      // Define measured avg current flowing to the battery ** inductor current right now due to the position of the sensor is placed right after the center tap**
//        Iind_meas[i]     = 0.0;                      // Define measured current from the AC side
//        error_batt_oc[i] = 0;
//        error_bus_oc[i]  = 0;
//    }
//
//    reset_PIerror();
//    RST_gate_driver = 1;
    /*
     * reset duty cycle to 0.5
     */
//    duty_phaseA               = 0.0;
//    duty_phaseB               = 0.0;
//    duty_phaseC               = 0.0;
//
//
//
//    EPWM_dutyupdate(EPWM_A,duty_phase[0]);
//    EPWM_dutyupdate(EPWM_B,duty_phase[1]);
//    EPWM_dutyupdate(EPWM_C,duty_phase[2]);
//    protection_on           = 0;                                                  // reset the protection_on indicator                                                // reset the protection trigger count

//    error_batt_ov           = 0;
//    error_bus_ov            = 0;
//    error_batt_lv           = 0;


}

void reset_PIerror()
{
    //.......................Initialize error between measurements and reference...................
//    Vbatt_error         = 0.0;                      // The error between reference and measured battery voltage
//    Vbatt_error_I_k     = 0.0;                      // Accumulated error on battery voltage at current measurement
//    Vbatt_error_I_k_1   = 0.0;                      // Accumulated error on battery voltage at previous measurement
//    for (i=0;i<3;i++)
//    {
//        Ibatt_error[i]         = 0.0;                      // The error between reference and measured battery current
//        Ibatt_error_I_k[i]     = 0.0;                      // Accumulated error on battery current at current measurement
//        Ibatt_error_I_k_1[i]   = 0.0;                      // Accumulated error on battery current at previous measurement
//        Iind_error[i]          = 0.0;                      // The error between reference and measured battery current
//        Iind_error_I_k[i]      = 0.0;                      // Accumulated error on battery current at current measurement
//        Iind_error_I_k_1[i]    = 0.0;                      // Accumulated error on battery current at previous measurement
//    }


}

//...................calculate the execution time.....................//
void configTimer()
{
    EALLOW; //Allow for writing to protected registers

    //Configure CPU Timer to count down at SYSCLK rate. By using CPU Timer, the time to run a full loop
    //can be determined. The time is important because it allows for precise algorithm runtime determination.
    CpuTimer1Regs.PRD.all=0x0000FFFF; //Period set to (2^16) counts
    CpuTimer1Regs.TPR.bit.TDDR=0x00; //Lower half of timer pre-scalar
    CpuTimer1Regs.TPRH.bit.TDDRH=0x00; //Upper half of timer pre-scalar
    CpuTimer1Regs.TCR.bit.TRB=1; //Load decrement counter and counter every SYSCLK (5ns)

    EDIS; //Disable writing to protected registers
}

void PLL() //Get grid theta from measured three-phase voltages
{
    float Valpha = (1.0/3.0)*(2.0*Vcap_meas[0] - Vcap_meas[1] - Vcap_meas[2]);
    float Vbeta = (1.0/sqrt3)*(Vcap_meas[1] - Vcap_meas[2]);

    if(PLL_init==0)
    {
        theta = atan2(Vbeta,Valpha); //Get an estimate of theta from Valpha & Vbeta
        omega_PLL = omega; //Take a guess that we're operating at standard grid frequency (50/60Hz)
        PLL_init = 1; //Never enter this if statement again
    }

    float Vq = -Valpha*sin(theta) + Vbeta*cos(theta); //For the PLL, we only need the q component, per our definition
    float Vq_ref = 0.0; //We want Vq to be zero
    float eVq = Vq_ref - Vq; //Vq error signal

    Int_PLL = Ts*eVq + Int_PLL; //Integrator without anti-windup
    omega_PLL = Kp_PLL*eVq + Ki_PLL*Int_PLL; //Get omega from PLL via PI controller
//    omega_PLL = omega;

    theta = theta + Ts*omega_PLL; //Discrete-time integrator to then get theta from omega
    theta = theta - PI2*floor(theta/PI2); //Modulus operator to stay within [0,2pi]
}

void zeroVoltage() //Tries to push zero sequence voltage to Vdc/2
{
/*
    if(change_ZV == 1)
    {
        V0_ref; //Set from variable pane
    }

    else
    {
        V0_ref = 0.5*Vdc_control;//Vdc;
    }

    if(EN_AC_controller == 0)
    {
        V0_ref = 0.5*Vdc_initial;
    }
    else
    {
        V0_ref = 0.5*Vdc_control;
    }
*/
    V0_ref = 0.5*Vdc;//0.5*Vdc_control;

    ezs = V0_ref - Vcapdq0[2]; //Error signal used for PI controller

//    Int_zs = Ts*ezs + Int_zs; //Integrator without anti-windup
    Int_zs = Ts*(ezs+ezs_star) + Int_zs; //Integrator with anti-windup
    Idq0_ind_ref[2] = Kp_zs*ezs +  Ki_zs*Int_zs; //Get I0_ind_ref via PI controller

    //Saturate I0_ref to maximum permissible value
    float I0_sat = 0.0;

    if(Idq0_ind_ref[2] > I0_max) //If integrator output is bigger than maximum value
    {
        I0_sat = I0_max;
    }
    else if (Idq0_ind_ref[2] < -I0_max) //If integrator output is less than minimum value
    {
        I0_sat = -I0_max;
    }
    else
    {
        I0_sat = Idq0_ind_ref[2];
    }

    //Back calculation for anti-windup
    if(I0_sat<=Idq0_ind_ref[2] && I0_sat>0) //If Id<saturated (integrator too high) & we're positive, then act to reduce integrator
    {
        ezs_star = I0_sat - Idq0_ind_ref[2];
    }
    else if(I0_sat>=Idq0_ind_ref[2] && I0_sat<0) //If saturated>Id and we're negative, integrator too high--act to reduce
    {
        ezs_star = I0_sat - Idq0_ind_ref[2];
    }
    else //We fail to meet the necessary conditions; hence, no integrator compensation to be done.
    {
        ezs_star = 0.0;
    }

    Idq0_ind_ref[2] = I0_sat; //Saturate

//    Idq0_ind_ref[2] = -(1.0/3.0)*(Igrid_meas_cal[0]+Igrid_meas_cal[1]+Igrid_meas_cal[2]);

}

void reactivepower() //Get Iq_ind reference from the reactive power control. Note: We use input voltage and current since we DO NOT have the output current to the grid (limitation of the number of ADCs available)
{
    if (EN_Q == 1)
    {
////////New reactive PI controller////////////////////////////////////////////////////////////////////////////

        PQ_dq_Q = 1.5*(Vcapdq0[1]*Idq0_ind[0] - Vcapdq0[0]*Idq0_ind[1]); //Calculate (input) reactive power: Q = 3/2*(VqId - VdIq)

        PQ[1] = 1.5*(Vbeta_cap*Ialpha_ind - Valpha_cap*Ibeta_ind); //Calculate (input) reactive power: Q = 3/2*(VbetaIalpha - ValphaIbeta)
        float Q_error = PQ_ref[1] - PQ[1];

        //anti-windup
        if (((Idq0_ind_ref[1] >= Q_max) && (Q_error > 0)) | ((Idq0_ind_ref[1] <= -Q_max) && (Q_error < 0))) // saturated and keep saturating to make the integral error constant
            Q_error_int = Q_error_int_1;
        else
        {
            Q_error_int_1 = Q_error_int;
            Q_error_int = Q_error_int_1 + Ts * Q_error;
        }

        // reactive power controller reference update for inductor current controller
        Idq0_ind_ref[1] = Kp_Q*Q_error +  Ki_Q*Q_error_int;

        if (Idq0_ind_ref[1] >= Q_max)
        {
            Idq0_ind_ref[1] = Q_max;
        }
        else if (Idq0_ind_ref[1] <= -Q_max)
        {
            Idq0_ind_ref[1] = -Q_max;
        }
    //capture = 1;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////


////////Previous reactive PI controller/////////////////////////////////////////////////////////////////////////
        /*
        //PQ[1] = 1.5*(Vcapdq0[1]*Idq0_ind[0] - Vcapdq0[0]*Idq0_ind[1]); //Calculate (input) reactive power: Q = 3/2*(VqId - VdIq)

        PQ[1] = 1.5*(Vbeta_cap*Ialpha_ind - Valpha_cap*Ibeta_ind); //Calculate (input) reactive power: Q = 3/2*(VbetaIalpha - ValphaIbeta)

        float eq = PQ_ref[1] - PQ[1];

        //Integrators with anti-windup. Saturation computed in duty() function
        Int_Q = Ts*(eq+eQ_star) + Int_Q;

        //PI controller to obtain ig_q0 from reactive power
        Idq0_ind_ref[1] = Kp_Q*eq +  Ki_Q*Int_Q;

////////Direct reactive power control by calculation/////////////////////////////////////////////////////////////

        //float temp = (2.0/3.0)/(Vcapdq0[0]*Vcapdq0[0] + Vcapdq0[1]*Vcapdq0[1]); //This term is used twice; hence, pre-calculate

        //Idq0_ind_ref[1] = temp*(PQ_ref[0]*Vcapdq0[1] - PQ_ref[1]*Vcapdq0[0]); //Iq_ref

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        //Add saturation and anti-windup for reference generation

        //Saturate Iq_grid_ref to maximum permissible value
        float Q_sat = 0.0;

        if(Idq0_ind_ref[1] > Q_max) //If integrator output is bigger than maximum value
        {
            Q_sat = Q_max;
            Idq0_ind_ref[1]=Q_max;
        }
        else if (Idq0_ind_ref[1] < -Q_max) //If integrator output is less than minimum value
        {
            Q_sat = -Q_max;
            Idq0_ind_ref[1]=-Q_max;
        }
        else
        {
            Q_sat = Idq0_ind_ref[1];
        }

        //Back calculation for anti-windup
        if(Q_sat<=Idq0_ind_ref[1] && Q_sat>0) //If Q<saturated (integrator too high) & we're positive, then act to reduce integrator
        {
            eQ_star = Q_sat - Idq0_ind_ref[1];
        }
        else if(Q_sat>=Idq0_ind_ref[1] && Q_sat<0) //If saturated>Q and we're negative, integrator too high--act to reduce
        {
            eQ_star = Q_sat - Idq0_ind_ref[1];
        }
        else //We fail to meet the necessary conditions; hence, no integrator compensation to be done.
        {
            eQ_star = 0.0;
        }
        */
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    }
    else
    {
        Q_error_int = 0.0;
        PQ_ref[1] = 0.0;
    }

    Idq0_ind_ref[1] = Idq0_ind_ref[1];
}

void dcVoltage() //Tries to push Vdc to Vdc_ref by giving a power reference
{
    float eVdc = Vdc_ref - Vdc; //Vdc error signal

    //Int_Vdc = Ts*eVdc + Int_Vdc; //Integrator without anti-windup
    Int_Vdc = Ts*(eVdc-eVdc_star) + Int_Vdc; //Integrator with anti-windup
    //PQ_ref[0] = Kp_Vdc*eVdc +  Ki_Vdc*Int_Vdc; //If Kp & Ki are large

    //DC voltage control gives Id_ind
    /*
    Idq0_ind_ref[0] = Kp_Vdc*eVdc +  Ki_Vdc*Int_Vdc; //If Kp and Ki are small

    //Saturate Id_ref to maximum permissible value
    float Id_sat = 0.0;

    if(Idq0_ind_ref[0] > Id_max) //If integrator output is bigger than maximum value
    {
        Id_sat = Id_max;
    }
    else if (Idq0_ind_ref[0] < -Id_max) //If integrator output is less than minimum value
    {
        Id_sat = -Id_max;
    }
    else
    {
        Id_sat = Idq0_ind_ref[0];
    }

    //Back calculation for anti-windup
    if(Id_sat<=Idq0_ind_ref[0] && Id_sat>0) //If Id<saturated (integrator too high) & we're positive, then act to reduce integrator
    {
        eVdc_star = Id_sat - Idq0_ind_ref[0];
    }
    else if(Id_sat>=Idq0_ind_ref[0] && Id_sat<0) //If saturated>Id and we're negative, integrator too high--act to reduce
    {
        eVdc_star = Id_sat - Idq0_ind_ref[0];
    }
    else //We fail to meet the necessary conditions; hence, no integrator compensation to be done.
    {
        eVdc_star = 0.0;
    }

    Idq0_ind_ref[0] = Id_sat; //Saturate
    */


    //DC voltage control gives Id_grid
    Idq0_ind_ref[0] = Kp_Vdc*eVdc +  Ki_Vdc*Int_Vdc; //If Kp and Ki are small

    //Saturate Id_ref to maximum permissible value
    float Id_sat = 0.0;

    if(Idq0_ind_ref[0] > Id_max) //If integrator output is bigger than maximum value
    {
        Id_sat = Id_max;
    }
    else if (Idq0_ind_ref[0] < -Id_max) //If integrator output is less than minimum value
    {
        Id_sat = -Id_max;
    }
    else
    {
        Id_sat = Idq0_ind_ref[0];
    }

    //Back calculation for anti-windup
    if(Id_sat<=Idq0_ind_ref[0] && Id_sat>0) //If Id<saturated (integrator too high) & we're positive, then act to reduce integrator
    {
        eVdc_star = Id_sat - Idq0_ind_ref[0];
    }
    else if(Id_sat>=Idq0_ind_ref[0] && Id_sat<0) //If saturated>Id and we're negative, integrator too high--act to reduce
    {
        eVdc_star = Id_sat - Idq0_ind_ref[0];
    }
    else //We fail to meet the necessary conditions; hence, no integrator compensation to be done.
    {
        eVdc_star = 0.0;
    }

    Idq0_ind_ref[0] = Id_sat; //Saturate

}

void indCurrent() //Inductor current control algorithm: generate Vdq0 for duty cycle and PWM
{
    if (EN_startup==1 && EN_AC_controller==1 && Flag_inrush==1)
    {
        InrushCurrentCounter++;
        if (InrushCurrentCounter<=0)
        {
            Idq0_ind_ref[0]=0.0;
            Idq0_ind_ref[1]=0.0;
            Idq0_ind_ref[2]=0.0;
        }
        else
        {
            InrushCurrentCounter=0;
            Int_Vdc=0.0;
            Q_error_int=0.0;
            Int_zs=0.0;
            Flag_inrush=0;
        }
    }

    //Calculate error signals
    ed = Idq0_ind_ref[0] - Idq0_ind[0];
    eq = Idq0_ind_ref[1] - Idq0_ind[1];
    e0 = Idq0_ind_ref[2] - Idq0_ind[2];

    //Integrators with anti-windup. Saturation computed in duty() function
    Intdq0_ind[0] = Ts*(ed+edq_ind_star[0]) + Intdq0_ind[0];
    Intdq0_ind[1] = Ts*(eq+edq_ind_star[1]) + Intdq0_ind[1];
    Intdq0_ind[2] = Ts*(e0+edq_ind_star[2]) + Intdq0_ind[2];

    //PI controller to obtain Vdq0 from Idq
    V_PI[0] = Kp_ind*ed + Ki_ind*Intdq0_ind[0];
    V_PI[1] = Kp_ind*eq + Ki_ind*Intdq0_ind[1];
    V_PI[2] = Kp_ind*e0 + Ki_ind*Intdq0_ind[2];

    //Feed-forward term (add capacitor voltage)
    //Vdq0[0] = Vdq0[0] + Vcapdq0[0];
    //Vdq0[1] = Vdq0[1] + Vcapdq0[1];
    //Vdq0[2] = Vdq0[2] + Vcapdq0[2];

    //Vdq0[0] = V_PI[0] + Vcapdq0[0];
    //Vdq0[1] = V_PI[1] + Vcapdq0[1];
    //Vdq0[2] = V_PI[2] + Vcapdq0[2];

    //Vdq0[0] = V_PI[0] + Vconst[0];
    //Vdq0[1] = V_PI[1] + Vconst[1];
    //Vdq0[2] = V_PI[2] + Vconst[2];

    //Vdq0[0] = V_PI[0];
    //Vdq0[1] = V_PI[1];
    //Vdq0[2] = V_PI[2];
}

void duty_AC()
{
    //Feedforward capacitor voltage (after startup)
/*
    if (EN_AC_controller == 0)
    {
        Vcapdq0[2] = Vdc_initial/2;
    }
    else if(EN_AC_controller == 1)
    {
        Vcapdq0[2] = Vdc_control/2;
    }
*/
    Vdq0[0] = V_PI[0] + Vcapdq0_initial[0];
    Vdq0[1] = V_PI[1] + Vcapdq0_initial[1];
    Vdq0[2] = V_PI[2] + Vcapdq0_initial[2];//0.0;


    //Vdq0[0] = V_PI[0] + Vconst[0];
    //Vdq0[1] = V_PI[1] + Vconst[1];
    //Vdq0[2] = V_PI[2] + Vconst[2];

    //Vdq0[0] = V_PI[0];
    //Vdq0[1] = V_PI[1];
    //Vdq0[2] = V_PI[2];

    //Precompute theta for "future" transforms and respective trig functions
    float theta15 = theta + 1.5*Ts*omega; //omega_PLL bounces around too much. Use omega, assuming f = gridFreq
    float sine15 = sin(theta15);
    float cosine15 = cos(theta15);

    //Inverse Park transform
    float Valpha = Vdq0[0]*cosine15 - Vdq0[1]*sine15;
    float Vbeta = Vdq0[0]*sine15 + Vdq0[1]*cosine15;
    float Vgamma = Vdq0[2];

    //Inverse Clarke
    Vabc[0] = Valpha + Vgamma;
    Vabc[1] = 0.5*(-Valpha + sqrt3*Vbeta) + Vgamma;
    Vabc[2] = -0.5*(Valpha + sqrt3*Vbeta) + Vgamma;
/*
    //dead time compensation
    if (Iind_meas_cal[0]>0)
    {
        Vabc[0] = Vabc[0] + Vdc*0.015;
    }
    else if (Iind_meas_cal[0]<0)
    {
        Vabc[0] = Vabc[0] - Vdc*0.015;
    }

    if (Iind_meas_cal[1]>0)
    {
        Vabc[1] = Vabc[1] + Vdc*0.015;
    }
    else if (Iind_meas_cal[1]<0)
    {
        Vabc[1] = Vabc[1] - Vdc*0.015;
    }

    if (Iind_meas_cal[2]>0)
    {
        Vabc[2] = Vabc[2] + Vdc*0.015;
    }
    else if (Iind_meas_cal[2]<0)
    {
        Vabc[2] = Vabc[2] - Vdc*0.015;
    }
*/
    //Modulation indices per phase. Local variables, so they are discarded when the function ends
    float iVdc = 1.0/Vdc_control; //Inverse of the DC voltage. For ZS control system, 1.0/Vdc; for no ZS, 2.0/Vdc

    if (EN_AC_controller == 0)
    {
        iVdc = 1.0/(2*Vmgrid_rated);//2.0/Vdc_initial;
    }
    else if(EN_AC_controller == 1)
    {

        iVdc = 1/Vdc;//Vdc_duty;

    }
    float modABC[3] = {0.0, 0.0, 0.0}; //Array for modulation indices
    modABC[0] = iVdc*Vabc[0];
    modABC[1] = iVdc*Vabc[1];
    modABC[2] = iVdc*Vabc[2];

    //Force modulation indices to stay within [-1 or {0,1}]. FOR loop to avoid several consecutive IF statements
    //For ZS control, lower limit is 0; for no ZS control, lower limit is -1
    unsigned short i = 0;
    for(i=0; i<3; i++)
    {
        if(modABC[i]>=1.0)
        {
            modABC[i] = 1.0;
        }
        else if(modABC[i]<=0.0)
        {
            modABC[i] = 0.0;
        }
    }

    //Calculate duty cycle. Stays in the range [0,1]. Unlike the motor drive where we centre about 0V, in this configuration we settle about Vdc/2. This changes modulation indices and duty cycle generation.
    //For ZS control, dutyABC = modABC; for no ZS control, dutyABC = 0.5*(modABC+1)
    dutyABC[0] = modABC[0];//0.5*(modABC[0]+1);
    dutyABC[1] = modABC[1];//0.5*(modABC[1]+1);
    dutyABC[2] = modABC[2];//0.5*(modABC[2]+1);

    duty_phase[0] = dutyABC[0]; //AC side phase A duty;
    duty_phase[1] = dutyABC[1]; //AC side phase B duty[1];
    duty_phase[2] = dutyABC[2]; //AC side phase C duty[2];

    //dutyABC[0] = 1.0; //modABC[0];
    //dutyABC[1] = 1.0; //modABC[1];
    //dutyABC[2] = 1.0; //modABC[2];

    float Vabc_sat[3] = {modABC[0]*Vdc, modABC[1]*Vdc, modABC[2]*Vdc}; //Convert modulation indices to phase voltages

    float Valphabeta_sat[3] = { (1.0/3.0)*(2.0*Vabc_sat[0] - Vabc_sat[1] - Vabc_sat[2]), (1.0/sqrt3)*(Vabc_sat[1] - Vabc_sat[2]), ((1.0/3.0)*(Vabc_sat[0] + Vabc_sat[1] + Vabc_sat[2])) }; //Clarke transform--phase to alpha-beta
    float Vdq0_sat[3] = {Valphabeta_sat[0]*cosine15 + Valphabeta_sat[1]*sine15, -Valphabeta_sat[0]*sine15 + Valphabeta_sat[1]*cosine15, Valphabeta_sat[2]}; //Park transform--alpha-beta to dq

    //Use "saturated" modulation indices to find "saturated" Vdq0 for anti-windup via back calculation (see: Bempardo lecture notes)
    //For ZS control, modABC*Vdc; for no ZS control, modABC*Vdc*0.5
    //float Vabc_sat[3] = {modABC[0]*Vdc, modABC[1]*Vdc, modABC[2]*Vdc}; //Convert modulation indices to phase voltages
    //float Valphabeta_sat[3] = { (1.0/3.0)*(2.0*Vabc_sat[0] - Vabc_sat[1] - Vabc_sat[2]), (1.0/sqrt3)*(Vabc_sat[1] - Vabc_sat[2]), ((1.0/3.0)*(Vabc_sat[0] + Vabc_sat[1] + Vabc_sat[2])) }; //Clarke transform--phase to alpha-beta
    //float Vdq0_sat[3] = {Valphabeta_sat[0]*cosine15 + Valphabeta_sat[1]*sine15, -Valphabeta_sat[0]*sine15 + Valphabeta_sat[1]*cosine15, Valphabeta_sat[2]}; //Park transform--alpha-beta to dq


    //Do checks to generate integrator compensation
    for(i=0; i<3; i++)
    {
        if(Vdq0_sat[i]<=Vdq0[i] && Vdq0_sat[i]>0) //If Vdq0<saturated (integrator too high) & we're positive, then act to reduce integrator
        {
            edq_ind_star[i] = Vdq0_sat[i] - Vdq0[i];
        }

        else if(Vdq0_sat[i]>=Vdq0[i] && Vdq0_sat[i]<0) //If saturated>Vdq0 and we're negative, integrator too high--act to reduce
        {
            edq_ind_star[i] = Vdq0_sat[i] - Vdq0[i];
        }
        else //We fail to meet the necessary conditions; hence, no integrator compensation to be done.
        {
            edq_ind_star[i] = 0.0;
        }
     }
/*
    //Set voltages to saturated values (for use in observers/control)
    Vabc[0] = Vabc_sat[0];
    Vabc[1] = Vabc_sat[1];
    Vabc[2] = Vabc_sat[2];
    Vdq0[0] = Vdq0_sat[0];
    Vdq0[1] = Vdq0_sat[1];
    Vdq0[2] = Vdq0_sat[2];
*/
}


