#include "os/os_task.h"
#include "bsp/pwm.h"
#include "bsp/adc.h"
#include "bsp/mc_hall_gpio.h"
#include "foc/core/foc_core.h"
#include "foc/motor/current.h"
#include "foc/core/e_ctrl.h"
#include "foc/foc_config.h"
#include "bsp/timer_count32.h"
#include "libs/time_measure.h"
#include "libs/logger.h"
#include "foc/core/PI_Controller.h"
pmsm_foc_t pmsm_foc = {0};

extern void PMSM_FOC_Init(void);
extern ExtU *PMSM_FOC_GetInputs(void);
extern ExtY *PMSM_FOC_GetOutputs(void);
extern void PMSM_FOC_Step(void);
extern P *PMSM_FOC_GetParams(void);
PI_Controller pi;

void PMSM_FOC_CoreInit(void) {
	mc_hall_init();
	eCtrl_init(0, 0); //default
	PMSM_FOC_Init();
	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
	pmsm_foc.FOC_P   = PMSM_FOC_GetParams();
	pmsm_foc.FOC_P->i_pwm_count = FOC_PWM_Half_Period;
	pmsm_foc.FOC_P->n_polePairs = 4;
	pmsm_foc.FOC_P->id_fieldWeakMax = S16Q5(-50);
	pmsm_foc.FOC_P->i_dqMax = S16Q5(150);
	pmsm_foc.FOC_P->V_modulation = S16Q14(0.95f);
	pmsm_foc.FOC_Out->n_Sector = 1;
	PMSM_FOC_iBusLimit(S16Q5(Default_iDC_Limit));
	PMSM_FOC_SpeedLimit(S32Q4(Default_Spd_Limit));
}

static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
	if (pmsm_foc.b_lockmot) {
		pwm_update_duty(0, 0, 0);
		pwm_update_sample(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period, 1);
	}else {
		current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->n_Sector-1);
	
		pwm_update_duty(pmsm_foc.FOC_Out->n_Duty[0], pmsm_foc.FOC_Out->n_Duty[1], pmsm_foc.FOC_Out->n_Duty[2]);
	
		//pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
	}
}


void PMSM_FOC_Schedule(void) {

	phase_current_sample(&pmsm_foc.FOC_In->adc_Phase[0], &pmsm_foc.FOC_In->adc_Phase[1], &pmsm_foc.FOC_In->adc_Phase[2]);
	pmsm_foc.FOC_In->sys_ticks = hall_get_hwcount(pmsm_foc.FOC_In->hall_abc);

	if (pmsm_foc.FOC_In->n_ctrlMod == SPD_MODE) {
		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_RefSpd());
		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_FinalTorque());
	}else if(pmsm_foc.FOC_In->n_ctrlMod == TRQ_MODE){
		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_FinalSpd());
		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_RefTorque());
	}
	PI_Controller_run(&pi, 111);
	
	//PMSM_FOC_Step();

	pmsm_foc.FOC_Out->n_Duty[0] = pmsm_foc.FOC_Out->n_Duty[1] = pmsm_foc.FOC_Out->n_Duty[2] = FOC_PWM_Half_Period;
	
	PMSM_FOC_PWMCurrent_Update();
}

void PMSM_FOC_Start(u8 nCtrlMode) {
	if (pmsm_foc.b_FocEna) {
		return;
	}
	pmsm_foc.b_FocEna = true;
	pmsm_foc.FOC_In->b_motEna = true;
	pmsm_foc.FOC_In->n_ctrlMod = nCtrlMode;
	pmsm_foc.FOC_In->b_cruiseEna = 0;
	pmsm_foc.FOC_In->spd_Target = 0;
	pmsm_foc.FOC_In->idq_Target = 0;
	pmsm_foc.FOC_In->vdq_Target[0] = 0;
	pmsm_foc.FOC_In->vdq_Target[1] = 0;
}

void PMSM_FOC_Stop(void) {
	if (!pmsm_foc.b_FocEna) {
		return;
	}
	pmsm_foc.b_FocEna = false;
	memset(pmsm_foc.FOC_In, 0, sizeof(ExtU));
}

bool PMSM_FOC_Is_Start(void) {
	return pmsm_foc.FOC_In->b_motEna;
}

void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
	pmsm_foc.FOC_In->iDC_Limit = (ibusLimit);
}

void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
	pmsm_foc.FOC_In->spd_Limit = (speedLimit);
}

float PMSM_FOC_GetSpeedLimit(void) {
	return S32Q4toF(pmsm_foc.FOC_In->spd_Limit);
}

void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
	pmsm_foc.FOC_In->vDC = (vbusVol);
}

void PMSM_FOC_SetCtrlMode(uint8_T mode) {
	pmsm_foc.FOC_In->n_ctrlMod = mode;
}

