#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/motor.h"
#include "foc/core/svpwm.h"
#include "foc/core/torque.h"
#include "foc/samples.h"
#include "app/nv_storage.h"
#include "bsp/pwm.h"
#include "libs/logger.h"
#include "math/fir.h"
PMSM_FOC_Ctrl _gFOC_Ctrl;
static Fir_t phase1, phase2;
static bool g_focinit = false;
static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
	float c,s;
#if 0
	SinCos_Lut(angle, &s, &c);
#else
	s = _gFOC_Ctrl.out.sin;
	c = _gFOC_Ctrl.out.cos;
#endif

	alpha_beta->a = dq->d * c - dq->q * s;
	alpha_beta->b = dq->d * s + dq->q * c;
}

static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
	alpha_beta->a = A;
	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
}

static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
	float c,s;
#if 0
	SinCos_Lut(angle, &s, &c);
#else
	s = _gFOC_Ctrl.out.sin;
	c = _gFOC_Ctrl.out.cos;
#endif
	dq->d = alpha_beta->a * c + alpha_beta->b * s;
	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
}

#define VD_PRIO_HIGH
static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t *out) {
	float sq_vdq = vdq->d * vdq->d + vdq->q * vdq->q;
	float vDC_m = vDC * module;
	float sq_vDC = vDC_m * vDC_m;
	if (sq_vdq > sq_vDC) {
#ifdef VD_PRIO_HIGH		
		out->d = vdq->d;
		out->q = sqrtf(sq_vDC - out->d*out->d);
		return 1.1f;
#else
		float r = sqrtf(sq_vDC / sq_vdq);
		out->d = vdq->d * r;
		out->q = vdq->q * r;
		return r;
#endif
	}
	out->d = vdq->d;
	out->q = vdq->q;
	return 1.0f; // s16q5 32 means int 1
}

static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
	float cp = c->s_Cp;
	c->s_FinalTgt = target; 
	c->s_Step = (c->s_FinalTgt - cp) / (float)time;
	if ((c->s_Step == 0) && ((c->s_FinalTgt - cp) != 0.0f)) {
		if (c->s_FinalTgt - cp > 0) {
			c->s_Step = 0.001;	
		}else if (c->s_FinalTgt - cp < 0){
			c->s_Step = -0.001;
		}
	}
}

static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
	if (++c->n_StepCount == c->n_CtrlCount) {
		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;
			}
		}
		c->n_StepCount = 0;
	}
	return c->s_Cp;
}

static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
	c->n_CtrlCount = count;
	c->n_StepCount = 0;
	c->s_Cp = 0;
	c->s_FinalTgt = 0;
	c->s_Step = 0;
}

static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, float target) {
	FOC_Set_DqRamp(c, target, (/*IDQ_CTRL_TS/SPD_CTRL_TS - 1*/CURRENT_LOOP_RAMP_COUNT));
}

static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
	FOC_Set_DqRamp(c, target, (VDQ_RAMP_FINAL_TIME/VDQ_RAMP_TS));
}


static void PMSM_FOC_Reset_PID(void) {
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_id, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_iq, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_fw, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_power, 0);
}

static void PMSM_FOC_Conf_PID(void) {
	_gFOC_Ctrl.pi_ctl_id->kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
	_gFOC_Ctrl.pi_ctl_id->ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
	_gFOC_Ctrl.pi_ctl_id->kb = nv_get_foc_params()->pid_conf[PID_D_id].kb;
	_gFOC_Ctrl.pi_ctl_id->DT = (1.0f/(float)IDQ_CTRL_TS);

	_gFOC_Ctrl.pi_ctl_iq->kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
	_gFOC_Ctrl.pi_ctl_iq->ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
	_gFOC_Ctrl.pi_ctl_iq->kb = nv_get_foc_params()->pid_conf[PID_Q_id].kb;
	_gFOC_Ctrl.pi_ctl_iq->DT = (1.0f/(float)IDQ_CTRL_TS);

	_gFOC_Ctrl.pi_ctl_trq->kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
	_gFOC_Ctrl.pi_ctl_trq->ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
	_gFOC_Ctrl.pi_ctl_trq->kb = nv_get_foc_params()->pid_conf[PID_TRQ_id].kb;
	_gFOC_Ctrl.pi_ctl_trq->DT = (1.0f/(float)SPD_CTRL_TS);

	_gFOC_Ctrl.pi_ctl_spd->kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
	_gFOC_Ctrl.pi_ctl_spd->ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
	_gFOC_Ctrl.pi_ctl_spd->kb = nv_get_foc_params()->pid_conf[PID_Spd_id].kb;
	_gFOC_Ctrl.pi_ctl_spd->DT = (1.0f/(float)SPD_CTRL_TS);

	_gFOC_Ctrl.pi_ctl_power->kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
	_gFOC_Ctrl.pi_ctl_power->ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
	_gFOC_Ctrl.pi_ctl_power->kb = nv_get_foc_params()->pid_conf[PID_Pow_id].kb;
	_gFOC_Ctrl.pi_ctl_power->DT = (1.0f/(float)SPD_CTRL_TS);

	_gFOC_Ctrl.pi_ctl_lock->kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
	_gFOC_Ctrl.pi_ctl_lock->ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
	_gFOC_Ctrl.pi_ctl_lock->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
	_gFOC_Ctrl.pi_ctl_lock->DT = (1.0f/(float)SPD_CTRL_TS);

}

