//********************************************************************************
//								     Include
//********************************************************************************
#include "MDRFxx_SFR.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.
//No special declared variables, the default is data

xdata unsigned int Heat_Cnt = 0;
xdata unsigned int Heat_Enalbe_Cnt = 0;
xdata unsigned int Heat_Level_Temperture_Cmd = 0;

xdata unsigned char Speed_High_Cnt;
xdata unsigned char Speed_Low_Cnt;
xdata unsigned char Speed_Level = 0;
xdata unsigned char Heat_Level = 0;

bit SPEED_ZC_OLD;
bit ZERO_OLD;
bit HEAT_SW_OLD;
bit HEAT_SW_FLAG;
xdata unsigned int HEAT_SW_Cnt = 0;

bit VDC_BUS_LOW_200V_F = 0;
bit SW_HOT_COOL = 0; // 1 : HOT ; 0 : COOL

unsigned char Error_Status;
unsigned char HeatErrorState;


xdata	int SW_HOT_Cnt=0;
xdata	unsigned char SW_HOT_Cnt0=0;

xdata	unsigned char led_Cnt=0;
xdata	unsigned char led_th=0;

//xdata unsigned int Pi_Cnt = 0;
//xdata int Pi_Out_Set = 0;
//xdata int Pi_Err = 0;
//xdata long Pi_Sum = 0;
//xdata long Pi_U = 0;
//xdata long Pi_Out = 0;
//xdata long Pi_Exc = 0;
//void Pi_Ctrl(int ref,int set)
//{
//		Pi_Err = ref - set;
//		Pi_U = Pi_Sum + Pi_kp * Pi_Err;
//		if(Pi_U > Out_max)      Pi_Out = Out_max;
//		else if(Pi_U < Out_min) Pi_Out = Out_min;
//		else                    Pi_Out = Pi_U;
//		Pi_Out_Set = Pi_Out>>11;
//		if(Pi_Out_Set >= 20) Pi_Out_Set = 20;
//		Pi_Sum = Pi_Sum + (Pi_ki * Pi_Err);//- (pi_kc*pi_exc);
//		if(Pi_Sum >= Out_max) Pi_Sum = Out_max;
//		if(Pi_Sum < Out_min)  Pi_Sum = Out_min;
//}

#define brakestep		12
#define brakestep2	brakestep*2
#define brakestep3	brakestep*3
#define brakestep4	brakestep*4
#define brakestep5	brakestep*5
#define brakestep6	brakestep*6
#define brakestep7	brakestep*7
#define brakestep8	brakestep*8
#define brakestep9	brakestep*9
#define brakestep10	brakestep*10

xdata unsigned int Brake_Cnt;
xdata unsigned int Brake_Duty;

