/*
 * Control.c
 *
 *  Created on: Aug 18, 2022
 *      Author: 210055498
 */
//#ifdef RELEASE
//#pragma CODE_SECTION(adca1_isr, ".TI.ramfunc");
//#endif

// Externs from CAN.c file
extern union GUI_CommandUnion   GuiCmd;
extern float                    GuiFloat1;
extern float                    GuiFloat2;
extern union DAC_MessageUnion   DAC[4];

#include <F2837xD_device.h>
#include <F2837xD_Pie_defines.h>
//#include <F2837xD_EPwm_defines.h>
//#include <F2837xD_epwm.h>
//#include <F2837xD_Examples.h>
#include "Func.h"
#include "C1_Def.h"
#include "C1_Can.h"
#include "C1_Struct.h"
#include "C1_Proto.h"
#include "C1_Vars.h"

//#ifdef RELEASE
#pragma CODE_SECTION(ADC_A1_Interrupt, ".TI.ramfunc");
#pragma CODE_SECTION(EPWM1_Interrupt, ".TI.ramfunc");
#pragma CODE_SECTION(USER_Interrupt, ".TI.ramfunc");
#pragma CODE_SECTION(DacCalc, ".TI.ramfunc");
#pragma CODE_SECTION(ForegroundFaults, ".TI.ramfunc");
//#endif

//*********************************************************************
// PWM1 Interrupt clears ADCA.1 interrupt and enabled DMA controller.
// This needs to be done only once to start DMA transfer in sync with
// 1st ADC conversion within Control period (EPWM1 INT is placed at
// the beginning and the middle of switching period)
//*********************************************************************
interrupt void EPWM1_Interrupt(void)
{
   GpioDataRegs.GPBSET.bit.GPIO59 = 1;  // Debug Set DIG Out B - pin 110 on eval board
   EALLOW;
   AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;   // make sure INT1 flag is cleared
   AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;     // enable ADCA.1 interrupt
//   DmaRegs.CH1.CONTROL.bit.RUN = 1;         // start DMA channel 1
   EPwm5Regs.ETSEL.bit.SOCAEN = 1;          // Enable SOCA

   PieVectTable.EPWM1_INT = USER_Interrupt; // Attach USER INT to EPWM1 INT after the first run
   EDIS;

   // Clear INT flag
   EPwm1Regs.ETCLR.bit.INT = 1;
   // Acknowledge this interrupt to receive more interrupts from group 3
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
   GpioDataRegs.GPBCLEAR.bit.GPIO59 = 1;  // Debug Clear DIG Out B
   AdcIntCounter = 0;
}

