#include "pc_porting.h"
#include "AI8051U.h"
#include "AI8051U_Uart.h"
#include "string.h"
#include "robot_arm.h"
#include "intrins.h"
#include "buzzer.h"
#include "stdio.h"
#include "led.h"

PCHandleTypeDef pc;

uint8_t xdata pc_buff[MAX_PC_RB_BUFFER_LENGTH] = {0};	
static uint8_t tx1_state = 0;

static void pc_delay(void)	//@40MHz 20ms
{
	unsigned long edata i;

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

void uart1_send_buffer(uint8_t* buf, uint16_t len)
{
	if(tx1_state == 0)
	{
		uint16_t data_len = len - 1;
		uint16_t arry_num;
		tx1_state = 1;
		if(len > Uart1_Tx)
		{
			return;
		}
		DMA_UR1T_AMT = (uint8_t)data_len;
		DMA_UR1T_AMTH = (uint8_t)(data_len >> 8);
		for(arry_num = 0; arry_num < len; arry_num++)
		{
			uart1_tx_buff[arry_num] = buf[arry_num];
		}
		DMA_UR1T_CR = 0xc0;		/* 使能UR1T_DMA功能 */
		tx1_state = 1;			/* 等待数据发送完成 */
	}
}

static void pc_packet_transmit(FunctionStatus cmd, uint8_t* _pdata, uint8_t len)
{
	uint8_t i;
	uint8_t _data[30];
	
	if(len == 0)
	{
		_data[0] = PC_PACKET_HEADER;
		_data[1] = PC_PACKET_HEADER;
		_data[2] = 2;
		_data[3] = cmd;

		uart1_send_buffer(_data, 4);
	}
	else
	{
		_data[0] = PC_PACKET_HEADER;
		_data[1] = PC_PACKET_HEADER;
		_data[2] = 2 + len;
		_data[3] = cmd;		
		
		for(i = 0; i < len; i++)
		{
			_data[4 + i] = _pdata[i];
		}
		
		uart1_send_buffer(_data, 4 + len);
	}
}

static void unpack(PCHandleTypeDef* self)
{
	uint8_t i;
	uint8_t rec_data[MAX_PC_READ_BUFFER_LENGTH];
	
	uint32_t readlen = 0;
	uint32_t available;
	static uint8_t buffer_rec_finish = 0;
	
	available = rb_getfull(&self->rb);
	
	available = available > MAX_PC_READ_BUFFER_LENGTH ? MAX_PC_READ_BUFFER_LENGTH: available;

	while(available > 0)
	{
		switch(self->packet_status)
		{	
			/* Step1: 找帧头 */
			case PACKET_HEADER_1:
				rb_read(&self->rb, rec_data, 1);
				self->packet_status = rec_data[0] == PC_PACKET_HEADER ? PACKET_HEADER_2: PACKET_HEADER_1;
				if(rec_data[0] == PC_PACKET_HEADER)
				{
					self->packet.packet_header[0] = rec_data[0];
				}
				break;	

			case PACKET_HEADER_2:
				rb_read(&self->rb, rec_data, 1);
				self->packet_status = rec_data[0] == PC_PACKET_HEADER ? PACKET_DATA_LENGTH: PACKET_HEADER_1;
				if(rec_data[0] == PC_PACKET_HEADER)
				{
					self->packet.packet_header[1] = rec_data[0];
				}					
				break;	

			case PACKET_DATA_LENGTH:
				rb_read(&self->rb, rec_data, 1);
				self->packet.data_len = rec_data[0];
				
				if(self->packet.data_len < 2)
				{
					self->packet_status = PACKET_HEADER_1;
					self->packet.data_len = rec_data[0];
				}
				else
				{
					self->packet_status = PACKET_FUNCTION;
				}
				break;

			case PACKET_FUNCTION:
				rb_read(&self->rb, rec_data, 1);
				self->packet_status = rec_data[0] < CMD_FUNC_NULL ? PACKET_DATA : PACKET_HEADER_1;
				self->packet.cmd = rec_data[0];	
				break;

			case PACKET_DATA:
				readlen = rb_read(&self->rb, rec_data, self->packet.data_len - 2);
				for(i = 0; i < readlen; i++)
				{
					self->packet.buffer[i] = rec_data[i];
				}
				buffer_rec_finish = 1;
				break;	

			default:
				self->packet_status = PACKET_HEADER_1;
				break;					
		}

		switch(self->packet.cmd)
		{
			case 1:
				self->status = CMD_VERSION_QUERY;
				break;
			
			case 2:
				self->status = CMD_SERVO_OFFSET_READ;
				break;
			
			case 3:
				self->status = CMD_MULT_SERVO_MOVE;
				break;

			case 4:
				self->status = CMD_COORDINATE_SET;
				break;
			
			case 6:
				self->status = CMD_ACTION_GROUP_RUN;
				action_group_reset();
				break;
			
			case 7:
				self->status = CMD_FULL_ACTION_STOP;
				break;
			
			case 8:
				self->status = CMD_FULL_ACTION_ERASE;
				break;

			case 10:
				self->status = CMD_SERVO_OFFSET_SET;
				break;

			case 11:
				self->status = CMD_SERVO_OFFSET_DOWNLOAD;
				break;		

			case 12:
				self->status = CMD_SERVOS_RESET;
				break;	
			
			case 13:
				self->status = CMD_ANGLE_BACK_READING;
				break;
			
			case 25:
				self->status = CMD_ACTION_DOWNLOAD;
				break;
			
			default:
				self->status = CMD_FUNC_NULL;
				break;	
		}
	
		if(buffer_rec_finish == 1)
		{
			buffer_rec_finish = 0;
			self->packet_status = PACKET_HEADER_1;
			break;
		}
		
		available = rb_getfull(&self->rb);
	}
}

void pc_init()
{	
	
	memset(&pc, 0, sizeof(PCHandleTypeDef));
	pc.rb.buf = pc_buff;
	pc.rb.len = MAX_PC_RB_BUFFER_LENGTH;
	
	uart1_init(9600);
	
}

void pc_handler()
{
	uint8_t i;
	uint8_t send_data[20];
	int8_t read_val[6];
	int current_duty;
	
	static uint8_t stop_flag = 0;
	static float x_value = DEFAULT_X, y_value = DEFAULT_Y, z_value = DEFAULT_Z, stop_x_value,stop_y_value, stop_z_value;	
	
	unpack(&pc);

    switch (pc.status)
    {
		case CMD_VERSION_QUERY:
			send_data[0] = SERVO_TYPE;
			send_data[1] = SOFTWARE_VERSION;	
			pc_delay();
			pc_packet_transmit(CMD_VERSION_QUERY, send_data, 2);
			memset(send_data, 0, sizeof(send_data));
			pc.status = CMD_FUNC_NULL;
			break;
		
		case CMD_SERVO_OFFSET_READ:
			robot_arm_offset_read(read_val);
			for (i = 0; i < 6; i++)
			{
				send_data[(2 * i)] = 1 + i;
				send_data[(2 * i) + 1] = (uint8_t)read_val[i];
			}
			pc_delay();
			pc_packet_transmit(CMD_SERVO_OFFSET_READ, send_data, 12);
			memset(send_data, 0, sizeof(send_data));
			pc.status = CMD_FUNC_NULL;
			break;

        case CMD_MULT_SERVO_MOVE:
			pc.servos_count = pc.packet.buffer[0];
			pc.running_time = BYTE_TO_HW(pc.packet.buffer[2], pc.packet.buffer[1]);
			for (i = 0; i < pc.servos_count; i++)
			{
				pc.set_id = pc.packet.buffer[3 + i * 3];
				pc.set_duty = BYTE_TO_HW(pc.packet.buffer[5 + i * 3], pc.packet.buffer[4 + i * 3]);
				robot_arm_knot_run(pc.set_id, pc.set_duty, pc.running_time + 40);
			}	
			pc.status = CMD_FUNC_NULL;
            break;

		case CMD_COORDINATE_SET:
			x_value += ((float)(int8_t)pc.packet.buffer[0]) / 10.0f;
			y_value += ((float)(int8_t)pc.packet.buffer[1]) / 10.0f;
			z_value += ((float)(int8_t)pc.packet.buffer[2]) / 10.0f;
			if(stop_flag == 1)
			{
				buzzer_toggle(100, 100, 1);
				x_value = stop_x_value;
				y_value = stop_y_value;
				z_value = stop_z_value;
				stop_flag = 0;
			}
			
			if(robot_arm_coordinate_set(x_value, y_value, z_value, -45.0f, 60) == 0)
			{
				stop_flag = 1;
			}
			else
			{
				stop_x_value = x_value;
				stop_y_value = y_value;
				stop_z_value = z_value;			
			}

			pc.status = CMD_FUNC_NULL;
			break;
		
        case CMD_ACTION_GROUP_RUN:
            pc.action_group_index = pc.packet.buffer[0];
            pc.running_times = BYTE_TO_HW(pc.packet.buffer[2], pc.packet.buffer[1]);
            if(action_group_run(pc.action_group_index, pc.running_times) == 1)
			{
				pc.status = CMD_FUNC_NULL;
			}
            break;

        case CMD_FULL_ACTION_STOP:
            action_group_stop();
			pc.status = CMD_FUNC_NULL;
            break;

        case CMD_FULL_ACTION_ERASE:
            action_group_erase();
			pc_delay();
			pc_delay();
            pc_packet_transmit(CMD_FULL_ACTION_ERASE, send_data, 0);
			pc.status = CMD_FUNC_NULL;
            break;    
			
		case CMD_SERVO_OFFSET_SET:
			robot_arm_offset_set(pc.packet.buffer[0], (int8_t)pc.packet.buffer[1]);
			pc.status = CMD_FUNC_NULL;
			break;
		
		case CMD_SERVO_OFFSET_DOWNLOAD:
			robot_arm_offset_save();
			pc_delay();
			pc_packet_transmit(CMD_SERVO_OFFSET_DOWNLOAD, send_data, 0);
			pc.status = CMD_FUNC_NULL;
			buzzer_toggle(100, 100, 1);
			break;
		
		case CMD_SERVOS_RESET:
			robot_arm_reset(1000);
			x_value = DEFAULT_X;
			y_value = DEFAULT_Y;
			z_value = DEFAULT_Z;
			pc.status = CMD_FUNC_NULL;
			break;

		case CMD_ANGLE_BACK_READING:
			for(i = 0; i < 6; i++)
			{
				current_duty = robot_arm_get_knot_current_duty(i + 1);
				send_data[i * 3] = i + 1;
				send_data[(i * 3) + 1] = GET_LOW_BYTE(current_duty);
				send_data[i * 3 + 2] = GET_HIGH_BYTE(current_duty);
			}
			pc_packet_transmit(CMD_ANGLE_BACK_READING, send_data, 18);
			pc.status = CMD_FUNC_NULL;
			break;
			
			break;			
        case CMD_ACTION_DOWNLOAD:
            pc.action_group_index = pc.packet.buffer[0];
            pc.action_frame_sum = pc.packet.buffer[1];
            pc.action_frame_index = pc.packet.buffer[2];
            action_group_save(pc.action_group_index, 
                              pc.action_frame_sum, 
                              pc.action_frame_index, 
							  pc.packet.buffer + 3,
                              ACTION_FRAME_SIZE);
			pc_delay();
			pc_packet_transmit(CMD_ACTION_DOWNLOAD, send_data, 0);
			if(pc.action_frame_index == pc.action_frame_sum - 1)
			{
				buzzer_toggle(100, 100, 1);
			}
			pc.status = CMD_FUNC_NULL;
            break;

        default:
            break;
    }
}

void stop_pc_receive()
{
	DMA_UR1R_STA = 0x00;
}

void start_pc_receive()
{
	DMA_UR1R_STA = 0xa1;
}

//void Uart1_isr(void) interrupt 4
//{
//	uint8_t rx_byte_h = 0;
//	uint8_t rx_byte_l = 0;
//	uint32_t rx_buf_size = 0;
//	/* 超时中断 */
//	if(UR1TOSR & 0x01)
//	{
//		/* 清除超时中断标志位 */
//		UR1TOSR = 0x80;
//		rx_byte_h = DMA_UR1R_DONEH;
//		rx_byte_l = DMA_UR1R_DONE;
//		/*更新写入的数据大小*/
//		rx_buf_size = ((uint16_t)rx_byte_h << 8) | rx_byte_l;
//		rb_write(&pc.rb, uart1_rx_buff, rx_buf_size);
//		DMA_UR1R_CR = 0;
//		DMA_UR1R_STA = 0x00;
//		/* 重新打开接收 */
//		DMA_UR1R_CR = 0xa1;
//	}
//}

//void uart1_tx_dma_isr(void) interrupt 50
//{
//	DMA_UR1T_STA = 0x00;
//	tx1_state = 0;
//}