void SW_HOT_COOL_LED(void){
	if(SW_HOT_COOL){//&& (ZERO_Check == 0)
		SW_HOT_Cnt0++;
		if(SW_HOT_Cnt0 > 1){// Led Duty Period
			SW_HOT_Cnt0 = 0;
			SW_HOT_Cnt++;
		}
		led_th = (abs(SW_HOT_Cnt)/128);// Led Duty
		led_Cnt++;
		
		#if (Uart_Debug == 0)
			if(led_th < led_Cnt){
				led_a = 0;
				led_b = 0;
				led_c = 1;
			}
			else{
				led_a = 1;
				led_b = 0;
				led_c = 0;
			}
		#endif
	}
}
void Brake_Motor(void)
{
			Brake_Cnt++;
			if(Brake_Cnt < brakestep2)
			{
				Brake_Duty = 600;
			}
			else if((Brake_Cnt >= brakestep2)&&(Brake_Cnt < brakestep3))
			{
					Brake_Duty = 700;
			}
			else if((Brake_Cnt >= brakestep3)&&(Brake_Cnt < brakestep4))
			{
					Brake_Duty = 0;
			}
			else if((Brake_Cnt >= brakestep4)&&(Brake_Cnt < brakestep5))
			{
					Brake_Duty = 700;
			}
			else if((Brake_Cnt >= brakestep5)&&(Brake_Cnt < brakestep6))
			{
					Brake_Duty = 800;
			}
			else if((Brake_Cnt >= brakestep6)&&(Brake_Cnt < brakestep7))
			{
					Brake_Duty = 1000;
			}
			else if((Brake_Cnt >= brakestep7)&&(Brake_Cnt < brakestep8))
			{
					Brake_Duty = 1100;
			}
			else if(Brake_Cnt >= brakestep8)
			{
					Brake_Duty = MPWMDATA_REGS;
					Brake_Cnt = brakestep9+1;
					MPWMCONT = 0x74;
					SYNC = 0x55;
			}
			//MOTOR_CONT1 &= ~(0x40);
			MOTOR_CONT1 &= ~(MPWMSEL);// Enable PWM User Mode
			SFR_PAGE = 1;	//MPWM_Duty_U
			MPWMDATA = Brake_Duty;
			SYNC = 0x55;
			SFR_PAGE = 2;	//MPWM_Duty_V
			MPWMDATA = Brake_Duty;
			SYNC = 0x55;
			SFR_PAGE = 3;	//MPWM_Duty_W
			MPWMDATA = Brake_Duty;
			SYNC = 0x55;
}

void i2c_Init (void)
{
		unsigned char i2c_Buf;
		PINCONG1 &= 0xCF;
		PINCONG1 |= 0x10;
		i2c_Buf = i2c_readbyte(0x01);
		PINCONG1 &= 0xCF;
		PINCONG1 |= 0x20;
		i2c_Cnt = i2c_Buf;
		if((i2c_Buf >= 0) && (i2c_Buf <= 2)){
			i2c_Cnt = i2c_Buf;
		}else{ // eeprom init
			i2c_Cnt = 0;
			PINCONG1 &= 0xCF;
			PINCONG1 |= 0x10;
			i2c_writebyte(0x01,i2c_Cnt);
			PINCONG1 &= 0xCF;
			PINCONG1 |= 0x20;
		}
}

void Key_Scan (unsigned char Level)
{
	// HEAT key press
	if(HEAT_SW == 0) HEAT_SW_Cnt++;
	if(HEAT_SW_Cnt >= 50) HEAT_SW_Cnt = 50; // 50 * 16ms
	
	if(HEAT_SW_OLD != HEAT_SW){
		if(HEAT_SW == 1){ // switch release update this
			if(HEAT_SW_Cnt >= 15){ // Long press 100 * 16ms (Pay attention to avoid continuous store eeprom after long press)
				//i2c_Cnt = Heat_Lever = 0;
				//i2c_Save = 1;
				SW_HOT_COOL = 1;
			}else if((HEAT_SW_Cnt >= 1) && (HEAT_SW_Cnt < 15)){ // Short press
				if(++Heat_Level >= 3) Heat_Level = 0;
				i2c_Cnt = Heat_Level;
				i2c_Save = 1;
				SW_HOT_COOL = 0;
			}
			HEAT_SW_Cnt = 0; // Pay attention to avoid continuous store eeprom after long press
		}else{
			HEAT_SW_FLAG = 1;// Press FLAG
		}
	}
	HEAT_SW_OLD = HEAT_SW;
	
	if(Level == 0){
		SystemState = 0;
		MotorErrorState = 0;
		SpeedCmd = 0;
	}else if(Level == 1){
		SystemState = 1;
		SpeedCmd = Speed_Level_1;
	}else if(Level == 2){
		SystemState = 1;
		SpeedCmd = Speed_Level_2;
	}
}

