#include "hal/hal.h"
#include "hal/pwm.h"
#include "libs/task.h"
#include "foc_core.h"
#include "foc_api.h"
#include "foc_stm.h"
#include "phase_current.h"
#include "park_clark.h"
#include "hall_sensor.h"
#include "circle_limitation.h"
#include "svpwm.h"

motor_foc_t g_foc = {
	.motor_param = {
		.poles = 2,
		.ld = 0.578477f,
		.lq = 5.78477f,
		.rs = 1.088f,
		.inertia = 3.319367f,
		.b_emf = 4.332566f,
	},
	.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,
	},	
};
#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_v.Vd = ref_out->Vd;
	foc->dq_v.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	
}

#if defined (CCMRAM)
#if defined (__ICCARM__)
#pragma location = ".ccmram"
#elif defined (__CC_ARM)
__attribute__( ( section ( ".ccmram" ) ) )
#endif
#endif
/* FOC 主控制任务 */
void FOC_Fast_Task(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;
	phase_time_t         phase_time;
	u32 sample_point;

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

	Debug_Log(foc);

	Debug_dq(&sample_dq);
}

/* 计算电流环的参考输入 */
void Foc_Calc_Current_Ref(motor_foc_t *foc) {
	static int count = 0;
	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
		if (((count) % 10) == 0) {
			printf("vq_out = %f, %f, %f\n", vq_out, speed_ref, speed_feedback);
		}
		count++;
		
	}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 >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
		u16 current_rpm = foc_get_speed();
		u16 ref_rpm = foc->speed_command;
		foc->speed_command = -1;
		
		if (ref_rpm + RPM_FOR_CLOSE_LOOP < current_rpm){
			ramp_set_target(&foc->voltage_ramp, foc->dq_command.Vq, speed_to_voltage(ref_rpm), SPEED_RAMP_DURATION);
			ramp_exc(&foc->current_ramp);
			foc->mode = FOC_MODE_OPEN_LOOP;
		}
	}
}

void foc_brake_handler(void) {
	g_foc.foc_fault = foc_brake_error;
}

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

#if defined (CCMRAM)
#if defined (__ICCARM__)
#pragma location = ".ccmram"
#elif defined (__CC_ARM)
__attribute__( ( section ( ".ccmram" ) ) )
#endif
#endif

void current_sample_handler(void) {
	if (g_foc.current_samp.is_calibrating) {
		phase_current_offset(&g_foc.current_samp);
	}else {
		FOC_Fast_Task(&g_foc);
	}
}

void foc_slow_task_handler(void) {
	FOC_Normal_Task(&g_foc);
}

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;	
}