#include "common.h"
#include "drv_adas.h"
#include "drv_can.h"
#include "app_rs485_1.h"
#include "app_rs485_2.h"
#include "app_can.h"
#include "app.h"
#include "app_adas.h"


uint16_t array_juli_1[ADAS_ARRAY_JU_LI_MAX];
uint16_t array_juli_1_index = 0;

uint16_t array_juli_2[ADAS_ARRAY_JU_LI_MAX];
uint16_t array_juli_2_index = 0;

DELAY_COMMON adas_delay;
uint8_t adas_receive_stag = ADAS_RECEIVE_0;
uint16_t ju_li_1 = 0xFFFF;
uint16_t ju_li_2 = 0xFFFF;
uint8_t current_pwm_no = ADAS_PWM_0;


void Sort(uint16_t *arr,uint16_t size)
{
	uint16_t i,j,tmp;

	for(i = 0; i < size - 1;i++)
	{
		for(j = 0; j < size - i - 1;j++)
		{
			if(arr[j] > arr[j+1])
			{
				tmp = arr[j];
				arr[j] = arr[j+1];
				arr[j+1] = tmp;			
			}
		}
	}
}

void ADAS_Measure_Finish_Ju_Li_1(void)
{
	Sort(array_juli_1,ADAS_ARRAY_JU_LI_MAX);
	ju_li_1 = array_juli_1[ADAS_ARRAY_JU_LI_MAX>>1];
}

void ADAS_Measure_Finish_Ju_Li_2(void)
{
	Sort(array_juli_2,ADAS_ARRAY_JU_LI_MAX);
	ju_li_2 = array_juli_2[ADAS_ARRAY_JU_LI_MAX>>1];
}

void ADAS_Measure_Ju_Li_1(void)
{
	 uint32_t time;
	 uint16_t temp_juli = 0;
	 
	if(current_pwm_no != ADAS_PWM_1)
		return;
	
	if(gpio_input_bit_get(GPIOC,GPIO_PIN_8) == 1&&adas_receive_stag == ADAS_RECEIVE_1)
	{
	
		time = timer_counter_read(TIMER0);
		if(time > ADAS_PWM_SENDING_MAX_TIME)
		{
			//<30CM
			temp_juli = ADAS_JU_LI_JING;
			adas_receive_stag = ADAS_RECEIVE_0;
			current_pwm_no = ADAS_PWM_0;
			timer_disable(TIMER0);
		}
		else
			adas_receive_stag = ADAS_RECEIVE_2;
	}
	else if(gpio_input_bit_get(GPIOC,GPIO_PIN_8) == 0&&adas_receive_stag == ADAS_RECEIVE_2)
	{
		time = timer_counter_read(TIMER0);
		time >>= 1;

		temp_juli = (uint16_t)((double)time*YIN_SU/10);
		adas_receive_stag = ADAS_RECEIVE_0;
		current_pwm_no = ADAS_PWM_0;
		timer_disable(TIMER0);
	}

	//
	if(adas_receive_stag == ADAS_RECEIVE_0)
	{
		array_juli_1[array_juli_1_index++] = temp_juli;

		if(array_juli_1_index >= ADAS_ARRAY_JU_LI_MAX)
		{
			array_juli_1_index = 0;

			g_event |= ADAS_MEAS_1_FINISH_EVENT;
		}
	}
}

void ADAS_Measure_Ju_Li_2(void)
{
	uint32_t time;
	uint16_t temp_juli = 0;
	
	if(current_pwm_no != ADAS_PWM_2)
		return;
	
	if(gpio_input_bit_get(GPIOC,GPIO_PIN_9) == 1&&adas_receive_stag == ADAS_RECEIVE_1)
	{
	
		time = timer_counter_read(TIMER0);
		if(time > ADAS_PWM_SENDING_MAX_TIME)
		{
			//<30CM
			temp_juli = ADAS_JU_LI_JING;
			adas_receive_stag = ADAS_RECEIVE_0;
			current_pwm_no = ADAS_PWM_0;
			timer_disable(TIMER0);
		}
		else
			adas_receive_stag = ADAS_RECEIVE_2;
	}
	else if(gpio_input_bit_get(GPIOC,GPIO_PIN_9) == 0&&adas_receive_stag == ADAS_RECEIVE_2)
	{
		time = timer_counter_read(TIMER0);
		time >>= 1;

		temp_juli = (uint16_t)((double)time*YIN_SU/10);
		adas_receive_stag = ADAS_RECEIVE_0;
		current_pwm_no = ADAS_PWM_0;
		timer_disable(TIMER0);
	}

	//
	if(adas_receive_stag == ADAS_RECEIVE_0)
	{
		array_juli_2[array_juli_2_index++] = temp_juli;

		if(array_juli_2_index >= ADAS_ARRAY_JU_LI_MAX)
		{
			array_juli_2_index = 0;

			g_event |= ADAS_MEAS_2_FINISH_EVENT;
		}
	}
}


void ADAS_PWM_1_Enable(void)
{
	 timer_disable(TIMER1);

	 ADAS_PWM_Initial(ADAS_PWM_1);

	 timer_enable(TIMER2);

	 timer_enable(TIMER0);

	 timer_counter_value_config(TIMER0,0);
	 
	 adas_receive_stag = ADAS_RECEIVE_1;
	 
	 //ju_li_1 = 0;

	 current_pwm_no = ADAS_PWM_1;
}

