#include <string.h>
#include "bsp/bsp.h"
#include "bsp/mc_hall_gpio.h"
#include "os/co_task.h"
#include "os/timer.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "math/fast_math.h"
#include "hall_sensor.h"
#include "foc/foc_api.h"
#include "app/nv_storage.h"
#include "bsp/timer_count32.h"


#define HALL_READ_TIMES 3

static void _hall_detect_task(void *args);

#define PWM_T  (0.00003333f)

/* 
* 测量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 210
/* 
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[] = {THETA_NONE, 121/*1*/, 240/*2*/, 190/*3*/, 13/*4*/, 58/*5*/, 306/*6*/, THETA_NONE};
static hall_t _hall;


measure_time_t g_meas_hall = {.exec_max_time = 6,};

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


static void __inline _hall_put_sample(u32 ticks) {
	hall_sample_t *s = &_hall.samples;
	s->ticks_sum -= s->ticks[s->index];
	s->ticks[s->index] = ticks;
	s->ticks_sum += s->ticks[s->index];
	
	s->index += 1;
	if (s->index >= SAMPLE_MAX_COUNT) {
		s->full = true;
		s->index = 0;
	}
}

static float __inline _hall_avg_speed(void){
	hall_sample_t *s = &_hall.samples;
	if (s->ticks_sum == 0.0f) {
		return 0.0f;
	}
	return (((float)PHASE_60_DEGREE * (float)SAMPLE_MAX_COUNT) / (us_2_s(s->ticks_sum)));
}

static void hall_sensor_default(void) {
	memset(&_hall, 0, sizeof(_hall));
	_hall.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
}

void hall_sensor_init(void) {
	mc_hall_init();
	hall_sensor_default();
	read_hall(_hall.state, _hall.measured_el_angle);
	co_task_create(_hall_detect_task, NULL, 512);
}

void hall_sensor_clear(void) {
	hall_sensor_default();
	read_hall(_hall.state, _hall.measured_el_angle);
}

static void _hall_detect_task(void *args) {
	while(1) {
		if (_hall.working) {
			u32 ticks_now = timer_count32_get();
			u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
			if (delta_us >= (1200*1000)) {
				hall_sensor_clear();
			}
		}
		co_task_delay(100);
	}
}


u16 *hall_phase_angle(void) {
	return _hall_table;
}

float hall_sensor_get_theta(void){
	if (_hall.is_override_angle) {
		return _hall.override_el_angle;
	}
	if (!_hall.working) {
		read_hall(_hall.state, _hall.measured_el_angle);
		return _hall.measured_el_angle;
	}
	os_disable_irq();
	float angle_step = _hall.el_speed_avg * PWM_T;
	float compensated_step = angle_step + _hall.el_speed_compensate * PWM_T;
	if (_hall.estimate_delta_angle <= PHASE_60_DEGREE) {
		_hall.estimate_delta_angle += compensated_step;
		if (_hall.direction == POSITIVE) {
			_hall.estimate_el_angle += compensated_step;
			if (_hall.estimate_el_angle > PHASE_360_DEGREE) {
				_hall.estimate_el_angle -= PHASE_360_DEGREE;
			}
		}else {
			_hall.estimate_el_angle -= compensated_step;
			if (_hall.estimate_el_angle < 0) {
			_hall.estimate_el_angle += PHASE_360_DEGREE;
			}
		}
	}
	os_enable_irq();
	return _hall.estimate_el_angle;
}

/*
两个hall中断之间估计的走过的角度减去60度，就是估计偏差，通过偏差修正当前的速度,
如果估计的比实际的小，当前速度需要加上一个修正值，如果比实际大，就要减去一个修正值
*/
static __inline__ void _hall_obsever_correct(float delta_s) {
	float delta_angle = 0;
	if (_hall.estimate_angle_aliment) {
		delta_angle = (float)PHASE_60_DEGREE - _hall.estimate_delta_angle;
		_hall.el_speed_compensate = delta_angle / delta_s;
		_hall.estimate_el_angle = _hall.measured_el_angle;
	}else {
		_hall.estimate_el_angle = _hall.measured_el_angle;
		_hall.el_speed_compensate = 0.0f;
	}
	_hall.estimate_delta_angle = 0.0f;
}

void hall_sensor_set_theta(bool override, float theta){
	_hall.is_override_angle = override;
	_hall.override_el_angle = theta;
}

float hall_sensor_get_speed(void) {
	_hall.rpm = _hall.el_speed / 360.0f * 60.0f;
	return _hall.rpm;
}

