#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 "foc/mc_config.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
#define SMOOTH_COUNT 0

/* 
1,5,4,6,2,3
0,1,2,3,4,5
//////
2,6,4,5,1,3
0,1,2,3,4,5
////
2,3,1,5,4,6
0,1,2,3,4,5
*/
static s8 hall_2_pos[] = {7,2,0,1,4,3,5,7};
static hall_t g_hall;

measure_time_t g_meas_hall = {.exec_max_time = 6,};

#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
#define HALL_MAX_TIME 1000000UL

static u32 stop_cnt = 0;
static u32 g_delta_cnt = 0;
static u32 g_trns_cnt = 0;
static u32 g_min_delta;
//static float g_low_off[6];
//static u32 g_low_off_cnt[6];
static u8 __INLINE hall_read_state(void) {
	u8 hall_a = 0, hall_b = 0, hall_c = 0;
	for (int i = 0; i < HALL_READ_TIMES; i++) {
		hall_a += gpio_hall_a_value();
		hall_b += gpio_hall_b_value();
		hall_c += gpio_hall_c_value();
	}
	u8 state = 0;
	if (hall_a > (HALL_READ_TIMES/2 + 1)) {
		state = 1;
	}
	if (hall_b > (HALL_READ_TIMES/2 + 1)) {
		state = state | (1<<1);
	}
	if (hall_c > (HALL_READ_TIMES/2 + 1)) {
		state = state | (1 << 2);
	}
	return state;
}

static void hall_init_low_pos(void) {
	u8 state = hall_read_state();
	s16 pos = hall_2_pos[state];
	if (pos == 7) {
		g_hall.sig_errors ++;
		return;
	}
	g_hall.state = state;
	if (g_hall.dir == POSITIVE) {
		g_hall.low_res_pos = pos + 0.5f;
	}else {
		g_hall.low_res_pos = pos - 0.5f;
	}
}


static void __INLINE hall_put_sample(u32 ticks) {
	hsample_t *s = &g_hall.samples;
	g_hall.last_delta_ticks = ticks;
	s->ticks_sum -= s->ticks[s->index];
	s->ticks[s->index] = ticks;
	s->ticks_sum += s->ticks[s->index];
	s->index += 1;
	if (s->filled < SAMPLE_MAX_COUNT) {
		s->filled++;
	}
	if (s->index >= SAMPLE_MAX_COUNT) {
		s->index = 0;
	}
}


static float __INLINE hall_elec_angle_vel(void){
	hsample_t *s = &g_hall.samples;
	if (s->filled < SAMPLE_MAX_COUNT) {
		return (PHASE_60_DEGREE) / us_2_s(g_hall.last_delta_ticks);
	}
	if (s->ticks_sum == 0) {
		return 0.0f;
	}
	return (PHASE_60_DEGREE * SAMPLE_MAX_COUNT) / us_2_s(s->ticks_sum);
}

void hall_debug_log(void) {
	sys_debug("angle dir %d, stat %d, lowres %f, err %d,%d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors);
	sys_debug("D: %d,%d,%d,%d\n", stop_cnt, g_delta_cnt, g_trns_cnt, g_min_delta);
	sys_debug("RPM: %f\n", hall_get_velocity());
#if 0
	if (g_low_off_cnt[0] < 10) {
		return;
	}
	int value[6];
	u32 mask = cpu_enter_critical();
	for (int i = 0; i < 6; i++) {
		value[i] = (int)(g_low_off[i]/(float)g_low_off_cnt[i]);
		g_low_off[i] = 0;
		g_low_off_cnt[i] = 0;
	}
	cpu_exit_critical(mask);
	sys_debug("off:%d,%d,%d,%d,%d,%d\n", value[0], value[1], value[2], value[3], value[4], value[5]);
#endif
}

void hall_init(void) {
	g_hall.phase_offset = mc_conf()->m.encoder_offset;
	g_hall.mot_poles = mc_conf()->m.poles;
	g_hall.b_trns_det = false;
	g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
	g_hall.angle_smooth_step = 0;
	g_hall.samples.ticks_sum = 0;
	g_hall.position = 0;
	g_hall.samples.index = 0;
	g_hall.samples.filled = 0;
	g_hall.elec_angle_vel = 0;
	g_hall.prev_dir = g_hall.dir = POSITIVE;
	g_min_delta = 1000000 * 100;
	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
		g_hall.samples.ticks[i] = HALL_MAX_TIME;
	}
	for (int i = 0; i < 6; i++) {
		//g_low_off[i] = 0;
		//g_low_off_cnt[i] = 0;
	}
	g_hall.samples.ticks_sum = HALL_MAX_TIME * SAMPLE_MAX_COUNT;

	if (!g_hall.inited) {
		g_hall.inited = true;
		gpio_hall_init();
	}
	hall_init_low_pos();
	stop_cnt = 0;
}

void  hall_set_direction(s8 dir) {
	if (dir == g_hall.dir) {
		return;
	}
	g_hall.dir = g_hall.prev_dir = dir;
	hall_init_low_pos();
}

#if SMOOTH_COUNT > 0
static float get_angle_diff(float a1, float a2) {
	float diff = a1 - a2;
	float abs_diff = ABS(diff);
	if (abs_diff >= PHASE_180_DEGREE) {
		return (PHASE_360_DEGREE - abs_diff);
	}else {
		return diff;
	}
}
#endif

