#include "process.h"
#include "gpio.h"
#include "ac780x_gpio.h"
#include "string.h"
#include "adc.h"
#include "Motor.h"
#include "W25Q64.h"
#include "AngleSensor.h"
#include "motor_control.h"
#include "cfg.h"
#include "Rtcx.h"
#include "storage.h"
#include "IB_Reader.h"
#include "math.h"


uint8_t		g_devicebusy = 0;
uint16_t	g_blinkLedTime = 0;									/*LED闪烁频率控制时间*/
uint16_t	g_blinkLedTgtTime = BLINK_LED_DFTT;	/*LED目标闪烁频率*/
uint16_t  g_IBLed_count = 0;
uint16_t	g_period1000ms = 0;
uint8_t		g_detectTime = 0;										/*状态检测时间*/
uint16_t   g_angleRead_Interval = 5;          //读取编码器角度间隔 
uint16_t   g_angleRead_Count = 0;             //读取编码器角度计数

uint16_t   g_ibRead_count =0;     //ibutton 油品读取计数 

uint32_t	g_poweroff_count = 0;				/*外部电源停止后、每秒计数加1*/

#define STATUS_DETECTINTERVAL  (10)  /*10ms */


uint8_t g_runstate; //运行状态，切换LED灯光 

volatile uint8_t g_lockstatus = STATUS_UNKOWN;
volatile uint8_t g_coverstatus = STATUS_COVERCLOSE;

//电机状态 
uint8_t g_motorstate = MOTOR_INIT;
uint8_t  g_runReady = 0;
uint16_t g_runTime = 0;

static float g_target_position = 0;  // 动作时，PID目标位置
static uint8_t g_motor_dir = MOTOR_TUNR_NULL;  		 // 电机动作时的转动方向 ，0 不动 1 正向， 2 反向

//状态持续计数，用来起到滤波作用 
//uint8_t tmp_status;
uint8_t g_lockcount[STATUS_LOCKALL]={0};
uint8_t g_covercount[STATUS_COVERALL]={0};

static float s_angle = 0.0;
//static float tmp_angle = 0.0;
static uint8_t get_lockstatus(void);
//static PID g_pid = {0}; 
//static uint8_t tmp_speed;
static uint8_t tmp_pwmout;
//static uint8_t tmp_time = 0;

static void update_runstate()
{
		switch(g_lockstatus){
			case STATUS_INTERMEDIATE:
				g_runstate = STATE_INTERMEDIATE;
				break;
			case STATUS_LOCK:
				g_runstate = STATE_LOCK;
				break;
			case STATUS_UNLOCK:
				g_runstate = STATE_UNLOCK;
				break;
			case STATUS_SAMPLE:
				g_runstate = STATE_SAMPLE;
				break;
			case STATUS_UNKOWN:
				g_runstate = STATE_UNKOWN;
				break;
			case STATUS_ENCODER_ERROR:
				g_runstate = STATE_EXCEPTION;
				break;
			default:
				break;
		};
		
		if(STATUS_COVEROPEN == g_coverstatus){
				g_runstate = STATE_OPENCOVER;
		}
	
}

void Process_Init(void)
{
		/*初始化控制变量.*/
		g_blinkLedTime 	  = 0;
		g_blinkLedTgtTime = BLINK_LED_DFTT;
	
		g_detectTime = 0;
		g_runstate = STATE_LOCK;
		g_angleRead_Interval = 5;
		g_angleRead_Count = 0;
		g_ibRead_count =0;
	
//		s_angle = AngleSensor_GetAngle();
//		g_lockstatus=get_lockstatus();
//		g_coverstatus=Gpio_IsOpenCover()>0?STATUS_COVEROPEN:STATUS_COVERCLOSE;

		g_motorstate = MOTOR_READY;
	
		/*上电默认LED点亮.*/
//		update_runstate();
}

