/*
 * 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();

   InitGpio();
   InitMemory();
   BLUE_LED_ON;
   InitDataRam();
   InitDAC();
   InitADC();
   InitVars();
   InitEPwm();
//   InitDMA();
   InitCANA();
   InitPie();
   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;
   WHITE_LED_OFF;

//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
         }
      }
      //******************************************
      // 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;
            WHITE_LED_ON;
            SubState = IN;
            /* no break */
         case (IN) :
            if (MasterReset == 1) {
               nPWM_RESET_ON;
               SubState = EXIT;
            }
            break;
         case (EXIT) :
            State = STANDBY_STATE;
            FaultCode.All = 0;
            nPWM_RESET_OFF;
            WHITE_LED_OFF;
            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);
   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;

   TemperaturePhaseA.N = (int)Cal.

   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;
   }
}

