#ifndef _PI_Contrller_H__
#define _PI_Contrller_H__
#include "math/fast_math.h"
#include "bsp/bsp.h"

typedef struct {
	float  kp;
	float  ki;
	float  kd;
	float  max;
	float  min;
	float  ui;
	float  sat;
	float  ts;
	bool   is_sat;
}PI_Controller;

static __INLINE void PI_Controller_Change_Kpi(PI_Controller *pi, float kp, float ki) {
	pi->kp = kp;
	pi->ki = ki;
}


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;
}

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->ts;
	pi->ui = fclamp(pi->ui + integral, pi->min, pi->max);
	float out = pi->ui + kp_err;
	float sat_out = fclamp(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_RunVel(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (kp_err) * pi->ki;
	float integral = ki_err * pi->ts;

	pi->ui = pi->ui + integral;
	float out = pi->ui + kp_err;
	if (out > pi->max) {
		out = pi->max;
		pi->ui = out - kp_err;
	}else if (out < pi->min) {
		out = pi->min;
		pi->ui = out - kp_err;
	}
	return out;
}


/* 电流环，PI 串联结构 */
static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float ff) {
	float kp_err = (err) * pi->kp;
	float ki_err = (kp_err) * pi->ki;
	float integral = ki_err * pi->ts;

	pi->ui = pi->ui + integral;
	float out = pi->ui + kp_err + ff;
	if (out > pi->max) {
		out = pi->max;
		pi->is_sat = true;
		pi->ui = out - (kp_err + ff);
	}else if (out < pi->min) {
		out = pi->min;
		pi->is_sat = true;
		pi->ui = out - (kp_err + ff);
	}else {
		pi->is_sat = false;
	}
	return out;
}


static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
	float kp_err = (err) * pi->kp;
	float ki_err = (kp_err) * pi->ki;
	float integral = ki_err * pi->ts;

	pi->ui = pi->ui + integral;
	float out = pi->ui + kp_err;
	if (out > pi->max) {
		out = pi->max;
		pi->is_sat = true;
		pi->ui = out - kp_err;
	}else if (out < pi->min) {
		out = pi->min;
		pi->is_sat = true;
		pi->ui = out - kp_err;
	}else {
		pi->is_sat = false;
	}
	return 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 ts;
}PLL_t;

static __INLINE void PLL_Reset(PLL_t *pll, float sample) {
	pll->observer = sample;
	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->ts;
	pll->out += pll->ki * delta * pll->ts;
	return pll->out;
}

#endif	/*_PI_Contrller_H__*/