//********************************************************************************
//								     Include
//********************************************************************************
#include "MDSF05_SFR.h"
#include "XSFR_GLOBAL.h"
#include "Allhex.h"
//********************************************************************************
//								       RAM
//********************************************************************************
//The variables defined by data are placed in the first 128 bytes (0x00~0x7F) address space
//The variables defined by idata are placed in the (0x00~0xFF) address space
//The variable defined by xdata are placed in the external extended RAM  (generally refers to the external 0xF000~0xF1FF space, the specific space size varies with the MCU , and some MCU do not have external RAM)

//The variable defined by data is the fastest, followed by idata, and xdata is slower than the first two
//Variables that are often used are declared with data/idata; variables that are not commonly used are declared with xdata.

#if (Vbus_Protect == 1)
	uint8 MotorErrorState = 0;
	// uint8 MotorErrorState = OverVbus | UnderVbus;
	xdata uint16 OverVbusProtectCount = BUS_VOLT_DURATION;
	xdata uint16 UnderVbusProtectCount = BUS_VOLT_DURATION;
#else
	uint8 MotorErrorState = 0;
#endif

#if (Temperature_Protect == 1)
	xdata uint16 OverTemperatureProtectCount = TEMPERATURE_DURATION;
#endif

#if (FaultLock_Protect == 1)
	xdata uint16 	MotorFaultDelayCount 	= 0;
#endif

#if (POCP_Protect == 1)
	xdata uint16 POCP_ProtectCount = PHASE_OCP_DURATION;
#endif

xdata 	int16 	IaFb;
xdata 	int16 	IbFb;
xdata	uint16	IA_ABS;
xdata	uint16	IB_ABS;
xdata	uint16	IC_ABS;

xdata	uint16	IA_ABS_MAX;
xdata	uint16	IB_ABS_MAX;
xdata	uint16	IC_ABS_MAX;

xdata	uint8	LACK_OF_PHASE_Delay_TIMER;
xdata	uint8	LACK_OF_PHASE_TIMER;

xdata	uint8	IA_err_counter;
xdata	uint8	IB_err_counter;
xdata	uint8	IC_err_counter;
//********************************************************************************
//								    OVP/UVP
//********************************************************************************
#if (Vbus_Protect == 1)
void Vbus_Protect_Fun (uint16 Adc_Vbus){
	if(Adc_Vbus > OVER_BUS_VOLT_VALUE){
		if(OverVbusProtectCount >= BUS_VOLT_DURATION){
			OverVbusProtectCount = BUS_VOLT_DURATION; 
			MotorErrorState |= OverVbus;
		}
		else
			OverVbusProtectCount += 10;
	}
	else{
		if(Adc_Vbus < CLEAR_OVER_BUS_VOLT_VALUE){
			if(OverVbusProtectCount < 1){
				OverVbusProtectCount = 0;
				MotorErrorState &= ~(OverVbus);
			}
			else
				OverVbusProtectCount -= 10;
		}
	}

	if(Adc_Vbus < UNDER_BUS_VOLT_VALUE){
		if(UnderVbusProtectCount >= BUS_VOLT_DURATION){
			UnderVbusProtectCount = BUS_VOLT_DURATION; 
			MotorErrorState |= UnderVbus;
		}
		else
			UnderVbusProtectCount += 10;
	}
	else{
		if(Adc_Vbus > CLEAR_UNDER_BUS_VOLT_VALUE){
			if(UnderVbusProtectCount < 1){
				UnderVbusProtectCount = 0;
				MotorErrorState &= ~(UnderVbus);
			}
			else
				UnderVbusProtectCount -= 10;
		}
	}
}
#endif
//********************************************************************************
//								     OTP
//********************************************************************************
#if (Temperature_Protect == 1)
void Temperature_Protect_Fun (uint16 Adc_Temperature){
	if(Adc_Temperature > OVER_TEMPERATURE_VALUE){
		if(OverTemperatureProtectCount >= TEMPERATURE_DURATION){
			OverTemperatureProtectCount = TEMPERATURE_DURATION; 
			MotorErrorState |= OverTemperature;
		}
		else
			OverTemperatureProtectCount += 10;
	}
	else{
		if(Adc_Temperature < CLEAR_OVER_TEMPERATURE_VALUE){
			if(OverTemperatureProtectCount < 1){
				OverTemperatureProtectCount = 0;
				MotorErrorState &= ~(OverTemperature);
			}
			else
				OverTemperatureProtectCount -= 10;
		}
	}
}
#endif
//********************************************************************************
//								 Lock Protect
//********************************************************************************
#if (FaultLock_Protect == 1)
xdata	uint16	mot_lock_cnt; 
void FaultLock_Fun (void){
	if(MotorState == M_RUN){
		if(MotorFaultDelayCount > FAULTLOCK_DELAY_DURATION){
			if(EstimatedSpeed > OverSpeed){
				mot_lock_cnt += 1;
				// MotorFaultState = 1;
				// MotorErrorState = FaultLock;
			}
			
			else if(EstimatedSpeed < UnderSpeed){
				mot_lock_cnt += 1;
				// MotorFaultState = 2;
				// MotorErrorState = FaultLock;
			}
			else{
				if(mot_lock_cnt>0)
					mot_lock_cnt -= 1;
			}
			
			if(mot_lock_cnt>50){
				mot_lock_cnt = 0;
				MotorErrorState = FaultLock;
			}
			// if((IqPiOut < 0) && (IqFb < 0)){
				// mot_lock_cnt += 1;
				// MotorFaultState = 3;
				// MotorErrorState = FaultLock;
			// }
			
			// if(IqFb >IqPiOut){
				// MotorFaultState = 4;
				// MotorErrorState = FaultLock;
			// }
		}
		else
			MotorFaultDelayCount += 10;
	}
	else{
		mot_lock_cnt = 0;
		MotorFaultDelayCount = 0;
		if(MotorState != M_ERROR)
			MotorFaultState = 0;
	}
}
#endif
//********************************************************************************
//								      OCP
//********************************************************************************
void AOCP_Protect_Fun (void){
	if(MotorState == M_IPD){
		MotorErrorState &= ~(AOCP);
	}
	else{
		MotorErrorState |= AOCP;
	}
}
//********************************************************************************
//								    Ibus OCP
//********************************************************************************
// void IBUS_OCP_Protect_Fun (void){
	