static void PMSM_FOC_UserInit(void) {
	memset(&_gFOC_Ctrl.userLim, 0, sizeof(_gFOC_Ctrl.userLim));
	_gFOC_Ctrl.userLim.s_iDCLim = nv_get_foc_params()->s_maxiDC;
	_gFOC_Ctrl.userLim.s_motRPMLim = nv_get_foc_params()->s_maxRPM;//(MAX_SPEED);
	_gFOC_Ctrl.userLim.s_torqueLim = nv_get_foc_params()->s_maxTorque;//MAX_TORQUE;
	_gFOC_Ctrl.userLim.s_PhaseCurrLim = nv_get_foc_params()->s_PhaseCurrLim;
	_gFOC_Ctrl.userLim.s_vDCMaxLim = nv_get_foc_params()->s_maxvDC;
	_gFOC_Ctrl.userLim.s_vDCMinLim = _gFOC_Ctrl.userLim.s_vDCMaxLim / 3;
	_gFOC_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_maxBrkCurrent;
	_gFOC_Ctrl.userLim.s_PhaseeVoleBrkLim = _gFOC_Ctrl.hwLim.s_PhaseVolMax - 20;
}

void PMSM_FOC_CoreInit(void) {
	Fir_init(&phase1);
	Fir_init(&phase2);
	_gFOC_Ctrl.pi_ctl_id = &PI_Ctrl_ID;
	_gFOC_Ctrl.pi_ctl_iq = &PI_Ctrl_IQ;
	_gFOC_Ctrl.pi_ctl_spd = &PI_Ctrl_Spd;
	_gFOC_Ctrl.pi_ctl_fw = &PI_Ctrl_fw;
	_gFOC_Ctrl.pi_ctl_trq = &PI_Ctrl_trq;
	_gFOC_Ctrl.pi_ctl_lock = &PI_Ctrl_lock;
	_gFOC_Ctrl.pi_ctl_power = &PI_Ctrl_Power;

	PMSM_FOC_Conf_PID();
	
	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
	
	_gFOC_Ctrl.hwLim.s_iDCMax = CONFIG_MAX_VBUS_CURRENT;
	_gFOC_Ctrl.hwLim.s_motRPMMax = CONFIG_MAX_MOT_RPM;
	_gFOC_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_MAX_PHASE_CURR;
	_gFOC_Ctrl.hwLim.s_PhaseVolMax = CONFIG_MAX_PHASE_VOL;
	_gFOC_Ctrl.hwLim.s_vDCMax      = CONFIG_MAX_VBUS_VOLTAGE;
	_gFOC_Ctrl.hwLim.s_torqueMax  = CONFIG_MAX_TORQUE;

	if (!g_focinit) {
		PMSM_FOC_UserInit();
		g_focinit = false;
	}
	_gFOC_Ctrl.params.n_modulation = nv_get_foc_params()->n_modulation;//SVM_Modulation;
	_gFOC_Ctrl.params.n_PhaseFilterCeof = nv_get_foc_params()->n_PhaseFilterCeof;//(0.2f);
	//_gFOC_Ctrl.params.n_TrqVelLimGain = nv_get_foc_params()->n_TrqVelLimGain;
	_gFOC_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
	_gFOC_Ctrl.in.s_manualAngle = INVALID_ANGLE;
	_gFOC_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxvDC;//(MAX_vDC);

	_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	_gFOC_Ctrl.out.f_vdqRation = 0;

	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[0], 1);
	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[1], 1);

	FOC_DqRamp_init(&_gFOC_Ctrl.vdq_ctl[0], (IDQ_CTRL_TS/VDQ_RAMP_TS));
	FOC_DqRamp_init(&_gFOC_Ctrl.vdq_ctl[1], (IDQ_CTRL_TS/VDQ_RAMP_TS));	
	PMSM_FOC_Reset_PID();
}

