#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

u32 stop_cnt = 0;
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;
	g_hall.prev_dir = g_hall.dir = POSITIVE;
	g_hall.low_res_pos = pos + 0.5f;
}


static void __INLINE hall_put_sample(u32 ticks, float angle) {
	hsample_t *s = &g_hall.samples;
	g_hall.last_delta_ticks = ticks;
	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 float __INLINE hall_elec_angle_vel(void){
	hsample_t *s = &g_hall.samples;
	if (s->ticks_sum == 0) {
		return 0.0f;
	}
	return s->angles_sum / us_2_s(s->ticks_sum);
}

void hall_debug_log(void) {
	sys_debug("angle dir %d, stat %d, lowres %f, err %d,%d, sp %d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.sig_errors, g_hall.noise_errors, stop_cnt);
}


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.samples.angles_sum = 0;
	g_hall.position = 0;
	g_hall.samples.full = false;
	g_hall.samples.index = 0;
	g_hall.elec_angle_vel = 0;
	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
		g_hall.samples.ticks[i] = 0;
		g_hall.samples.angles[i] = 0;
	}
	if (!g_hall.inited) {
		g_hall.inited = true;
		gpio_hall_init();
	}
	hall_init_low_pos();
	stop_cnt = 0;
}

#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 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];
	g_hall.state = state;
	s16 delta_pos = pos - pos_prev;
	g_hall.low_res_pos = pos;
	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 {
			//keep prev dir value
		}
		g_hall.edge_ticks = task_ticks_abs();
	}else {
		g_hall.sig_errors ++;
	}
	return true;
}

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

float hall_get_elec_angle(void) {
	float angle = g_hall.elec_angle + g_hall.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.full && (delta_ticks / g_hall.last_delta_ticks >= 1.3f)) {
		stop_cnt ++;
		g_hall.elec_angle_vel = g_hall.elec_angle_vel * 0.99f;
		if (g_hall.elec_angle_vel < 10) {
			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 void hall_calc_mot_velocity(u32 prev_ticks) {
	u32 delta_cnt = time_delta_us(prev_ticks, NULL);
	if (!g_hall.samples.full && g_hall.samples.index == 0) {
		if (delta_cnt <= 1000) {
			delta_cnt = 1000;
		}
	}
	hall_put_sample(delta_cnt, PHASE_60_DEGREE);
	float elec_vel;
	if (g_hall.b_trns_det) {
		elec_vel = PHASE_60_DEGREE/(us_2_s(delta_cnt));
		LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 0.5f);
	}else {
		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;
	float del_abs = ABS(velocity_raw - g_hall.velocity_raw);
	if (del_abs > 140) {
		g_hall.b_trns_det = true;
	}else if (del_abs < 100) {
		g_hall.b_trns_det = false;
	}
	g_hall.velocity_raw = velocity_raw;
	LowPass_Filter(g_hall.velocity_filted, velocity_raw, 1.0f);
}


float hall_offset_detect(float *off) {
	return 0.0f;
}

void HALL_IRQHandler(void) {
	u32 mask = cpu_enter_critical();
	u32 prev_ticks = g_hall.edge_ticks;
	if (!hall_update_low_pos()) {
		goto hall_end;
	}
	if (time_delta_us(prev_ticks, NULL) == 0) {
		g_hall.noise_errors++;
		goto hall_end;
	}
	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(prev_ticks);
hall_end:
	cpu_exit_critical(mask);
	return;
}