#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);
static void _hall_init_el_angle(void);

#define PWM_T  (0.000033f)

#define HALL_PLACE_OFFSET (199)
/* 
100
101
001
011
010
110
4,5,1,3,2,6,4
*/

static hall_sensor_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)


static void __inline _hall_put_sample(u32 ticks) {
	hall_sample_t *s = &_sensor_hander.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 = &_sensor_hander.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(&_sensor_hander, 0, sizeof(_sensor_hander));
	_sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
	_hall_init_el_angle();
}

void hall_sensor_init(void) {
	mc_hall_init();
	hall_sensor_default();
	co_task_create(_hall_detect_task, NULL, 512);
}

void hall_sensor_clear(void) {
	hall_sensor_default();
}

static void _hall_detect_task(void *args) {
	while(1) {
		if (_sensor_hander.el_speed != 0) {
			u32 ticks_now = timer_count32_get();
			u32 delta_us = timer_count32_getus(ticks_now, _sensor_hander.hall_ticks);
			if (delta_us >= (1200*1000)) {
				hall_sensor_clear();
			}
		}
		co_task_delay(100);
	}
}

float hall_sensor_get_theta(void){
	if (_sensor_hander.is_override_angle) {
		return _sensor_hander.override_el_angle;
	}

	float angle_step = _sensor_hander.el_speed * PWM_T;
	float compensated_step = angle_step + _sensor_hander.el_speed_compensate * PWM_T * 0.05f;
	_sensor_hander.estimate_delta_angle += compensated_step;
#if 1
	if (_sensor_hander.direction == POSITIVE) {
		_sensor_hander.estimate_el_angle += compensated_step;
	}else {
		_sensor_hander.estimate_el_angle -= compensated_step;
	}
#else
	if (_sensor_hander.estimate_delta_angle >= PHASE_60_DEGREE) {
		if (_sensor_hander.direction == POSITIVE) {
			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle + PHASE_60_DEGREE;
		}else {
			_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle - PHASE_60_DEGREE;
		}
	}
#endif
	return _sensor_hander.estimate_el_angle;
}


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

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

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

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

static void _hall_init_el_angle(void) {
	_sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
  	switch ( _sensor_hander.hall_stat )
  	{
    case STATE_5:
      	_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE/2;
      	break;
    case STATE_1:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + PHASE_60_DEGREE / 2;
      	break;
    case STATE_3:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + PHASE_60_DEGREE / 2;
      	break;
    case STATE_2:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_120_DEGREE - PHASE_60_DEGREE / 2;
      	break;
    case STATE_6:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE - PHASE_60_DEGREE / 2;
      	break;
    case STATE_4:
		_sensor_hander.measured_el_angle = _sensor_hander.phase_offset - PHASE_60_DEGREE / 2;
      	break;
    default:
      	/* Bad hall sensor configutarion so update the speed reliability */
      	_sensor_hander.sensor_error = true;
      	break;
 	}
  	/* Initialize the measured angle */
  	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
	_sensor_hander.hall_ticks = timer_count32_get();
}

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) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
			}else if (state_prev == STATE_3) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
			}
			break;
		case STATE_2:
			if (state_prev == STATE_3) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
			}else if (state_prev == STATE_6) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
			}
			break;
		case STATE_3:
			if (state_prev == STATE_1) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_120_DEGREE;
			}else if (state_prev == STATE_2) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_180_DEGREE;
			}
			break;
		case STATE_4:
			if (state_prev == STATE_6) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
			}else if (state_prev == STATE_5) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset;
			}
			break;
		case STATE_5:
			if (state_prev == STATE_4) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset;
			}else if (state_prev == STATE_1) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset + PHASE_60_DEGREE;
			}
			break;
		case STATE_6:
			if (state_prev == STATE_2) {
				_sensor_hander.direction = POSITIVE;
				theta_now = _sensor_hander.phase_offset - PHASE_120_DEGREE;
			}else if (state_prev == STATE_4) {
				_sensor_hander.direction = NEGATIVE;
				theta_now = _sensor_hander.phase_offset - PHASE_60_DEGREE;
			}
			break;
		default:
			return 0xFFFFFFFF;
	}
	return theta_now;
}

void hall_sensor_handler(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 = timer_count32_get();	

	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
	if (theta_now == 0xFFFFFFFF) {
		return;
	}

	u32 delta_us = timer_count32_getus(hall_ticks_now, _sensor_hander.hall_ticks);
	if (delta_us == 0) {
		return;
	}

	float delta_time = us_2_s(delta_us);
	_hall_put_sample(delta_us);
	if (_sensor_hander.samples.full) {
		_sensor_hander.el_speed_avg = _hall_avg_speed();
		///_sensor_hander.estimate_angle_aliment = true;
	}else {
		_sensor_hander.el_speed_avg = (float)PHASE_60_DEGREE / delta_time;
	}

	os_disable_irq();
	
	_sensor_hander.measured_el_angle = theta_now;	
	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;

	if (_sensor_hander.estimate_angle_aliment) {
		float delta_angle = (float)PHASE_60_DEGREE - _sensor_hander.estimate_delta_angle;
		_sensor_hander.el_speed_compensate = delta_angle / delta_time;
	}else {
		_sensor_hander.el_speed_compensate = 0.0f;
	}
	_sensor_hander.estimate_delta_angle = 0.0f;
	_sensor_hander.el_speed = _sensor_hander.el_speed_avg;

	os_enable_irq();
	
	_sensor_hander.hall_stat = hall_stat_now;
	_sensor_hander.hall_ticks = hall_ticks_now;
	
	time_measure_end(&g_meas_hall);
}