#include <string.h>
#include "libs/task.h"
#include "hal/pwm.h"
#include "hal/hal.h"
#include "foc/foc.h"
#include "foc/park_clark.h"
#include "foc/svpwm.h"
#include "foc/foc_task.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;
u16 _hall_table[] = {0xFFFF, 292, 47, 1, 180, 227, 113, 0xFFFF};

void foc_init(void) {
	/* init pwm hardware timer */
	m_foc.state = IDLE;
	m_foc.gate_output = false;
	memcpy(m_foc.hall_table, _hall_table, sizeof(_hall_table));
	
	PWM_TimerInit();

	task_start(foc_main_task_handler, 0);

	foc_hall_detect(6.0f, (u16 *)m_foc.hall_table);
}

void foc_motor_spin(dq_t *dq_v, float angle) {
	alpha_beta_t alphabeta;
	phase_time_t phase_time;
	u8 sector = 0xFF;
	Rev_Park(dq_v, degree_2_pi(angle), &alphabeta);
	svpwm(&alphabeta, MAX_VBUS, FOC_PWM_period/2, &phase_time, &sector);
	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, FOC_PWM_period/2);
}

int foc_hall_detect(float current, u16 *hall_table){
	dq_t dq_v = {.d = 0.0f, .q = 0.0f};
	foc_start_pwm(true);
	for (int i = 0;i < 1000;i++) {
		dq_v.d = (float)i * current / 1000.0f;
		foc_motor_spin(&dq_v, 0);
		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++) {
			foc_motor_spin(&dq_v, j);
			task_udelay(10 * 1000);

			int hall = HALL_Read(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--) {
			foc_motor_spin(&dq_v, j);
			task_udelay(10 * 1000);

			int hall = HALL_Read(7);
			float s, c;
			normal_sincosf(degree_2_pi(j), &s, &c);
			sin_hall[hall] += s;
			cos_hall[hall] += c;
			hall_iterations[hall]++;
		}
	}
	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++;
		}
	}
	foc_start_pwm(false);
	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){

}

void hall_detect_handler(void) {

}

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;
	}
}