void ADAS_PWM_2_Enable(void)
{
	 timer_disable(TIMER1);

	 ADAS_PWM_Initial(ADAS_PWM_2);
	 
	 timer_enable(TIMER2);

	 timer_enable(TIMER0);

	 timer_counter_value_config(TIMER0,0);
	 
	 adas_receive_stag = ADAS_RECEIVE_1;

	 //ju_li_2 = 0;

	 current_pwm_no = ADAS_PWM_2;
}

void ADAS_Timeout(void)
{
	
	if(adas_delay.set)
	{
		++adas_delay.count;
		if(adas_delay.count >= ADAS_ZONG_TIME)
		{
			g_event |= ADAS_PWM_1_TIMEOUT_EVENT;
			adas_delay.count = 0;
		}
		else if(adas_delay.count == ADAS_LIANG_TIME)
		{
			g_event |= ADAS_PWM_2_TIMEOUT_EVENT;
		}
	}
}

void ADAS_Enable(uint8_t enable)
{
	if(enable)
	{
		adas_delay.set = 1;
		adas_delay.count = ADAS_ZONG_TIME;
	}
	else
	{
		adas_delay.set = 0;
		adas_delay.count = 0;
	}
}
 void ADAS_Initial(void)
{
	 ADAS_Drv_Initial();
	 
	 ADAS_Enable(0);
}


void Can_ADAS_Self_Send(CAN_FRAME*can_adas_frame)
{
	uint16_t len;
	uint8_t *buf = can_adas_frame->data;

	len = 0;
	buf[len++] = (uint8_t)(KEY_ADAS_SELF_UP >> 0);
	buf[len++] = (uint8_t)(KEY_ADAS_SELF_UP >> 8);
	buf[len++] = (uint8_t)(ju_li_1 >> 0);
	buf[len++] = (uint8_t)(ju_li_1 >> 8);
	buf[len++] = (uint8_t)(ju_li_2 >> 0);
	buf[len++] = (uint8_t)(ju_li_2 >> 8);

	//
	can_adas_frame->head.rsp = FRAME_PT_NO_RSP;
	can_adas_frame->head.dest = CTR_ID;
	can_adas_frame->head.sour = SELF_ID;
	can_adas_frame->head.index = 1;
	can_adas_frame->head.total = (len - 1)/8 + 1;
	can_adas_frame->head.pro   =FRAME_PRO_D;
	//
	can_adas_frame->len = len;

	//
	if(!Send_Data_Can(can_adas_frame,ADAS_SELF))
	{
		//g_event |= ADAS_RESEND_CMD_EVENT;
		//return 0;
	}
	
}

int8_t Handle_Can_Adas_CMD(CAN_FRAME*can_adas_frame)
{
	uint16_t key;
	uint8_t *buf = can_adas_frame->data;

	memcpy(&key,can_adas_frame->data,sizeof(key));

	switch(key)
	{
		case KEY_ADAS_COMMON:
			break;
		
		case KEY_ADAS_ENABLE:
			switch(buf[2])
			{
				case 0:
					ADAS_Enable(0);
					
					break;
				case 1:
					ADAS_Enable(1);
					
					break;
				default:
					return 0;
			}
			break;
	
		case KEY_ADAS_SELF_UP:
			return 0;	
		default:
			return 0;

	}
	
	return 1;
	
}

int8_t Rsp_Can_Adas_CMD(CAN_FRAME*can_adas_frame)
{
	uint16_t key,len;
	uint8_t *buf = can_adas_frame->data;
	
	memcpy(&key,can_adas_frame->data,sizeof(key));

	switch(key)
	{
		case KEY_ADAS_COMMON:
			if((can_adas_frame->head.rsp != FRAME_PT_NEED_RSP))
				return 1;
			len = 0;
			buf[len++] = (uint8_t)(KEY_ADAS_COMMON >> 0);
			buf[len++] = (uint8_t)(KEY_ADAS_COMMON >> 8);
			buf[len++] = 0;
			buf[len++] = adas_delay.set;
			buf[len++] = (uint8_t)(ju_li_1 >> 0);
			buf[len++] = (uint8_t)(ju_li_1 >> 8);
			buf[len++] = (uint8_t)(ju_li_2 >> 0);
			buf[len++] = (uint8_t)(ju_li_2 >> 8);
			
			break;	
		case KEY_ADAS_ENABLE:
			if((can_adas_frame->head.rsp != FRAME_PT_NEED_RSP))
				return 1;
			
			len = 0;
			buf[len++] = (uint8_t)(KEY_ADAS_ENABLE >> 0);
			buf[len++] = (uint8_t)(KEY_ADAS_ENABLE >> 8);
			buf[len++] = 0;
			buf[len++] = adas_delay.set;
			break;	
		case KEY_ADAS_SELF_UP:
			return 0;
		default:
			return 0;
	}
	//
	can_adas_frame->head.rsp = FRAME_PT_RSP;
	can_adas_frame->head.dest = can_adas_frame->head.sour;
	can_adas_frame->head.sour = SELF_ID;
	can_adas_frame->head.index = 1;
	can_adas_frame->head.total = (len - 1)/8 + 1;
	can_adas_frame->head.pro   =FRAME_PRO_D;
	//
	can_adas_frame->len = len;

	//
	if(!Send_Data_Can(can_adas_frame,ADAS_RSP))
	{
		g_event |= ADAS_RESEND_CMD_EVENT;
		return 0;
	}
	
	return 1;

}