//#define PHASE_LFP_FIR
#define PHASE_LFP
static __INLINE void PMSM_FOC_Update_Hardware(void) {
	AB_t vAB;
	if (!_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
	}else {
		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_hallAngle;
	}
	SinCos_Lut(_gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.sin, &_gFOC_Ctrl.out.cos);
	
	_gFOC_Ctrl.in.s_motRPM = motor_encoder_get_speed() / _gFOC_Ctrl.params.n_poles;
	_gFOC_Ctrl.in.s_vDC = get_vbus_float();
	//sample current
	phase_current_get(_gFOC_Ctrl.in.s_iABC);
	get_phase_vols(_gFOC_Ctrl.in.s_vABC);
	_gFOC_Ctrl.in.s_vABC[0] -= _gFOC_Ctrl.in.s_vDC/2.0f;
	_gFOC_Ctrl.in.s_vABC[1] -= _gFOC_Ctrl.in.s_vDC/2.0f;
	_gFOC_Ctrl.in.s_vABC[2] -= _gFOC_Ctrl.in.s_vDC/2.0f; 
	Clark(_gFOC_Ctrl.in.s_vABC[0], _gFOC_Ctrl.in.s_vABC[1], _gFOC_Ctrl.in.s_vABC[2], &vAB);
	
	Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealVdq);

#ifdef PHASE_LFP
	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[0], _gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.params.n_PhaseFilterCeof);
	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[1], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.params.n_PhaseFilterCeof);
	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[2], _gFOC_Ctrl.in.s_iABC[2], _gFOC_Ctrl.params.n_PhaseFilterCeof);
#elif defined PHASE_LFP_FIR
	_gFOC_Ctrl.in.s_iABCFilter[1] = Fir_Filter(&phase1, _gFOC_Ctrl.in.s_iABC[1]);
	_gFOC_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, _gFOC_Ctrl.in.s_iABC[2]);
	_gFOC_Ctrl.in.s_iABCFilter[0] = -(_gFOC_Ctrl.in.s_iABCFilter[1] + _gFOC_Ctrl.in.s_iABCFilter[2]);
#endif
}

static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
	/* update id pi ctrl */
	_gFOC_Ctrl.params.maxvDQ.d = _gFOC_Ctrl.in.s_vDC;//MAX_vDC;
	_gFOC_Ctrl.params.minvDQ.d = -_gFOC_Ctrl.in.s_vDC;//MAX_vDC;
	_gFOC_Ctrl.params.maxvDQ.q = _gFOC_Ctrl.in.s_vDC;//MAX_vDC;
	_gFOC_Ctrl.params.minvDQ.q = -_gFOC_Ctrl.in.s_vDC;//MAX_vDC;

	if (_gFOC_Ctrl.params.maxvDQ.d != _gFOC_Ctrl.pi_ctl_id->max) {
		_gFOC_Ctrl.pi_ctl_id->max = _gFOC_Ctrl.params.maxvDQ.d;
	}
	if (_gFOC_Ctrl.params.minvDQ.d != _gFOC_Ctrl.pi_ctl_id->min) {
		_gFOC_Ctrl.pi_ctl_id->min = _gFOC_Ctrl.params.minvDQ.d;
	}
	/* update iq pi ctrl */
	if (_gFOC_Ctrl.params.maxvDQ.q != _gFOC_Ctrl.pi_ctl_iq->max) {
		_gFOC_Ctrl.pi_ctl_iq->max = _gFOC_Ctrl.params.maxvDQ.q;
	}
	if (_gFOC_Ctrl.params.minvDQ.q != _gFOC_Ctrl.pi_ctl_iq->min) {
		_gFOC_Ctrl.pi_ctl_iq->min = _gFOC_Ctrl.params.minvDQ.q;
	}	
}
extern int jtag_plot;
float encoder_last_offset(void);
static __INLINE void PMSM_FOC_Plot_Debug(void) {
	if (_gFOC_Ctrl.ctrl_count % 8 == 0) {
		//plot_1data16(_gFOC_Ctrl.out.test_sample);
		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
		//plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
		//if (jtag_plot == 2) {
			//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.q));
			//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
			//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x10(_gFOC_Ctrl.in.s_iABC[1]), FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.q));
			//plot_2data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABCFilter[0]);
			//plot_3data16(_gFOC_Ctrl.in.s_motRPM, _gFOC_Ctrl.idq_ctl[1].s_Cp * 10, FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.q));
			//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x10(_gFOC_Ctrl.in.s_vDC), FtoS16x10(get_throttle_float()));
		plot_3data16(FtoS16x10(_gFOC_Ctrl.out.s_OutVdq.q), FtoS16x10(_gFOC_Ctrl.out.s_RealVdq.q), FtoS16x10(_gFOC_Ctrl.out.s_FilteriDC));
		//}
		//plot_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
		//plot_1data16(_gFOC_Ctrl.in.s_motRPM);
	}
}

