#include "tim.h"

#include "FOC.h"

#if FOC_ARM_MATH
#include "arm_math.h"
#endif
#include "math_utils.h"

const float sqrt3 = 1.73205080756f;

const int PWM_ARR = 1600; // PWM的计数周期
float foc_Udc = 24.0f;	  // 电机的母线电压
float foc_ULimit = 5.0f;

/// @brief 返回当前FOC的设置,以结构体的形式
/// @return 返回速度KFP滤波器的FOC_PARA结构体
FOC_PARA foc_para_Get(void)
{
	FOC_PARA config = {0};
	config.foc_Udc = foc_Udc;
	config.foc_ULimit = foc_ULimit;
	return config;
}

/// @brief 设置FOC,以结构体的形式传入,传入结构体会赋值到最终结构体中(即需要在外部构建该结构体)
/// @param config KFP滤波器设置,FOC_PARA结构体;foc_Udc和foc_ULimit须要(>0)
/// @return 返回操作结果, 0 - 成功 , -1 - 失败(输入非法)
int foc_para_Set(FOC_PARA config)
{
	if ((config.foc_Udc > 0) && (config.foc_ULimit > 0))
	{
		foc_Udc = config.foc_Udc;
		foc_ULimit = config.foc_ULimit;
		return 0;
	}else{
		return -1;
	}
}

// 使能TIMx的通道y
void PWM_Init(void)
{
#if MOT_DEV_TARGET == STM32_TARGET_A12_F4
	LL_TIM_OC_SetCompareCH1(TIM3, 0);
	LL_TIM_OC_SetCompareCH2(TIM3, 0);
	LL_TIM_OC_SetCompareCH3(TIM3, 0);

	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH1);
	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH2);
	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH3);

	LL_TIM_OC_SetCompareCH1(TIM3, 0);
	LL_TIM_OC_SetCompareCH2(TIM3, 0);
	LL_TIM_OC_SetCompareCH3(TIM3, 0);
#endif

#if MOT_DEV_TARGET == STM32_TARGET_A2_F1
	LL_TIM_OC_SetCompareCH4(TIM2, 0);
	LL_TIM_OC_SetCompareCH3(TIM3, 0);
	LL_TIM_OC_SetCompareCH4(TIM3, 0);

	LL_TIM_CC_EnableChannel(TIM2, LL_TIM_CHANNEL_CH4);
	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH3);
	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH4);

	LL_TIM_OC_SetCompareCH4(TIM2, 0);
	LL_TIM_OC_SetCompareCH3(TIM3, 0);
	LL_TIM_OC_SetCompareCH4(TIM3, 0);
#endif
}

// 输入参数0.0f ~ 1.0f，输出3路PWM
inline void Set_PWM(float _CCR1, float _CCR2, float _CCR3)
{
#if MOT_DEV_TARGET == STM32_TARGET_A12_F4
	LL_TIM_OC_SetCompareCH1(TIM3, _CCR1 * PWM_ARR); // 交换
	LL_TIM_OC_SetCompareCH2(TIM3, _CCR3 * PWM_ARR);
	LL_TIM_OC_SetCompareCH3(TIM3, _CCR2 * PWM_ARR);
#endif

#if MOT_DEV_TARGET == STM32_TARGET_A2_F1
	LL_TIM_OC_SetCompareCH4(TIM2, _CCR1 * PWM_ARR); // 交换
	LL_TIM_OC_SetCompareCH3(TIM3, _CCR3 * PWM_ARR);
	LL_TIM_OC_SetCompareCH4(TIM3, _CCR2 * PWM_ARR);
#endif
}

