/*
 * main.c
 */

/* This must be here before all include files */
#define LOCATE_VARS

#include <math.h>
#include <string.h>
#include <F2837xD_device.h>
#include <F28x_Project.h>
#include <F2837xD_EPwm.h>
#include "Func.h"
#include "C1_Proto.h"
#include "C1_Def.h"
#include "C1_Can.h"
#include "C1_Struct.h"
#include "C1_Vars.h"

extern union GUI_CommandUnion   GuiCmd;
extern float                    GuiFloat1;
extern float                    GuiFloat2;
extern unsigned int             NewGuiCmdFlag;
extern union DAC_MessageUnion   DAC[4];

extern unsigned int RamFuncsRunStart;
extern unsigned int RamFuncsLoadStart;
extern unsigned int RamFuncsLoadSize;
extern unsigned int RamConstRunStart;
extern unsigned int RamConstLoadStart;
extern unsigned int RamConstLoadSize;

void main(void)
{
   // Disable CPU interrupts
   DINT;
   // Disable CPU interrupts and clear all CPU interrupt flags:
   IER = 0x0000;
   IFR = 0x0000;

#ifdef RELEASE
   memcpy(&RamFuncsRunStart, &RamFuncsLoadStart, (size_t)&RamFuncsLoadSize);
   memcpy(&RamConstRunStart, &RamConstLoadStart, (size_t)&RamConstLoadSize);
#endif

   InitSysCtrl();
   // Initialize the PIE (Peripheral Interrupt Expansion) control registers to their default state.
   // The default state is all PIE interrupts disabled and flags are cleared.
   // This function is found in the F2837xD_PieCtrl.c file.
   InitPieCtrl();
   // Initialize the PIE vector table with pointers to the default TI shell Interrupt Service Routines (ISR).
   // The shell ISR routines are found in F2837xD_DefaultIsr.c.
   // This function is found in F2837xD_PieVect.c.
   InitPieVectTable();

   InitXBar();
   InitGpio();
   InitMemory();
   InitDataRam();
   InitDAC();
   InitADC();
   InitVars();
   InitEPwm();
//   InitDMA();
   InitCANA();
   InitPie();
   InitECap();
   InitPeripheralClocksCustom(); // At the end of initialization turn off unused peripherals

   EPwm1Regs.ETSEL.bit.INTEN = 1;      // Enable PWM1 INT to sync ADC conversions with switching cycle

   // Enable global Interrupts and higher priority real-time debug events:
   EINT;
   ERTM;

//State = RUN_STATE; // debug code
//SubState = ENTRY;
//RunConverter = 0xFFFF;
//Mode = ModeCmd = RUN_MODE;
//SubState = ENTRY;

   while (1) {
      while (BackgroundRunFlag != 0xFFFF) // wait until the control task triggers the background task
         ;
      BackgroundRunFlag = 0;

      RxCANA();
      TxCANA();

      // Process CAN command message when new message arrives
      if (NewGuiCmdFlag == 1) {
         GuiCmdCounter = NewGuiCmdFlag = 0;
         if ((GuiCmd.Bit.nStopStart) && (!nStopStartPrev)) {
            RunConverter = 0xFFFF;  // Set to FFFF if rising edge of GuiCommand.Bit.StopStart
         }
         else if (GuiCmd.Bit.nStopStart == 0) {
            RunConverter = 0;
         }
         nStopStartPrev = GuiCmd.Bit.nStopStart;
         MasterReset = GuiCmd.Bit.MasterReset;
      }
      else {  // If CAN communication with GUI stops, initiate a fault
         GuiCmdCounter++;
         if (GuiCmdCounter > GuiCmdTimeoutCount) {
            //FaultCode.Bit.CAN_Comm = 1; // FIXME
         }
      }

      BackgroundFaults();

      //******************************************
      // Governing State Machine                 *
      //******************************************
      switch (State) {
      case (STANDBY_STATE) :
         switch (SubState) {
         case (ENTRY) :
            ModeCmd = STANDBY_MODE;
            SubState = IN;
            /* no break */
         case (IN) :
            if (FaultCode.All != 0) {
               ModeCmd = FAULT_MODE;
               State = FAULT_STATE;
            }
            else if (RunConverter == 0xFFFF) {
               State = RUN_STATE;
            }
            else {
               break;
            }
            /* no break */
         case (EXIT) :
            StatePrev = STANDBY_STATE;
            SubState = ENTRY;
            break;
         }
         break;

      case (RUN_STATE) :
         switch (SubState) {
         case (ENTRY) :
            ModeCmd = RUN_MODE;
            SubState = IN;
            /* no break */
         case (IN) :
            if (FaultCode.All != 0) {
               ModeCmd = FAULT_MODE;
               State = FAULT_STATE;
            }
            else if (RunConverter != 0xFFFF) {
               ModeCmd = STANDBY_MODE;
               State = STANDBY_STATE;
            }
            else {
               break;
            }
            /* no break */
         case (EXIT) :
            StatePrev = RUN_STATE;
            SubState = ENTRY;
            break;
         }
         break;

      case (FAULT_STATE) :
         switch (SubState) {
         case (ENTRY) :
            // Force One time trip zone event - turns off all switches
            EPwm2Regs.TZFRC.bit.OST = 1;
            EPwm2Regs.TZFRC.bit.OST = 1;
            RunConverter = 0;
            SubState = IN;
            /* no break */
         case (IN) :
            if (MasterReset == 1) {
               SubState = EXIT;
            }
            break;
         case (EXIT) :
            State = STANDBY_STATE;
            FaultCode.All = 0;
            ModeCmd = STANDBY_MODE;
            StatePrev = FAULT_STATE;
            SubState = ENTRY;
            break;
         }
         break;
      }

      LedCounter++;
      if (LedCounter >= LedCounts) {
         LedCounter = 0;
         LED_G_TOGGLE;
         LED_Y_TOGGLE;
         LED_R_TOGGLE;
      }
      RunTimeCounter++;
      if (RunTimeCounter >= Cal.BackgroundFreq) {
         RunTimeCounter = 0;
         RunTime++;
      }
   }
}

