#include "hal/hal.h"
#include "hal/pwm.h"
#include "libs/task.h"
#include "foc_task.h"
#include "foc.h"
#include "phase_current.h"
#include "park_clark.h"
#include "hall_sensor.h"
#include "svpwm.h"

#if 1
static void __inline foc_update_theta(motor_foc_t *foc) {
	float angle = 0.0f;
	if (foc->override.is_theta) {
		angle = foc->override.theta;
	}else {
		angle = hall_sensor_get_theta();
	}
	foc->motor_s.angle = angle;
	foc->motor_s.theta = degree_2_pi(foc->motor_s.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

/* 输出dq电压给反park,最后给svpwm */
static void __inline Foc_Dq_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
	if (foc->mode == FOC_MODE_PI_DQ || foc->mode == FOC_MODE_PI_FULL) {
		ref_out->d = pi_control(&foc->PI_id, foc->dq_ref.d - sampled->d);
		ref_out->q = pi_control(&foc->PI_iq, foc->dq_ref.q - sampled->q);
	}else {
		ref_out->d = foc->dq_ref.d;
		ref_out->q = foc->dq_ref.q;
	}
	if (foc->override.is_vdq) {
		ref_out->d = foc->override.vdq.d;
		ref_out->q = foc->override.vdq.q;
	}
}

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);
	/* 采集3相电流 */
	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_s.theta, &sample_dq);
	/* 处理D,Q电流环，速度环低频运行，不在此处处理*/
	Foc_Dq_PI_Contrl(foc, &sample_dq, &v_dq);
	/* d-q坐标转 alpha-beta坐标，输入给pwm */
	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
	/* 生成 pwm，模拟正弦波,此处vbus需要动态采集 */
	SVPWM_7(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
	/* 通过扇区和pwm duty 选择合适的3相电流采样点 */
	sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
	/* 死区补偿 */
	DeadTime_Compensation(c_sample, &phase_time);
	/* 更新duty和采样点到硬件TIM1中 */
	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);

	Debug_Log(foc);

	Debug_dq(&sample_dq);
}

//输出dq电流，给电流环
static void __inline Foc_Speed_PI_Control(motor_foc_t *foc) {
	if (foc->mode == FOC_MODE_PI_SPEED || foc->mode == FOC_MODE_PI_FULL){
		float speed_ref = ramp_get_target(&foc->speed_ramp);
		float vq_out = pi_control(&foc->PI_speed, speed_ref - foc->motor_s.rpm);
		foc->dq_ref.q = vq_out;
	}
}

void FOC_Normal_Task(motor_foc_t *foc) {
	switch (foc->state) {
		case START:
			PWM_TurnOnLowSides();
			FOC_STM_NextState(CURRENT_CALIBRATE);
			break;
		case CURRENT_CALIBRATE:
			foc_current_calibrate();
			FOC_STM_NextState(READY_TO_RUN);
			break;
		case READY_TO_RUN:
			foc_pwm_start(true);
			FOC_STM_NextState(RAMPING_START);
			ramp_exc(&foc->start_ramp);
			foc_overide_vdq(true);
			break;
		case RAMPING_START:
			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->start_ramp));
			if (ramp_complete(&foc->start_ramp)) {
				FOC_STM_NextState(RUNNING);
			}
			break;
		case RUNNING:
			Foc_Speed_PI_Control(foc);
			break;
		case ANY_STOP:
			ramp_clear(&foc->start_ramp);
			foc_clear();
			FOC_STM_NextState(IDLE);
			break;
		default:
			break;
	}

}