#include <string.h>
#include "os/os_type.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"
#include "foc/core/foc_api.h"
#include "foc/core/park_clark.h"
#include "foc/core/svpwm.h"
#include "foc/core/foc_fsm.h"
#include "foc/core/foc_core.h"
#include "foc/motor/current.h"
#include "foc/motor/hall.h"
#include "foc/motor/motor.h"
#include "foc/samples.h"
#include "foc/commands.h"

extern motor_foc_t g_foc;
static void foc_defulat_value(void);

void foc_init(void) {
	foc_defulat_value();
	adc_init();
	pwm_3phase_init();
	hall_sensor_init();
	foc_core_init();
	foc_command_init();
}

static void foc_defulat_value(void){
	g_foc.state = IDLE;
	g_foc.mosfec_gate = false;
	g_foc.vbus = MAX_VBUS_VOLTAGE;
	g_foc.mode = FOC_MODE_OPEN_LOOP;
	g_foc.alpha_beta.alpha = 0;
	g_foc.alpha_beta.beta = 0;
	g_foc.dq_command.Id = 0;
	g_foc.dq_command.Iq = 0;
	g_foc.foc_fault = foc_success;
	g_foc.speed_command.speed = -1;
	g_foc.speed_command.delta_ts = 0;
	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
	g_foc.sector = 0;
	g_foc.dq_last.Id = 0;
	g_foc.dq_last.Iq = 0;
	phase_current_init(&g_foc.current_samp);
	ramp_ctrl_init(&g_foc.voltage_ramp);
	ramp_ctrl_init(&g_foc.current_ramp);
	ramp_ctrl_init(&g_foc.speed_ramp);
	pi_clear(&g_foc.id_controller);
	pi_clear(&g_foc.iq_controller);
	pi_clear(&g_foc.speed_controller);
}

static bool gas_detect_speed_signal(void) {
	return false;
}

current_samp_t *foc_get_current_sample(void) {
	return &g_foc.current_samp;
}


float speed_to_voltage(u16 rpm) {
	return foc_get_vbus_voltage() * rpm / MAX_SPEED_RPM;
}

float speed_to_current(u16 rpm) {
	return MAX_CURRENT * rpm / MAX_SPEED_RPM;
}


void foc_stop(void) {
	pwm_stop();
	foc_start_adc(false);
	g_foc.mosfec_gate = false;
	foc_defulat_value();
	hall_sensor_clear();
}

void foc_start_adc(bool start) {
	if (start) {
		adc_start_convert();
	}else {
		adc_stop_convert();
	}
}

void foc_clear(void) {
	g_foc.alpha_beta.alpha = 0;
	g_foc.alpha_beta.beta = 0;
	g_foc.dq_command.Id = 0;
	g_foc.dq_command.Iq = 0;
	g_foc.foc_fault = foc_success;
	g_foc.speed_command.speed = -1;
	g_foc.speed_command.delta_ts = 0;
	memset(&g_foc.phase_time, 0, sizeof(g_foc.phase_time));
	g_foc.sector = 0;
	g_foc.dq_last.Id = 0;
	g_foc.dq_last.Iq = 0;
	phase_current_init(&g_foc.current_samp);
	ramp_ctrl_init(&g_foc.voltage_ramp);
	ramp_ctrl_init(&g_foc.current_ramp);
	ramp_ctrl_init(&g_foc.speed_ramp);
	pi_clear(&g_foc.id_controller);
	pi_clear(&g_foc.iq_controller);
	pi_clear(&g_foc.speed_controller);
}

u32 foc_get_speed(void) {
	u32 speed = motor_get_speed()/(motor_params.poles);
	return abs(speed);
}


void foc_set_dq_command(float d, float q) {
	g_foc.dq_command.Vd = d;
	g_foc.dq_command.Vq = q;
}

void foc_set_voltage_ramp(float final){
	sys_debug("voltage %f -> %f\n", g_foc.dq_last.Vq, final);
	ramp_set_target(&g_foc.voltage_ramp, g_foc.dq_last.Vq, final, START_RAMP_DURATION);
	ramp_exc(&g_foc.voltage_ramp);
}

void foc_set_speed_ramp(u16 rpm){
	if ((g_foc.speed_command.delta_ts == 0) || (g_foc.speed_command.delta_ts > START_RAMP_DURATION)) {
		g_foc.speed_command.delta_ts = START_RAMP_DURATION;
	}
	ramp_set_target(&g_foc.speed_ramp, foc_get_speed(), rpm, g_foc.speed_command.delta_ts);
	ramp_exc(&g_foc.speed_ramp);
}

void foc_set_controller_mode(control_mode_t mode) {
	g_foc.mode = mode;
}

void foc_set_speed(u16 rpm, u32 delta_ms) {
	if (g_foc.state == IDLE || g_foc.state == ANY_STOP) {
		return;
	}
	g_foc.speed_command.speed = rpm;
	g_foc.speed_command.delta_ts = delta_ms;
	if (g_foc.mode == FOC_MODE_OPEN_LOOP) {
		foc_set_voltage_ramp(speed_to_voltage(rpm));
	}
}

foc_fault_t foc_start_motor(void){
	if (gas_detect_speed_signal()) {
		return foc_start_with_gas_error;
	}
	return foc_fsm_next_state(START);
}

foc_fault_t foc_stop_motor(void) {
	if (foc_get_speed() > 0) {
		return foc_stop_with_speed_error;
	}
	return foc_fsm_next_state(ANY_STOP);
}

bool foc_motor_is_started(void) {
	return g_foc.state >= START;
}

void foc_current_calibrate(void){
	g_foc.current_samp.adc_offset_a = 0;
	g_foc.current_samp.adc_offset_b = 0;
	g_foc.current_samp.adc_offset_c = 0;

	phase_current_init(&g_foc.current_samp);
	g_foc.current_samp.is_calibrating_offset = true;
	g_foc.current_samp.sector = SECTOR_5;
	adc_current_sample_config(g_foc.current_samp.sector);
	motor_start();
	while(g_foc.current_samp.offset_sample_count != 0){};
#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
	motor_stop();
	cpu_udelay(100);
	phase_current_init(&g_foc.current_samp);	
	g_foc.current_samp.sector = SECTOR_1;
	adc_current_sample_config(g_foc.current_samp.sector);
	motor_start();
	while(g_foc.current_samp.offset_sample_count != 0){};
#endif
	g_foc.current_samp.is_calibrating_offset = false;
}

float foc_get_vbus_voltage(void){
	return g_foc.vbus;
}