/**************************************************************************
* NAME: pid.h      		                                                  *
* AUTHOR: Johan Gärtner                                                   *
*
* PURPOSE: This file contains pid functions		                          *
* INFORMATION: pidUAV is the final pid controller which only takes        *
* 			   pidProfile_t as input. pidProfile_t will be updated and 	  *
* 			   contains all the information about the current state of    *
* 			   the pid.													  *
* GLOBAL VARIABLES:                                                       *
* Variable        Type          Description                               *
* --------        ----          -----------                               *
*
* **************************************************************************/
#ifndef FLIGHT_PID_H_
#define FLIGHT_PID_H_

#include<stdio.h>
#include<stdint.h>

#include "Flight/filter.h"

#define PTERM_SCALE 0.032029f				/*P-term used as a scale value to the PID controller*/
#define ITERM_SCALE 0.244381f				/*I-term used as a scale value to the PID controller*/
#define DTERM_SCALE 0.000529f				/*D-term used as a scale value to the PID controller*/

#define XYZ_AXIS_COUNT 3					/*The maximum number of DOF that belongings to the PID*/

#define THROTTLE 0							/*Index terms to the PID*/
#define ROLL 0								/*Index terms to the PID*/
#define PITCH 1								/*Index terms to the PID*/
#define YAW 2								/*Index terms to the PID*/

/*Struct that belongs to a certain PID controller*/
typedef struct pidProfile_s {

	bool pidStabilisationEnabled;			/*Enables/Dissables PID controller*/

	uint8_t ID_profile;						/*ID of a certain PID, shall be referenced to a certain sensor*/
	uint8_t DOF;							/*DOF = degrees of freedom*/

	uint8_t P[XYZ_AXIS_COUNT];				/*PID value*/
	uint8_t I[XYZ_AXIS_COUNT];				/*PID value*/
	uint8_t D[XYZ_AXIS_COUNT];				/*PID value*/

	int16_t Max_PID_Term;					/*The max and minimum output value of the PID*/
	int16_t Max_Error_Term;					/*The max and minimum output value of the PID error*/

	int16_t Max_P_Term;						/*The max and minimum output value of the PTerm*/
	int16_t Max_I_Term;						/*The max and minimum output value of the ITerm*/
	int16_t Max_D_Term;						/*The max and minimum output value of the DTerm*/

	uint16_t dterm_lpf;  					/*Derivative low pass filter in hz, 80 - 90 is a good init value (same as clean flight)*/
	uint16_t yaw_lpf;						/*Proportional (p term) low pass filter for yaw axis in hz, 80 - 90 is a good init value (same as clean flight)*/

	pt1Filter_t deltaFilter[XYZ_AXIS_COUNT];/*Struct, Contains filter value for each PID of D term, do not to be init*/
	pt1Filter_t yawFilter;					/*Struct, Contains filter value for each PID of p term for yaw axis, do not to be init*/

	float Integral[XYZ_AXIS_COUNT];			/*Buffer which contains integral values, do not to be init */
	float LastError[XYZ_AXIS_COUNT];		/*Buffer which contains last error values, do not to be init */
	float PID_Out[XYZ_AXIS_COUNT];			/*Output values of PID loop*/
	float dT;								/*Delta time of each PID cycle*/

} pidProfile_t;

/**************************************************************************
* BRIEF: Constrain float values within a defined limit                    *
* INFORMATION: Used in PID loop to limit values                           *
**************************************************************************/
float constrainf(float amt, int low, int high);

/**************************************************************************
* BRIEF: Update current sensor values				                      *
* INFORMATION:                       									  *
**************************************************************************/
void getCurrentValues(float *sensorValues, uint8_t ID_profile);

/**************************************************************************
* BRIEF: Update desired values from rc command		                      *
* INFORMATION:                       									  *
**************************************************************************/
void getPointRate(float *desiredCommand, uint8_t ID_profile);

/**************************************************************************
* BRIEF: Initializes the pid profile PID controller				          *
* INFORMATION: Recommended to use if unexpected values occur of profile	  *
**************************************************************************/
void pidUAVInit(pidProfile_t *pidProfile);

/**************************************************************************
* BRIEF: Dynamic PID controller, able to handle several PID controller    *
* 		 connected to different profiles.
* INFORMATION:                       									  *
**************************************************************************/
void pidUAV(pidProfile_t *pidProfile);

#endif /* FLIGHT_PID_H_ */