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

#define HALL_READ_TIMES 3
/* 
* 测量HALL_PLACE_OFFSET通用方式就是ST推荐的通过外力带动电机，
* 测量电机U相反电动势和hall 1的上升沿之间的差值
* 这里使用的是先通过hall_sensor_calibrate测量hall1，2，3，4，5，6
* 对应的角度(偏差比较大),然后启动电机，让HALL_PLACE_OFFSET
* 从0开始增加，每增加1度观察电机电流(看直流电源),
* 找到一个电机平稳转动并且电流最小的角度作为HALL_PLACE_OFFSET
*/
#define HALL_PLACE_OFFSET 213.0f
/* 
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 u16 _hall_table[] = {0xFFFF, 257/*1*/, 36/*2*/, 344/*3*/, 159/*4*/, 222/*5*/, 88/*6*/, 0xFFFF};
static hall_t _hall;
static hall_sample_t h_samples;

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

static void _hall_put_sample(float angle, u32 ticks) {
	hall_sample_t *s = &h_samples;
	s->index += 1;
	if (s->index >= SAMPLE_MAX_COUNT) {
		s->full = true;
		s->index = 0;
	}
	s->angle[s->index] = angle;
	s->ticks[s->index] = ticks;
}

static float __inline _hall_avg_speed(void){
	hall_sample_t *s = &h_samples;
	float t_angle = 0.0f;
	u32   t_ticks = 0;
	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
		t_angle += s->angle[i];
		t_ticks += s->ticks[i];
	}
	if (t_ticks == 0.0f) {
		return 0.0f;
	}
	return (t_angle / tick_2_s(t_ticks));
}

void hall_sensor_init(void) {
	memset(&_hall, 0, sizeof(_hall));
	read_hall(_hall.state, _hall.theta);
#ifdef HALL_PLACE_OFFSET	
	_hall.phase_offset = HALL_PLACE_OFFSET;
#endif
}


static void _detect_timeout(void){
	u32 now = get_seconds();
	if (now > _hall.second + 4) {//4s内没有霍尔中断，速度清零
		if (_hall.degree_per_s > 0) {
			_hall.degree_per_s = 0;
			_hall.e_rpm = _hall.e_filted_rpm = 0;
		}
	}
}

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


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

float hall_sensor_avg_speed(void) {
	_detect_timeout();
	return _hall.e_filted_rpm;
}


int hall_sensor_calibrate(float current, u16 *hall_table){
	foc_overide_set_theta(0.0f);
	foc_overide_theta(true);

	foc_overide_set_vdq(0.0f, 0.0f);
	foc_overide_vdq(true);
	
	foc_pwm_start(true);
	HAL_ADC1_InJ_StartConvert();
	for (int i = 0;i < 1000;i++) {
		foc_overide_set_vdq((float)i * current / 1000.0f, 0.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));
	task_udelay(50 * 1000);
	// Forwards
	for (int i = 0;i < 3;i++) {
		for (int j = 0;j < 360;j++) {
			foc_overide_set_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--) {
			foc_overide_set_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);
	foc_overide_theta(false);
	foc_overide_vdq(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;	
}

#ifdef HALL_PLACE_OFFSET
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;

	if (!_hall.working) {		
		if(theta_now != 0xFFFF) {
			_hall.working = true;
			_hall.state = state_now;
			_hall.theta = theta_now;
			_hall.ticks = task_ticks_abs();
		}
		return;
	}

	switch (state_now) {
		case STATE_1:
			if (state_prev == STATE_5) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + 60.0f;
			}else if (state_prev == STATE_3) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + 120.0f;
			}
			break;
		case STATE_2:
			if (state_prev == STATE_3) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + 180.0f;
			}else if (state_prev == STATE_6) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + 240.0f;
			}
			break;
		case STATE_3:
			if (state_prev == STATE_1) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + 120.0f;
			}else if (state_prev == STATE_2) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + 180.0f;
			}
			break;
		case STATE_4:
			if (state_prev == STATE_6) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + 300.0f;
			}else if (state_prev == STATE_5) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset;
			}
			break;
		case STATE_5:
			if (state_prev == STATE_4) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset;
			}else if (state_prev == STATE_1) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + 60.0f;
			}
			break;
		case STATE_6:
			if (state_prev == STATE_2) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + 240.0f;
			}else if (state_prev == STATE_4) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + 300.0f;
			}
			break;
		default:
			return;
	}


	float delta_time = tick_2_s(delta_ticks(_hall.ticks));
	if (delta_time == 0.0f) { //may be errors ???
		return;
	}
	float delta_theta = (_hall.direction == POSITIVE)?60.0f : -60.0f;
	_hall_put_sample(delta_theta, delta_ticks(_hall.ticks));
	if (!h_samples.full) {
		_hall.degree_per_s = delta_theta / delta_time;
	}else {
		_hall.degree_per_s = _hall_avg_speed();
		_hall.e_filted_rpm = _hall.degree_per_s / 360.0f * 60.0f; //电角速度
	}
	_hall.ticks = task_ticks_abs();
	_hall.second = get_seconds();
	_hall.theta = theta_now;
	_hall.state = state_now;
	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
}

#else
void hall_sensor_handler1(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);
	//{0xFFFF, 257/*1*/, 36/*2*/, 344/*3*/, 159/*4*/, 222/*5*/, 88/*6*/, 0xFFFF};
	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;
	}
	delta_theta = 60.0f;
	_hall_put_sample(delta_theta, delta_ticks(_hall.ticks));
	if (!h_samples.full) {
		_hall.degree_per_s = delta_theta / delta_time;
	}else {
		_hall.degree_per_s = _hall_avg_speed();
		_hall.e_filted_rpm = _hall.degree_per_s / 360.0f * 60.0f; //电角速度
	}
	_hall.ticks = task_ticks_abs();
	_hall.theta += delta_theta;
	_hall.state = state_now;
	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f; //电角速度
	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
}
#endif