#include <string.h>
#include "bsp/bsp_driver.h"
#include "os/os_task.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "math/fast_math.h"
#include "foc/motor/hall.h"
#include "app/nv_storage.h"
#include "libs/time_measure.h"
#include "app/nv_storage.h"
#include "libs/logger.h"

//#define USE_DETECTED_ANGLE 1

#define HALL_READ_TIMES 9

static u32 _hall_detect_task(void *args);
static void _hall_init_el_angle(void);


//#define HALL_PLACE_OFFSET (360-25)//(230) //(345) //315
#define HALL_PLACE_OFFSET (230)

/* 
4,5,1,3,2,6,4
*/

hall_t _sensor_hander;

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) //s32q14

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

static u32 __inline__ _hall_get_angle_ticks(void) {
	hall_sample_t *s = &_sensor_hander.samples;
	if (!s->full) {
		return s->ticks[s->index-1];
	}else {
		return s->ticks_sum/SAMPLE_MAX_COUNT;
	}
}

static float __inline _hall_angle_speed(void){
	hall_sample_t *s = &_sensor_hander.samples;
	if (s->ticks_sum == 0) {
		return 0.0f;
	}
	
	if (!s->full) {
		return s->angles[s->index - 1] / us_2_s(s->ticks[s->index-1]);
	}else {
		return s->angles_sum / us_2_s(s->ticks_sum);
	}
}

void hall_debug_log(void) {
	sys_debug("angle dir %d\n", _sensor_hander.direction);
}

/*
static bool __inline _hall_data_empty(void) {
	hall_sample_t *s = &_sensor_hander.samples;
	if ((!s->full) && (s->index == 0)){
		return true;
	}
	return false;
}
*/

static void hall_sensor_default(s8 direction) {
	memset(&_sensor_hander, 0, sizeof(_sensor_hander));
	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
	for (int i = 0; i < 8; i++) {
		_sensor_hander.angle_table[i] = (nv_get_motor_params()->hall_table[i]);
	}
	_sensor_hander.manual_angle = 0x3FF;
	_sensor_hander.direction = direction;
	_sensor_hander.running_dir = _sensor_hander.direction;
	_hall_init_el_angle();
}

void hall_sensor_init(void) {
	hall_sensor_default(POSITIVE);
	mc_hall_init();
	shark_task_create(_hall_detect_task, NULL);
}

void hall_set_direction(s8 direction) {
	hall_sensor_default(direction);
}

void hall_sensor_clear(void) {
	hall_set_direction(POSITIVE);
}

static u32 _hall_detect_task(void *args) {
	if (_sensor_hander.el_speed != 0) {
		u32 ticks_now = task_ticks_abs();
		if (ticks_now > _sensor_hander.hall_ticks) {
			if (task_delta_ticks(ticks_now) > 2000*1000) {
				hall_sensor_clear();
			}
		}
	}
	return 0;
}

#if 1
int hall_e_count = 0;
float hall_sensor_get_theta(bool detect_err){
	hall_e_count++;
	float angle_add = _sensor_hander.delta_angle_ts;
	if (_sensor_hander.comp_count > 0) {
		_sensor_hander.comp_count--;
		angle_add += _sensor_hander.angle_comp_ts;
	}
	_sensor_hander.estimate_delta_angle += angle_add;

	float el_angle = _sensor_hander.estimate_delta_angle;
	if (el_angle > _sensor_hander.next_delta_angle) {
		el_angle = _sensor_hander.next_delta_angle;
	}
	if (_sensor_hander.direction == POSITIVE) {
		el_angle = _sensor_hander.estimate_el_angle + el_angle;
	}else {
		el_angle = _sensor_hander.estimate_el_angle - el_angle;
	}
	rand_angle(el_angle);
	if (hall_e_count%5 == 0) {
		plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
	}
	return (el_angle);
}
#else
static int test_loop = 0;
float hall_sensor_get_theta(void){
	float angle_add = _sensor_hander.delta_angle_ts;
	float comp_angle = 0;
	if (_sensor_hander.comp_count > 0) {
		_sensor_hander.comp_count--;
		comp_angle = _sensor_hander.angle_comp_ts;
	}

	if (_sensor_hander.direction == POSITIVE) {
		_sensor_hander.estimate_delta_angle += angle_add;
		_sensor_hander.estimate_el_angle += (angle_add + comp_angle);
	}else {
		_sensor_hander.estimate_delta_angle -= angle_add;
		_sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
	}
	rand_angle(_sensor_hander.estimate_el_angle);
	test_loop ++;
	if (test_loop % 20 == 0) {
		//plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
	}
	return ( _sensor_hander.estimate_el_angle);
}

