#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"

static void charge_cap_timer_handler(timer_t *t);
static u32 foc_main_task_handler(void);

static timer_t charge_cap_timer = {.handler = charge_cap_timer_handler};
static motor_foc_t m_foc;

void foc_init(void) {
	memset(&m_foc, 0, sizeof(m_foc));
	m_foc.state = IDLE;
	m_foc.gate_output = false;

	hall_sensor_init();
	/* init pwm hardware timer */
	PWM_TimerInit();

	task_start(foc_main_task_handler, 0);
}

int foc_hall_detect(float current, u16 *hall_table){
	foc_start_pwm(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_start_pwm(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;	
}

static void charge_cap_timer_handler(timer_t *t) {
	if (m_foc.state == CHARGER_BOOT_CAP) {
		m_foc.state = READY_TO_RUN;
	}
}
static u32 foc_main_task_handler(void) {
	switch (m_foc.state) {
		case START:
			m_foc.state = CHARGER_BOOT_CAP;
			timer_post(&charge_cap_timer, 10);
			PWM_TurnOnLowSides();
			break;
		case CHARGER_BOOT_CAP:
			break;
		case READY_TO_RUN:
			break;
		default:
			break;
	}
	return 1;
}

void foc_brake_handler(void) {
	if (m_foc.state == CHARGER_BOOT_CAP) {
		timer_post(&charge_cap_timer, 10);
		PWM_TurnOnLowSides();
	}
}

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_start_pwm(bool start) {
	if (start != m_foc.gate_output) {
		if (start) {
			PWM_Start();
		}else {
			PWM_Stop();
		}
		m_foc.gate_output = start;
	}
}