#include <math.h>
#include "bsp/bsp_drvier.h"
#include "foc/motor/current.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/mc_error.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "math/fast_math.h"
current_samp_t g_cs;

#define NB_OFFSET_SAMPLES 32
#define SENSOR_SAMPLES 10000
#define Rvbus  0.0005f
#define Gvbus (13.1f) //母线电流的运放 
#define Rds_Defualt  0.005f//欧
#define Gmos  (1.7f)//mos 电流的运放
#define Sample_R Rds_Defualt
#define Lower_Pass_p 0.2f

#define VBUS_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gvbus)
#define MOSds_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gmos)

#define current_i(v, r) ((v)/(r))
/*
va = iv*kv - ia*k
vb = iv*kv - ib*k
vc = iv*kv - ic*k  -> vb+vc = 2iv*kv - (ib+ic)*k ->vb+vc = 2iv*kv + ia*k -->va+vb+vc = 
ia = -(ib+ic)
*/
void phase_current_init(void) {
	current_samp_t *cs = &g_cs;
	cs->sample_count = NB_OFFSET_SAMPLES + 1;
	cs->adc_ia = 0;
	cs->adc_ib = 0;
	cs->adc_ic = 0;
}

void phase_current_offset_calibrate(void){
	g_cs.adc_offset_a = 0;
	g_cs.adc_offset_b = 0;
	g_cs.adc_offset_c = 0;

	phase_current_init();
#ifdef HIGH_SIDE_CURRENT_SENSOR
	g_cs.c_phases = PHASE_BC;
#else
	g_cs.c_phases = PHASE_AB;
#endif
	g_cs.c_ignore_phase = IGNORE_NONE;
	adc_current_sample_config(g_cs.c_phases);
	g_cs.is_calibrating_offset = true;
}

void phase_current_calibrate_wait(void) {
	while(g_cs.is_calibrating_offset || g_cs.is_calibrating_sensor) {
		wdog_reload();
	}
}

#ifdef HIGH_SIDE_CURRENT_SENSOR
void phase_current_sensor_start_calibrate(float calibrate_current) {
	bool calibrate = false;
	if (calibrate_current > 0) {
		calibrate = true;
		g_cs.sensor_samples_1 = 0;
		g_cs.sensor_samples_2 = 0;
		g_cs.sample_count = SENSOR_SAMPLES + 1;
		g_cs.calibrate_current = calibrate_current;
	}
	
	g_cs.is_calibrating_sensor = calibrate;
}

bool phase_current_sensor_do_calibrate(void) {
	current_samp_t *cs = &g_cs;
	if (!cs->is_calibrating_sensor) {
		return false;
	}
	s32 phase_current1, phase_current2;
	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
	if(cs->c_phases == PHASE_BC) {
		if (cs->sample_count > 0) {
			cs->sample_count--;
			if (cs->sample_count <= SENSOR_SAMPLES) {
				cs->sensor_samples_1 += (phase_current1 - cs->adc_offset_b);
				cs->sensor_samples_2 += (phase_current2 - cs->adc_offset_c);
			}
		}else {
			cs->sensor_samples_1 = cs->sensor_samples_1 / (float)SENSOR_SAMPLES;
			cs->sensor_samples_2 = cs->sensor_samples_2 / (float)SENSOR_SAMPLES;
			cs->sensor_k1 = g_cs.calibrate_current/cs->sensor_samples_1;
			cs->sensor_k2 = g_cs.calibrate_current/cs->sensor_samples_2;
			cs->sensor_k1 = ABS(cs->sensor_k1);
			cs->sensor_k2 = ABS(cs->sensor_k2);
			cs->is_calibrating_sensor = false;
		}
	}
	return cs->is_calibrating_sensor;
}
#else
void phase_current_sensor_start_calibrate(float calibrate_current) {

}

bool phase_current_sensor_do_calibrate(void) {
	return false;
}
#endif