void Process_RunPrd(void)
{
	
	/*周期性地检查LED闪烁,LED2和LED3同时闪烁.*/
	if (g_blinkLedTime >= g_blinkLedTgtTime)
	{
		g_blinkLedTime = 0;
		g_blinkLedTgtTime = BLINK_LED_DFTT;
		
		g_coverstatus=Gpio_IsOpenCover();
	
		update_runstate();
		
		switch(g_runstate){
			case STATE_LOCK:
				REDLED_OFF;
				GREENLED_TOGGLE;
				break;
			case STATE_UNLOCK:
				GREENLED_OFF;
				REDLED_TOGGLE;
				break;
			case STATE_INTERMEDIATE:
				REDGREEN_TOGGLE;
				break;
			case STATE_SAMPLE:
				REDLED_OFF;
				GREENLED_TOGGLE;
				g_blinkLedTgtTime = 200;
				break;
			case STATE_UNKOWN:
				REDLED_OFF;
			  GREENLED_ON;
				break;
			case STATE_OPENCOVER:
			case STATE_EXCEPTION:
			default:
				GREENLED_OFF;
			  REDLED_ON;
				break;
		
		};
		
		if(g_ibRead_count >= 1000){
			g_ibRead_count = 0;
			
			//在控制电机转动时， 不对ibutton 进行读取 
			if((MOTOR_READY == g_motorstate) || (MOTOR_STOPED == g_motorstate)){
				IBRead_OilType();
			}
			
		}
		
		if(g_IBLed_count >= 500){
			g_IBLed_count = 0;
			if(OIL_TYPE_NULL != IBGet_OilType()){
					IB_RED_OFF;
					IB_GREEN_TOGGLE;
			}else{
					IB_GREEN_OFF;
					IB_RED_TOGGLE;
			}
		
		}
	
		//printADCValue();
		//printMotorCurrent();
		//printf(" Motor Current:%f mA \r\n", getMotorCurrent());
		
		//W25Q64_PrintInfo();
		//AngleSensor_PrintInfo();

		
	}
	
	
	//
	if(g_period1000ms >= 1000){
		g_period1000ms=0;
		
		//Storage_CountReduce();
		
		//g_poweroff_count++;
		printf(" opencover : %d \r\n", g_coverstatus);
		
		
		//printMotorCurrent();
		
		//printf(" Battery Voltage:%f V \r\n", getBatteryVoltage());
		
		//RTCx_PrintDateTime();
		//AngleSensor_PrintInfo();
		//IB_Print();
		
	}

	
}

static uint8_t get_motor_dir(float target){
	
	if(target > s_angle ){
		return MOTOR_TUNR_P; // 正转 
	}else if(target < s_angle){
		return MOTOR_TUNR_N; // 反转 
	}
	
	return MOTOR_TUNR_NULL; 
}

void Process_Timer1_CB(void)
{
		if (g_blinkLedTime < 0xFFFF)
		{
				g_blinkLedTime++;
		}
		
		if(g_detectTime < 0xFF){
				g_detectTime++;
		}
		
		
		if(g_runReady){
			g_runTime++;
		}
		
		if(g_period1000ms < 0xFFFF)
		{
			g_period1000ms++;
		}
		
		if(g_ibRead_count < 0xFFFF){
			g_ibRead_count++;
		}
		
		if(g_IBLed_count < 0xFFFF){
			g_IBLed_count++;
		}
		
		g_angleRead_Count++;
		if(g_angleRead_Count >= g_angleRead_Interval){
			
				g_angleRead_Count=0;
				AngleSensor_Read();
		
		}
}

void Process_Timer0_CB(void)
{
	AngleSensor_Read();
	
}

