#include "MC_FOC_Driver.h"
#include "MC_type.h"
#include "MC_Globals.h"
#include "MC_PID_regulators.h"
#include "MC_Clarke_park.h"
#include "stm32f10x_svpwm_3shunt.h"

#define FluxWeak_Control 0

//全局变量定义

int32_t IqFdbFlt = 0;
int32_t IdFdbFlt = 0;
int32_t VoltSquareFlt = 0;
int32_t UqVoltFlt = 0;
int32_t UdVoltFlt = 0;

void FOC_Model(int16_t Ref, int16_t MotorSpeed, uint16_t SVM_Angle)	  //电流环处理函数，严格按照框图理解
{  
	int32_t UqVoltTmp;
	int16_t UdVoltTmp;
	static int16_t IqFdb = 0;
	static int16_t IdFdb = 0;
	#if FluxWeak_Control
	int32_t VoltSquare = 0;    //弱磁控制预留
	int32_t VoltMax = 0; 
	#endif
	int32_t IdFluxLessRef = 0;
	int32_t UqCal = 0;
	
	Curr_Components Stat_Curr_a_b;       // Stator currents Ia,Ib
	Curr_Components Stat_Curr_alfa_beta; // Ialpha & Ibeta, Clarke's transformations of Ia & Ib
	Volt_Components Stat_Volt_alfa_beta; // Valpha & Vbeta, RevPark transformations of Vq & Vd

  /**********STARTS THE VECTOR CONTROL *********************** */ 		
	Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues();			//读取2相的电流值
  Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b);	// 得到Ialpha和Ibeta,Clark变换
 	Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, SVM_Angle);  // Stat_Curr_q_d为当前的Id和Iq值//SVM_Angle
																                         // 给定值为 Stat_Curr_q_d_ref_ref
	
	IqFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component1 << 10)) - IqFdbFlt)>>10;  // KFlt = 2ms
	IqFdb = IqFdbFlt >> 10;
	
	IdFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component2 << 10)) - IdFdbFlt)>>10;   // KFlt = 2ms
	IdFdb = IdFdbFlt >> 10;
	
	#if FluxWeak_Control // Debug 没调好,暂时关闭
		/* 增加弱磁处理 add Bike 20171007 */
		Tmp = ((int32_t)Stat_Volt_q_d.qV_Component1 * Stat_Volt_q_d.qV_Component1)  + ((int32_t)Stat_Volt_q_d.qV_Component2 * Stat_Volt_q_d.qV_Component2);  //2*Q30
		Tmp =  Tmp >> 10; //Q20
		VoltSquareFlt += ((Tmp<<4) - VoltSquareFlt) >> 12;
		VoltSquare = VoltSquareFlt >> 4;
		VoltMax = 314641;
		IdFluxLessRef = PID_Regulator(VoltMax, VoltSquare, &PID_Weak_InitStructure) >> 10;
	#else
		IdFluxLessRef = 0;
	#endif
	
	UqVoltTmp =  PID_Regulator(Ref,\
														 IqFdb,\
			                       &PID_Torque_InitStructure); // 电流闭环输出q轴电压
	UqVoltFlt += ((UqVoltTmp << 9) - UqVoltFlt) >> 3;	

	/* 
		UqCal = DbSpdMotor*VMax*0.7/1020
		      = DbSpdMotor*VMax/(1020/0.7)
					= DbSpdMotor*VMax/1020
	*/
	#if 1    //加入前馈
	uint16_t Cal_K;
	Cal_K = (MC_MotorParam.Rate_Speed * 183) >> 7; //电机转速 * 1.43
	UqCal = ((int32_t)MotorSpeed * MAX_MODULE) / ((Cal_K < 1000) ? 1000 : Cal_K);
	UqVoltTmp = UqCal + (UqVoltFlt >> 9);
	#else    //去掉前馈
	UqVoltTmp = UqVoltFlt >> 9;
	#endif
	
	UqVoltTmp = (UqVoltTmp > ((int32_t)MAX_MODULE)) ? MAX_MODULE : UqVoltTmp;
	Stat_Volt_q_d.qV_Component1 = UqVoltTmp;

	UdVoltTmp = PID_Regulator(IdFluxLessRef,\
														IdFdb,\
	                          &PID_Flux_InitStructure);	// 电流闭环输出d轴电压
	
	UdVoltFlt	+= ((UdVoltTmp << 9) - UdVoltFlt) >> 3;		
	Stat_Volt_q_d.qV_Component2 =  UdVoltFlt >> 9;
																							
  //circle limitation
  RevPark_Circle_Limitation(&Stat_Volt_q_d); 		// 电压极限圈限制？ 会不会出现波动情况?
  /*Performs the Reverse Park transformation,
  i.e transforms stator voltages Vqs and Vds into Valpha and Vbeta on a 
  stationary reference frame*/
  Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);
  /*Valpha and Vbeta finally drive the power stage*/ 
  SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta); //实际的电流输出控制  
}

void FOC_Enable(void)
{
  FOC_Status = FOC_Status_RUN;
}

void FOC_Disable(void)
{
  FOC_Status = FOC_Status_WAIT;
	Stat_Curr_q_d.qI_Component1 = 0;
	Stat_Curr_q_d.qI_Component2 = 0;
	Stat_Volt_q_d.qV_Component1 = 0;
	Stat_Volt_q_d.qV_Component2 = 0;
}