interrupt void ADC_A1_Interrupt(void)
{
   // Toggle GPIO DIG_OUTB for debug
   GpioDataRegs.GPBSET.bit.GPIO58 = 1; // Set Dig Out C - pin 108 on eval board

   // SOC0 => A0 - IindA     B2 -  IindC      C2 - VoutA
   // SOC1 => A2 - IindB     B3 -  VoutC      C3 - Vdc
   // SOC2 => A3 - VoutB     B15 - Case_Tempr C4 - B_Tempr
   // SOC3 => A1 - Iext_curr
   // SOC4 => A4 - A_Tempr
   // SOC5 => A5 - C_Tempr

   // AdcBuffer[0] =  IindA,
   // AdcBuffer[1] =  IindB,
   // AdcBuffer[2] =  IindC
   // AdcBuffer[3] =  VoutA,
   // AdcBuffer[4] =  VoutB,
   // AdcBuffer[5] =  VoutC
   // AdcBuffer[6] =  Vdc,
   // AdcBuffer[7] =  Iext_curr
   // AdcBuffer[8] =  Case_Tempr
   // AdcBuffer[9] =  A_Tempr
   // AdcBuffer[10] = B_Tempr
   // AdcBuffer[11] = C_Tempr

   pAdcBuffer = &AdcBuffer[AdcIntCounter * 12];
   *pAdcBuffer = AdcaResultRegs.ADCRESULT0; // ADCA SOC0 IindA
   pAdcBuffer++;
   *pAdcBuffer = AdcaResultRegs.ADCRESULT1; // ADCA SOC1 IindB
   pAdcBuffer++;
   *pAdcBuffer = AdcbResultRegs.ADCRESULT0; // ADCB SOC0 IindC
   pAdcBuffer++;
   *pAdcBuffer = AdccResultRegs.ADCRESULT0; // ADCC SOC0 VoutA
   pAdcBuffer++;
   *pAdcBuffer = AdcaResultRegs.ADCRESULT2; // ADCA SOC2 VoutB
   pAdcBuffer++;
   *pAdcBuffer = AdcbResultRegs.ADCRESULT1; // ADCB SOC1 VoutC
   pAdcBuffer++;
   *pAdcBuffer = AdccResultRegs.ADCRESULT1; // ADCC SOC1 Vdc
   pAdcBuffer++;
   *pAdcBuffer = AdcaResultRegs.ADCRESULT3; // ADCA SOC3 Iext_curr
   pAdcBuffer++;
   *pAdcBuffer = AdcbResultRegs.ADCRESULT2; // ADCB SOC2 Case_Tempr
   pAdcBuffer++;
   *pAdcBuffer = AdcaResultRegs.ADCRESULT4; // ADCA SOC4 A_Tempr
   pAdcBuffer++;
   *pAdcBuffer = AdccResultRegs.ADCRESULT2; // ADCC SOC2 B_Tempr
   pAdcBuffer++;
   *pAdcBuffer = AdcaResultRegs.ADCRESULT5; // ADCA SOC5 C_Tempr

   AdcIntCounter = (++AdcIntCounter) & 0x3;

   // Clear ADCINT1
   AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
   // To receive more interrupts from this PIE group, acknowledge this interrupt.
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;

   GpioDataRegs.GPBCLEAR.bit.GPIO58 = 1; // Set Dig Out C - pin 108 on eval board
}

//interrupt void DMA_CH1_Interrupt(void)
//{
//   // Toggle GPIO DIG_OUTA for debug
//   GpioDataRegs.GPBTOGGLE.bit.GPIO57 = 1;   // toggle Dig Out A - pin 106 on eval board
//
//   // Clear INT flag for this timer
//   DmaRegs.CH1.CONTROL.bit.PERINTCLR = 1;
//   // Acknowledge this interrupt to receive more interrupts from group 3
//   PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
//   // request USER1_TRAP interrupt that runs control code
//   asm("   TRAP #20;");
//}