void InitVars()
{
   // Time Base CLK is by default CPU_FREQ/2 (100MHz for 200MHz CPU CLK setup).
   // It can be changed to CPU_FREQ (200MHz) if PERCLKDIVSEL.EPWMCLKDIV = 0
   // However, check the data sheet for maximum ePWM clock frequency
   // If you change PWM clock, code below must be modified to reflect the change
   PeriodCounter = (unsigned int) (0.25 * Cal.CoreClockFreq / Cal.SwitchingFreq); // 0.25 * 200e6 / 50e3 = 1000
   AdcCounter = PeriodCounter / (unsigned int) Cal.OversamplingRate;
   Tadc = 2.0 * __divf32((float) AdcCounter, Cal.CoreClockFreq);
   PeriodCounter = AdcCounter * (unsigned int) Cal.OversamplingRate;
   PeriodCounterFloat = (float)PeriodCounter;
   Tsmp = 2.0 * __divf32((float)PeriodCounter, Cal.CoreClockFreq); // PWM runs at half clock
   SwitchingFrequency = __divf32(1.0, Tsmp);
   DeadTimeCounts = (unsigned int) (0.5 * Cal.DeadTime * Cal.CoreClockFreq); // 0.5 * 500e-9 * 200e6 = 50
   PeriodCounterMax = PeriodCounter - DeadTimeCounts;
   PeriodCounterMin = DeadTimeCounts;
   BackgroundCounts = (unsigned int)__divf32(1.0,  Tsmp * Cal.BackgroundFreq);
   Tsmpb = BackgroundCounts * Tsmp;
   LedCounts = (unsigned int)(Cal.LedPeriod * Cal.BackgroundFreq);
   GuiCmdTimeoutCount = Cal.CanTimeout * Cal.BackgroundFreq;

   CurrZReg.Kp = Cal.CurrZRegDef.Kp;
   CurrZReg.Ki = Cal.CurrZRegDef.Ki;
   CurrZReg.Tsmp = &Tsmp;

   {  // bipolar signals
      register float tmp = __divf32(1.0, 2047.0 * Cal.OversamplingRate);
      Gain.Iind = Cal.InductorCurrentRange * tmp;
      //Gain.Iload = Cal.LoadCurrentRange * tmp;
      Gain.BodeExc = tmp;
      // unipolar signals
      tmp = __divf32(1.0, 4095.0 * Cal.OversamplingRate);
      Gain.Vout = Cal.VoltageOutputRange * tmp;
      Gain.Vdc = Cal.VdcRange * tmp;
      Gain.Iext = Cal.ExtCurrentRange * tmp;
   }

   IndOvercurrentCounts = (int)__divf32(Cal.IndOvercurrentTimeout, Tsmp);
   VdcOvervoltageCounts = (int)__divf32(Cal.DcOvervoltageTimeout, Tsmp);
   VdcOutUndervoltageCounts = (int)__divf32(Cal.DcOutUndervoltageTimeout, Tsmp);

//   BackgroundCounter = BackgroundRunFlag = 0;
//   BodeSwitch = 0;
//   BodeDistMag = 0.0;
   RunConverter = 0;
   nStopStartPrev = 0;
   NewGuiCmdFlag = 0;
   MasterReset = 0;
   GuiCmdCounter = 0;

   TempPhA_LookupTbl.N = TempPhB_LookupTbl.N = TempPhC_LookupTbl.N = (int)Cal.RtdTempLookup1D1StructDef.N;
   TempPhA_LookupTbl.Step = TempPhB_LookupTbl.Step = TempPhC_LookupTbl.Step = Cal.RtdTempLookup1D1StructDef.Step;
   TempPhA_LookupTbl.InvStep = TempPhB_LookupTbl.InvStep = TempPhC_LookupTbl.InvStep = __divf32(1.0, TempPhA_LookupTbl.Step);
   TempPhA_LookupTbl.X0 = TempPhB_LookupTbl.X0 = TempPhC_LookupTbl.X0 = Cal.RtdTempLookup1D1StructDef.X0;
   TempPhA_LookupTbl.Data = TempPhB_LookupTbl.Data = TempPhC_LookupTbl.Data = &Cal.RtdTempLookup1D1StructDef.Data[0];

   LPFilt1stCoef(&TempPhA_LPF, Cal.TempFreqLPF_def, Tsmpb);
   TempPhC_LPF.D0 = TempPhB_LPF.D0 = TempPhA_LPF.D0;
   TempPhC_LPF.N0 = TempPhB_LPF.N0 = TempPhA_LPF.N0;
   TempPhC_LPF.N1 = TempPhB_LPF.N1 = TempPhA_LPF.N1;
   TempPhC_LPF.Zminus1 = TempPhB_LPF.Zminus1 = TempPhA_LPF.Zminus1 = 0.0;

   State = STANDBY_STATE;
   SubState = ENTRY;
   Mode = STANDBY_MODE;
   SubMode = ENTRY;

   DAC[0].Bit.Type = NOT_USED;
   DAC[0].Bit.Addr = (unsigned long)&State;
   DAC[0].Bit.Zero = 0;
   DAC[0].Bit.Gain = 0.0;
   DAC[0].Bit.Offset = 1.0;

   DAC[1].Bit.Type = NOT_USED;
   DAC[1].Bit.Addr = (unsigned long)&SubState;
   DAC[1].Bit.Zero = 0;
   DAC[1].Bit.Gain = 0.0;
   DAC[1].Bit.Offset = 0.0;

   DAC[2].Bit.Type = NOT_USED;
   DAC[2].Bit.Addr = (unsigned long)&Mode;
   DAC[2].Bit.Zero = 0;
   DAC[2].Bit.Gain = 0.0;
   DAC[2].Bit.Offset = 1.0;

   DAC[3].Bit.Type = NOT_USED;  // Gasha: DAC Ch 4 does not exist on the TI evaluation board.
   DAC[3].Bit.Addr = (unsigned long)&SubMode;
   DAC[3].Bit.Zero = 0;
   DAC[3].Bit.Gain = 0.0;
   DAC[3].Bit.Offset = 0.0;

   SoftwareVersion = 0x0101;

}

