#include <string.h>
#include "bsp/bsp.h"
#include "libs/task.h"
#include "math/fast_math.h"
#include "hall_sensor.h"

#define HALL_READ_TIMES 7
/* 
100
101
001
011
010
110
4,5,1,3,2,6,4
*/
static u16 _hall_table[] = {0xFFFF, 292/*1*/, 54/*2*/, 1/*3*/, 180/*4*/, 229/*5*/, 115/*6*/, 0xFFFF};
static hall_t _hall;

#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
#define tick_2_s(tick) ((float)tick / (float)SYSTEM_CLOCK)

static u32 __inline delta_ticks(u32 prev) {
	u32 now = task_ticks_abs();
	if (now >= prev) {
		return (now - prev);
	}
	return (0xFFFFFFFFU - prev + now) + 1;
}

void hall_sensor_init(void) {
	memset(&_hall, 0, sizeof(_hall));
	read_hall(_hall.state, _hall.theta);
}

float hall_sensor_get_theta(void){
	if (!_hall.working) {
		read_hall(_hall.state, _hall.theta);
		return _hall.theta;
	}
	float est_theta = tick_2_s(delta_ticks(_hall.ticks)) * _hall.degree_per_s + _hall.theta;
	fast_norm_angle(&est_theta);
	return est_theta;
}

float hall_sensor_get_speed(void) {
	return _hall.e_rpm;
}


void hall_sensor_handler(void) {
	u8 state_now = get_hall_stat(HALL_READ_TIMES);
	float theta_now = _hall_table[state_now];
	u8 state_prev = _hall.state;
	float theta_prev = _hall.theta;
	if (!_hall.working) {		
		if(theta_now != 0xFFFF) {
			_hall.working = true;
			_hall.state = state_now;
			_hall.theta = theta_now;
			_hall.ticks = task_ticks_abs();
		}
		return;
	}
	//printf("hall %d, %d\n", state_now, state_prev);

	float delta_theta = 360.0f;
	switch (state_now) {
		case STATE_1:
			if (state_prev == STATE_5) {
				_hall.direction = POSITIVE;
				delta_theta = theta_now - theta_prev;
			}else if (state_prev == STATE_3) {
				_hall.direction = NEGATIVE;
				delta_theta = 360 - theta_now + theta_prev;
			}
			break;
		case STATE_2:
			if (state_prev == STATE_3) {
				_hall.direction = POSITIVE;
				delta_theta = theta_now - theta_prev;
			}else if (state_prev == STATE_6) {
				_hall.direction = NEGATIVE;
				delta_theta = theta_prev - theta_now;
			}
			break;
		case STATE_3:
			if (state_prev == STATE_1) {
				_hall.direction = POSITIVE;
				delta_theta = 360 - theta_prev + theta_now;
			}else if (state_prev == STATE_2) {
				_hall.direction = NEGATIVE;
				delta_theta = theta_prev - theta_now;
			}
			break;
		case STATE_4:
			if (state_prev == STATE_6) {
				_hall.direction = POSITIVE;
				delta_theta = theta_now - theta_prev;
			}else if (state_prev == STATE_5) {
				_hall.direction = NEGATIVE;
				delta_theta = theta_prev - theta_now;
			}
			break;
		case STATE_5:
			if (state_prev == STATE_4) {
				_hall.direction = POSITIVE;
				delta_theta = theta_now - theta_prev;
			}else if (state_prev == STATE_1) {
				_hall.direction = NEGATIVE;
				delta_theta = theta_prev - theta_now;
			}
			break;
		case STATE_6:
			if (state_prev == STATE_2) {
				_hall.direction = POSITIVE;
				delta_theta = theta_now - theta_prev;
			}else if (state_prev == STATE_4) {
				_hall.direction = NEGATIVE;
				delta_theta = theta_prev - theta_now;
			}
			break;
		default:
			break;
	}
	if (delta_theta == 360.0f) { //no vilid hall 
		return;
	}

	float delta_time = tick_2_s(delta_ticks(_hall.ticks));
	if (delta_time == 0.0f) { //may be errors ???
		return;
	}
	_hall.degree_per_s = delta_theta / delta_time;
	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, state_now);
	_hall.ticks = task_ticks_abs();
	_hall.theta = theta_now;
	_hall.state = state_now;
	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
}