/*
 * 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                    GuiFloat0;
extern float                    GuiFloat1;
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(USER1_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)
{
   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
   EPwm3Regs.ETSEL.bit.SOCAEN = 1;         // enable SOCA
   EPwm1Regs.ETSEL.bit.INTEN = 0;          // Disable PWM1 INT
   IER &= ~M_INT3;                         // Disable INT3 (PWM1 INT is the only one used in this group)
   EDIS;

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

interrupt void ADC_A1_Interrupt(void)  // FIXME Gasha: not sure if I need it since clearing the interrupt is not required.
{
   // Toggle GPIO DIG_OUTB for debug
   GpioDataRegs.GPBSET.bit.GPIO58 = 1; // Set Dig Out C - pin 108 on eval board

//   pAdcBuffer = &AdcBuffer[8 * AdcIntCounter];
//   *pAdcBuffer = AdcaResultRegs.ADCRESULT0;
//   pAdcBuffer++;
//   *pAdcBuffer = AdcaResultRegs.ADCRESULT1;
//   pAdcBuffer++;
//   *pAdcBuffer = AdcbResultRegs.ADCRESULT0;
//   pAdcBuffer++;
//   *pAdcBuffer = AdcbResultRegs.ADCRESULT1;
//   pAdcBuffer++;
//   *pAdcBuffer = AdccResultRegs.ADCRESULT0;
//   pAdcBuffer++;
//   *pAdcBuffer = AdccResultRegs.ADCRESULT1;
//   pAdcBuffer++;
//   *pAdcBuffer = AdcdResultRegs.ADCRESULT0;
//   pAdcBuffer++;
//   *pAdcBuffer = AdcdResultRegs.ADCRESULT1;
//
//   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;

//   if (AdcIntCounter == 0) {
//      // request USER1_TRAP interrupt that runs control code
//      asm("   TRAP #20;");
//   }

   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(USER1_Interrupt, HPI);
interrupt void USER1_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
   Iind1 =   (float)(AdcBuffer[0] + AdcBuffer[8] +  AdcBuffer[16] + AdcBuffer[24]) * Gain.Iind1 - Cal.InductorCurrentRange;
   Iind2 =   (float)(AdcBuffer[1] + AdcBuffer[9] +  AdcBuffer[17] + AdcBuffer[25]) * Gain.Iind2 - Cal.InductorCurrentRange;
   Iload =   (float)(AdcBuffer[2] + AdcBuffer[10] + AdcBuffer[18] + AdcBuffer[26]) * Gain.Iload - Cal.LoadCurrentRange;
   Vdc =     (float)(AdcBuffer[3] + AdcBuffer[11] + AdcBuffer[19] + AdcBuffer[27]) * Gain.VdcIn;
   BodeExc = (float)(AdcBuffer[5] + AdcBuffer[13] + AdcBuffer[21] + AdcBuffer[29]) * Gain.BodeExc - 1.0;
   VdcOut =  (float)(AdcBuffer[7] + AdcBuffer[15] + AdcBuffer[23] + AdcBuffer[31]) * Gain.VdcOut;
   InvVdc =  __divf32(1.0, __fmax(Vdc, 100.0));

ModeCmd = Mode = RUN_MODE; // fixme
   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) :
         Iload_HPF.Zminus1 = 0.0;
         VoltReg.Integral = CurrDiffReg.Integral = 0.0;
         CurrSumReg.Integral = __fmin(__divf32(270.0, __fmax(Vdc, 50.0)), 0.95);
         EPwm1Regs.CMPA.bit.CMPA = PeriodCounter - DeadTimeCounts;  // Set compare A value
         EPwm2Regs.CMPA.bit.CMPA = PeriodCounter - DeadTimeCounts;  // Set compare A value
         EPwm1Regs.TZCLR.bit.OST = 1; // Clear trip bit - enables PWM
         EPwm2Regs.TZCLR.bit.OST = 1; // Clear trip bit - enables PWM
         nPWM1_EN_ON;
         SubMode = IN;
         break;
      case (IN) :
         // Voltage Loop
         VoltReg.Error = Cal.VdcRef - Vdc;
         if (BodeSwitch == 3) {
            BodeY = -VoltReg.Error;
            VoltReg.Error += BodeExc * BodeDistMag;
            BodeX = VoltReg.Error;
         }
         // Limit FFv gain factor between 0 and 1
         FFv = Cal.VoltFF * __fmax(__fmin(__divf32((Vdc - Cal.VdcLow), (Cal.VdcHi - Cal.VdcLow)), 1.0), 0.0);
         VoltReg.FF = DigFilt1(Iload, &Iload_HPF) * FFv;
         IindRef = VLPI(&VoltReg);

         // Sum Current Loop
         IindSum = Iind1 + Iind2;
         CurrSumReg.Error = IindRef - IindSum;
         if (BodeSwitch == 1) {
            BodeY = -CurrSumReg.Error;
            CurrSumReg.Error += BodeExc * BodeDistMag;
            BodeX = CurrSumReg.Error;
         }
         CurrSumReg.FF = Cal.CurrentFF * VdcOut;
         CurrSumReg.UL = Cal.CurrSumULGain * Vdc;
         CurrSumReg.LL = Cal.CurrSumLLGain * Vdc;
         VoutSumCmd = VLPI(&CurrSumReg);

         // Current Diff Loop
         IindDiff = Iind1 - Iind2;
         CurrDiffReg.Error = IindDiff;
         if (BodeSwitch == 2) {
            BodeY = -CurrDiffReg.Error;
            CurrDiffReg.Error += BodeExc * BodeDistMag;
            BodeX = CurrDiffReg.Error;
         }
         VoutDiffCmd = VLPI(&CurrDiffReg);

         Duty1Cmd = (VoutSumCmd - VoutDiffCmd) * InvVdc;
         Duty2Cmd = (VoutSumCmd + VoutDiffCmd) * InvVdc;
         Duty1CMPACounts = (unsigned int)__max(__min((int)(Duty1Cmd * PeriodCounterFloat), PeriodCounterMax), PeriodCounterMin);
         Duty2CMPACounts = (unsigned int)__max(__min((int)(Duty2Cmd * PeriodCounterFloat), PeriodCounterMax), PeriodCounterMin);
         EPwm1Regs.CMPA.bit.CMPA = Duty1CMPACounts;
         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;
//         }
         break;
      case (EXIT) :
         EPwm1Regs.TZFRC.bit.OST = 1;  // Force One time trip zone event - turns off all switches
         EPwm2Regs.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;
   }

   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)
{
   if (Iind1 >= Cal.IndOvercurrent) {
      if (Ind1OvercurrentCounter < Ind1OvercurrentCounts) {
         Ind1OvercurrentCounter++;
      }
      else {
         FaultCode.Bit.OvercurrentInd1 = 1;
      }
   }
   else {
      Ind1OvercurrentCounter--;
      Ind1OvercurrentCounter = __max(Ind1OvercurrentCounter, 0);
   }

   if (Iind2 >= Cal.IndOvercurrent) {
      if (Ind2OvercurrentCounter < Ind2OvercurrentCounts) {
         Ind2OvercurrentCounter++;
      }
      else {
         FaultCode.Bit.OvercurrentInd2 = 1;
      }
   }
   else {
      Ind2OvercurrentCounter--;
      Ind2OvercurrentCounter = __max(Ind2OvercurrentCounter, 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;
}
