//=================================================================================================
//
// 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 "../PDEF_ProjectDefinitions.h"
#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 1000 // Refresh rate in ms

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

#define EVENT_FLAGS_ALL             ( EVENT_TIMER_UPDATE )

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

BOOL boEnableOutput = FALSE;

//=================================================================================================
// 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
  osPriorityNormal,                 // 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.
//=================================================================================================


//=================================================================================================
// 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_boSetTemperature
// Description:   Sets the Peltier Controller to a Specific Temperature
// Parameters:    None
// Returns:       Boolean TRUE if successful
//-------------------------------------------------------------------------------------------------
BOOL PECO_boSetTemperature( S16 Temperature ){
	BOOL boOK = TRUE;
	
	boOK &= boSetPeltierVoltage( Temperature );
	
	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);
	boEnableOutput = boEnable;
}

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

//=================================================================================================
// 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 last_error = 0;
  FLOAT integral = 0;
  FLOAT dT = REFRESH; // 1s
	
	while ( TRUE )
  {

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

    if( u32Flags & EVENT_TIMER_UPDATE )
    {

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

      FLOAT kp = VARH_flGetVariableData( VARH_ePID_kp );
      FLOAT ki = VARH_flGetVariableData( VARH_ePID_ki );
      FLOAT kd = VARH_flGetVariableData( VARH_ePID_kd );
      FLOAT max = VARH_flGetVariableData( VARH_ePID_Max );
      FLOAT min = VARH_flGetVariableData( VARH_ePID_Min );
      
      FLOAT Tnow = VARH_flGetVariableData( VARH_eTemp_C );
      FLOAT Tset = VARH_flGetVariableData( VARH_ePID_Temp );

      // -error
      FLOAT error = Tnow - Tset;

      // proportional term
      FLOAT P = kp * error;
      
      // integral term
      integral += error * dT;
      FLOAT I = ki * integral;
      
      // derivative term
      FLOAT D = kd * ( error - last_error ) / dT;
      last_error = error;
      
      // total
      FLOAT output = P + I + D;
      
      // limitter
      if ( output > max ){
        output = max;
      } else if ( output < min ){
        output = min;
      }

      boSetPeltierVoltage( output ); // set the output

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

    }

	
	}

}

//-------------------------------------------------------------------------------------------------
// 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 < -3 ) Voltage = -3;
	
	ANPO_boSetVoltage( ( ( (FLOAT)Voltage ) + 20.088) / 34.103 );

	return( boOK );
}

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