xdata unsigned int ZERO_Count = 0;
xdata unsigned char ZERO_Check_Cnt = 0;
xdata unsigned int Heat_Control_Cnt = 0;
idata unsigned int Heat_Sin = 0;
idata unsigned int Heat_Ac = 0;
idata unsigned int Heat_Duty_Cmd = 0;
bit ZERO_Check = 0;
idata signed int SW_LPF_IN;
idata signed int SW_LPF_OUT = 1023;
idata signed int SW_LPF_PREOUT = 1023;
void Heat_Control (void)
{
		//idata unsigned int i;
		//for(i=0;i<10;i++);
		// Confirm the motor is running before star up heating
		if((MotorState == M_RUN)&&(Error_Status == 0)){
//				SW_LPF_IN     = Heat_Temperture_avg;
//				SW_LPF_OUT    = SW_LPF_PREOUT + (((signed long)381 * (SW_LPF_IN - SW_LPF_PREOUT)) >> 15);
//				SW_LPF_PREOUT = SW_LPF_OUT;
			if(Heat_Temperture_avg > Heat_Level_Error){
				if((Heat_Temperture_avg > Heat_Level_Temperture_Cmd) && (Heat_Level_Temperture_Cmd < 1023))
				{
					if(Heat_Cnt <= Heat_Sin) HEAT = HEAT_ON; // 2:1 sine-wave
					else              HEAT = HEAT_OFF;
				}else               HEAT = HEAT_OFF;
			}else{
				// Heat_Temperture_Error
				HEAT = HEAT_OFF;
			}
			
			// Enabe temperature feedback
			if(++Heat_Control_Cnt >= 10)
			{
				Heat_Control_Cnt = 0;
				// Pi Control
				//Pi_Ctrl(Heat_Lever_Temperture_Cmd,Heat_Temperture_avg);
				//Heat_Enalbe_Cnt = Pi_Out_Set;
				// open loop
				if(Heat_Enalbe_Cnt > Heat_Duty_Cmd) Heat_Enalbe_Cnt -= 1;
				else if(Heat_Enalbe_Cnt < Heat_Duty_Cmd) Heat_Enalbe_Cnt += 1;
				// close loop
//				if(Heat_Temperture_avg < Heat_Lever_Temperture_Cmd)
//				{
//					Heat_Enalbe_Cnt++;
//				}else{
//					Heat_Enalbe_Cnt--;
//				}
			}
			
			if(Heat_Enalbe_Cnt >= Heat_Period_Max)      Heat_Enalbe_Cnt = Heat_Period_Max;
			else if(Heat_Enalbe_Cnt <= Heat_Period_Min) Heat_Enalbe_Cnt = Heat_Period_Min;
		}else{
			Heat_Enalbe_Cnt = Heat_Period_Max;
			HEAT = HEAT_OFF;
		}
		//if(Heat_Cnt >= 5) Heat_Cnt = 0;
		if(Heat_Cnt >= Heat_Enalbe_Cnt) Heat_Cnt = 0;
}

void Heat_Level_Fun (void)
{
	if(SW_HOT_COOL == 0)
	{
		if(Heat_Level == 0){
			SW_HOT_COOL = 0;
			Heat_Duty_Cmd = Heat_Period_Max;
			Heat_Level_Temperture_Cmd = 1023;
		}else if(Heat_Level == 1){
			Heat_Sin = Heat_Sin_1;
			Heat_Duty_Cmd = Heat_Duty_1;
			Heat_Level_Temperture_Cmd = Heat_Level_1;
		}else if(Heat_Level == 2){
			Heat_Sin = Heat_Sin_2;
			Heat_Duty_Cmd = Heat_Duty_2;
			Heat_Level_Temperture_Cmd = Heat_Level_2;
		}
	}
	else
	{
		if(SW_HOT_Cnt > 0)
		{
			Heat_Sin = Heat_Sin_2;
			Heat_Duty_Cmd = Heat_Duty_2;
			Heat_Level_Temperture_Cmd = Heat_Level_2;
		}
		else
		{
			Heat_Duty_Cmd = Heat_Period_Max;
			Heat_Level_Temperture_Cmd = 1023;
		}
	}
}

