#include <string.h>
#include "libs/task.h"
#include "bsp/bsp.h"
#include "foc/foc_api.h"
#include "foc/park_clark.h"
#include "foc/svpwm.h"
#include "foc/foc_core.h"
#include "foc/foc_stm.h"
#include "foc/phase_current.h"
#include "foc/hall_sensor.h"
#include "foc/vbus_sensor.h"
#include "foc/ntc_sensor.h"
#include "foc/gas_sensor.h"

extern motor_foc_t mFOC;
static u32 foc_measure_task(void);
static void foc_defulat_value(void);

void foc_init(void) {
	foc_defulat_value();
	
	HAL_ADC1_Enable();
	/* init pwm hardware timer */
	PWM_TimerEnable();
	/* enable tim4 to run the foc normal task */
	TIM4_Enable();

	hall_sensor_init();
	vbus_sensor_init();
	ntc_sensor_init();

	task_start(foc_measure_task, 0);
}

static void foc_defulat_value(void){
	mFOC.state = IDLE;
	mFOC.mosGate = false;
	mFOC.vbus = MAX_VBUS_VOLTAGE;
	mFOC.state = IDLE;
	mFOC.mode = FOC_MODE_OPEN_LOOP;
	mFOC.alpha_beta.alpha = 0;
	mFOC.alpha_beta.beta = 0;
	mFOC.dq_ref.Id = 0;
	mFOC.dq_ref.Iq = 0;
	mFOC.foc_fault = foc_success;
	mFOC.is_ready = false;
	mFOC.rpm_ref = -1;
	memset(&mFOC.phase_time, 0, sizeof(mFOC.phase_time));
	mFOC.sector = 0;
	mFOC.dq_v.Id = 0;
	mFOC.dq_v.Iq = 0;
	phase_current_init(&mFOC.current_samp);
	ramp_ctrl_init(&mFOC.voltage_ramp);
	ramp_ctrl_init(&mFOC.current_ramp);
	ramp_ctrl_init(&mFOC.speed_ramp);
	pi_clear(&mFOC.PI_id);
	pi_clear(&mFOC.PI_iq);
	pi_clear(&mFOC.PI_speed);	
}

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_clear(void) {
	PWM_Stop();
	mFOC.mosGate = false;
	foc_defulat_value();
	hall_sensor_init();
}

u32 foc_get_speed(void) {
	float speed = hall_sensor_avg_speed()/(mFOC.motor_p.poles);
	//printf("avg speed %f, %d\n", speed, mFOC.motor_p.poles);
	return abs(speed);
}

bool foc_is_ready(void){
	return mFOC.is_ready;
}

foc_fault_t foc_set_ready(bool ready) {
	if (ready == foc_is_ready()) {
		return foc_success;
	}
	normal_task_enable(false);
	if (foc_stm_nextstate(ready?START:ANY_STOP) == NoError) {
		mFOC.is_ready = ready;
		normal_task_enable(true);
		return foc_success;
	}	
	normal_task_enable(true);
	return foc_not_allowed;
}


void foc_set_voltage_ramp(float final){
	printf("voltage %f\n", final);
	ramp_set_target(&mFOC.voltage_ramp, mFOC.dq_v.Vq, final, START_RAMP_DURATION);
}

void foc_set_speed_ramp(u16 rpm){
	ramp_set_target(&mFOC.speed_ramp, foc_get_speed(), rpm, SPEED_RAMP_DURATION);
}

/*
void foc_set_start_ramp(float v) {
	ramp_set_target(&mFOC.voltage_ramp, mFOC.dq_v.Vq, v, START_RAMP_DURATION);
}*/

void foc_set_ref_speed(u16 rpm) {
	normal_task_enable(false);
	if (mFOC.state == IDLE || mFOC.state == ANY_STOP) {
		normal_task_enable(true);
		return;
	}
	mFOC.rpm_ref = rpm;
	if (mFOC.mode == FOC_MODE_OPEN_LOOP) {
		foc_set_voltage_ramp(speed_to_voltage(rpm));
		ramp_set_target(&mFOC.speed_ramp, rpm, rpm, SPEED_RAMP_DURATION);
		ramp_exc(&mFOC.voltage_ramp);
	}
	normal_task_enable(true);
}

foc_fault_t foc_start_motor(void){
	return foc_stm_nextstate(START);
}

foc_fault_t foc_stop_motor(void) {
	return foc_stm_nextstate(ANY_STOP);
}

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

	PWM_Disable_Channels();
	//foc_pwm_start(false);

	task_udelay(10);
	
	phase_current_init(&mFOC.current_samp);
	mFOC.current_samp.is_calibrating = true;
	mFOC.current_samp.sector = SECTOR_5;
	foc_pwm_start(true);
	HAL_ADC1_InJ_StartConvert();
	while(mFOC.current_samp.offset_sample_count != 0){};
	
	foc_pwm_start(false);
	task_udelay(100);
	phase_current_init(&mFOC.current_samp);	
	mFOC.current_samp.sector = SECTOR_1;
	foc_pwm_start(true);
	while(mFOC.current_samp.offset_sample_count != 0){};
	mFOC.current_samp.is_calibrating = false;
	foc_pwm_start(false);
	PWM_Enable_Channels();
}

void foc_overide_theta(bool enable){
	mFOC.override.is_theta = enable;
}

void foc_overide_vdq(bool enable){
	mFOC.override.is_vdq = enable;
}

void foc_overide_set_theta(float theta){
	mFOC.override.theta = theta;
}


void foc_overide_set_vdq(float d, float q){
	mFOC.override.vdq.Vd = d;
	mFOC.override.vdq.Vq = q;
	//printf("%f, %f\n", d, q);
}

float foc_get_vbus_voltage(void){
	return mFOC.vbus;
}
static u32 foc_measure_task(void){
	vbus_sample_voltage();
	ntc_sensor_sample();
	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
	wdog_reload();
	return 1;
}