#pragma INTERRUPT(USER_Interrupt, HPI);
interrupt void USER_Interrupt(void)
{
   EINT; // Enable interrupts so that ADC interrupt can run
   GpioDataRegs.GPBSET.bit.GPIO59 = 1;  // Debug Set DIG Out B - pin 110 on eval board

   // Average samples from AdcBuffer
   // SOC0 => A0 - IindA      B2 - IindC       C2 - VoutA
   // SOC1 => A2 - IindB      B3 - VoutC       C3 - Vdc
   // SOC2 => A3 - VoutB      B15 - Case_Tempr C4 - B_Tempr
   // SOC3 => A1 - Iext_curr
   // SOC4 => A4 - A_Tempr
   // SOC5 => A5 - C_Tempr

   // AdcBuffer[0] = IindA,
   // AdcBuffer[1] = IindB,
   // AdcBuffer[2] = IindC
   // AdcBuffer[3] = VoutA,
   // AdcBuffer[4] = VoutB,
   // AdcBuffer[5] = VoutC
   // AdcBuffer[6] = Vdc,
   // AdcBuffer[7] = Iext_curr
   // AdcBuffer[8] = Case_TemprRaw
   // AdcBuffer[9] = A_TemprRaw
   // AdcBuffer[10] = B_TemprRaw
   // AdcBuffer[11] = C_TemprRaw

   IindAbc.a =    (float)(AdcBuffer[0]  + AdcBuffer[12] + AdcBuffer[24] + AdcBuffer[36]) * Gain.Iind - Cal.InductorCurrentRange;
   IindAbc.b =    (float)(AdcBuffer[1]  + AdcBuffer[13] + AdcBuffer[25] + AdcBuffer[37]) * Gain.Iind - Cal.InductorCurrentRange;
   IindAbc.b =    (float)(AdcBuffer[2]  + AdcBuffer[14] + AdcBuffer[26] + AdcBuffer[38]) * Gain.Iind - Cal.InductorCurrentRange;
   VoutAbc.a =    (float)(AdcBuffer[3]  + AdcBuffer[15] + AdcBuffer[27] + AdcBuffer[39]) * Gain.Vout - Cal.VoltageOutputRange;
   VoutAbc.b =    (float)(AdcBuffer[4]  + AdcBuffer[16] + AdcBuffer[28] + AdcBuffer[40]) * Gain.Vout - Cal.VoltageOutputRange;
   VoutAbc.c =    (float)(AdcBuffer[5]  + AdcBuffer[17] + AdcBuffer[29] + AdcBuffer[41]) * Gain.Vout - Cal.VoltageOutputRange;
   Vdc =          (float)(AdcBuffer[6]  + AdcBuffer[18] + AdcBuffer[30] + AdcBuffer[42]) * Gain.Vdc;
   Iext =         (float)(AdcBuffer[7]  + AdcBuffer[19] + AdcBuffer[31] + AdcBuffer[43]) * Gain.Iext - Cal.ExtCurrentRange;
   TemprCaseRaw = (float)(AdcBuffer[8]  + AdcBuffer[20] + AdcBuffer[32] + AdcBuffer[44]);
   TemprARaw =    (float)(AdcBuffer[9]  + AdcBuffer[21] + AdcBuffer[33] + AdcBuffer[45]);
   TemprBRaw =    (float)(AdcBuffer[10] + AdcBuffer[22] + AdcBuffer[34] + AdcBuffer[46]);
   TemprCRaw =    (float)(AdcBuffer[11] + AdcBuffer[23] + AdcBuffer[35] + AdcBuffer[47]);

   GpioDataRegs.GPBCLEAR.bit.GPIO59 = 1;  // Debug Clear DIG Out B - pin 110 on eval board
   GpioDataRegs.GPBSET.bit.GPIO59 = 1;  // Debug Set DIG Out B - pin 110 on eval board
   InvVdc =  __divf32(1.0, __fmax(Vdc, 500.0));

   //Vdc = 340.0;
   //Iind1 = 50.0;
   //Iind2  = 50.1;
   //Iload = 100.0;

   // create reference sin and cos always
   Theta.Theta += Frequency * TWO_PI * Tsmp;   // used in DAC debugging
   if (Theta.Theta >= TWO_PI) {
      Theta.Theta -= TWO_PI;
   }
   Theta.Sin = __sin(Theta.Theta);
   Theta.Cos = __cos(Theta.Theta);

   switch (Mode) {
   case (STANDBY_MODE):
      switch (SubMode) {
      case (ENTRY) :
         SubMode = IN;
         /* no break */
      case (IN) :
         if (ModeCmd == FAULT_MODE) {
            Mode = FAULT_MODE;
         }
         else if (ModeCmd == RUN_MODE) {
            Mode = RUN_MODE;
         }
         else {
            break;
         }
         /* no break */
      case (EXIT) :
         SubMode = ENTRY;
         break;
      }
      break;

   case (RUN_MODE):
      switch (SubMode) {
      case (ENTRY) :
         EPwm1Regs.CMPA.bit.CMPA = PeriodCounter/2; // Set compare A value
         EPwm2Regs.CMPA.bit.CMPA = PeriodCounter/2; // Set compare A value
         EPwm3Regs.CMPA.bit.CMPA = PeriodCounter/2; // Set compare A value
         EPwm1Regs.TZCLR.bit.OST = 1; // Clear trip bit - enables PWM
         EPwm2Regs.TZCLR.bit.OST = 1; // Clear trip bit - enables PWM
         EPwm3Regs.TZCLR.bit.OST = 1; // Clear trip bit - enables PWM
         SubMode = IN;
         break;
      case (IN) :
         VmagCmd = __fsat(VmagCmd, 0.577350269 * Vdc, -0.577350269 * Vdc);
         VcmdXy.x = -VmagCmd * Theta.Sin; // align voltage vector with Q axis
         VcmdXy.y = VmagCmd * Theta.Cos;
         xy2abc(&VcmdXy, &VcmdAbc);

         // Z axis current controller to reduce low frequency common mode current
         abc2xyz(&IindAbc, &IindXyz);
         xyz2dqo(&IindXyz, &IindDqo, &Theta);

         CurrZReg.Error = -IindXyz.z;
         CurrZReg.UL = Vdc * 0.3;
         CurrZReg.LL = -CurrZReg.UL;
         VoutZCmd = VLPI(&CurrZReg);

         DutyAbc.a = __fsat((VcmdAbc.a + VoutZCmd) * InvVdc, 0.98, 0.02);
         DutyAbc.b = __fsat((VcmdAbc.b + VoutZCmd) * InvVdc, 0.98, 0.02);
         DutyAbc.c = __fsat((VcmdAbc.c + VoutZCmd) * InvVdc, 0.98, 0.02);

         Duty1CMPACounts = (unsigned int)((DutyAbc.a + 0.5) * PeriodCounterFloat);
         Duty2CMPACounts = (unsigned int)((DutyAbc.b + 0.5) * PeriodCounterFloat);
         Duty3CMPACounts = (unsigned int)((DutyAbc.c + 0.5) * PeriodCounterFloat);

         EPwm1Regs.CMPA.bit.CMPA = Duty1CMPACounts;
         EPwm2Regs.CMPA.bit.CMPA = Duty2CMPACounts;
         EPwm2Regs.CMPA.bit.CMPA = Duty2CMPACounts;

         // State exit conditions
         if ((ModeCmd == FAULT_MODE) || (FaultCode.All != 0)) {
            Mode = FAULT_MODE;
         }
         else if (ModeCmd == STANDBY_MODE) {
            Mode = STANDBY_MODE;
         }
         else {
            break;
         }
         /* no break */
      case (EXIT) :
         EPwm1Regs.TZFRC.bit.OST = 1;  // Force One time trip zone event - turns off all switches
         EPwm2Regs.TZFRC.bit.OST = 1;
         EPwm3Regs.TZFRC.bit.OST = 1;
         SubMode = ENTRY;
         break;
      }
      break;
   case (FAULT_MODE):
      switch (SubMode) {
      case (ENTRY) :
         SubMode = IN;
         /* no break */
      case (IN) :
         if (ModeCmd == STANDBY_MODE) {
            Mode = STANDBY_MODE;
         }
         else {
            break;
         }
         /* no break */
      case (EXIT) :
         SubMode = ENTRY;
         break;
      }
      break;
   }

   ForegroundFaults();
   DacCalc();

   BackgroundCounter++;
   if (BackgroundCounter >= BackgroundCounts) {
      BackgroundCounter = 0;
      BackgroundRunFlag = 0xFFFF;
   }

   EPwm1Regs.ETCLR.bit.INT = 1;
   // Acknowledge this interrupt to receive more interrupts from group 3
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;

   GpioDataRegs.GPBCLEAR.bit.GPIO59 = 1;  // Debug Clear DIG Out B
}

