#include "os/co_task.h"
#include "bsp/pwm.h"
#include "bsp/adc.h"
#include "foc/core/foc_core.h"
#include "foc/core/foc_api.h"
#include "foc/core/foc_fsm.h"
#include "foc/motor/current.h"
#include "foc/core/park_clark.h"
#include "foc/motor/hall.h"
#include "foc/motor/ntc.h"
#include "foc/motor/motor.h"
#include "foc/core/circle_limitation.h"
#include "foc/samples.h"
#include "foc/core/svpwm.h"
#include "bsp/timer_count32.h"
#include "libs/logger.h"

motor_foc_t g_foc = {
	.id_controller = {
		.Kp_gain = 9,
		.Ki_gain = 1071,
		.max_output = MAX_VBUS_VOLTAGE,
		.min_output = -MAX_VBUS_VOLTAGE,		
	},
	.iq_controller = {
		.Kp_gain = 10,
		.Ki_gain = 1080,
		.max_output = MAX_VBUS_VOLTAGE,
		.min_output = -MAX_VBUS_VOLTAGE,
	},
	.speed_controller = {
		.Kp_gain = 1,
		.Ki_gain = 200,
		.max_output = MAX_CURRENT,
		.min_output = -MAX_CURRENT,
	},	
};


static void foc_sample_task(void*);

void foc_core_init(void) {
	samples_init();
	ntc_sensor_init();
	co_task_create(foc_sample_task, NULL, 256);
}

#if 1
static void __inline foc_update_theta(motor_foc_t *foc) {
	foc->motor_stat.angle = hall_sensor_get_theta();
	foc->motor_stat.theta = degree_2_pi(foc->motor_stat.angle);
}

#else
static void __inline foc_update_theta(motor_foc_t *foc) {
	static float angle = 0.0f;
	static bool first_s = false;
	if (!first_s) {
		first_s = true;
		angle = hall_sensor_get_theta();
	}else {
		angle += 0.5f;
	}
	fast_norm_angle(&angle);
	foc->motor_s.angle = angle;
	foc->motor_s.theta = degree_2_pi(angle);
}
#endif


static void __inline foc_calc_voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
	//float vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
	//float vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
	if (foc->mode == FOC_MODE_CURRENT_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP) {
		ref_out->Vd = pi_control(&foc->id_controller, foc->dq_command.Id - sampled->Id);
		ref_out->Vq = pi_control(&foc->iq_controller, foc->dq_command.Iq - sampled->Iq);
		//printf("vd = %f, vq = %f\n", vd, vq);
	}else {
		ref_out->Vd = foc->dq_command.Vd;
		ref_out->Vq = foc->dq_command.Vq;
	}
	foc->dq_last.Vd = ref_out->Vd;
	foc->dq_last.Vq = ref_out->Vq;
}

static void __inline deadtime_compensation(current_samp_t *c_sample, phase_time_t *time) {
#if 0
    /* Dead time compensation */
    if ( c_sample->Ia > 0)
    {
      time->A -= TDead;
    }
    else
    {
      time->A += TDead;
    }

    if ( c_sample->Ib > 0 )
    {
      time->B -= TDead;
    }
    else
    {
      time->B += TDead;
    }

    if ( c_sample->Ic > 0 )
    {
      time->C -= TDead;
    }
    else
    {
      time->C += TDead;
    }
#endif
}

static void __inline Debug_Log(motor_foc_t *foc){
#if 0
	static int count;
	if (count++ % 10 == 0) {
		//printf("$%d %d %d %d %d;",(int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
		//	(int)(foc->current_samp.Ic * 1000.0f), (int)foc->sector * 100, (int)foc->motor_s.angle);
		printf("$%d;", (int)hall_sensor_get_speed());
	}
#endif	
}

static void __inline Debug_dq(dq_t *dq){
#if 0
	static int count;
	if (count++ % 10 == 0) {
		printf("$%d %d;",(int)(dq->d * 1000.0f), (int)(dq->q * 1000.0f));
	}
#endif	
}