#endif

float hall_sensor_get_speed(void) {
	return (_sensor_hander.rpm) * _sensor_hander.running_dir;
}

int hall_offset_increase(int inc) {
	inc = inc << 19;
	if (_sensor_hander.phase_offset + inc >= PHASE_360_DEGREE) {
		_sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - PHASE_360_DEGREE;
	}else {
		_sensor_hander.phase_offset += inc;
	}
	return _sensor_hander.phase_offset;
}

s32 *hall_get_table(void) {
	return _sensor_hander.angle_table;
}

static float sin_hall[8];
static float cos_hall[8];
static int hall_iterations[8];
void hall_detect_angle(s16 angle) {
	_sensor_hander.manual_angle = (angle);
	int hall = get_hall_stat(HALL_READ_TIMES);
	float s, c;
	normal_sincosf(degree_2_pi(angle), &s, &c);
	sin_hall[hall] += s;
	cos_hall[hall] += c;
	hall_iterations[hall]++;
}

bool hall_detect_angle_finish(void) {
	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);
			_sensor_hander.angle_table[i] = (ang);
		} else {
			_sensor_hander.angle_table[i] = (0x3FF);
			fails++;
		}
	}
	if (fails == 2) {
		nv_save_hall_table(_sensor_hander.angle_table);
	}
	memset(sin_hall, 0, sizeof(sin_hall));
	memset(cos_hall, 0, sizeof(cos_hall));
	memset(hall_iterations, 0, sizeof(hall_iterations));	
	return fails == 2;
}

void hall_detect_offset(s16 angle) {
	_sensor_hander.manual_angle = (angle);
}

bool hall_detect_offset_finish(void) {
	int fails = 0;
	for(int i = 0;i < 8;i++) {
		if (hall_iterations[i] > 20) {
			_sensor_hander.angle_table[i] = _sensor_hander.angle_table[i] / hall_iterations[i];
		}
	}
	return (fails == 2);
}
static void _hall_init_el_angle(void) {
	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
#ifdef USE_DETECTED_ANGLE
	if (_sensor_hander.hall_stat == 0 || _sensor_hander.hall_stat == 7) {
		_sensor_hander.sensor_error ++;
		return;
	}
	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + _sensor_hander.angle_table[_sensor_hander.hall_stat];
#else
	s32 sector_center = PHASE_60_DEGREE/2;
  	switch ( _sensor_hander.hall_stat )
  	{
    case STATE_5:
      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + sector_center;
      	break;
    case STATE_1:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + sector_center;
      	break;
    case STATE_3:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + sector_center;
      	break;
    case STATE_2:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + sector_center;
      	break;
    case STATE_6:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + sector_center;
      	break;
    case STATE_4:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + sector_center;
      	break;
    default:
      	/* Bad hall sensor configutarion so update the speed reliability */
      	_sensor_hander.sensor_error ++;
      	return;
 	}
#endif
	_sensor_hander.sensor_error = 0;
  	/* Initialize the measured angle */
	_sensor_hander.measured_el_angle += PHASE_60_DEGREE;
	rand_angle(_sensor_hander.measured_el_angle);
  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
	_sensor_hander.hall_ticks = task_ticks_abs();
}

static __inline__ float _get_angle(u8 state, float added) {
#ifdef USE_DETECTED_ANGLE
	return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
#else
	return _sensor_hander.phase_offset + added;
#endif
}
/* 4,5,1,3,2,6,4 */
static float _hall_position(u8 state_now, u8 state_prev) {
	float theta_now =  0xFFFFFFFF;
	switch (state_now) {
		case STATE_1:
			if (state_prev == STATE_5) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
			}else if (state_prev == STATE_3) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
			}
			break;
		case STATE_2:
			if (state_prev == STATE_3) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
			}else if (state_prev == STATE_6) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
			}
			break;
		case STATE_3:
			if (state_prev == STATE_1) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
			}else if (state_prev == STATE_2) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
			}
			break;
		case STATE_4:
			if (state_prev == STATE_6) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
			}else if (state_prev == STATE_5) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
			}
			break;
		case STATE_5:
			if (state_prev == STATE_4) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
			}else if (state_prev == STATE_1) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
			}
			break;
		case STATE_6:
			if (state_prev == STATE_2) {
				_sensor_hander.running_dir = POSITIVE;
				theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
			}else if (state_prev == STATE_4) {
				_sensor_hander.running_dir = NEGATIVE;
				theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
			}
			break;
		default:
			_sensor_hander.sensor_error ++;
			return 0xFFFFFFFF;
	}
	if (theta_now != 0xFFFFFFFF) {
		rand_angle(theta_now);
	}
	return theta_now;
}

