#include <math.h>
#include "bsp/adc.h"
#include "bsp/pwm.h"
#include "foc/motor/current.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "libs/utils.h"
#include "libs/logger.h"
#include "math/fix_math.h"
static current_samp_t g_cs;

#define NB_OFFSET_SAMPLES 32

#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->offset_sample_count = NB_OFFSET_SAMPLES + 1;
	cs->adc_ia = 0;
	cs->adc_ib = 0;
	cs->adc_ic = 0;
}

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

	phase_current_init();
	g_cs.is_calibrating_offset = true;
	g_cs.sector = SECTOR_5;
	adc_current_sample_config(g_cs.sector);
}

void phase_current_wait_cali(void) {
	while(g_cs.is_calibrating_offset) {
		wdog_reload();
	}
}

bool phase_current_offset(void) {
	current_samp_t *cs = &g_cs;
	if (!g_cs.is_calibrating_offset) {
		return false;
	}
	s32 phase_current1 = 0 , phase_current2 = 0;
	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
	if (cs->offset_sample_count == (NB_OFFSET_SAMPLES + 1)) {
		cs->offset_sample_count --;
		return true;
	}
	if (cs->offset_sample_count > 0) {
		cs->offset_sample_count--;
		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
			cs->adc_offset_b += phase_current1;
			cs->adc_offset_a += phase_current2;
			if (cs->offset_sample_count == 0) {
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
				cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
			}
		}
		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
			cs->adc_offset_c += phase_current2;
			if (cs->offset_sample_count == 0) {
				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
			}
		}
	}else {
		if (cs->sector == SECTOR_5) {
			cs->sector = SECTOR_1;
			phase_current_init();
			adc_current_sample_config(cs->sector);
		}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;
}

#define LowPass_filter 1.0f
void phase_current_get(s16 *iABC){
	current_samp_t *cs = &g_cs;
	s32 phase_current1, phase_current2;

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

	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
		/* Current on Phase C is not accessible */
		/* Ia = PhaseAOffset - ADC converted value) */
		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
		cs->adc_ia = (phase_current2 - cs->adc_offset_a);
		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
		/* 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->sector == SECTOR_2 || cs->sector == SECTOR_3) {
		/* 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] = S16_mul(cs->adc_ia, ADC_TO_CURR_ceof, 9);
	iABC[1] = S16_mul(cs->adc_ib, ADC_TO_CURR_ceof, 9);
	iABC[2] = S16_mul(cs->adc_ic, ADC_TO_CURR_ceof, 9);
}


void phase_current_point(void *p){
	FOC_OutP *out = p;
	current_samp_t *cs = &g_cs;
	u32 low_side_low_duty = FOC_PWM_Half_Period - out->n_lowDuty;
	u32 low_side_mid_duty = FOC_PWM_Half_Period - out->n_midDuty;
	cs->sector = out->n_Sector;
	out->n_Sample1 = FOC_PWM_Half_Period + 1;
	out->n_Sample2 = FOC_PWM_Half_Period + 1;
	/*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
	if (low_side_low_duty * 2 >= TSampleMIN) { //可以采样
		if (low_side_low_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
			out->n_Sample1 = FOC_PWM_Half_Period - 1;
			cs->sector = SECTOR_1;
		}else {
			u32 Samp_p = out->n_lowDuty + TSampleBefore;
			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
			}else {
				out->n_Sample1 = Samp_p;
			}
		}
	}else if (low_side_mid_duty * 2 >= TSampleMIN){
		if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
			out->n_Sample1 = FOC_PWM_Half_Period - 1;
		}else {
			u32 Samp_p = out->n_midDuty + TSampleBefore;
			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
			}else {
				out->n_Sample1 = Samp_p;
			}
		}
	}
	out->n_Sector = cs->sector;
}


void phase_current_adc_triger(void){
	adc_enable_ext_trigger();
}