bool phase_current_offset(void) {
	current_samp_t *cs = &g_cs;
	if (!cs->is_calibrating_offset) {
		return false;
	}
	s32 phase_current1 = 0 , phase_current2 = 0;
	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
	if (cs->sample_count == (NB_OFFSET_SAMPLES + 1)) {
		cs->sample_count --;
		return true;
	}
	if (cs->sample_count > 0) {
		cs->sample_count--;
		if (cs->c_phases == PHASE_AB && cs->sample_count >= 0) {
			cs->adc_offset_a += phase_current1;
			cs->adc_offset_b += phase_current2;
			if (cs->sample_count == 0) {
				cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
			}
		}
		if (cs->c_phases == PHASE_BC && cs->sample_count >= 0) {
			cs->adc_offset_c += phase_current2;
#ifdef HIGH_SIDE_CURRENT_SENSOR
			cs->adc_offset_b += phase_current1;
#endif
			if (cs->sample_count == 0) {
				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
#ifdef HIGH_SIDE_CURRENT_SENSOR
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
#endif
			}
		}
	}else {
		if (cs->c_phases == PHASE_AB) {
			cs->c_phases = PHASE_BC;
			phase_current_init();
			adc_current_sample_config(cs->c_phases);
		}else {
			cs->is_calibrating_offset = false;
			sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
		}

	}
	return true;
}


bool phase_curr_offset_check(void) {
	if ((g_cs.adc_offset_b > ADC_FULL_MAX/2 + 100) || (g_cs.adc_offset_c > ADC_FULL_MAX/2 + 100)) {
		err_add_record(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b));
		return true;
	}
	if ((g_cs.adc_offset_b < ADC_FULL_MAX/2 - 100) || (g_cs.adc_offset_c < ADC_FULL_MAX/2 - 100)) {
		err_add_record(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_c, g_cs.adc_offset_b));
		return true;
	}
	return false;
}

#if 0
void phase_current_get(float *iABC){
	current_samp_t *cs = &g_cs;
	s32 phase_current1, phase_current2;

	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);

	if (cs->c_phases == PHASE_AB) {
		/* Current on Phase C is not accessible */
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
	}else if (cs->c_phases == PHASE_BC) {
		/* Current on Phase A is not accessible 	*/
		/* Ib = PhaseBOffset - ADC converted value) */
		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
	}else if (cs->c_phases == PHASE_AC) {
		/* Current on Phase B is not accessible 	*/
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
	}
	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
}

