#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/core/foc_core.h"
#include "foc/foc_config.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/delay.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"

static bool _motor_started = false;
static float _motor_throttle = 0;

bool mc_start(u8 mode) {
	if (_motor_started) {
		return true;
	}
	if (mode > TRQ_MODE) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (PMSM_FOC_GetSpeed() > 10) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	pwm_turn_on_low_side();
	task_udelay(500);
	phase_current_calibrate();
	PMSM_FOC_Start(mode);
	pwm_start();
	adc_start_convert();
	_motor_throttle = 0;
	_motor_started = true;
	return true;
}

bool mc_stop(void) {
	if (!_motor_started) {
		return true;
	}
	if (PMSM_FOC_GetSpeed() > 10) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}	
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	_motor_started = false;
	return true;
}

bool mc_lock_motor(bool lock) {
	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (lock) {
		adc_stop_convert();
		pwm_stop();
	}else {
		pwm_start();
		adc_start_convert();
	}
	return true;
}


bool mc_throttle_released(void) {
	return get_throttle_float() < THROTTLE_LOW_VALUE;
}

static float _get_speed_from_throttle(float throttle) {
	float f_throttle = (throttle);
	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
		return 0;
	}
	float delta = f_throttle - (THROTTLE_LOW_VALUE);

	float ration = delta / (THROTTLE_MAX_VALUE - THROTTLE_LOW_VALUE);

	return (THROTTLE_MIN_RPM + PMSM_FOC_GetSpeedLimit() * ration);
}

static float _get_idq_from_throttle(float throttle) {
	float f_throttle = (throttle);
	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
		return 0;
	}
	float delta = f_throttle - (THROTTLE_LOW_VALUE);

	float ration = delta / (THROTTLE_MAX_VALUE - THROTTLE_LOW_VALUE);

	return (THROTTLE_MIN_IDQ + MAX_iDQ * ration);
}

static void _brake_io_process(void) {
	bool brake = gpio_get_brake();
	for (int i = 0; i < 10; i++) {
		if (brake != gpio_get_brake()) {
			return;
		}
	}
	PMSM_FOC_Brake(brake);
}

u32 mc_main_task(void *param) {
	if (_motor_started) {
		if (get_throttle_float() != _motor_throttle) {
			_motor_throttle = get_throttle_float();
			float speed_Ref = _get_speed_from_throttle(_motor_throttle);
			PMSM_FOC_Set_Speed(speed_Ref);
			float torque_idq = _get_idq_from_throttle(_motor_throttle);
			PMSM_FOC_Set_Current(torque_idq);
		}
		_brake_io_process();
	}
	return 0;
}