#include "ps2_porting.h"
#include "AI8051U.h"
#include "AI8051U_Uart.h"
#include "robot_arm.h"
#include "buzzer.h"
#include "led.h"
#include "intrins.h"
#include "string.h"
#include "global.h"

PS2HandleTypeDef ps2;

uint8_t xdata ps2_buff[MAX_PS2_RB_BUFFER_LENGTH] = {0};	

float map_x, map_y, map_z;

void Delay20ms(void)	//@40MHz
{
	unsigned long edata i;

	_nop_();
	_nop_();
	i = 199998UL;
	while (i) i--;
}


static float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return out_min + (x - in_min) * ((out_max - out_min) / (in_max - in_min));
}


static void ps2_run(PS2HandleTypeDef* self)
{
	if(self->mode == PS2_SINGLE_SERVO_MODE)
	{
		if(self->keyvalue.button0.bits0.bit_triangle)
		{
			robot_arm_knot_run(1, PS2_SET_MAX_DUTY, self->action_running_time);
		}	
		else if(self->keyvalue.button0.bits0.bit_cross)
		{
			robot_arm_knot_run(1, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		
		else if(self->keyvalue.button0.bits0.bit_triangle == 0 && self->keyvalue.button0.bits0.bit_cross == 0)
		{
			if(self->last_keyvalue.button0.bits0.bit_triangle != 0 || self->last_keyvalue.button0.bits0.bit_cross != 0)
			{
				robot_arm_knot_stop(1);
			}
		}

		if(self->keyvalue.button0.bits0.bit_square)
		{
			robot_arm_knot_run(2, PS2_SET_MAX_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_circle)
		{
			robot_arm_knot_run(2, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_square == 0 && self->keyvalue.button0.bits0.bit_circle == 0)
		{
			if(self->last_keyvalue.button0.bits0.bit_square != 0 || self->last_keyvalue.button0.bits0.bit_circle != 0)
			{
				robot_arm_knot_stop(2);
			}
		}

		if(self->keyvalue.button0.bits0.bit_r1)
		{
			robot_arm_knot_run(3, PS2_SET_MAX_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_r2)
		{
			robot_arm_knot_run(3, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_r1 == 0 && self->keyvalue.button0.bits0.bit_r2 == 0)
		{
			if(self->last_keyvalue.button0.bits0.bit_r1 != 0 || self->last_keyvalue.button0.bits0.bit_r2 != 0)
			{
				robot_arm_knot_stop(3);
			}
		}

		if(self->keyvalue.button0.bits0.bit_l1 == 1)
		{
			robot_arm_knot_run(4, PS2_SET_MAX_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_l2 == 1)
		{
			robot_arm_knot_run(4, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.button0.bits0.bit_l1 == 0 && self->keyvalue.button0.bits0.bit_l2 == 0)
		{
			if(self->last_keyvalue.button0.bits0.bit_l1 != 0 || self->last_keyvalue.button0.bits0.bit_l2 != 0)
			{
				robot_arm_knot_stop(4);
			}
		}
		
		if(self->keyvalue.bit_up == 1)
		{
			robot_arm_knot_run(5, PS2_SET_MAX_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.bit_down == 1)
		{
			robot_arm_knot_run(5, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.bit_up == 0 && self->keyvalue.bit_down == 0)
		{
			if(self->last_keyvalue.bit_up != 0 || self->last_keyvalue.bit_down != 0)
			{
				robot_arm_knot_stop(5);
			}
		}

		if(self->keyvalue.bit_left == 1)
		{
			robot_arm_knot_run(6, PS2_SET_MAX_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.bit_right == 1)
		{
			robot_arm_knot_run(6, PS2_SET_MIN_DUTY, self->action_running_time);
		}
		else if(self->keyvalue.bit_left == 0 && self->keyvalue.bit_right == 0)
		{
			if(self->last_keyvalue.bit_left != 0 || self->last_keyvalue.bit_right != 0)
			{
				robot_arm_knot_stop(6);
			}
		}

		if(self->keyvalue.button1.bits1.bit_start == 1)
		{
			robot_arm_reset(self->action_running_time);
		}

		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.bit_up == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(0, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.bit_down == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(1, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.bit_left == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(2, 1))
				{
					action_group_reset();
					break;
				}
			}
		}		

		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.bit_right == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(3, 1))
				{
					action_group_reset();
					break;
				}
			}
		}	

		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_l1 == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(4, 1))
				{
					action_group_reset();
					break;
				}
			}
		}	
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_l2 == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(5, 1))
				{
					action_group_reset();
					break;
				}
			}
		}	
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_triangle == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(6, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_cross == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(7, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_square == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(8, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_circle == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(9, 1))
				{
					action_group_reset();
					break;
				}
			}
		}

		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_r1 == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(10, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_select == 1 && self->keyvalue.button0.bits0.bit_r2 == 1)
		{
			buzzer_toggle(100, 100, 1);
			while(1)
			{
				if(action_group_run(11, 1))
				{
					action_group_reset();
					break;
				}
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_leftjoystick_press)
		{
			buzzer_toggle(100, 100, 1);
			led_flash(2, 100, 100, 1);
			if(self->action_running_time > 400)
			{
				self->action_running_time  -= 200;
			}
		}
		
		if(self->keyvalue.button1.bits1.bit_rightjoystick_press)
		{
			buzzer_toggle(100, 100, 1);
			led_flash(2, 100, 100, 1);
			if(self->action_running_time < 10000)
			{
				self->action_running_time += 200;
			}
		}
	}
	
	if(self->mode == PS2_COORDINATE_MODE)
	{
		map_x = map((float)((uint32_t)self->keyvalue.left_joystick_y), 0.0f, 255.0f, -10.0f, 10.0f);
		map_y = map((float)((uint32_t)self->keyvalue.left_joystick_x), 0.0f, 255.0f, -10.0f, 10.0f);
		map_z = map((float)((uint32_t)self->keyvalue.right_joystick_y), 0.0f, 255.0f, -10.0f, 10.0f);
		
		robot_arm_coordinate_set(15 - map_x, -map_y, 15 - map_z, 0.0f, 500);
	}
}


static void ps2_unpack(PS2HandleTypeDef* self)
{
	uint8_t i;

	uint32_t readlen = 0;
	uint32_t available;
	uint8_t rec_data[PS2_PACKET_LENGTH];
	static uint8_t buffer_index = 0;	
	
	available = rb_getfull(&self->rb);
	if (available != 0)
	{
		available = available > PS2_PACKET_LENGTH ? PS2_PACKET_LENGTH: available;
		if(self->packet_status == PS2_PACKET_HEADER_1)			/* Step1: 找帧头 */
		{
			readlen = rb_read(&self->rb, rec_data, 1);
			self->packet_status = rec_data[0] == PS2_PACKET_HEADER ? PS2_PACKET_HEADER_2: PS2_PACKET_HEADER_1;
			self->packet.packet_header[0] = rec_data[0];			
		}
		
		if (self->packet_status == PS2_PACKET_HEADER_2)			/* Step2:获取一帧的长度信息 */
		{
			readlen = rb_read(&self->rb, rec_data, 2);
			for(i = 0; i < readlen; i++)
			{
				switch(self->packet_status)
				{
					case PS2_PACKET_HEADER_2:
						self->packet_status = rec_data[i] == PS2_PACKET_HEADER ? PS2_PACKET_DATA_LENGTH: PS2_PACKET_HEADER_1;
						self->packet.packet_header[1] = rec_data[i];
						break;		
					
					case PS2_PACKET_DATA_LENGTH:
						self->packet_status = rec_data[i] != 0 ? PS2_PACKET_DATA: PS2_PACKET_HEADER_1;
						self->packet.data_len = rec_data[i];
						buffer_index = 0;
						break;			
					
					default:
						self->packet_status = PS2_PACKET_HEADER_1;
						break;						
				}
			}
		}
		
		if (self->packet_status == PS2_PACKET_DATA)			/* Step3:获取一帧的数据信息 */
		{
			readlen = rb_read(&self->rb, rec_data, self->packet.data_len - 1);
			for(i = 0; i < readlen; i++)
			{
				self->packet.buffer[buffer_index] = rec_data[i];
				buffer_index++;	
				if(buffer_index == self->packet.data_len - 2)
				{
					self->packet_status = PS2_PACKET_HEADER_1;
					self->keyvalue.button0.buffer0 = self->packet.buffer[2];
					self->keyvalue.button1.buffer1 = self->packet.buffer[3];
					if(self->keyvalue.button1.bits1.bit_mode == 1 && self->last_keyvalue.button1.bits1.bit_mode == 0)
					{
						self->mode_button_status = !self->mode_button_status; 
					}
					
					self->mode = self->mode_button_status == 0 ? PS2_SINGLE_SERVO_MODE : PS2_COORDINATE_MODE;				
				}
			}			
		}
		
		switch(self->mode)
		{
			case PS2_SINGLE_SERVO_MODE:
				self->keyvalue.bit_left = self->packet.buffer[5] == 0x00 ? 1 : 0;
				self->keyvalue.bit_right = self->packet.buffer[5] == 0xFF ? 1 : 0;
				self->keyvalue.bit_up = self->packet.buffer[6] == 0x00 ? 1 : 0;
				self->keyvalue.bit_down = self->packet.buffer[6] == 0xFF ? 1 : 0;
				break;
			
			case PS2_COORDINATE_MODE:
				self->keyvalue.left_joystick_x = self->packet.buffer[5];
				self->keyvalue.left_joystick_y = self->packet.buffer[6];
				self->keyvalue.right_joystick_x = self->packet.buffer[7];
				self->keyvalue.right_joystick_y = self->packet.buffer[8];
				switch(self->packet.buffer[4])
				{
					case 0x00:
						self->keyvalue.bit_up = 1;
						break;
					
					case 0x01:
						self->keyvalue.bit_up = 1;
						self->keyvalue.bit_right = 1;
						break;
					
					case 0x02:
						self->keyvalue.bit_right = 1;
						break;
					
					case 0x03:
						self->keyvalue.bit_down = 1;
						self->keyvalue.bit_right = 1;
						break;
					
					case 0x04:
						self->keyvalue.bit_down = 1;
						break;
					
					case 0x05:
						self->keyvalue.bit_down = 1;
						self->keyvalue.bit_left = 1;
						break;
					
					case 0x06:
						self->keyvalue.bit_left = 1;
						break;
					
					case 0x07:
						self->keyvalue.bit_up = 1;
						self->keyvalue.bit_left = 1;
						break;
					
					case 0x0F:
						self->keyvalue.bit_up = 0;
						self->keyvalue.bit_down = 0;
						self->keyvalue.bit_left = 0;
						self->keyvalue.bit_right = 0;
						break;
				}
				break;
				
			default:
				break;
		}
		ps2_run(self);	
		memcpy(&self->last_keyvalue, &self->keyvalue, sizeof(PS2KeyValueObjectTypeDef));	
	}
}


void ps2_init()
{
	memset(&ps2, 0, sizeof(PS2HandleTypeDef));
	ps2.action_running_time = 1000;
	rb_init(&ps2.rb, ps2_buff, sizeof(ps2_buff));
	uart2_init(9600);
}

void ps2_handler()
{
	ps2_unpack(&ps2);
}

//void Uart2_isr(void) interrupt 8
//{
//	uint8_t rx_byte_h = 0;
//	uint8_t rx_byte_l = 0;
//	uint32_t rx_buf_size = 0;
//	
//	/* 超时中断 */
//	if(UR2TOSR & 0x01)
//	{
//		/* 清除超时中断标志位 */
//		UR2TOSR = 0x80;
//		rx_byte_h = DMA_UR2R_DONEH;
//		rx_byte_l = DMA_UR2R_DONE;
//		/*更新写入的数据大小*/
//		rx_buf_size = ((uint16_t)rx_byte_h << 8) | rx_byte_l;
//		rb_write(&ps2.rb, uart2_rx_buff, rx_buf_size);
//		DMA_UR2R_CR = 0;
//		DMA_UR2R_STA = 0x00;
//		/* 重新打开接收 */
//		DMA_UR2R_CR = 0xa1;
//	}
//}
