#ifndef _PI_Contrller_H__
#define _PI_Contrller_H__
#include "math/fast_math.h"
#include "bsp/bsp.h"
#if 0
typedef struct {
	s16q5_t  kp;
	s16q5_t  ki;
	s16q5_t  max;
	s16q5_t  min;
	s16q5_t  Ui;
	s16q14_t DT;
}PI_Controller;

static __INLINE void PI_Controller_Reset(PI_Controller *pi, s16q5_t init) {
	pi->Ui = init;
}

static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
	s16q5_t kp_err = S16_mul(err,pi->kp, 5);
	s16q5_t ki_err = S16_mul(err,pi->ki, 5);
	s16q5_t integral = S16_mul(ki_err, pi->DT, 14);
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	s32q14_t out = pi->Ui + kp_err;
	return MATH_sat(out, pi->min, pi->max);
}
#else
typedef struct {
	float  kp;
	float  ki;
	float  kd;
	float  max;
	float  min;
	float  Ui;
	float  sat;
	float  DT;
	bool   is_sat;
	float  last_err;
	float  b_dstart;
}PI_Controller;

static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min) {
	pi->max = max;
	pi->min = min;
}
static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
	pi->Ui = (init);
	pi->sat = 0.0f;
	pi->is_sat = false;
	pi->b_dstart = false;
}

static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (err) * pi->ki;
	float integral = ki_err * pi->DT;
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	float out = pi->Ui + kp_err;
	float sat_out =  (MATH_sat(out, pi->min, pi->max));
	if (out != sat_out) {
		pi->is_sat = true;
	}else {
		pi->is_sat = false;
	}
	return sat_out;
}

static __INLINE float PI_Controller_RunLimit(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (err) * pi->ki;
	float integral = ki_err * pi->DT;
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	float out = pi->Ui + kp_err;
	float sat_out = MATH_sat(out, pi->min, pi->max);
	
	return sat_out;
}


static __INLINE float PI_Controller_RunWithDiff(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (err) * pi->ki;
	float integral = ki_err * pi->DT;
	if (!pi->b_dstart) {
		pi->last_err = err;
		pi->b_dstart = true;
	}
	
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	float out = pi->Ui + kp_err + pi->kd * (err - pi->last_err);
	float sat_out =  (MATH_sat(out, pi->min, pi->max));
	if (out != sat_out) {
		pi->is_sat = true;
	}else {
		pi->is_sat = false;
	}
	pi->last_err = err;
	return sat_out;
}

#if 0
static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (err) * pi->ki;
	float integral = ki_err * pi->DT;
	pi->Ui = pi->Ui + integral + pi->sat * pi->kd;
	float out = pi->Ui + kp_err ;
	float out_sat = MATH_sat(out, pi->min, pi->max);
	if (out != out_sat) {
		pi->is_sat = true;
	}else {
		pi->is_sat = false;
	}
	pi->sat = out_sat - out;

	return out_sat;
}
#endif

static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);
	float ki_err = (kp_err) * pi->ki;
	float integral = ki_err * pi->DT;
	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
	float out = pi->Ui + kp_err;
	float sat_out =  (MATH_sat(out, pi->min, pi->max));
	if (out != sat_out) {
		pi->is_sat = true;
	}else {
		pi->is_sat = false;
	}
	return sat_out;
}

static __INLINE float _fmod(float v, s32 m) {
	int v_i = (int)v;
	int m_i = v_i % m;
	return (v-v_i) + m_i;
}

typedef struct {
	float observer;
	float kp;
	float ki;
	float Ui;
	float out;
	s32   max_wp;
	bool  ob_wp;
	float DT;
}PLL_t;

static __INLINE void PLL_Reset(PLL_t *pll) {
	pll->observer = 0.0f;
	pll->out = 0.0f;
	pll->ob_wp = false;
}
/*
Transfer func:
ki*s/(s*s + kp*s + ki)
ki = (kp/2) * (kp/2)
Wn = kp/2 ==> kp = 2 * Wn, Wn为PLL的带宽
*/
static __INLINE float PLL_run(PLL_t *pll, float sample, float comp) {
	float observer = pll->observer - comp;
	if (comp < 0) {
		observer = -comp - pll->observer;
	}
	float delta = sample - observer;
	pll->observer = observer + (pll->out + pll->kp * delta) * pll->DT;
	pll->out += pll->ki * delta * pll->DT;
	return pll->out;
}

#endif
#endif	/*_PI_Contrller_H__*/