void PMSM_FOC_Schedule(void) {
	AB_t vAB;
#ifdef PHASE_LFP	
	float *iabc = _gFOC_Ctrl.in.s_iABCFilter;
#elif defined PHASE_LFP_FIR
	float *iabc = _gFOC_Ctrl.in.s_iABCFilter;
#else
	float *iabc = _gFOC_Ctrl.in.s_iABC;
#endif

	_gFOC_Ctrl.ctrl_count++;

	PMSM_FOC_Update_Hardware();

	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {

		PMSM_FOC_Update_PI_Idq();
	
		Clark(iabc[0], iabc[1], iabc[2], &vAB);

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

		float target_d = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[0]);
		float err = target_d - _gFOC_Ctrl.out.s_RealIdq.d;
		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(_gFOC_Ctrl.pi_ctl_id, err);

		float target_q = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[1]);
		err = target_q - _gFOC_Ctrl.out.s_RealIdq.q;
		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(_gFOC_Ctrl.pi_ctl_iq, err);
		_gFOC_Ctrl.out.test_targetIQ = target_q;

	}else {
		_gFOC_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]);
		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]);	
	}

	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.params.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);

	phase_current_point(&_gFOC_Ctrl.out);
	
	pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases);

#ifdef NO_SAMPLE_IDC
	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.d, _gFOC_Ctrl.out.s_RealIdq.d, 0.01f);
	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.q, _gFOC_Ctrl.out.s_RealIdq.q, 0.01f);
#endif
	PMSM_FOC_Plot_Debug();
}

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("VBUS: %f, %d, %d\n", S16Q5toF(get_vbus_sfix5()), _gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2);
	//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 */
u8 PMSM_FOC_CtrlMode(void) {
	u8 preMode = _gFOC_Ctrl.out.n_RunMode;
	if (!_gFOC_Ctrl.in.b_motEnable) {
		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || _gFOC_Ctrl.in.b_cruiseEna){
		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT_BRK) {
		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT_BRK;
	}else {
		if (!_gFOC_Ctrl.in.b_cruiseEna) {
			_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
		}
	}
	if (preMode != _gFOC_Ctrl.out.n_RunMode) {
		if ((preMode == CTRL_MODE_SPD) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, _gFOC_Ctrl.in.s_targetTorque);
		}else if ((preMode == CTRL_MODE_CURRENT) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {
			
		}	
	}
	return _gFOC_Ctrl.out.n_RunMode;
}

/* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
static __INLINE float PMSM_FOC_Limit_Power(float maxTrq) {
	PI_Ctrl_Power.max = maxTrq;
	float errRef = _gFOC_Ctrl.userLim.s_iDCLim - _gFOC_Ctrl.out.s_FilteriDC;
	return PI_Controller_run(_gFOC_Ctrl.pi_ctl_power, errRef);
}
static __INLINE void PMSM_FOC_idq_Assign(void) {
	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || _gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
		if (_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
			float s, c;
			normal_sincosf(degree_2_pi(_gFOC_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
			_gFOC_Ctrl.in.s_targetIdq.d = _gFOC_Ctrl.in.s_targetCurrent * c;
			_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetCurrent * s;
		}else {
			_gFOC_Ctrl.in.s_targetIdq.d = 0;
			_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetCurrent;
		}
	}else if ((_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
		torque_get_idq(_gFOC_Ctrl.in.s_targetTorque, _gFOC_Ctrl.in.s_motRPM, &_gFOC_Ctrl.in.s_targetIdq);
	}

	FOC_Set_iDqRamp(&_gFOC_Ctrl.idq_ctl[0], _gFOC_Ctrl.in.s_targetIdq.d);
	FOC_Set_iDqRamp(&_gFOC_Ctrl.idq_ctl[1], _gFOC_Ctrl.in.s_targetIdq.q);
}

/*called in media task */
void PMSM_FOC_idqCalc(void) {
	PMSM_FOC_Get_iDC();
	if (_gFOC_Ctrl.in.b_motLock) {
		float vel_count = motor_encoder_get_vel_count();
		float errRef = 0 - vel_count;
		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_lock ,errRef);
		PMSM_FOC_idq_Assign();
		return;
	}
	if ((_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) || (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK)) {
		_gFOC_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
		if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
			if (eCtrl_get_FinalCurrent() < 0.0001f && _gFOC_Ctrl.in.s_motRPM < MIN_RPM_EXIT_EBRAKE) {
				_gFOC_Ctrl.in.s_targetCurrent = 0;
			}
		}
	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
		float refTorque = eCtrl_get_FinalTorque();
		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
		_gFOC_Ctrl.pi_ctl_trq->min = -refTorque;
		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (_gFOC_Ctrl.in.s_motRPM < MIN_RPM_EXIT_EBRAKE)) {
			_gFOC_Ctrl.pi_ctl_trq->max = 0;
			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
		}
		float errRef = _gFOC_Ctrl.userLim.s_motRPMLim - _gFOC_Ctrl.in.s_motRPM;
		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_trq, errRef);
		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
		_gFOC_Ctrl.pi_ctl_spd->min = -_gFOC_Ctrl.userLim.s_PhaseCurrLim;
		float refSpeed = eCtrl_get_RefSpeed();
		if (_gFOC_Ctrl.in.b_cruiseEna) {
			refSpeed = _gFOC_Ctrl.in.s_cruiseRPM;
		}
		if ((eCtrl_get_FinalSpeed() == 0) && (_gFOC_Ctrl.in.s_motRPM < MIN_RPM_EXIT_EBRAKE)) {
			_gFOC_Ctrl.pi_ctl_spd->max = 0;
			_gFOC_Ctrl.pi_ctl_spd->min = 0; //防止倒转
		}
		float errRef = refSpeed - _gFOC_Ctrl.in.s_motRPM;
		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_spd, errRef);
		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
	}
	PMSM_FOC_idq_Assign();
}

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

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

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

void PMSM_FOC_iBusLimit(float ibusLimit) {
	_gFOC_Ctrl.userLim.s_iDCLim = (ibusLimit);
}

void PMSM_FOC_SpeedLimit(float speedLimit) {
	if (speedLimit > _gFOC_Ctrl.hwLim.s_motRPMMax) {
		speedLimit = _gFOC_Ctrl.hwLim.s_motRPMMax;
	}
	_gFOC_Ctrl.userLim.s_motRPMLim = (speedLimit);
}

float PMSM_FOC_GetSpeedLimit(void) {
	return _gFOC_Ctrl.userLim.s_motRPMLim;
}

void PMSM_FOC_TorqueLimit(float torqueLimit) {
	if (torqueLimit > _gFOC_Ctrl.hwLim.s_torqueMax) {
		torqueLimit = _gFOC_Ctrl.hwLim.s_torqueMax;
	}
	_gFOC_Ctrl.userLim.s_torqueLim = torqueLimit;
}
float PMSM_FOC_GetTorqueLimit(void) {
	return _gFOC_Ctrl.userLim.s_torqueLim;
}

void PMSM_FOC_SeteBrkPhaseCurrent(float curr) {
	_gFOC_Ctrl.userLim.s_iDCeBrkLim = curr;
}

float PMSM_FOC_GeteBrkPhaseCurrent(void) {
	return _gFOC_Ctrl.userLim.s_iDCeBrkLim ;
}

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

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

u8 PMSM_FOC_GetCtrlMode(void) {
	return _gFOC_Ctrl.in.n_ctlMode;
}

void PMSM_FOC_PhaseCurrLim(float lim) {
	if (lim > _gFOC_Ctrl.hwLim.s_PhaseCurrMax) {
		lim = _gFOC_Ctrl.hwLim.s_PhaseCurrMax;
	}
	_gFOC_Ctrl.userLim.s_PhaseCurrLim = lim;
}