// }
//********************************************************************************
//								   Phase  OCP
//********************************************************************************
#if (POCP_Protect == 1)
void Phase_OCP_Protect_Fun (void){
	int IcFb;
	IaFb = dFB_Ia_DRH << 8;
	IaFb |= dFB_Ia_DRL;
	
	IaFb = abs(IaFb);
	
	IbFb = dFB_Ib_DRH << 8;
	IbFb |= dFB_Ib_DRL;
	IbFb = abs(IbFb);
	
	IcFb = -IaFb - IbFb;
	if((abs(IaFb) > PHASE_OCP_VALUE) || (abs(IbFb) > PHASE_OCP_VALUE) || (abs(IcFb) > PHASE_OCP_VALUE)){
		if(POCP_ProtectCount >= PHASE_OCP_DURATION){
			POCP_ProtectCount = PHASE_OCP_DURATION; 
			MotorErrorState |= POCP;
		}
		else
			POCP_ProtectCount += 10;
	}
	else{
		if(POCP_ProtectCount < 1){
			POCP_ProtectCount = 0;
			// MotorErrorState &= ~(POCP);
		}
		else
			POCP_ProtectCount -= 10;
	}
}
#endif
//********************************************************************************
//							Lack of Phase Protection
//********************************************************************************
#if (dLACK_OF_PHASE_PROTECTION == 1)
void LACK_OF_PHASE_Function(void){

	if(MotorState == M_RUN){
		// IaFb = (unsigned int)(((unsigned int)dFB_Ia_DRH * 256) + (dFB_Ia_DRL));
		// IbFb = (unsigned int)(((unsigned int)dFB_Ib_DRH * 256) + (dFB_Ib_DRL));
		IaFb = dFB_Ia_DRH << 8;
		IaFb |= dFB_Ia_DRL;
		
		IbFb = dFB_Ib_DRH << 8;
		IbFb |= dFB_Ib_DRL;
	
		IA_ABS = abs(IaFb);	
		IB_ABS = abs(IbFb);
		IC_ABS = abs(-IaFb-IbFb);
	
		LACK_OF_PHASE_Delay_TIMER+=1;
		if(LACK_OF_PHASE_Delay_TIMER>=2){	//10ms
			LACK_OF_PHASE_Delay_TIMER = 0;
			
			if(IA_ABS>IA_ABS_MAX)
				 IA_ABS_MAX = IA_ABS;
			if(IB_ABS>IB_ABS_MAX)
				 IB_ABS_MAX = IB_ABS;	 
			if(IC_ABS>IC_ABS_MAX)
				 IC_ABS_MAX = IC_ABS;
			
			LACK_OF_PHASE_TIMER+=1;
			if(LACK_OF_PHASE_TIMER>100){	//1s
				LACK_OF_PHASE_TIMER = 0;
				
				if(IA_ABS_MAX < dLACK_PHASE_MIN_CURRENT && IB_ABS_MAX > dLACK_PHASE_MAX_CURRENT && IC_ABS_MAX > dLACK_PHASE_MAX_CURRENT)	
					IA_err_counter++;
				else
					IA_err_counter = 0;
				
				if(IB_ABS_MAX < dLACK_PHASE_MIN_CURRENT && IA_ABS_MAX > dLACK_PHASE_MAX_CURRENT && IC_ABS_MAX > dLACK_PHASE_MAX_CURRENT)
					IB_err_counter++;
				else
					IB_err_counter = 0;
				
				if(IC_ABS_MAX < IC_LACK_OF_PHASE_Current && IB_ABS_MAX > dLACK_PHASE_MAX_CURRENT && IA_ABS_MAX > dLACK_PHASE_MAX_CURRENT)
					IC_err_counter++;
				else
					IC_err_counter = 0;
				
				IA_ABS_MAX = 0;
				IB_ABS_MAX = 0;
				IC_ABS_MAX = 0;
				
				// if(IA_err_counter > 1|| IB_err_counter > 1 || IC_err_counter > 2)
					// MotorErrorState |= POCP;
			}
		}
	}
}
#endif