// FOC核心函数：输入Uq、Ud和电角度，输出三路PWM
void setPhaseVoltage(float Uq, float Ud, float angle_el)
{
	Uq = _constraint(Uq, -fabsf(foc_ULimit), fabsf(foc_ULimit));
	// angle_el = _normalizeAngle(angle_el); // 使用MT6701计算电角度时，已限制角度范围

	static float Ts = 1.0f;
	float Ta, Tb, Tc;
	float t1, t2, t3, t4, t5, t6, t7;
	float sum, k_svpwm;

#if FOC_ARM_MATH
	// Park逆变换
	float U_alpha = -Uq * arm_sin_f32(angle_el) + Ud * arm_cos_f32(angle_el);
	float U_beta = Uq * arm_cos_f32(angle_el) + Ud * arm_sin_f32(angle_el);
#endif

	// Park逆变换
	float U_alpha = -Uq * sinf(angle_el) + Ud * cosf(angle_el);
	float U_beta = Uq * cosf(angle_el) + Ud * sinf(angle_el);

	// 扇区判断
	float K = sqrt3 * Ts / foc_Udc; // SVPWM调制比
	float u1 = U_beta * K;
	float u2 = (0.8660254f * U_alpha - 0.5f * U_beta) * K; // sqrt(3)/2 = 0.8660254
	float u3 = (-0.8660254f * U_alpha - 0.5f * U_beta) * K;

	uint8_t sector = (u1 > 0.0f) + ((u2 > 0.0f) << 1) + ((u3 > 0.0f) << 2); // sector = A + 2B + 4C

	// 非零矢量和零矢量作用时间的计算
	switch (sector)
	{
	case 3: // 扇区1
		t4 = u2;
		t6 = u1;
		sum = t4 + t6;
		if (sum > Ts) // 过调制处理
		{
			k_svpwm = Ts / sum;
			t4 *= k_svpwm;
			t6 *= k_svpwm;
		}
		t7 = (Ts - t4 - t6) / 2.0f;
		Ta = t4 + t6 + t7;
		Tb = t6 + t7;
		Tc = t7;
		break;
	case 1: // 扇区2
		t2 = -u2;
		t6 = -u3;
		sum = t2 + t6;
		if (sum > Ts)
		{
			k_svpwm = Ts / sum;
			t2 *= k_svpwm;
			t6 *= k_svpwm;
		}
		t7 = (Ts - t2 - t6) / 2.0f;
		Ta = t6 + t7;
		Tb = t2 + t6 + t7;
		Tc = t7;
		break;
	case 5: // 扇区3
		t2 = u1;
		t3 = u3;
		sum = t2 + t3;
		if (sum > Ts)
		{
			k_svpwm = Ts / sum;
			t2 *= k_svpwm;
			t3 *= k_svpwm;
		}
		t7 = (Ts - t2 - t3) / 2.0f;
		Ta = t7;
		Tb = t2 + t3 + t7;
		Tc = t3 + t7;
		break;
	case 4: // 扇区4
		t1 = -u1;
		t3 = -u2;
		sum = t1 + t3;
		if (sum > Ts)
		{
			k_svpwm = Ts / sum;
			t1 *= k_svpwm;
			t3 *= k_svpwm;
		}
		t7 = (Ts - t1 - t3) / 2.0f;
		Ta = t7;
		Tb = t3 + t7;
		Tc = t1 + t3 + t7;
		break;
	case 6: // 扇区5
		t1 = u3;
		t5 = u2;
		sum = t1 + t5;
		if (sum > Ts)
		{
			k_svpwm = Ts / sum;
			t1 *= k_svpwm;
			t5 *= k_svpwm;
		}
		t7 = (Ts - t1 - t5) / 2.0f;
		Ta = t5 + t7;
		Tb = t7;
		Tc = t1 + t5 + t7;
		break;
	case 2: // 扇区6
		t4 = -u3;
		t5 = -u1;
		sum = t4 + t5;
		if (sum > Ts)
		{
			k_svpwm = Ts / sum;
			t4 *= k_svpwm;
			t5 *= k_svpwm;
		}
		t7 = (Ts - t4 - t5) / 2.0f;
		Ta = t4 + t5 + t7;
		Tb = t7;
		Tc = t5 + t7;
		break;
	default:
		Ta = 0;
		Tb = 0;
		Tc = 0;
		break;
	}

	// FOC_log("[Ta,Tb,Tc]:%f,%f,%f\r\n", Ta, Tb, Tc);
	Set_PWM(Ta, Tb, Tc); // 输出三路PWM，驱动无刷电机转动
}