float PMSM_FOC_GetPhaseCurrLim(void) {
	return _gFOC_Ctrl.userLim.s_PhaseCurrLim;
}

void PMSM_FOC_SetOpenVdq(float vd, float vq) {
	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[0], vd);
	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[1], vq);
}


bool PMSM_FOC_EnableCruise(bool enable) {
	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
		float motSpd = PMSM_FOC_GetSpeed();
		if (motSpd < MIN_CRUISE_RPM) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		_gFOC_Ctrl.in.s_cruiseRPM = 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 == CTRL_MODE_SPD));
}

bool PMSM_FOC_Set_Speed(float rpm) {
	if (_gFOC_Ctrl.in.b_cruiseEna) {
		return false;
	}
	eCtrl_set_TgtSpeed(min(ABS(rpm), _gFOC_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
	return true;
}

bool PMSM_FOC_Set_epmMode(bool epm) {
	if (_gFOC_Ctrl.in.b_epmMode != epm) {
		if (PMSM_FOC_GetSpeed() != 0.0f) {
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
		_gFOC_Ctrl.in.b_epmMode = epm;
		if (epm) {
			PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxEpmRPM);
			eCtrl_set_TgtSpeed(0);
			PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
		}else {
			PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
			PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxRPM);
		}
	}
	return true;
}

bool PMSM_FOC_is_epmMode(void) {
	return _gFOC_Ctrl.in.b_epmMode;
}

bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir) {
	if (!_gFOC_Ctrl.in.b_epmMode) {
		return false;
	}
	if (move) {
		if (_gFOC_Ctrl.in.epmDirection != EPM_Dir_None) {
			return false;
		}
		_gFOC_Ctrl.in.epmDirection = dir;
	}else {
		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
	}
	return true;
}

EPM_Dir_t PMSM_FOC_Get_epmDir(void) {
	return _gFOC_Ctrl.in.epmDirection;
}

bool PMSM_FOC_Set_Current(float is) {
	eCtrl_set_TgtCurrent(is);
	return true;
}

bool PMSM_FOC_Set_Torque(float trq) {
	eCtrl_set_TgtTorque(trq);
	return true;
}

bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		_gFOC_Ctrl.in.s_cruiseRPM = rpm;
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_MTPA_Calibrate(bool enable) {
	if (enable) {
		_gFOC_Ctrl.in.b_MTPA_calibrate = true;
		_gFOC_Ctrl.in.s_manualAngle = 0;
	}else {
		_gFOC_Ctrl.in.s_manualAngle = INVALID_ANGLE;
		_gFOC_Ctrl.in.b_MTPA_calibrate = false;
	}
}

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

void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
	dq->d = _gFOC_Ctrl.in.s_targetIdq.d;
	dq->q = _gFOC_Ctrl.in.s_targetIdq.q;
}

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


void PMSM_FOC_LockMotor(bool lock) {
	if (_gFOC_Ctrl.in.b_motLock != lock) {		
		motor_encoder_lock_pos(lock);
		PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
		_gFOC_Ctrl.in.b_motLock = lock;
	}
}

void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {

}


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

void PMSM_FOC_SetCriticalError(u8 err) {
	_gFOC_Ctrl.out.n_CritiCalErrMask |= (1u << err);
}

void PMSM_FOC_ClrCriticalError(u8 err) {
	_gFOC_Ctrl.out.n_CritiCalErrMask &= ~(1u << err);
}

u32 PMSM_FOC_GetCriticalError(void) {
	return _gFOC_Ctrl.out.n_CritiCalErrMask;
}
//获取母线电流
float PMSM_FOC_Get_iDC(void) {
	float vd = _gFOC_Ctrl.out.s_OutVdq.d;
	float vq = _gFOC_Ctrl.out.s_OutVdq.q;
	float id = _gFOC_Ctrl.out.s_FilterIdq.d;
	float iq = _gFOC_Ctrl.out.s_FilterIdq.q;
	
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 2/3(iq x vq + id x vd);
	*/
	float m_pow = (vd * id + vq * iq); //s32q10
	float raw_idc = m_pow / _gFOC_Ctrl.in.s_vDC * 1.5f; //s16q5
	LowPass_Filter(_gFOC_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);
	return _gFOC_Ctrl.out.s_FilteriDC;
}

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