void Speed_Level_Fun (void)
{
	if(SPEED_ZC_OLD != SPEED_ZC){
		Speed_Low_Cnt = 0;
		Speed_High_Cnt++;
		if(Speed_High_Cnt >= 3){ // 30ms
			Speed_High_Cnt = 3;
			Speed_Level = 2;
		}
	}else{
		Speed_Low_Cnt++;
		if(Speed_Low_Cnt >= 30){ // 30ms
			Speed_High_Cnt = 0;
			Speed_Low_Cnt = 30;
			Speed_Level = 1;
		}
	}
	SPEED_ZC_OLD = SPEED_ZC;
}

void Error_Priority(void)
{
	// 0:Normal 1:Stalling 2:Heat over temperature 3:Mos over temperature 4:Lack phase 5:Over current 6:Heat open circuit 7:Under voltage 8:Motor error 9:Over voltage
	// Priority: 0->highest ~ 9->Lowest
	if((MotorErrorState == 0x00) && (HeatErrorState == 0x00)){
		Error_Status = 0;
	}else if(MotorErrorState == FaultLock){
		Error_Status = 1;
	}else if(HeatErrorState == HeatOverTemperture){
		Error_Status = 2;
	}else if(MotorErrorState == OverTemperture){
		Error_Status = 3;
	}else if((MotorErrorState == LackPhase) || (MotorErrorState == LackPhaseRun)){
		Error_Status = 4;
	}else if(MotorErrorState == AOCP){
		Error_Status = 5;
	}else if(HeatErrorState == HeatOpenCircuit){
		Error_Status = 6;
	}else if(MotorErrorState == UnderVbus){
		Error_Status = 7;
	}else if(MotorState == M_ERROR){
		Error_Status = 8;
	}else if(MotorErrorState == OverVbus){
		Error_Status = 9;
	}
}

#if (Uart_Debug == 0)
xdata unsigned int Led_Delay_Cnt = 0;
xdata unsigned int LED_F_cnt = 0;
xdata unsigned int LED_S_cnt = 0;
void Led_Control(unsigned char Enable,unsigned char Error_Temp)
{
	if(Enable == 0)
	{
		// Error_Temp
		// 0:Normal 1:Stalling 2:Heat over temperature 3:Mos over temperature 4:Lack phase 5:Over current 6:Heat open circuit 7:Under voltage 8:Motor error 9:Over voltage
		switch(Error_Temp)
		{
			case 0:
				LED_F_cnt = 0;
				Led_Show();
				break;
			case 1:
				LED_F_cnt++;
				if(LED_F_cnt >= 250)
				{
					LED_F_cnt = 0;
					led_a = 0;
					led_b = 0;
					led_c = ~led_c; 
				} 
				break;
			case 2:
				LED_S_cnt++;
				if(LED_S_cnt >= 2000)
				{
					LED_S_cnt = 0; 
				}
				Led_Flash(1,2,200); 
				break;
			case 3:
				LED_S_cnt++;
				if(LED_S_cnt >= 2000)
				{
					LED_S_cnt = 0; 
				}
				Led_Flash(1,3,200); 
				break;
			case 4:
				LED_F_cnt++;
				if(LED_F_cnt >= 250)
				{
					LED_F_cnt = 0;
					led_a = 0;
					led_b = ~led_b;
					led_c = 0; 
				} 
				break; 
			case 5:
				LED_S_cnt++;
				if(LED_S_cnt >= 2000)
				{
					LED_S_cnt = 0; 
				}
				Led_Flash(2,2,200); 
				break;
			case 6:
				LED_S_cnt++;
				if(LED_S_cnt >= 2000)
				{
					LED_S_cnt = 0; 
				}
				Led_Flash(2,3,200); 
				break; 
			case 7:
				LED_F_cnt++;
				if(LED_F_cnt >= 250)
				{
					LED_F_cnt = 0;
					led_a = ~led_a;
					led_b = 0;
					led_c = 0; 
				} 
				break; 
//			case 8:
//				LED_S_cnt++;
//				if(LED_S_cnt >= 400)
//				{
//					LED_S_cnt = 0;
//				}
//				LED_FLASH(3,2,40);
//				break;
			case 9:
				LED_S_cnt++;
				if(LED_S_cnt >= 2000)
				{
					LED_S_cnt = 0; 
				}
				Led_Flash(3,3,200); 
				break; 
		}
	}
	else
	{
					led_a = 0;
					led_b = 0;
					led_c = 0; 
	}
}