static __inline__ s16 get_mid_duty(u32 max_hi_side, FOC_OutP *out) {
	u32 min_low_side = FOC_PWM_Half_Period - max_hi_side;
	if (min_low_side >= MAX(TSampleBefore, TADC)) { //可以采样
		if ((min_low_side > TSampleBefore) && (min_low_side > TADC)) {
			out->n_Sample1 = FOC_PWM_Half_Period - 1;
			return 0;
		}else {
			u32 Samp_p = max_hi_side + TSampleBefore;
			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
				return 1;
			}else {
				out->n_Sample1 = Samp_p;
				return 2;
			}
		}
	}
	return -1;
}
void phase_current_point(void *p){
	FOC_OutP *out = p;
	current_samp_t *cs = &g_cs;
	u32 max_hi_side = MAX(out->n_Duty[0], MAX(out->n_Duty[1], out->n_Duty[2]));

	out->n_Sample1 = FOC_PWM_Half_Period + 1;
	out->n_Sample2 = FOC_PWM_Half_Period + 1;

	if (get_mid_duty(max_hi_side, out) != 0) {
		u32 mid_hi_side;
		out->n_Sample1 = FOC_PWM_Half_Period + 1;
		out->n_Sample2 = FOC_PWM_Half_Period + 1;		
		if (out->n_Duty[0] == max_hi_side) {
			//sample B, C
			mid_hi_side = MAX(out->n_Duty[1], out->n_Duty[2]);
			cs->c_phases = PHASE_BC;
		}else if (out->n_Duty[1] == max_hi_side) {
			//sample A, C
			mid_hi_side = MAX(out->n_Duty[0], out->n_Duty[2]);
			cs->c_phases = PHASE_AC;
		}else {
			//sample A, B
			mid_hi_side = MAX(out->n_Duty[0], out->n_Duty[1]);
			cs->c_phases = PHASE_AB;
		}
		get_mid_duty(mid_hi_side, out);
	}else {
		cs->c_phases = PHASE_AB;
	}
	out->n_CPhases = cs->c_phases;
}
#else
#define LOW_FP_COEF 0.1f
void phase_current_get(float *iABC){
	current_samp_t *cs = &g_cs;
	s32 phase_current1, phase_current2;

	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);

	if (cs->c_phases == PHASE_AB) {
		/* Current on Phase C is not accessible */
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
		LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
		LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
		cs->adc_ic_filter = -(cs->adc_ia_filter + cs->adc_ib_filter);

		if (cs->c_ignore_phase == IGNORE_NONE) {

		}else if (cs->c_ignore_phase == IGNORE_A) {
			//LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
			cs->adc_ia = cs->adc_ia_filter;
		}else if (cs->c_ignore_phase == IGNORE_B) {
			//LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
			cs->adc_ib = cs->adc_ib_filter;
		}else if (cs->c_ignore_phase == IGNORE_ALL) {
			cs->adc_ia = cs->adc_ia_filter;
			cs->adc_ib = cs->adc_ib_filter;
		}
		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
	}else if (cs->c_phases == PHASE_BC) {
		/* Current on Phase A is not accessible 	*/
		/* Ib = PhaseBOffset - ADC converted value) */
		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
#ifndef HIGH_SIDE_CURRENT_SENSOR		
		LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
		LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
		cs->adc_ia_filter = -(cs->adc_ib_filter + cs->adc_ic_filter);
		if (cs->c_ignore_phase == IGNORE_NONE) {

		}else if (cs->c_ignore_phase == IGNORE_B) {
			//LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
			cs->adc_ib = cs->adc_ib_filter;
		}else if (cs->c_ignore_phase == IGNORE_C) {
			//LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
			cs->adc_ic = cs->adc_ic_filter;
		}else if (cs->c_ignore_phase == IGNORE_ALL) {
			cs->adc_ib = cs->adc_ib_filter;
			cs->adc_ic = cs->adc_ic_filter;
		}
		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
#endif
	}else if (cs->c_phases == PHASE_AC) {
		/* Current on Phase B is not accessible 	*/
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
		LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
		LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
		cs->adc_ib_filter = -(cs->adc_ia_filter + cs->adc_ic_filter);

		if (cs->c_ignore_phase == IGNORE_NONE) {

		}else if (cs->c_ignore_phase == IGNORE_A) {
			//LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
			cs->adc_ia = cs->adc_ia_filter;
		}else if (cs->c_ignore_phase == IGNORE_C) {
			//LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
			cs->adc_ic = cs->adc_ic_filter;
		}else if (cs->c_ignore_phase == IGNORE_ALL) {
			cs->adc_ia = cs->adc_ia_filter;
			cs->adc_ic = cs->adc_ic_filter;
		}
		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
	}
#ifdef HIGH_SIDE_CURRENT_SENSOR
#ifdef CONFIG_PWM_UV_SWAP
	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof1;
	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof2;
	iABC[0] = -(iABC[1] + iABC[2]);
#else
	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof1;
	iABC[0] = -cs->adc_ic * ADC_TO_CURR_ceof2;
	iABC[2] = -(iABC[1] + iABC[0]);
#endif
#else
	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
#endif
}

