#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

static u16 _hall_table[] = {0xFFFF, 292/*1*/, 47/*2*/, 1/*3*/, 180/*4*/, 227/*5*/, 113/*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 (0xFFFFFFFF - 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) {
		return THETA_NONE;
	}
	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 0.0f;
}

void hall_sensor_handler(void) {
	u8 hall = get_hall_stat(HALL_READ_TIMES);
	float theta = _hall_table[hall];
	if (!_hall.working) {		
		if(theta != 0xFFFF) {
			_hall.working = true;
			_hall.state = hall;
			_hall.theta = theta;
			_hall.ticks = task_ticks_abs();
		}
		return;
	}

	if (theta == 0xFFFF || _hall.theta == theta) { //may be hall noise, drop it
		return;
	}
	float delta_theta = theta - _hall.theta;
	float theta_abs = abs(delta_theta);
	if (theta_abs > 70 || theta_abs < 40) { //may be hall noise, drop it
		return;
	}
	if (delta_theta < 0) {
		_hall.direction = NEGATIVE;
	}else {
		_hall.direction = POSITIVE;
	}
	float delta_time = tick_2_s(delta_ticks(_hall.ticks));
	if (delta_time == 0.0f) { //may be errors ???
		return;
	}
	_hall.degree_per_s = theta_abs / delta_time;

	_hall.ticks = task_ticks_abs();
	_hall.theta = theta;
}