void Led_Show(void)
{
	if(!SW_HOT_COOL){
		if(Heat_Level == 0){
			led_a = 0;
			led_b = 0;
			led_c = 1;
		}else if(Heat_Level == 1){
			led_a = 0;
			led_b = 1;
			led_c = 0;
		}else if(Heat_Level == 2){//&&(VDC_BUS_LOW_200V_F == 0)
			led_a = 1;
			led_b = 0;
			led_c = 0;
		}
	}
}

void Led_Flash(unsigned char num,unsigned char cnt,unsigned int time)
{
	unsigned char k;
	unsigned int j;
	j = time*(cnt+2);
	k = (LED_S_cnt/time)%2;
	if(LED_S_cnt < j){
		if(num == 1){
			if(k == 0){
				led_a = 0;
				led_b = 0;
				led_c = 1;
			}else{
				led_a = 0;
				led_b = 0;
				led_c = 0;
			}
		}else if(num == 2){
			if(k == 0){
				led_a = 0;
				led_b = 1;
				led_c = 0;
			}else{
				led_a = 0;
				led_b = 0;
				led_c = 0;
			}
		}else if(num == 3){
			if(k == 0){
				led_a = 1;
				led_b = 0;
				led_c = 0;
			}else{
				led_a = 0;
				led_b = 0;
				led_c = 0;
			}
		}
	}else{
				led_a = 0;
				led_b = 0;
				led_c = 0;
	}
}
#endif
#if 0
void heat_out(void){
	if(heat_level == 1)
	{
		trig_ter = 927;
		trig_out_pwm = 0;
		pi_err = 0;
		pi_u = 0;
		pi_out = 0;
		pi_out_set = 0;
		pi_sum = 0;
	}//
	else if(heat_level == 2)
	{
		trig_ter = 770;
	}//
	else if(heat_level == 3)
	{
		trig_ter = 530;
	}//
	
	if(Triac_ctr_f){
		Triac_ctr_f = 0;
		pi_ctrl(NTC_data,trig_ter);//trig_ter NTC_data_avg
		trig_out_temp = pi_out_set;
	}
	
	if(!SW_HOT_COOL)       trig_out_pwm = trig_out_temp;
	else                   trig_out_pwm = trig_ter_temp;
	if(NTC_data_avg < 500) trig_out_pwm = 0;
}

void NTC_Protect(void){
	//NTC_data_avg = NTC_data;
	if((heat_level > 1) && (delay_heat_time >= 50)){
		if(NTC_data_avg <= 150){ //
			NTCP_cnt++;
			if(NTCP_cnt >= 255){
				NTCP_cnt = 255;
				//fan_SW = 5;
				//if(Error_temp == 0) Error_temp = 2;
			}
		}
		else if(NTC_data_avg >= 1020){ //
		{
			NTCOP_cnt++;
			if(NTCOP_cnt >= 255){
				NTCOP_cnt = 255;
				//fan_SW = 5;
				//Error_temp = 6;
			}
		}
	}
}

void speed_out(void)
{
	if(speed_level == 1)
	{
		speed_set = VDC_BUS_data*18;
		if(speed_set > 8500) speed_set = 8500;
	}
	else if(speed_level == 2)
	{
		speed_set = VDC_BUS_data*18;
		if(speed_set > 10500) speed_set = 10500;
	}
}
#endif
