#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 "bsp/timer_count32.h"
#include "libs/time_measure.h"
#include "libs/logger.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);

void PMSM_FOC_CoreInit(void) {
	mc_hall_init();
	PMSM_FOC_Init();
	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
	PMSM_FOC_iBusLimit(Default_DC_iLimit);
	PMSM_FOC_SpeedLimit(Default_Spd_Limit);
}

static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
	current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->sector);
	
	pwm_update_duty(pmsm_foc.FOC_Out->PWM[0], pmsm_foc.FOC_Out->PWM[1], pmsm_foc.FOC_Out->PWM[2]);
	
	pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
}

static __INLINE bool PMSM_FOC_BrakeCtrl(void) {

}

static __INLINE void PMSM_FOC_Controller(void) {
	u8  hall[3];
	pwm_clear_updata();
	phase_current_sample(&pmsm_foc.FOC_In->adc_a, &pmsm_foc.FOC_In->adc_b);
	pmsm_foc.FOC_In->hw_count = hall_get_hwcount(hall);
	pmsm_foc.FOC_In->hall_a = hall[0];
	pmsm_foc.FOC_In->hall_b = hall[1];
	pmsm_foc.FOC_In->hall_c = hall[2];
	if (!PMSM_FOC_BrakeCtrl()) {
		if (pmsm_foc.FOC_In->n_ctrlModReq == SPD_MODE) {
			pmsm_foc.FOC_In->speed_target = i2sFix1((s16)ramp_get_target(&pmsm_foc.speed_ramp));
			pmsm_foc.FOC_In->current_target = 0;
		}else if(pmsm_foc.FOC_In->n_ctrlModReq == TRQ_MODE){
			pmsm_foc.FOC_In.speed_target = i2sFix1((s16)pmsm_foc.speed_ramp.final_point);
			pmsm_foc.FOC_In.current_target = i2sFix6((s16)ramp_get_target(&pmsm_foc.current_ramp));
		}
	}
	
	PMSM_FOC_Step();

	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_ctrlModReq = nCtrlMode;
	pmsm_foc.FOC_In->b_cruiseEna = 0;
	pmsm_foc.FOC_In->foc_calibrate = 0;
	pmsm_foc.FOC_In->speed_target = 0;
	pmsm_foc.FOC_In->current_target = 0;
	pmsm_foc.FOC_In->vdq_open_target[0] = 0;
	pmsm_foc.FOC_In->vdq_open_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));
}

void PMSM_FOC_iBusLimit(int16_T ibusLimit) {
	pmsm_foc.FOC_In->DC_iLimit = i2sFix6(ibusLimit);
}

void PMSM_FOC_SpeedLimit(int16_T speedLimit) {
	pmsm_foc.FOC_In->speed_limit = i2sFix2(speedLimit);
}

s16 PMSM_FOC_GetSpeedLimit(void) {
	return sFix2ToI(pmsm_foc.FOC_In->speed_limit);
}

void PMSM_FOC_VbusVoltage(int16_T vbusVol) {
	pmsm_foc.FOC_In->vbus_voltage = i2sFix6(vbusVol);
}

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

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

bool PMSM_FOC_EnableCruise(boolean_T enable) {
	if (enable) {
		if (sFix2ToI(pmsm_foc.FOC_Out->rpm) < 100) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), pmsm_foc.FOC_Out->rpm, 500);
	}
	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->running_mode == SPD_MODE));
}

bool PMSM_FOC_Set_Speed(s16 rpm, u32 ramp) {
	if (!pmsm_foc.FOC_In->b_cruiseEna) {
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, ramp);
	}
	return true;
}

bool PMSM_FOC_Set_Current(s16 current, u32 ramp) {
	if (pmsm_foc.FOC_In.n_ctrlModReq != TRQ_MODE) {
		return false;
	}
	ramp_set_target(&pmsm_foc.current_ramp, ramp_get_target(&pmsm_foc.current_ramp), current, ramp);
	return true;
}

bool PMSM_FOC_Set_CruiseSpeed(s16 rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, 0);
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd) {
	uint16_T foc_cali = 0;
	if (b_caliHall) {
		foc_cali = FOC_CALIMOD_HALL;
	}
	if ((pmsm_foc.FOC_In->foc_calibrate & FOC_CALIMOD_HALL) == foc_cali) {
		return;
	}
	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
		return;
	}
	pmsm_foc.FOC_In->foc_calibrate &= ~(FOC_CALIMOD_HALL);
	pmsm_foc.FOC_In->b_motEna = b_caliHall;
	pmsm_foc.FOC_In->foc_calibrate |= foc_cali;
	pmsm_foc.FOC_In->vdq_open_target[0] = i2sFix6(open_vd);
	pmsm_foc.FOC_In->vdq_open_target[1] = 0;
	pmsm_foc.FOC_In->n_ctrlModReq = OPEN_MODE;
}

s16 PMSM_FOC_GetSpeed(void) {
	return sFix2ToI(pmsm_foc.FOC_Out->rpm);
}


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


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

void foc_brake_handler(bool brake) {
	pmsm_foc.b_brake_in = brake;
	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
		pmsm_foc.FOC_In->b_cruiseEna = false;
	}
}

void foc_pwm_up_handler(void){
	phase_current_adc_triger();
}

measure_time_t g_meas_foc = {.exec_max_time = 15,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void mc_phase_current_irq(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	TIME_MEATURE_START();
	PMSM_FOC_Controller();
	TIME_MEATURE_END();
}