#include "arm_math.h"
#include "PMSM_FOC_Core.h"
#include "PMSM_FOC_Params.h"
#include "foc/core/e_ctrl.h"
#include "math/fix_math.h"
#include "math/fast_math.h"
#include "foc/motor/current.h"
#include "foc/motor/hall.h"
#include "foc/core/svpwm.h"
#include "bsp/pwm.h"
#include "libs/logger.h"

static PMSM_FOC_Ctrl _gFOC_Ctrl;


static __INLINE void RevPark(DQ_t *dq, s16q5_t angle, AB_t *alpha_beta) {
	s16q14_t c,s;
	SinCos_Lut(angle, &s, &c);

	alpha_beta->a = S16_mul(dq->d, c, 14)  - S16_mul(dq->q, s, 14);
	alpha_beta->b = S16_mul(dq->d, s, 14) + S16_mul(dq->q, c, 14);
}

static __INLINE void Clark(s16q5_t A, s16q5_t B, s16q5_t C, AB_t *alpha_beta){
	alpha_beta->a = (2 * A - B - C) / 3;
	alpha_beta->b = S16_mul(ONE_BY_SQRT3_Q14, (B - C), 14);
}

static __INLINE void Park(AB_t *alpha_beta, s16q5_t angle, DQ_t *dq) {
	s16q14_t c,s;
	SinCos_Lut(angle, &s, &c);

	dq->d = S16_mul(alpha_beta->a, c, 14) + S16_mul(alpha_beta->b, s, 14);
	dq->q = S16_mul(-alpha_beta->a, s, 14) + S16_mul(alpha_beta->b, c, 14);
}

static __INLINE s16q14_t Circle_Limitation(DQ_t *vdq, s16q5_t vDC, s16q14_t module, DQ_t *out) {
	u32 sq_vdq = (u32)vdq->d * vdq->d + (u32)vdq->q * vdq->q;
	s16q5_t vDC_m = S16_mul(vDC, module, 14);
	u32 sq_vDC = (u32)vDC_m * vDC_m;
	if (sq_vdq > sq_vDC) {
		s16q14_t r = S16Q14(sqrtf((float)sq_vDC / (float)sq_vdq));
		out->d = S16_mul(vdq->d, r, 14);
		out->q = S16_mul(vdq->q, r, 14);
		return r;
	}
	out->d = vdq->d;
	out->q = vdq->q;
	return S16Q14(1); // s16q5 32 means int 1
}

static __INLINE void FOC_Set_DqRamp(idq_Ctrl *c, s16q5_t target) {
	s32q14_t cp = c->s_Cp;
	c->s_FinalTgt = (s32)target << 9; // to s32q14
	c->s_Step = (c->s_FinalTgt - cp) / (IDQ_CTRL_TS/SPD_CTRL_TS);
	if (c->s_Step == 0) {
		if (c->s_FinalTgt - cp > 0) {
			c->s_Step = S32Q14(1);	
		}else {
			c->s_Step = S32Q14(-1);
		}
	}
}

static __INLINE s32q14_t FOC_Get_DqRamp(idq_Ctrl *c) {
	c->s_Cp += c->s_Step;
	if (c->s_Step < 0) {
		if (c->s_Cp < c->s_FinalTgt) {
			c->s_Cp = c->s_FinalTgt;
		}
	}else {
		if (c->s_Cp > c->s_FinalTgt) {
			c->s_Cp = c->s_FinalTgt;
		}
	}
	return c->s_Cp;
}

static void PMSM_FOC_Reset_PID(void) {
	PI_Controller_Reset(_gFOC_Ctrl.id_ctl, 0);
	PI_Controller_Reset(_gFOC_Ctrl.iq_ctl, 0);
	PI_Controller_Reset(_gFOC_Ctrl.spd_ctl, 0);
	PI_Controller_Reset(_gFOC_Ctrl.fw_ctl, 0);
}

void PMSM_FOC_CoreInit(void) {
	_gFOC_Ctrl.id_ctl = &PI_Ctrl_ID;
	_gFOC_Ctrl.iq_ctl = &PI_Ctrl_IQ;
	_gFOC_Ctrl.spd_ctl = &PI_Ctrl_Spd;
	_gFOC_Ctrl.fw_ctl = &PI_Ctrl_fw;
	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
	_gFOC_Ctrl.in.n_poles = 4;
	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDQ);
	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDC);
	_gFOC_Ctrl.in.s_maxRPM = S32Q14(MAX_SPEED);
	_gFOC_Ctrl.in.s_vDC    = S16Q5(MAX_vDC);
	_gFOC_Ctrl.in.n_modulation = S16Q14(0.95f);
	_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
	_gFOC_Ctrl.out.f_vdqRation = S16Q14(0.9f);
	_gFOC_Ctrl.in.s_manualAngle = 0xFFFF;

	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl, 0);
	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl + 1, 0);
	PMSM_FOC_Reset_PID();
}

