/*
 * pi.h
 *
 * Created: 01/02/2021 21:36:11
 *  Author: Nick-XMG
 */ 
#ifndef CONTROL_H_
#define CONTROL_H_

// ----------------------------------------------------------------------
// 	history
// ----------------------------------------------------------------------
// 	01.02.2021 - initial Revision

// ----------------------------------------------------------------------
// 	header files
// ----------------------------------------------------------------------
#include "arm_math.h"
#include "atmel_start.h"
#include "utilities.h"
// ----------------------------------------------------------------------
// 	#defines
// ----------------------------------------------------------------------
// ----------------------------------------------------------------------
// 	global variables
// ----------------------------------------------------------------------

#ifdef __cplusplus
extern "C" {
	#endif
	
/* PI Structure */
typedef volatile struct
{
	volatile float32_t Kp;          // the proportional gain for the PI controller
	volatile float32_t Ki;          // the integral gain for the PI controller

	volatile float32_t Ref_pu;      // the reference value [pu]
	volatile float32_t Fbk_pu;      // the feedback value [pu]
	volatile float32_t Ff_pu;       // the feedForward value [pu]

	volatile float32_t OutMin_pu;   // the saturation low limit for the controller output [pu]
	volatile float32_t OutMax_pu;   // the saturation high limit for the controller output [pu]
	volatile float32_t Out_pu;      // the controller output [pu]

	volatile float32_t Ui;          // the integrator value for the PI controller
	volatile float32_t error;          // the integrator value for the PI controller
	volatile bool SatFlag;   // flag to signal controller saturation
} PI_t;

/* PID Structure */
typedef volatile struct
{
	volatile float32_t  Kp;                //!< the proportional gain for the PID controller
	volatile float32_t  Ki;                //!< the integral gain for the PID controller
	volatile float32_t  Kd;                //!< the derivative gain for the PID controller

	volatile float32_t  Ref_pu;          //!< the reference input value
	volatile float32_t  Fbk_pu;        //!< the feedback input value
	volatile float32_t  Ff_pu;         //!< the feedforward input value
	
	volatile float32_t  OutMin_pu;            //!< the minimum output value allowed for the PID controller
	volatile float32_t  OutMax_pu;            //!< the maximum output value allowed for the PID controller
	volatile float32_t	Out_pu;      // the controller output [pu]
	
	volatile float32_t  Ui;                //!< the integrator start value for the PID controller
	volatile float32_t  Ud;
	volatile float32_t error;          // the integrator value for the PI controller
	
	//FILTER_FO_Handle derFilterHandle;   //!< the derivative filter handle
	//FILTER_FO_Obj    derFilter;         //!< the derivative filter object
} PID_t;


// ----------------------------------------------------------------------
// 	functions
// ----------------------------------------------------------------------
// function to set default values for the object

//inline float clamp(const float32_t d, const float32_t min, const float32_t max) {
	//const float t = d < min ? min : d;
	//return t > max ? max : t;
//}


inline void PI_objectInit(volatile PI_t *pPi_obj)
{
	PI_t *obj = (PI_t *)pPi_obj;

	// Function initializes the object with default values
	//fix16_t Kp = fix16_from_float(1.0);
	obj->Kp			= 0.0f;
	obj->Ki			= 0.0f;
	obj->Fbk_pu		= 0.0f;
	obj->Ref_pu		= 0.0f;
	obj->Ff_pu		= 0.0f;
	obj->Out_pu		= 0.0f;
	obj->OutMax_pu	= 0.0f;
	obj->OutMin_pu	= 0.0f;
	obj->Ui			= 0.0f;
	obj->SatFlag	= false;
}

inline void PID_objectInit(volatile PID_t *pPiD_obj)
{
	PID_t *obj = (PID_t *)pPiD_obj;

	// Function initializes the object with default values
	//fix16_t Kp = fix16_from_float(1.0);
	obj->Kp			= 0.0f;
	obj->Ki			= 0.0f;
	obj->Kd			= 0.0f;
	obj->Fbk_pu		= 0.0f;
	obj->Ref_pu		= 0.0f;
	obj->Ff_pu		= 0.0f;
	obj->Out_pu		= 0.0f;
	obj->OutMax_pu	= 0.0f;
	obj->OutMin_pu	= 0.0f;
	obj->Ui			= 0.0f;
	obj->Ud			= 0.0f;
}

// ----------------------------------------------------------------------
//	PI Calculation
// ----------------------------------------------------------------------
static inline void PI_run_series(volatile PI_t *pPi_obj)
{
	PI_t *obj = (PI_t *)pPi_obj;
	volatile float32_t Up;

	// Compute the controller error
	obj->error = obj->Ref_pu - obj->Fbk_pu;

	// Compute the proportional term
	Up = obj->Kp *obj->error;

	// Compute the integral term in series form and saturate	
	obj->Ui = f_clamp((obj->Ui + (obj->Ki * Up)), obj->OutMin_pu, obj->OutMax_pu);

	// Saturate the output
 	obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu);
}

