#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"
#include "hall_sensor.h"
#include "motor_control.h"
#include "my_obs.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;
uint8_t ShiftOBS_Flag = 0;

//正常电流环
void FOC_Model(int16_t IqRef, int16_t IdRef, int16_t MotorSpeed, uint16_t SVM_Angle, TrueOrFalse_Flag_Struct_t OBS_Flag)
{
	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
	static int32_t IdFluxLessRef = 0, IqFluxLessRef = 0;
	int32_t UqCal = 0;
	static int32_t FOC_IqLim = 0;
	static uint32_t Angle_Ref = 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变换
	
	if(OBS_Flag == True)
	{		
		switch(ShiftOBS_Flag)
		{
			case 0:default://初始状态，采用霍尔电角度
			{
			  Angle_Ref = SVM_Angle;
				if(MC_RunInfo.MotorSpeed > 200) //高速时开始切换观测器电角度
				{
				  if((int32_t)((observer_stObsOutPu.uwElecThetaPu << 1) - SVM_Angle) > 1000)
						ShiftOBS_Flag = 1;     //速度超过200rpm后，开始切换
				}
				break;
			}
			case 1:
			{
				Angle_Ref += 100;
				if(Angle_Ref >= (observer_stObsOutPu.uwElecThetaPu << 1))
					ShiftOBS_Flag = 2;    //切换完成
				break;
			}
			case 2:
			{
			  Angle_Ref = (observer_stObsOutPu.uwElecThetaPu << 1);
				break;
			}			
		}
	}
	else
	{
	  ShiftOBS_Flag = 0;
		Angle_Ref = SVM_Angle;
	}
	
 	Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, Angle_Ref);  // 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((MC_HallSensorData.BackwardFlag==True)&&(MC_CalParam.AssistRunMode == MC_AssistRunMode_GAS))
	{
		if( ADC1_Result[ADC1_RANK_VIN] < 3000) //53853mV  17.951  3000
		{
			IqFluxLessRef = IqRef;
			if(IdFluxLessRef<(-4))
			{
				IdFluxLessRef += 4;
			}
			else IdFluxLessRef = 0;			
		}
		else if( ADC1_Result[ADC1_RANK_VIN] < 3210 ) //57622mV 3210
		{
			FOC_IqLim = 1050 - (ADC1_Result[ADC1_RANK_VIN] - 3000)*5;
			IqFluxLessRef = IqRef<FOC_IqLim ? IqRef : FOC_IqLim;
			IdFluxLessRef -= 2;
			if(IdFluxLessRef<-420) IdFluxLessRef = -420;
		}
		else
		{
			IqFluxLessRef=0;
			IdFluxLessRef = -420;		
		}	
	}
	else
	{
		if( ADC1_Result[ADC1_RANK_VIN] < 3100) //55648mV  17.951  3100
		{
			IqFluxLessRef = IqRef;
			if(IdFluxLessRef<(-4))
			{
				IdFluxLessRef += 4;
			}
			else IdFluxLessRef = 0;			
		}
		else if( ADC1_Result[ADC1_RANK_VIN] < 3310 ) //59417mV 3310
		{
			FOC_IqLim = 1050 - (ADC1_Result[ADC1_RANK_VIN] - 3100)*5;
			IqFluxLessRef = IqRef<FOC_IqLim ? IqRef : FOC_IqLim;
			IdFluxLessRef -= 2;
			if(IdFluxLessRef<-420) IdFluxLessRef = -420;
		}
		else
		{
			IqFluxLessRef=0;
			IdFluxLessRef = -420;		
		}	
	}	
	
	UqVoltTmp =  PID_Regulator(IqFluxLessRef,\
														 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;
	
	if(IqRef == 0)
	{
	  Stat_Volt_q_d.qV_Component1 = 0;
		IdFluxLessRef = IdRef;
	}

	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); //实际的电流输出控制  
	
	//OBS Calculate
	my_ObsCal(Stat_Volt_alfa_beta.qV_Component2 * (MC_RunInfo.BusVoltage * 577 / 100000) / 32767, 
	          Stat_Volt_alfa_beta.qV_Component1 * (MC_RunInfo.BusVoltage * 577 / 100000) / 32767, 
						Stat_Curr_alfa_beta.qI_Component1 * 1100 / 2048, 
						Stat_Curr_alfa_beta.qI_Component2 * 1100 / 2048, 
						MC_RunInfo.BusVoltage / 100, 
						&observer_stObsOutPu);
}

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