float hall_sensor_avg_speed(void) {
	return _hall.el_speed_avg / 360 * 60.0f;
}

int hall_offset_increase(int inc) {
	if (_hall.phase_offset + inc >= 360) {
		_hall.phase_offset = _hall.phase_offset + inc - 360;
	}else {
		_hall.phase_offset += inc;
	}
	return _hall.phase_offset;
}

int hall_sensor_calibrate(float voltage, u16 *hall_table){
	if (hall_table == NULL) {
		hall_table = _hall_table;
	}
	foc_set_controller_mode(FOC_MODE_OPEN_LOOP);
	hall_sensor_set_theta(true, 0.0f);
	foc_set_dq_command(0.0f, 0.0f);
	foc_pwm_start(true);
	for (int i = 0;i < 1000;i++) {
		foc_set_dq_command((float)i * voltage / 1000.0f, 0.0f);
		delay_ms(1);
	}
	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));
	delay_ms(2 * 1000);
	// Forwards
	for (int i = 0;i < 5;i++) {
		for (int j = 0;j < 360;j++) {
			hall_sensor_set_theta(true, j);
			delay_ms(5);

			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]++;
		}
	}
	delay_ms(2 * 1000);
	// Reverse
	for (int i = 0;i < 5;i++) {
		for (int j = 360;j >= 0;j--) {
			hall_sensor_set_theta(true, j);
			delay_ms(5);

			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);
	hall_sensor_set_theta(false, 0.0f);
	foc_set_dq_command(0.0f, 0.0f);
	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] = THETA_NONE;
			fails++;
		}
	}
	return fails == 2;	
}

static s32 _hall_position(u8 state_now, u8 state_prev) {
	s32 theta_now =  0xFFFFFFFF;
	switch (state_now) {
		case STATE_1:
			if (state_prev == STATE_5) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + PHASE_60_DEGREE;
			}else if (state_prev == STATE_3) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + PHASE_120_DEGREE;
			}
			break;
		case STATE_2:
			if (state_prev == STATE_3) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + PHASE_180_DEGREE;
			}else if (state_prev == STATE_6) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + PHASE_240_DEGREE;
			}
			break;
		case STATE_3:
			if (state_prev == STATE_1) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + PHASE_120_DEGREE;
			}else if (state_prev == STATE_2) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + PHASE_180_DEGREE;
			}
			break;
		case STATE_4:
			if (state_prev == STATE_6) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + PHASE_300_DEGREE;
			}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 + PHASE_60_DEGREE;
			}
			break;
		case STATE_6:
			if (state_prev == STATE_2) {
				_hall.direction = POSITIVE;
				theta_now = _hall.phase_offset + PHASE_240_DEGREE;
			}else if (state_prev == STATE_4) {
				_hall.direction = NEGATIVE;
				theta_now = _hall.phase_offset + PHASE_300_DEGREE;
			}
			break;
		default:
			return 0xFFFFFFFF;
	}
	if (theta_now >= 360) {
		theta_now -= 360;
	}
	return theta_now;
}

void hall_sensor_handler(void) {
	time_measure_start(&g_meas_hall);
	u8 state_now = get_hall_stat(HALL_READ_TIMES);
	u8 state_prev = _hall.state;
	u32 ticks_now = timer_count32_get();	
	if (!_hall.working) {		
		if(_hall_table[state_now] != 0xFFFF) {
			_hall.working = true;
			_hall.state = state_now;
			_hall.ticks = ticks_now;
			_hall.measured_el_angle = _hall_table[state_now];
			_hall.estimate_el_angle = _hall.measured_el_angle;
		}
		return;
	}
	s32 theta_now = _hall_position(state_now, state_prev);
	if (theta_now == 0xFFFFFFFF) {
		sys_debug("hall error\n");
		return;
	}

	u32 delta_us = timer_count32_getus(ticks_now, _hall.ticks);
	if (delta_us == 0) {
		sys_debug("hall time error\n");
		return;
	}

	os_disable_irq();
	
	float delta_time = us_2_s(delta_us);
	_hall_put_sample(delta_us);
	if (_hall.samples.full) {
		_hall.el_speed_avg = _hall_avg_speed();
		_hall.estimate_angle_aliment = true;
	}else {
		_hall.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
	}
	_hall.el_speed = _hall.el_speed_avg;
	_hall.measured_el_angle = theta_now;	
	_hall.state = state_now;
	_hall.ticks = ticks_now;

	_hall_obsever_correct(delta_time);

	os_enable_irq();

	time_measure_end(&g_meas_hall);
}