#include <string.h>
#include "libs/task.h"
#include "bsp/bsp.h"
#include "foc/foc.h"
#include "foc/park_clark.h"
#include "foc/svpwm.h"
#include "foc/foc_task.h"
#include "foc/phase_current.h"
#include "foc/hall_sensor.h"
#include "foc/vbus_sensor.h"
#include "foc/ntc_sensor.h"

static u32 foc_stm_task_handler(void);
static u32 foc_measure_task_handler(void);
static void foc_defulat_value(void);

static motor_foc_t m_foc;

void foc_init(void) {
	foc_defulat_value();
	hall_sensor_init();
	HAL_ADC1_Enable();
	/* init pwm hardware timer */
	PWM_TimerEnable();

	task_start(foc_measure_task_handler, 0);
	task_start(foc_stm_task_handler, 0);
	printf("test\n");
	foc_ramp_speed(400.0f, 10.0f);
	
	//foc_hall_detect(3.0f, m_foc.hall_table);
}

static void foc_defulat_value(void){
	memset(&m_foc, 0, sizeof(m_foc));
	m_foc.state = IDLE;
	m_foc.gate_output = false;
	m_foc.vbus = 12.0f;
	phase_current_init(&m_foc.current_samp);
}

void foc_ramp_speed(float rpm, u32 duration_ms) {
	float current = 8.0f;
	m_foc.dq_ref.q = 0;
	m_foc.dq_ref.d = 0;
	//m_foc.override_p.is_override_theta = true;
	foc_pwm_start(true);
	for (int i = 0;i < 100;i++) {
		m_foc.dq_ref.d = (float)i * current / 1000.0f;
		task_udelay(10);
	}
	m_foc.dq_ref.d = 0.0f;
	m_foc.dq_ref.q = 4.0f;
	
	while(1) {
		for (int i = 0; i < 360; i++) {
			m_foc.override_p.theta = i;
			task_udelay(100);
			int theta = hall_sensor_get_theta();
			if (i % 10 == 0) {
				printf("$%d;", theta);
			}
		}
	}
}

int foc_hall_detect(float current, u16 *hall_table){
	foc_pwm_start(true);
	m_foc.override_p.is_override_theta = true;
	m_foc.override_p.theta = 0.0f;
	m_foc.override_p.is_override_v_dq = true;
	m_foc.override_p.v_dq.d = .0f;
	m_foc.override_p.v_dq.q = .0f;
	
	for (int i = 0;i < 1000;i++) {
		m_foc.override_p.v_dq.d = (float)i * current / 1000.0f;
		task_udelay(1000);
	}
	float sin_hall[8];
	float cos_hall[8];
	int hall_iterations[8];
	memset(sin_hall, 0, sizeof(sin_hall));
	memset(cos_hall, 0, sizeof(cos_hall));
	memset(hall_iterations, 0, sizeof(hall_iterations));

	// Forwards
	for (int i = 0;i < 3;i++) {
		for (int j = 0;j < 360;j++) {
			m_foc.override_p.theta = j;
			task_udelay(10 * 1000);

			int hall = get_hall_stat(7);
			float s, c;
			normal_sincosf(degree_2_pi(j), &s, &c);
			sin_hall[hall] += s;
			cos_hall[hall] += c;
			hall_iterations[hall]++;
		}
	}

	// Reverse
	for (int i = 0;i < 3;i++) {
		for (int j = 360;j >= 0;j--) {
			m_foc.override_p.theta = j;
			task_udelay(10 * 1000);

			int hall = get_hall_stat(7);
			float s, c;
			normal_sincosf(degree_2_pi(j), &s, &c);
			sin_hall[hall] += s;
			cos_hall[hall] += c;
			hall_iterations[hall]++;
		}
	}
	foc_pwm_start(false);
	m_foc.override_p.is_override_theta = false;
	m_foc.override_p.is_override_v_dq = false;
	int fails = 0;
	for(int i = 0;i < 8;i++) {
		if (hall_iterations[i] > 30) {
			float ang = pi_2_degree(atan2f(sin_hall[i], cos_hall[i]));
			fast_norm_angle(&ang);
			hall_table[i] = (u16)ang;
		} else {
			hall_table[i] = 0xFFFF;
			fails++;
		}
	}
	return fails == 2;	
}

void foc_clear(void) {
	foc_defulat_value();
	PWM_Stop();
	m_foc.gate_output = false;
}

static u32 foc_measure_task_handler(void){
	vbus_sample_voltage();
	ntc_sensor_sample();
	return 1;
}

static u32 foc_stm_task_handler(void) {
	switch (m_foc.state) {
		case START:
			break;
		case CHARGER_BOOT_CAP:
			break;
		case READY_TO_RUN:
			break;
		default:
			break;
	}
	return 10;
}

void foc_brake_handler(void) {

}

void foc_pwm_up_handler(void){
	phase_current_adc_triger(&m_foc.current_samp);
}


void current_sample_handler(void) {
	foc_task(&m_foc);
}

void foc_pwm_start(bool start) {
	if (start == m_foc.gate_output) {
		return;
	}
	if (start) {
		PWM_Start();
	}else {
		PWM_Stop();
	}
	m_foc.gate_output = start;	
}