#ifdef USE_DETECTED_ANGLE
static __inline u8 _next_hall(u8 hall_now) {
	switch (hall_now) {
		case STATE_1:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_3;
			}else {
				return STATE_5;
			}
		case STATE_2:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_6;
			}else {
				return STATE_3;
			}
		case STATE_3:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_2;
			}else {
				return STATE_1;
			}
		case STATE_4:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_5;
			}else {
				return STATE_6;
			}
		case STATE_5:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_1;
			}else {
				return STATE_4;
			}
		case STATE_6:
			if (_sensor_hander.direction == POSITIVE) {
				return STATE_4;
			}else {
				return STATE_2;
			}
		default: //not reached here
			return STATE_1;
	}
} 


static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
	s32 delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
	if (_sensor_hander.direction == POSITIVE) {
		if (delta_angle < 0) { //process cross 360 degree
			delta_angle += PHASE_360_DEGREE;
		}
	}else if (_sensor_hander.direction == NEGATIVE) {
		if (delta_angle > 0) { //process cross 360 degree
			delta_angle -= PHASE_360_DEGREE;
		}
		delta_angle = -delta_angle;
	}
	return delta_angle;
}
#endif

void HALL_IRQHandler(void) {
	time_measure_start(&g_meas_hall);
	u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
	u8 hall_stat_prev = _sensor_hander.hall_stat;
	u32 hall_ticks_now = task_ticks_abs();	

	/*获取当前转子角度*/
	float theta_now = _hall_position(hall_stat_now, hall_stat_prev);
	if (theta_now == 0xFFFFFFFF) {
		return;
	}
	//plot_2data16(hall_stat_now*60, _sensor_hander.manual_angle);
	//获取两次中断的时间间隔，估计速度
	u32 delta_us = task_delta_ticks(hall_ticks_now);
	if (delta_us == 0) {
		return;
	}
	_sensor_hander.hall_ticks = hall_ticks_now;
	//获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
#ifdef USE_DETECTED_ANGLE
	float delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
	float next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
#else
	float delta_angle = PHASE_60_DEGREE;
	float next_delta_angle = delta_angle;
#endif
	float delta_time = us_2_s(delta_us);
	float prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
	float delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
	if (delta_el_speed*100/prev_imme_el_speed >= 40) { //即时速度增加40%,认为不稳定，需要使用即时速度估计转子位置
		_sensor_hander.trns_detect = true;
	}else {
		_sensor_hander.trns_detect = false;
	}
	_hall_put_sample(delta_us, delta_angle);
	u32 mask = cpu_enter_critical();
	if (_sensor_hander.samples.full) {
		//float estimate_delta_angle =  _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
		//plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.estimate_delta_angle>>19);
		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
		delta_us = _hall_get_angle_ticks();
		//_sensor_hander.comp_count = 50;
		//_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
		_sensor_hander.estimate_el_angle = theta_now;
	}else {
		_sensor_hander.comp_count = 0;
		_sensor_hander.angle_comp_ts = 0;
		_sensor_hander.estimate_el_angle = theta_now;
	}
	_sensor_hander.estimate_delta_angle = 0;
	_sensor_hander.delta_angle_ts = (next_delta_angle/(us_2_s(delta_us)/FOC_CTRL_US));
	_sensor_hander.next_delta_angle = next_delta_angle;
	_sensor_hander.measured_el_angle = theta_now;

	cpu_exit_critical(mask);
	_sensor_hander.hall_stat = hall_stat_now;
	_sensor_hander.hall_ticks = hall_ticks_now;
	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
	_sensor_hander.rpm = (_sensor_hander.el_speed / 360 * 60) * nv_get_motor_params()->poles; //s32q5
	//plot_3data16(_sensor_hander.rpm >> 5, _sensor_hander.el_speed>>5, delta_us);
	time_measure_end(&g_meas_hall);

	//plot_3data16(delta_us/10, hall_stat_prev * 10, _sensor_hander.hall_stat * 10);
}