static __INLINE void PMSM_FOC_Update_Encoder(void) {
	if (_gFOC_Ctrl.in.s_manualAngle != 0xFFFF) {
		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
	}else {
		_gFOC_Ctrl.in.s_motAngle = hall_sensor_get_theta();
	}
	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
}

/* MPTA, 弱磁, 功率限制 */
static __INLINE void PMSM_FOC_idq_Assign(void) {
	_gFOC_Ctrl.in.s_targetIdq.d = 0;
	_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetTrq;
	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl, _gFOC_Ctrl.in.s_targetIdq.d);
	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl+1, _gFOC_Ctrl.in.s_targetIdq.q);
}


void PMSM_FOC_Schedule(void) {
	AB_t vAB;
	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABC;

	PMSM_FOC_Update_Encoder();
	//sample current
	phase_current_get(iabc);
	if (_gFOC_Ctrl.out.n_RunMode != OPEN_MODE) {
		Clark(iabc[0], iabc[1], iabc[2], &vAB);

		Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);

		s32q14_t err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl) - (_gFOC_Ctrl.out.s_RealIdq.d << 9);
		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_run(_gFOC_Ctrl.id_ctl, err);
		
		err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl+1) - (_gFOC_Ctrl.out.s_RealIdq.q << 9);
		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.iq_ctl, err);
	}
	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.in.n_modulation, &_gFOC_Ctrl.out.s_OutVdq);

	RevPark(&_gFOC_Ctrl.out.s_OutVdq, _gFOC_Ctrl.in.s_motAngle, &vAB);

	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);

	//u32 sp = phase_current_point(&_gFOC_Ctrl.out.n_Sector , _gFOC_Ctrl.out.n_Duty, _gFOC_Ctrl.out.n_lowDuty, _gFOC_Ctrl.out.n_midDuty);
	//_gFOC_Ctrl.out.n_Sample1 = sp & 0xFFFF;
	//_gFOC_Ctrl.out.n_Sample2 = (sp>>16) & 0xFFFF;
	//pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
	//pwm_update_duty(FOC_PWM_Half_Period, FOC_PWM_Half_Period, FOC_PWM_Half_Period);
	//pwm_update_sample(sp&0xFFFF, (sp>>16)&0xFFFF, _gFOC_Ctrl.out.n_Sector);
	plot_3data16(_gFOC_Ctrl.in.s_iABC[0]>>5, _gFOC_Ctrl.in.s_iABC[1]>>5, _gFOC_Ctrl.in.s_iABC[2]>>5);
	//plot_1data16(_gFOC_Ctrl.in.s_iABC[2]);
}

void PMSM_FOC_LogDebug(void) {
	//sys_debug("Duty %d, %d, %d\n", _gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
	//sys_debug("Vdq %f, %f-->%f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.d), S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.q), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.d), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.q), S16Q14toF(_gFOC_Ctrl.out.f_vdqRation));
	sys_debug("iABC %f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_iABC[0]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[1]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[2]));
	//plot_1data16(_gFOC_Ctrl.in.s_iABC[0]);
	//sys_debug("sample %d, %d\n", _gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2);
}

/*called in media task */
void PMSM_FOC_CtrlMode(void) {
	if (!_gFOC_Ctrl.in.b_motEnable) {
		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
	}else if (!_gFOC_Ctrl.in.b_motEnable || _gFOC_Ctrl.in.n_ctlMode == OPEN_MODE) {
		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
	}else if (_gFOC_Ctrl.in.n_ctlMode == SPD_MODE || _gFOC_Ctrl.in.b_cruiseEna){
		_gFOC_Ctrl.out.n_RunMode = SPD_MODE;
	}else {
		_gFOC_Ctrl.out.n_RunMode = TRQ_MODE;
	}
}

/*called in media task */
void PMSM_FOC_idqCalc(void) {
	if (!_gFOC_Ctrl.in.b_motEnable) {
		return;
	}
	if (_gFOC_Ctrl.out.n_RunMode == OPEN_MODE) {
		return;
	}
	if (_gFOC_Ctrl.out.n_RunMode == TRQ_MODE) {
		_gFOC_Ctrl.in.s_targetTrq = eCtrl_get_RefTorque();
	}else {
		s32q14_t errRef = eCtrl_get_RefSpd() - _gFOC_Ctrl.in.s_motRPM;
		_gFOC_Ctrl.in.s_targetTrq = PI_Controller_run(_gFOC_Ctrl.spd_ctl, errRef);
		
	}
	PMSM_FOC_idq_Assign();
}

void PMSM_FOC_Start(u8 nCtrlMode) {
	if (_gFOC_Ctrl.in.b_motEnable) {
		return;
	}
	PMSM_FOC_CoreInit();
	_gFOC_Ctrl.in.b_motEnable = true;
}

void PMSM_FOC_Stop(void) {
	if (!_gFOC_Ctrl.in.b_motEnable) {
		return;
	}
	_gFOC_Ctrl.in.b_motEnable = false;
}

bool PMSM_FOC_Is_Start(void) {
	return _gFOC_Ctrl.in.b_motEnable;
}

void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
	_gFOC_Ctrl.in.s_maxiDC = (ibusLimit);
}