void DacCalc(void)
{
   register float tmp;

   for (unsigned int ch_num = 0; ch_num < 3; ch_num++) {
      switch (DAC[ch_num].Bit.Type) {
      case IEEE_FLOAT:
         if (DAC[ch_num].Bit.Zero == 0) {
            tmp = (*(float*)DAC[ch_num].Bit.Addr + DAC[ch_num].Bit.Offset) * DAC[ch_num].Bit.Gain;
            tmp = __fsat(tmp,4095.0,0.0);
            DacValue[ch_num] = (unsigned int)tmp;
         }
         else  {
            DacValue[ch_num] = (unsigned int)2048;
         }
         break;
      case UNSIGNED_16:
         if (DAC[ch_num].Bit.Zero == 0) {
            tmp = ((float)(*( volatile unsigned int*)DAC[ch_num].Bit.Addr) + DAC[ch_num].Bit.Offset) * DAC[ch_num].Bit.Gain;
            tmp = __fsat(tmp,4095.0,0.0);
            DacValue[ch_num] = (unsigned int)tmp;
         }
         else  {
            DacValue[ch_num] = (unsigned int)0;
         }
         break;
      case SIGNED_16:
         if (DAC[ch_num].Bit.Zero == 0) {
            tmp = ((float)(*(volatile int*)DAC[ch_num].Bit.Addr) + DAC[ch_num].Bit.Offset) * DAC[ch_num].Bit.Gain;
            tmp = __fsat(tmp,4095.0,0.0);
            DacValue[ch_num] = (unsigned int)tmp;
         }
         else  {
            DacValue[ch_num] = (unsigned int)2048;
         }
         break;
      case UNSIGNED_32:
         if (DAC[ch_num].Bit.Zero == 0) {
            tmp = ((float)(*(volatile unsigned long*)DAC[ch_num].Bit.Addr) + DAC[ch_num].Bit.Offset) * DAC[ch_num].Bit.Gain;
            tmp = __fsat(tmp,4095.0,0.0);
            DacValue[ch_num] = (unsigned int)tmp;
         }
         else  {
            DacValue[ch_num] = (unsigned int)0;
         }
         break;
      case SIGNED_32:
         if (DAC[ch_num].Bit.Zero == 0) {
            tmp = ((float)(*(volatile long*)DAC[ch_num].Bit.Addr) + DAC[ch_num].Bit.Offset) * DAC[ch_num].Bit.Gain;
            tmp = __fsat(tmp,4095.0,0.0);
            DacValue[ch_num] = (unsigned int)tmp;
         }
         else  {
            DacValue[ch_num] = (unsigned int)2048;
         }
         break;
      default:
         DacValue[ch_num] = 0;
         break;
      }
   }
   DacaRegs.DACVALS.all = DacValue[0];
   DacbRegs.DACVALS.all = DacValue[1];
   DaccRegs.DACVALS.all = DacValue[2];
}