void phase_current_point(void *p){
	FOC_OutP *out = p;
	current_samp_t *cs = &g_cs;
	s16  test_sample = 0;
	
	out->n_Sample1 = FOC_PWM_Half_Period + 1;
	out->n_Sample2 = FOC_PWM_Half_Period + 1;

	cs->c_ignore_phase = IGNORE_NONE;
#ifdef HIGH_SIDE_CURRENT_SENSOR
	cs->c_phases = PHASE_BC;
	out->n_Sample1 = FOC_PWM_Half_Period - 1;
#else
	u16 *duty = out->n_Duty;
	u16 max_hi_side = MAX(duty[0], MAX(duty[1], duty[2]));
	u16 min_low_side = FOC_PWM_Half_Period - max_hi_side;

	if (min_low_side > TSampleMIN) {
		u16 Samp_p = max_hi_side + TSampleBefore;
		if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
			out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
			test_sample = 1;
		}else {
			out->n_Sample1 = Samp_p;
			test_sample = 2;
		}
		cs->c_phases = PHASE_AB;
	}else {
		u16 lowA = FOC_PWM_Half_Period - duty[0];
		u16 lowB = FOC_PWM_Half_Period - duty[1];
		u16 lowC = FOC_PWM_Half_Period - duty[2];
		u16 t2, t3, t_pointer, m = 0;
		if (max_hi_side == duty[0]) { //A 相底边开通时间最短
			cs->c_phases = PHASE_BC;
			if (lowB > lowC) {
				t2 = (lowC - lowA)/2;
				t3 = (lowB - lowC)/2;
				t_pointer = duty[2]; //通过C计算
			}else {
				t2 = (lowB - lowA)/2;
				t3 = (lowC - lowB)/2;
				t_pointer = duty[1]; //通过B计算
				m = 1;
			}
			
			if (t2 > TSampleMIN) {
				out->n_Sample1 = t_pointer + TSampleBefore;
				test_sample = 3;
			}else if (t3 > TSampleMIN) {
				if (m == 0) {
					out->n_Sample1 = duty[1] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_C;
					test_sample = 4;
				}else {
					out->n_Sample1 = duty[2] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_B;
					test_sample = 5;
				}
			}else {
				out->n_Sample1 = FOC_PWM_Half_Period - 1;
				cs->c_ignore_phase = IGNORE_ALL;
				test_sample = 6;
			}
		}else if (max_hi_side == duty[1]) { //B 相底边开通时间最短
			cs->c_phases = PHASE_AC;
			if (lowA > lowC) {
				t2 = (lowC - lowB)/2;
				t3 = (lowA - lowC)/2;
				t_pointer = duty[2]; //通过C计算
			}else {
				t2 = (lowA - lowB)/2;
				t3 = (lowC - lowA)/2;
				t_pointer = duty[0]; //通过A计算
				m = 1;
			}
			
			if (t2 > TSampleMIN) {
				out->n_Sample1 = t_pointer + TSampleBefore;
				test_sample = 7;
			}else if (t3 > TSampleMIN) {
				if (m == 0) {
					out->n_Sample1 = duty[0] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_C;
					test_sample = 8;
				}else {
					out->n_Sample1 = duty[2] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_A;
					test_sample = 9;
				}
			}else {
				out->n_Sample1 = FOC_PWM_Half_Period - 1;
				cs->c_ignore_phase = IGNORE_ALL;
				test_sample = 10;
			}
		}else {  //C 相底边开通时间最短
			cs->c_phases = PHASE_AB;
			if (lowA > lowB) {
				t2 = (lowB - lowC)/2;
				t3 = (lowA - lowB)/2;
				t_pointer = duty[1]; //通过B计算
			}else {
				t2 = (lowA - lowC)/2;
				t3 = (lowB - lowA)/2;
				t_pointer = duty[0]; //通过A计算
				m = 1;
			}

			if (t2 > TSampleMIN) {
				out->n_Sample1 = t_pointer + TSampleBefore;
				test_sample = 11;
			}else if (t3 > TSampleMIN) {
				if (m == 0) {
					out->n_Sample1 = duty[0] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_B;
					test_sample = 12;
				}else {
					out->n_Sample1 = duty[1] + TSampleBefore;
					cs->c_ignore_phase = IGNORE_A;
					test_sample = 13;
				}
			}else {
				out->n_Sample1 = FOC_PWM_Half_Period - 1;
				cs->c_ignore_phase = IGNORE_ALL;
				test_sample = 14;
			}
		}
	}
#endif
	out->n_CPhases = cs->c_phases;
	out->test_sample = test_sample;
}

#endif

void phase_current_adc_triger(void){
	adc_enable_ext_trigger();
}