/* FOC 主控制任务 */
void do_motor_foc(motor_foc_t *foc){
	current_samp_t *c_sample = &foc->current_samp;
	alpha_beta_t sample_ab, pwm_ab;
	dq_t         sample_dq, v_dq;

	pwm_clear_updata();
	/* 更新电角度 */
	foc_update_theta(foc);
	/* 采集相电流 */
	phase_current_sample(c_sample);
		
	/* ABC三相坐标到alpha-beta坐标 */
	do_clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
	/* alpha-beta坐标系到D-Q旋转坐标系 */
	do_park(&sample_ab, foc->motor_stat.theta, &sample_dq);
	/* 电流环，输出电压给SVPWM */
	foc_calc_voltage(foc, &sample_dq, &v_dq);
	/* 确保电压在6个扇区的内切圆中 */
	circle_limitation(&v_dq, foc->vbus, 0.96f);
	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
	Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
	svpwm_get_phase_time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &c_sample->time, &foc->sector);
	/* 计算三相电流的采样点 */
	get_phase_sample_point(c_sample, foc->sector);
	/* 死区补偿 */
	deadtime_compensation(c_sample, &c_sample->time);
	/* 更新 TIM1的CCR0-2，生成互补pwm, 相电流更新采样点 */
	motor_update_duty(c_sample->time.A, c_sample->time.B, c_sample->time.C, c_sample->time.A_next, c_sample->time.B_next, c_sample->time.C_next);
	/* 更新采样点 */
	motor_update_sample(c_sample->time.Samp_p1, c_sample->time.Samp_p2, c_sample->sector);

	Debug_Log(foc);

	Debug_dq(&sample_dq);
}

/* 计算电流环的参考输入 */
void foc_calc_current(motor_foc_t *foc) {
	float speed_ref = ramp_get_target(&foc->speed_ramp);
	float speed_feedback = foc_get_speed();
	float vq_out = pi_control(&foc->speed_controller, speed_ref - speed_feedback);	
	if (foc->mode == FOC_MODE_SPEED_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP){
		foc->dq_command.Iq = vq_out;
		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0		
	}else {
		foc->dq_command.Iq = ramp_get_target(&foc->current_ramp);
		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
	}
}

void foc_speed_ramp(motor_foc_t *foc){
	if (foc->speed_command.speed >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
		u16 current_rpm = foc_get_speed();
		u16 ref_rpm = foc->speed_command.speed;
		foc->speed_command.speed = -1;
		
		if (ref_rpm + RPM_FOR_CLOSE_LOOP < current_rpm){
			foc_set_voltage_ramp(speed_to_voltage(ref_rpm));
			foc->mode = FOC_MODE_OPEN_LOOP;
		}else {
			foc_set_speed_ramp(ref_rpm);
		}
	}
}

void foc_brake_handler(bool brake) {
	g_foc.is_brake_in = brake;
}

void foc_pwm_up_handler(void){
	phase_current_adc_triger(&g_foc.current_samp);
}

measure_time_t g_meas_foc = {.exec_max_time = 15,};
/*ADC 电流采集中断，调用FOC的核心处理函数*/
void mc_phase_current_irq(void) {
	if (g_foc.current_samp.is_calibrating_offset) {
		phase_current_offset(&g_foc.current_samp);
		return;
	}
	time_measure_start(&g_meas_foc);
	do_motor_foc(&g_foc);
	time_measure_end(&g_meas_foc);
	if (g_meas_foc.intval_time < 62 || g_meas_foc.intval_time > 62) {
		//log_chan_value(2, g_meas_foc.intval_time);
	}
}

void foc_pwm_start(bool start) {
	if (start == g_foc.mosfec_gate) {
		return;
	}
	if (start) {
		pwm_start();
	}else {
		pwm_stop();
	}
	g_foc.mosfec_gate = start;	
}

/*10ms 定时任务，主要处理foc状态机(里面包含速度环)*/
void foc_normal_task(void) {
	foc_fsm(&g_foc);
}

static void foc_sample_task(void *args){
//	u64 ts = co_task_sys64_ts();
	while(1) {
		ntc_sensor_sample();
		g_foc.vbus = get_vbus_sample();
		wdog_reload();
		co_task_yield();
	}
}