void Process_MotorControl(float angle)
{
	//printf("P_M angle: %f \r\n", angle);
	s_angle = angle;
	
	if(s_angle < 0){ //处理异常
		if((MOTOR_READY != g_motorstate) && (MOTOR_STOPED != g_motorstate)){
				Motor_Brake();
				g_motorstate = MOTOR_STOPED;
				g_runReady =1;
				g_runTime=0;
		}
	}
	
	g_lockstatus=get_lockstatus();

	//处理开关锁  
	switch(g_motorstate){
		case MOTOR_READY:
			break;
		case MOTOR_LOCKING:
			if(/*(STATUS_LOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
				Motor_Brake();
				g_motorstate = MOTOR_STOPED;
				g_runReady =1;
				g_runTime=0;
			}else{

				if(angle < g_target_position){

						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
						if(MOTOR_TUNR_P == g_motor_dir){
							Motor_Positive(tmp_pwmout);
						}else if(MOTOR_TUNR_N == g_motor_dir){
							Motor_Negative(tmp_pwmout);
						}
						
						
				
				}else{
					Motor_Brake();
					g_motorstate = MOTOR_STOPED;
					g_runReady =1;
					g_runTime=0;
				}
				
			}
			
			break;
		case MOTOR_UNLOCKING:
			if(/*(STATUS_UNLOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
				Motor_Brake();
				g_motorstate = MOTOR_STOPED;
				g_runReady =1;
				g_runTime=0;
			}else{

				if(angle > g_target_position){

						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
						if(MOTOR_TUNR_P == g_motor_dir){
							Motor_Positive(tmp_pwmout);
						}else if(MOTOR_TUNR_N == g_motor_dir){
							Motor_Negative(tmp_pwmout);
						}
				
				}else{
					Motor_Brake();
					g_motorstate = MOTOR_STOPED;
					g_runReady =1;
					g_runTime=0;
				}
				
			}
			break;
			
		case MOTOR_SAMPLEING:
			if(/*(STATUS_SAMPLE == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
				Motor_Brake();
				g_motorstate = MOTOR_STOPED;
				g_runReady =1;
				g_runTime=0;
			}else{

				if(angle > g_target_position){
						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
						if(MOTOR_TUNR_P == g_motor_dir){
							Motor_Positive(tmp_pwmout);
						}else if(MOTOR_TUNR_N == g_motor_dir){
							Motor_Negative(tmp_pwmout);
						}
				
				}else{
					Motor_Brake();
					g_motorstate = MOTOR_STOPED;
					g_runReady =1;
					g_runTime=0;
				}
				
			}
			break;
		case MOTOR_STOPED:
			if(g_runTime >= 5000){ //5S
				g_motorstate = MOTOR_READY;
				g_angleRead_Interval = 500;
			}
			
			g_angleRead_Interval = 200;
			
			break;
		default:
			break;

	};

}


uint8_t get_lockstatus(void)
{
		static float _tmpf_min=0;
		static float _tmpf_max = 0;
		uint8_t status = STATUS_UNKOWN;
		if(s_angle<0){
			return STATUS_ENCODER_ERROR;
		}
		
	
		if((config->lock_threshold - config->unlock_threshold) > 10.0){
			
			if((s_angle > config->lock_threshold)){
					status = STATUS_LOCK;	
			}else if(s_angle < config->unlock_threshold){
					status = STATUS_UNLOCK;	
			}else {
					
				if(fabsf(config->sample_threshold1 - config->sample_threshold2) >  10.0 )
				{
					 if(config->sample_threshold1 > config->sample_threshold2){
							_tmpf_max = config->sample_threshold1;
							_tmpf_min = config->sample_threshold2;
						}else{
							_tmpf_max = config->sample_threshold2;
							_tmpf_min = config->sample_threshold1;
						}
						
						if((s_angle > _tmpf_min) && (s_angle < _tmpf_max)){
							status = STATUS_SAMPLE;	
						}else{
							status = STATUS_INTERMEDIATE;	
						}
						
				 }else{ //不支持取样
					status = STATUS_INTERMEDIATE;	
				 }
					
			}
				
		}
		
		return status;
}


uint16_t Process_GetOilType(void)
{
	return IBGet_OilType();
}

uint8_t Process_GetLockStatus(void)
{
		return get_lockstatus();
}
	
uint8_t Process_GetCoverStatus(void)
{
		if(Gpio_IsOpenCover()){
			return STATUS_COVEROPEN;
		}else{
			return STATUS_COVERCLOSE;
		}
}


uint8_t Process_OpLock(uint8_t speed)
{

	if((MOTOR_READY == g_motorstate) && (STATUS_LOCK != g_lockstatus)){

		g_motorstate = MOTOR_LOCKING;
		g_runReady =1;
		g_runTime=0;
		g_angleRead_Interval = 5;
		
		g_target_position = config->lock_threshold+3;
		MC_Init(s_angle);
		g_motor_dir = get_motor_dir(g_target_position);
		
		printf("Process_OpLock\r\n");
		

	}
	
	return 0;
}

uint8_t Process_OpUnlock(uint8_t speed)
{
//	uint8_t id = 0;
	
	if((MOTOR_READY == g_motorstate) && (STATUS_UNLOCK != g_lockstatus)){

		g_motorstate = MOTOR_UNLOCKING;
		g_runReady =1;
		g_runTime=0;
		g_angleRead_Interval = 5;
		
		g_target_position = config->unlock_threshold-3;
		MC_Init(s_angle);
		g_motor_dir = get_motor_dir(g_target_position);
		
		printf("Process_OpUnlock\r\n");
		
	}

	return 0;
}

uint8_t Process_OpSample(uint8_t speed)
{
//	uint8_t id = 0;
	// 参数不符合要求
	if(fabsf(config->sample_threshold1 - config->sample_threshold2) <=  10.0 ){
		return 0;
	}
	
	if((MOTOR_READY == g_motorstate) && (STATUS_LOCK == g_lockstatus)){

		g_motorstate = MOTOR_SAMPLEING;
		g_runReady =1;
		g_runTime=0;
		g_angleRead_Interval = 5;
		
	  g_target_position = (config->sample_threshold1 + config->sample_threshold2)/2;
		MC_Init(s_angle);
		g_motor_dir = get_motor_dir(g_target_position);
		
		printf("Process_OpSample \r\n");
		

	}

	return 0;
}

float Process_GetAngle(void)
{
	return s_angle;
}

uint8_t Process_AngleCalibration(void)
{
		float angle_raw = AngleSensor_GetAngleRaw();
	 if(angle_raw >= 0){
			config->angle_offset = (200 - angle_raw);
		  AngleSensor_Setoffset(config->angle_offset);
	 }
	 
	 return 0;
}


uint8_t Process_CalibrationThreshold(uint8_t param)
{
	if(0 == param){
		config->unlock_threshold = s_angle;
	}else if(1 == param){
		config->lock_threshold = s_angle;
	}else if(2 == param){
		config->sample_threshold1 = s_angle;
	}else if(3 == param){
		config->sample_threshold2 = s_angle;
	}
	
	return 0x00;
}

void Process_Storage(void)
{
		static uint8_t pre_lstatus = STATUS_INTERMEDIATE;
		static uint8_t pre_cstatus = STATUS_COVEROPEN;
	
		if(Gpio_IsDC24()){
			
			pre_lstatus = g_lockstatus;
			pre_cstatus = g_coverstatus;
			
			
		}else{
			
			if(pre_lstatus != g_lockstatus){
					if(STATUS_LOCK == g_lockstatus){
						
						Storage_AddItem(ITEM_RECORD, EVENT_MANUAL_LOCK);
					
					}else if(STATUS_UNLOCK == g_lockstatus){
						Storage_AddItem(ITEM_RECORD, EVENT_MANUAL_UNLOCK);
					}
				
					pre_lstatus = g_lockstatus;
			
			}
			
			if(pre_cstatus != g_coverstatus){
				if(STATUS_COVEROPEN == g_coverstatus){
					Storage_AddItem(ITEM_RECORD, EVENT_OPENCOVER);
				}
				
				pre_cstatus = g_coverstatus;
			
			}
			
		}
		
		

}

void Process_Poweroff(void)
{
	static float battery_v = 0.0;
		if(Gpio_IsDC24()){
			
			g_poweroff_count = 0;
		}else{
			
			battery_v = getBatteryVoltage();
			
			if((g_poweroff_count >= 2*60*60) || (battery_v < (3.3))){ // 2小时后或电池电压小于3.3V 关机 
				
					g_poweroff_count = 0;
					Storage_Save();
					mdelay(500);
					LDOEn_DISABLE;
					//NVIC_SystemReset();
					//printf(" NVIC_SystemReset  \r\n");
				
			}
		}

}

void Process_MotorP(uint8_t speed)
{
		if(speed > 0){
			Motor_Positive(speed);
		}else{
			Motor_Brake();
		}
		
}
void Process_MotorN(uint8_t speed)
{
		if(speed > 0){
			Motor_Negative(speed);
		}else{
			Motor_Brake();
		}
}