void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
	_gFOC_Ctrl.in.s_maxRPM = (speedLimit);
}

s32q4_t PMSM_FOC_GetSpeedLimit(void) {
	return _gFOC_Ctrl.in.s_maxRPM;
}

void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
	_gFOC_Ctrl.in.s_vDC = vbusVol;
}

void PMSM_FOC_SetCtrlMode(u8 mode) {
	_gFOC_Ctrl.in.n_ctlMode = mode;
}

void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
	_gFOC_Ctrl.in.s_targetVdq.d = vd;
	_gFOC_Ctrl.in.s_targetVdq.q = vq;
}


bool PMSM_FOC_EnableCruise(bool enable) {
	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
		s32q4_t motSpd = PMSM_FOC_GetSpeed();
		if (motSpd < MIN_CRUISE_RPM) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		eCtrl_set_TargetSpeed(motSpd);
		_gFOC_Ctrl.in.b_cruiseEna = enable;
	}
	
	return true;
}

bool PMSM_FOC_Is_CruiseEnabled(void) {
	return (_gFOC_Ctrl.in.b_cruiseEna && (_gFOC_Ctrl.out.n_RunMode == SPD_MODE));
}

bool PMSM_FOC_Set_Speed(s32q4_t rpm) {
	if (_gFOC_Ctrl.in.b_cruiseEna) {
		return false;
	}
	eCtrl_set_TargetSpeed(rpm);
	return true;
}

bool PMSM_FOC_Set_Trque(float Trq) {
	eCtrl_set_TrqCurrent(Trq);
	return true;
}

bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		eCtrl_set_TargetSpeed(rpm);
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_Set_Angle(s16 angle) {
	_gFOC_Ctrl.in.s_manualAngle = S16Q14(angle);
}

s32q4_t PMSM_FOC_GetSpeed(void) {
	return _gFOC_Ctrl.in.s_motRPM;
}


void PMSM_FOC_LockMotor(bool lock) {
	_gFOC_Ctrl.in.b_motLock = lock;
}

void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {
	_gFOC_Ctrl.spd_ctl->kp = S32Q14(kp);
	_gFOC_Ctrl.spd_ctl->ki = S32Q14(ki);
	_gFOC_Ctrl.spd_ctl->max = S32Q14(max);
	_gFOC_Ctrl.spd_ctl->min = S32Q14(min);
}

void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min) {
	_gFOC_Ctrl.id_ctl->kp = S32Q14(kp);
	_gFOC_Ctrl.id_ctl->ki = S32Q14(ki);
	_gFOC_Ctrl.id_ctl->max = S32Q14(max);
	_gFOC_Ctrl.id_ctl->min = S32Q14(min);
}

void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min) {
	_gFOC_Ctrl.iq_ctl->kp = S32Q14(kp);
	_gFOC_Ctrl.iq_ctl->ki = S32Q14(ki);
	_gFOC_Ctrl.iq_ctl->max = S32Q14(max);
	_gFOC_Ctrl.iq_ctl->min = S32Q14(min);
}

void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min) {

}

void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min) {
	_gFOC_Ctrl.fw_ctl->kp = S32Q14(kp);
	_gFOC_Ctrl.fw_ctl->ki = S32Q14(ki);
	_gFOC_Ctrl.fw_ctl->max = S32Q14(max);
	_gFOC_Ctrl.fw_ctl->min = S32Q14(min);
}

void PMSM_FOC_SetErrCode(u8 error) {
	if (_gFOC_Ctrl.out.n_Error != error) {
		_gFOC_Ctrl.out.n_Error = error;
	}
}

u8 PMSM_FOC_GetErrCode(void) {
	return _gFOC_Ctrl.out.n_Error;
}

//获取母线电流
s16q5_t PMSM_FOC_Get_iDC(void) {
	s32 vd = _gFOC_Ctrl.out.s_OutVdq.d;
	s32 vq = _gFOC_Ctrl.out.s_OutVdq.q;
	s32 id = _gFOC_Ctrl.out.s_RealIdq.d;
	s32 iq = _gFOC_Ctrl.out.s_RealIdq.q;
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 2/3(iq x vq + id x vd);
	*/
	s32 m_pow = (vd * id + vq * iq); //s32q10
	s16 iDC = m_pow / _gFOC_Ctrl.in.s_vDC; //s16q5
	return S16Q5toF(iDC) * 0.667f;
}

void PMSM_FOC_Brake(bool brake) {
	_gFOC_Ctrl.in.b_eBrake = brake;
	if (!_gFOC_Ctrl.in.b_motEnable) {
		return;
	}
	if (_gFOC_Ctrl.in.b_eBrake & _gFOC_Ctrl.in.b_cruiseEna) {
		_gFOC_Ctrl.in.b_cruiseEna = false;
	}
	eCtrl_brake_signal(brake);
}