// RAMD1 memory block contains uninitialized variables and heap space
// RAMD1 : origin = 0x00B800, length = 0x000800    // .ebss .esysmem
// Initialize it to 0 since TI compiler does not do it by default
void InitDataRam()
{
   unsigned int p;

   for (p = 0xB800; p < 0xC000; p++) {
     *(unsigned int*)p = 0;
   }
}

void BackgroundFaults()
{
   // Read ECap1,2,3 to get module temperatures
   ECap1Result = ECap1Regs.CAP1 + ECap1Regs.CAP2 + ECap1Regs.CAP3 + ECap1Regs.CAP4;
   ECap1Frequency = (0.25/1000.0) * __divf32(Cal.CoreClockFreq, (float)ECap1Result);
   TempPhA = Lookup1D1(ECap1Frequency, &TempPhA_LookupTbl);
   DigFilt1(TempPhA, &TempPhA_LPF);

   ECap2Result = ECap2Regs.CAP1 + ECap2Regs.CAP2 + ECap2Regs.CAP3 + ECap2Regs.CAP4;
   ECap2Frequency = (0.25/1000.0) * __divf32(Cal.CoreClockFreq, (float)ECap2Result);
   TempPhB = Lookup1D1(ECap2Frequency, &TempPhB_LookupTbl);
   DigFilt1(TempPhB, &TempPhB_LPF);

   ECap3Result = ECap3Regs.CAP1 + ECap3Regs.CAP2 + ECap3Regs.CAP3 + ECap3Regs.CAP4;
   ECap3Frequency = (0.25/1000.0) * __divf32(Cal.CoreClockFreq, (float)ECap3Result);
   TempPhC = Lookup1D1(ECap3Frequency, &TempPhC_LookupTbl);
   DigFilt1(TempPhC, &TempPhC_LPF);

   ECap1Regs.ECCTL2.bit.REARM = 1;  // Arm the eCap1
   ECap2Regs.ECCTL2.bit.REARM = 1;  // Arm the eCap2
   ECap3Regs.ECCTL2.bit.REARM = 1;  // Arm the eCap3
}