void ForegroundFaults(void)
{
   Imag = __sqrt(IindDqo.d * IindDqo.d + IindDqo.q * IindDqo.q + IindDqo.o * IindDqo.o);
   if (Imag >= Cal.IndOvercurrent) {
      if (IndOvercurrentCounter < IndOvercurrentCounts) {
         IndOvercurrentCounter++;
      }
      else {
         FaultCode.Bit.OvercurrentInd = 1;
      }
   }
   else {
      IndOvercurrentCounter--;
      IndOvercurrentCounter = __max(IndOvercurrentCounter, 0);
   }

   if (Vdc >= Cal.DcOvervoltage) {
      if (VdcOvervoltageCounter < VdcOvervoltageCounts) {
         VdcOvervoltageCounter++;
      }
      else {
         FaultCode.Bit.DcOvervoltage = 1;
      }
   }
   else {
      VdcOvervoltageCounter--;
      VdcOvervoltageCounter = __max(VdcOvervoltageCounter, 0);
   }

   if (Vdc < Cal.DcOutUndervoltage) {
      if (VdcOutUndervoltageCounter < VdcOutUndervoltageCounts) {
         VdcOutUndervoltageCounter++;
      }
      else {
         FaultCode.Bit.DcOutUndervoltage = 1;
      }
   }
   else {
      VdcOutUndervoltageCounter--;
      VdcOutUndervoltageCounter = __max(VdcOutUndervoltageCounter, 0);
   }

//   FaultCode.Bit.DesatA = FAULT1_A;
//   FaultCode.Bit.DesatB = FAULT1_B;
}
