//=================================================================================================
//
// Company:       Paul Scherrer Institut
//                5232 Villigen PSI
//                Switzerland
//
//-------------------------------------------------------------------------------------------------
//
// Project:       Peltier Controller V2
// Author:        Noah Piqué (noah.pique@psi.ch)
//
//-------------------------------------------------------------------------------------------------
//
// Module:        Peltier Controller
// Filename:      PECO_PeltierController.c
// Date:          Handled by Subversion (version control system)
// Revision:      Handled by Subversion (version control system)
// History:       Handled by Subversion (version control system)
//
//-------------------------------------------------------------------------------------------------
//
// Description:   This source file contains all functions dealing with the Peltier Controller Output
//
//=================================================================================================

//=================================================================================================
// Section:       INCLUDES
// Description:   List of required include files.
//=================================================================================================

#include "PECO_PeltierController.h"

// Application
#include "../Application/VARH_VariableHandler.h"

// Driver
#include "ANPO_AnalogPortsOut.h"
#include "DIPO_DigitalPorts.h"

// Toolbox
#include "../Toolbox/UTIL_Utility.h"

// include STM32 drivers
#include "stm32l4xx_hal.h"

#include "cmsis_os2.h"

//=================================================================================================
// Section:       DEFINITIONS
// Description:   Definition of local constants (visible by this module only).
//=================================================================================================

#define REFRESH 200 // Refresh rate in ms

#define EVENT_TIMER_UPDATE ((U32)(1 << 0))

#define EVENT_FLAGS_ALL (EVENT_TIMER_UPDATE)

#define MAX_POWER_PELTIER 100

//=================================================================================================
// Section:       MACROS
// Description:   Definition of local macros (visible by this module only).
//=================================================================================================

//=================================================================================================
// Section:       ENUMERATIONS
// Description:   Definition of local enumerations (visible by this module only).
//=================================================================================================

//=================================================================================================
// Section:       STRUCTURES
// Description:   Definition of local Structures (visible by this module only).
//=================================================================================================

//=================================================================================================
// Section:       LOCAL VARIABLES
// Description:   Definition of local variables (visible by this module only).
//=================================================================================================

LOCAL osThreadId_t m_pstThreadID = NULL;
LOCAL osTimerId_t m_pstUpdateTimer = NULL;
LOCAL osEventFlagsId_t m_pstEventID = NULL;

//=================================================================================================
// Section:       LOCAL CONSTANTS
// Description:   Definition of local constants (visible by this module only).
//=================================================================================================

LOCAL CONST osThreadAttr_t stTaskAttribute =
    {
        "PECO_Thread",    // name of the thread
        osThreadDetached, // attribute bits
        NULL,             // memory for control block
        0,                // size of provided memory for control block
        NULL,             // memory for stack
        1024,             // size of stack
        osPriorityHigh,   // initial thread priority (default: osPriorityNormal)
        0,                // TrustZone module identifier
        0,                // reserved (must be 0)
};

LOCAL CONST osTimerAttr_t stTimerAttribute =
    {
        "PECO_UpdateTimer", // name of the timer
        0,                  // attribute bits
        NULL,               // memory for control block
        0,                  // size of provided memory for control block
};

LOCAL CONST osEventFlagsAttr_t stEventAttribute =
    {
        "PECO_Event_Flags", // name of the event flags
        0,                  // attribute bits
        NULL,               // memory for control block
        0,                  // size of provided memory for control block
};

//=================================================================================================
// Section:       LOCAL FUNCTIONS (PROTOTYPES)
// Description:   Definition of local functions (visible by this module only).
//=================================================================================================

PRIVATE VOID vTask(PVOID arg);
BOOL boSetPeltierVoltage(FLOAT Voltage);
PRIVATE VOID vEventCallback(PVOID pvData);

//=================================================================================================
// Section:       EXTERNAL FUNCTIONS
// Description:   Definition of external (global) functions.
//=================================================================================================

//=================================================================================================
// Section:       EXTERNAL VARIABLES
// Description:   Definition of external (global) variables.
//=================================================================================================

extern IWDG_HandleTypeDef hiwdg;

//=================================================================================================
// Section:       GLOBAL FUNCTIONS
// Description:   Definition (implementation) of global functions.
//=================================================================================================

//-------------------------------------------------------------------------------------------------
// Function:      PECO_boInitializeModule
// Description:   Initializes the module. Function must be called once immediately after power-up.
// Parameters:    None
// Returns:       Boolean TRUE if successful
//-------------------------------------------------------------------------------------------------

BOOL PECO_boInitializeModule(VOID)
{
  BOOL boOK = TRUE;

  boOK &= ((m_pstThreadID = osThreadNew(vTask, NULL, &stTaskAttribute)) == NULL) ? FALSE : TRUE;
  boOK &= ((m_pstEventID = osEventFlagsNew(&stEventAttribute)) == NULL) ? FALSE : TRUE;
  boOK &= (m_pstUpdateTimer = osTimerNew(vEventCallback, osTimerPeriodic, (PVOID)EVENT_TIMER_UPDATE, &stTimerAttribute)) == NULL ? FALSE : TRUE;

  boSetPeltierVoltage(0);

  boOK &= (osTimerStart(m_pstUpdateTimer, REFRESH) == osOK) ? TRUE : FALSE;

  return (boOK);
}