static inline void PI_run_parallel(volatile PI_t *pPi_obj)
{
	PI_t *obj = (PI_t *)pPi_obj;
	volatile float32_t Up;
	
	// Compute the controller error
	obj->error = obj->Ref_pu - obj->Fbk_pu;
	
	// Compute the proportional term
	Up = obj->Kp * obj->error;
	
	// Compute the integral term in parallel form and saturate
	obj->Ui = f_clamp((obj->Ui + (obj->Ki * obj->error)), obj->OutMin_pu, obj->OutMax_pu);
	
	obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu);  // Saturate the output
} // end of PI_run_parallel() function


// ----------------------------------------------------------------------
//	PID Calculation
// ----------------------------------------------------------------------
static inline void PID_run_series(volatile PID_t *pPid_obj)
{
	PID_t *obj = (PID_t *)pPid_obj;
	volatile float32_t Up, Ud_tmp;
	
	// Compute the controller error
	obj->error = obj->Ref_pu - obj->Fbk_pu;
	
	// Compute the proportional term
	Up = obj->Kp * obj->error;
	
	if(obj->Ki>0.0){
		// Compute the integral term in parallel form and saturate
		obj->Ui = f_clamp((obj->Ui + (obj->Ki * Up)), obj->OutMin_pu, obj->OutMax_pu);
	}
	
	Ud_tmp = obj->Kd * obj->Ui; // Compute the derivative term
	
	obj->Ud = Ud_tmp; // Replace with filter

	obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ud + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu);  // Saturate the output
	
}

static inline void PID_run_parallel(volatile PID_t *pPid_obj)
{
	PID_t *obj = (PID_t *)pPid_obj;
	volatile float32_t Up, Ud_tmp;

	
	// Compute the controller error
	obj->error = obj->Ref_pu - obj->Fbk_pu;
	
	// Compute the proportional term
	Up = obj->Kp * obj->error;
	
	if(obj->Ki>0.0){
		// Compute the integral term in parallel form and saturate
		obj->Ui = f_clamp((obj->Ui + (obj->Ki * obj->error)), obj->OutMin_pu, obj->OutMax_pu);
	}
	
	Ud_tmp = obj->Kd * obj->error;   // Compute the derivative term

	obj->Ud = Ud_tmp; // Replace with filter

	obj->Out_pu = f_clamp((Up + obj->Ui + obj->Ud + obj->Ff_pu), obj->OutMin_pu, obj->OutMax_pu);  // Saturate the output
}


// ----------------------------------------------------------------------
// Calculate Current Parameters
// ----------------------------------------------------------------------

static inline float PI_calcKp(float32_t Ls_H, float32_t deviceCurrent_A, float32_t deviceVoltage_V,
float deviceCtrlPeriode_Sec)
{
	// calculation is based on "Betragsoptimum"
	// Kp = Ls/(2*tau)
	float32_t x1;
	float32_t y1;
	float32_t Kp;

	// multiplication with deviceCurrent_A is to get per unit values
	x1 = (float32_t)(Ls_H * deviceCurrent_A);

	y1 = (float32_t)(2.0f * deviceCtrlPeriode_Sec);

	// multiplication with deviceVoltage_V is to get per unit values
	y1 = (float32_t)(y1 * deviceVoltage_V);

	Kp = (x1 / y1);
	return Kp;
}

static inline float PI_calcKi(float Rs_Ohm, float Ls_H, float deviceCtrlPeriode_Sec)
{
	// calculation is based on "TI - MotorWare's documentation"
	float RsByLs = (float)(Rs_Ohm / Ls_H);
	float Ki = RsByLs * deviceCtrlPeriode_Sec;
	//fix16_t Ki = 0;
	return Ki;
}

// ----------------------------------------------------------------------
// 	something...
// ----------------------------------------------------------------------

// ----------------------------------------------------------------------
// 	end of file
// ----------------------------------------------------------------------

#ifdef __cplusplus
}
#endif /* extern "C" */
#endif /* BLDC_PI_H_ */