void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
	pmsm_foc.FOC_In->vdq_Target[0] = (vd);
	pmsm_foc.FOC_In->vdq_Target[1] = (vq);
}

bool PMSM_FOC_EnableCruise(boolean_T enable) {
	if (enable) {
		float motSpd = PMSM_FOC_GetSpeed();
		if (motSpd < MIN_CRUISE_RPM) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		eCtrl_set_TargetSpeed(motSpd);
	}
	pmsm_foc.FOC_In->b_cruiseEna = enable;
	return true;
}

bool PMSM_FOC_Is_CruiseEnabled(void) {
	return (pmsm_foc.FOC_In->b_cruiseEna && (pmsm_foc.FOC_Out->n_FocMode == SPD_MODE));
}

bool PMSM_FOC_Set_Speed(float rpm) {
	if (pmsm_foc.FOC_In->b_cruiseEna) {
		return false;
	}
	eCtrl_set_TargetSpeed(rpm);
	return true;
}

bool PMSM_FOC_Set_Current(float current) {
	eCtrl_set_TrqCurrent(current);
	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_HallCalibrate(boolean_T b_caliHall, s16 open_vd) {

	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
		return;
	}
	pmsm_foc.FOC_In->vdq_Target[0] = S16Q4(open_vd);
	pmsm_foc.FOC_In->vdq_Target[1] = 0;
	pmsm_foc.FOC_In->n_ctrlMod = OPEN_MODE;
}

void PMSM_FOC_Set_Vdq(s16 vd, s16 vq) {
	pmsm_foc.FOC_In->vdq_Target[0] = S16Q4(vd);
	pmsm_foc.FOC_In->vdq_Target[1] = S16Q4(vq);
}

void PMSM_FOC_Set_Angle(s16 angle) {
	pmsm_foc.FOC_In->set_Angle = S16Q14(angle);
}

float PMSM_FOC_GetSpeed(void) {
	return S32Q4toF(pmsm_foc.FOC_Out->f_MotRPM);
}


void PMSM_FOC_SetErrCode(u8 code) {
	pmsm_foc.error_code = code;
}


u8 PMSM_FOC_GetErrCode(void) {
	return pmsm_foc.error_code;
}

void PMSM_FOC_LockMotor(bool lock) {
	pmsm_foc.b_lockmot = lock;
}

void PMSM_FOC_SetSpdPid(float kp, float ki, float kb) {
	pmsm_foc.FOC_P->cf_nKp = S16Q10(kp);
	pmsm_foc.FOC_P->cf_nKi = S16Q14(ki);
	pmsm_foc.FOC_P->cf_nKb = S16Q14(kb);
}

void PMSM_FOC_SetIDPid(float kp, float ki, float kb) {
	pmsm_foc.FOC_P->cf_idKp = S16Q10(kp);
	pmsm_foc.FOC_P->cf_idKi = S16Q14(ki);
	pmsm_foc.FOC_P->cf_idKb = S16Q14(kb);
}

void PMSM_FOC_SetIQPid(float kp, float ki, float kb) {
	pmsm_foc.FOC_P->cf_iqKp = S16Q10(kp);
	pmsm_foc.FOC_P->cf_iqKi = S16Q14(ki);
	pmsm_foc.FOC_P->cf_iqKb = S16Q14(kb);
}

void PMSM_FOC_SetTrqPid(float kp, float ki, float kb) {

}

void PMSM_FOC_SetFW_I(float ki, float kb) {
	pmsm_foc.FOC_P->cf_Fw_Ki = S16Q10(ki);
	pmsm_foc.FOC_P->cf_Fw_Kb = S16Q10(kb);

}

s16 PMSM_FOC_Add_Hall_Offset(s16 offset) {
	pmsm_foc.FOC_P->i_hall_offset = S16Q4(offset);
	return (s16)S16Q4toF(pmsm_foc.FOC_P->i_hall_offset);
}

//获取母线电流
float PMSM_FOC_Get_iDC(void) {
	s32 vd = pmsm_foc.FOC_Out->f_Vdq[0];
	s32 vq = pmsm_foc.FOC_Out->f_Vdq[1];
	s32 id = pmsm_foc.FOC_Out->f_Idq[0];
	s32 iq = pmsm_foc.FOC_Out->f_Idq[1];
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 2/3(iq x vq + id x vd);
	*/
	s32 m_pow = (vd * id + vq * iq); //s32q10
	s16 iDC = m_pow / pmsm_foc.FOC_In->vDC; //s16q5
	return S16Q5toF(iDC) * 0.667f;
}

void PMSM_FOC_Brake(bool brake) {
	pmsm_foc.b_brake_in = brake;
	if (!pmsm_foc.FOC_In->b_motEna) {
		return;
	}
	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
		pmsm_foc.FOC_In->b_cruiseEna = false;
	}
	eCtrl_brake_signal(brake);
}