static __INLINE bool hall_update_low_pos(void) {
	u8 state = hall_read_state();
	s16 pos = hall_2_pos[state];
	if (pos == 7) {
		g_hall.sig_errors ++;
		return false;
	}
	s16 pos_prev = hall_2_pos[g_hall.state];
	s16 delta_pos = pos - pos_prev;
	if (delta_pos != 0) {
		s8  prev_dir = g_hall.dir;
		if (delta_pos == 1 || delta_pos == -5) {
			g_hall.dir = POSITIVE;
			g_hall.prev_dir = prev_dir;
		}else if (delta_pos == -1 || delta_pos == 5){
			g_hall.dir = NEGATIVE;
			g_hall.prev_dir = prev_dir;
		}else {
			if (g_hall.samples.filled > 0) {
				g_hall.sig_errors ++;
				return false;
			}
		}
		g_hall.edge_ticks = task_ticks_abs();
	}else {
		g_hall.sig_errors ++;
		return false;
	}	
	g_hall.state = state;
	
	g_hall.low_res_pos = pos;

	return true;
}

hall_t *hall_get(void) {
	return &g_hall;
}

float hall_get_elec_angle(void) {
	float phase_offset = g_hall.phase_offset;
	if (g_hall.dir == NEGATIVE) {
		phase_offset = PHASE_60_DEGREE + phase_offset;
	}
	float angle = g_hall.elec_angle + phase_offset;
	norm_angle_deg(angle);
	return angle;
}

float hall_update_elec_angle(void) {
	float delta_ticks = (float)time_delta_us(g_hall.edge_ticks, NULL);//上次hall变换到目前的时间
	float low_res = g_hall.low_res_pos;
	float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
	if (delta_pos > 1.0f) {
		delta_pos = 1.0f;
	}else if (delta_pos < -1.0f) {
		delta_pos = -1.0f;
	}
	float high_res_pos = delta_pos  + low_res;
	float elec_angle = high_res_pos * PHASE_60_DEGREE;
	float elec_smooth_angle;
#if SMOOTH_COUNT>0
	float delta_angle = delta_pos * PHASE_60_DEGREE;
	if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
		elec_smooth_angle = g_hall.elec_angle_edge + g_hall.angle_smooth_step * g_hall.angle_smooth_cnt + delta_angle;
		g_hall.angle_smooth_cnt++;
		if (g_hall.angle_smooth_step >= 0) {
			elec_smooth_angle = min(elec_smooth_angle, elec_angle);
		}else {
			elec_smooth_angle = MAX(elec_smooth_angle, elec_angle);
		}
	}else {
		elec_smooth_angle = elec_angle;
	}
#else
	elec_smooth_angle = elec_angle;
#endif
	norm_angle_deg(elec_smooth_angle);
	g_hall.elec_angle = elec_smooth_angle;
	g_hall.position += g_hall.elec_angle_vel * FOC_CTRL_US / g_hall.mot_poles;
	if ((g_hall.samples.filled > 1) && (delta_ticks / g_hall.last_delta_ticks >= 1.4f)) {
		stop_cnt ++;
		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.95f;
		if (g_hall.elec_angle_vel < 30) {
			g_hall.elec_angle_vel = 0;
		}
		float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
		g_hall.velocity_raw = velocity_raw;
		LowPass_Filter(g_hall.velocity_filted, velocity_raw, 1.0f);
	}
	return hall_get_elec_angle();
}

float hall_get_velocity(void) {
	return g_hall.velocity_filted;
}

float hall_get_position(void) {
	return g_hall.position;
}

static __INLINE void hall_calc_mot_velocity(u32 delta_cnt) {
	if (g_hall.samples.filled == 0) {
		hall_put_sample(HALL_MAX_TIME/10);
		return;
	}
	if (delta_cnt < g_min_delta) {
		g_min_delta = delta_cnt;
	}
	hall_put_sample(delta_cnt);
	g_hall.elec_angle_vel = hall_elec_angle_vel();
	float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
	g_hall.velocity_raw = velocity_raw;
	LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.9F);
}


float hall_offset_detect(float *off) {
	return 0.0f;
}
extern float foc_observer_sensorless_angle(void);
void HALL_IRQHandler(void) {
	g_delta_cnt++;
	//int pos_int;
	//float delta;
	u32 mask = cpu_enter_critical();
	u32 delta_cnt = time_delta_us(g_hall.edge_ticks, NULL);
	if (delta_cnt < 300) {
		g_hall.noise_errors++;
		goto hall_end;
	}
	if (!hall_update_low_pos()) {
		goto hall_end;
	}
#if 0
	pos_int = g_hall.low_res_pos;
	delta = g_hall.low_res_pos * 60.0f - foc_observer_sensorless_angle();
	norm_angle_deg(delta);
	g_low_off_cnt[pos_int]++;
	g_low_off[pos_int] += delta;
#endif
	g_hall.elec_angle_edge = g_hall.elec_angle;
#if SMOOTH_COUNT>0
	g_hall.delta_angle_edge = get_angle_diff(g_hall.low_res_pos * PHASE_60_DEGREE, g_hall.elec_angle_edge);
	if (ABS(g_hall.delta_angle_edge) >= 2.0f) {
		g_hall.angle_smooth_step = 0;//g_hall.delta_angle_edge/SMOOTH_COUNT;
		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
	}else {
		g_hall.angle_smooth_step = 0;
		g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
	}
#endif
	hall_calc_mot_velocity(delta_cnt);
hall_end:
	cpu_exit_critical(mask);
	return;
}