//-------------------------------------------------------------------------------------------------
// Function:      PECO_Enable
// Description:   Enables the Peltier Controller Output
// Parameters:    BOOL boEnable
// Returns:       None
//-------------------------------------------------------------------------------------------------
VOID PECO_Enable(BOOL boEnable)
{
  DIPO_vSetState(DIPO_eEN, boEnable);
  VARH_vSetVariableDataFromSystemU32(VARH_ePowerState, (U32)boEnable);
}

//-------------------------------------------------------------------------------------------------
// Function:      PECO_isEnabled
// Description:   Returns true if the Output is enabled
// Parameters:    None
// Returns:       BOOL boEnableOutput
//-------------------------------------------------------------------------------------------------
BOOL PECO_isEnabled(VOID)
{
  return (BOOL)VARH_u32GetVariableData(VARH_ePowerState);
}

//=================================================================================================
// Section:       LOCAL FUNCTIONS
// Descriptionn:  Definition (implementation) of local functions.
//=================================================================================================

//-------------------------------------------------------------------------------------------------
// Function:      vTask
// Description:   Task
// Parameters:    None
// Returns:       None
//-------------------------------------------------------------------------------------------------
VOID vTask(PVOID arg)
{

  UNUSED(arg);
  U32 u32Flags;

  FLOAT error_prev = 0;
  FLOAT Tc_prev = 0;
  FLOAT i_prev = 0;
  FLOAT d_prev = 0;

  FLOAT tau = 0.4f;


  FLOAT dT = REFRESH / 1000.0f; // 0.2s

  while (TRUE)
  {

    u32Flags = osEventFlagsWait(m_pstEventID, EVENT_FLAGS_ALL, osFlagsWaitAny, osWaitForever);

    if (u32Flags & EVENT_TIMER_UPDATE)
    {

      HAL_IWDG_Refresh(&hiwdg);

      if (PECO_isEnabled())
      {

        if (VARH_u32GetVariableData(VARH_eMode) == PECO_eConstTemp)
        {
          // TODO: check the measurements, everything ok?

          FLOAT power = VARH_flGetVariableData(VARH_ePeltier_P);
          if(power > MAX_POWER_PELTIER){
        	  PECO_Enable(FALSE);
          }


          // PID Regelung

          FLOAT Kp = VARH_flGetVariableData(VARH_ePID_kp);
          FLOAT Ki = VARH_flGetVariableData(VARH_ePID_ki);
          FLOAT Kd = VARH_flGetVariableData(VARH_ePID_kd);
          FLOAT Umax = VARH_flGetVariableData(VARH_ePID_Max);
          FLOAT Umin = VARH_flGetVariableData(VARH_ePID_Min);

          FLOAT Tc = VARH_flGetVariableData(VARH_eTemp_Module);
          FLOAT Ts = VARH_flGetVariableData(VARH_ePID_Temp);

          // calculate error
          FLOAT error = Tc - Ts;

          // proportional term
          FLOAT P = Kp * error;

          // integral term
          FLOAT I = i_prev + 0.5f * Ki * dT * (error + error_prev);

          // compute integral limits (anti-windup via dynamic integrator clamping)
          FLOAT limMinInt = 0.0f;
          FLOAT limMaxInt = 0.0f;

          if(Umax > P) limMaxInt = Umax - P;
          if(Umin < P) limMinInt = Umin - P;

          // clamp integrator
          if(I > limMaxInt) I = limMaxInt;
          else if (I < limMinInt) I = limMinInt;

          // derivative term
          FLOAT D = -(2.0f * Kd * (Tc - Tc_prev)	/* Note: derivative on measurement, therefore minus sign in front of equation! */
			+ (2.0f * tau - dT) * d_prev)
			/ (2.0f * tau + dT);

          // total
          FLOAT Uout = P + I + D;

          // limitter
          if (Uout > Umax) Uout = Umax;
          else if (Uout < Umin) Uout = Umin;

          Tc_prev = Tc;
          error_prev = error;
          d_prev = D;
          i_prev = I;

          boSetPeltierVoltage(Uout); // set the output
        }
        else if (VARH_u32GetVariableData(VARH_eMode) == PECO_eConst_Voltage)
        {
          boSetPeltierVoltage(VARH_flGetVariableData(VARH_eControlVoltage)); // set the output

          // reset PID Vars
          error_prev = 0;
          Tc_prev = 0;
          i_prev = 0;
          d_prev = 0;
        }
      }
      else
      {
    	// reset PID vars
    	error_prev = 0;
		Tc_prev = 0;
		i_prev = 0;
		d_prev = 0;
      }
    }
  }
}

//-------------------------------------------------------------------------------------------------
// Function:      boSetPeltierVoltage
// Description:   Sets the Peltier elements to a specific Voltage
// Parameters:    S8 Voltage (12V - -3V)
// Returns:       Boolean TRUE if successful
//-------------------------------------------------------------------------------------------------
BOOL boSetPeltierVoltage(FLOAT Voltage)
{
  BOOL boOK = TRUE;

  if (Voltage > 12)
    Voltage = 12;
  if (Voltage < -5)
    Voltage = -5;

  boOK &= ANPO_boSetVoltage(Voltage);

  return (boOK);
}

//-------------------------------------------------------------------------------------------------
// Function:      vEventCallback
// Description:   Callback for events
// Parameters:    None
// Returns:       None
//-------------------------------------------------------------------------------------------------
PRIVATE VOID vEventCallback(PVOID pvData)
{
  osEventFlagsSet(m_pstEventID, (U32)pvData);
}