/*
 * 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(DMA_CH1_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
   // SOC3 => A1 - Iext_curr

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

   pAdcBuffer = &AdcBuffer[AdcIntCounter * 8];
   *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

   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
   // SOC3 => A1 - Iext_curr

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

   IindAbc.a = (float)(AdcBuffer[0] + AdcBuffer[8] +   AdcBuffer[16] + AdcBuffer[24]) * Gain.Iind - Cal.InductorCurrentRange;
   IindAbc.b = (float)(AdcBuffer[1] + AdcBuffer[9] +   AdcBuffer[17] + AdcBuffer[25]) * Gain.Iind - Cal.InductorCurrentRange;
   IindAbc.b = (float)(AdcBuffer[2] + AdcBuffer[10] +  AdcBuffer[18] + AdcBuffer[26]) * Gain.Iind - Cal.InductorCurrentRange;
   VoutAbc.a = (float)(AdcBuffer[3] + AdcBuffer[11] +  AdcBuffer[19] + AdcBuffer[27]) * Gain.Vout - Cal.VoltageOutputRange;
   VoutAbc.b = (float)(AdcBuffer[4] + AdcBuffer[12] +  AdcBuffer[20] + AdcBuffer[28]) * Gain.Vout - Cal.VoltageOutputRange;
   VoutAbc.c = (float)(AdcBuffer[5] + AdcBuffer[13] +  AdcBuffer[21] + AdcBuffer[29]) * Gain.Vout - Cal.VoltageOutputRange;
   Vdc =       (float)(AdcBuffer[6] + AdcBuffer[14] +  AdcBuffer[22] + AdcBuffer[30]) * Gain.Vdc;
   Iext =      (float)(AdcBuffer[7] + AdcBuffer[15] +  AdcBuffer[23] + AdcBuffer[311]) * Gain.Iext - Cal.ExtCurrentRange;
   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;  // fixme
   //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
         nPWM1_EN_ON;
         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;
         nPWM1_EN_OFF;
         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;
}
