#include "app_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"
#ifdef MECANUM_CHASSIS
#include "mecanum_chassis.h"
#endif
		
#ifdef DIFFERENTIAL_CHASSIS
#include "differential_chassis.h"
#endif	

#ifdef MECANUM_CHASSIS
static int8_t rot;
static uint8_t speed;
static uint16_t angle;
#endif
	
#ifdef DIFFERENTIAL_CHASSIS
static float speed;
static float rot;	
#endif	

AppHandleTypeDef app;

uint8_t xdata app_buff[MAX_APP_RB_BUFFER_LENGTH] = {0};	

static uint8_t tx3_state = 0;
static uint32_t tickstart = 0;

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

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


void uart3_send_buffer(uint8_t* buf, uint16_t len)
{
	if(tx3_state == 0)
	{
		uint16_t data_len = len - 1;
		uint16_t arry_num;
		tx3_state = 1;
		if(len > Uart3_Tx)
		{
			return;
		}
		DMA_UR3T_AMT = (uint8_t)data_len;
		DMA_UR3T_AMTH = (uint8_t)(data_len >> 8);
		for(arry_num = 0; arry_num < len; arry_num++)
		{
			uart3_tx_buff[arry_num] = buf[arry_num];
		}
		DMA_UR3T_CR = 0xc0;		/* 使能UR3T_DMA功能 */
		tx3_state = 1;			/* 等待数据发送完成 */
	}
}


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

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

static void unpack(AppHandleTypeDef* self)
{
	uint8_t i;
	uint8_t rec_data[MAX_APP_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_APP_READ_BUFFER_LENGTH ? MAX_APP_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] == APP_PACKET_HEADER ? PACKET_HEADER_2: PACKET_HEADER_1;
				if(rec_data[0] == APP_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] == APP_PACKET_HEADER ? PACKET_DATA_LENGTH: PACKET_HEADER_1;
				if(rec_data[0] == APP_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 9:
				self->status = CMD_CHASSIS_CONTROL;
				tickstart = get_tick();
				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 app_init()
{
	/* 蓝牙开关控制使能 */
  P3M0 |= 0x80; 
	P3M1 &= ~0x80; 
	P37 = 1;	
	
	memset(&app, 0, sizeof(AppHandleTypeDef));
	app.rb.buf = app_buff;
	app.rb.len = MAX_APP_RB_BUFFER_LENGTH;
	
	uart1_init(9600);
	uart3_init(9600);
	
#ifdef MECANUM_CHASSIS
  mecanum_chassis_init();
#endif
		
#ifdef DIFFERENTIAL_CHASSIS
  differential_chassis_init();	
#endif
}


void app_handler()
{
	uint8_t i;
	uint8_t send_data[20];
	
	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(&app);

    switch (app.status)
    {
		case CMD_VERSION_QUERY:
			send_data[0] = SERVO_TYPE;
			send_data[1] = SOFTWARE_VERSION;	
			app_delay();
			app_packet_transmit(CMD_VERSION_QUERY, send_data, 2);
			memset(send_data, 0, sizeof(send_data));
			app.status = CMD_FUNC_NULL;
			break;

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

		case CMD_COORDINATE_SET:
			x_value += ((float)(int8_t)app.packet.buffer[0]) / 10.0f;
			y_value += ((float)(int8_t)app.packet.buffer[1]) / 10.0f;
			z_value += ((float)(int8_t)app.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;			
			}

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

		case CMD_CHASSIS_CONTROL:
			if(get_tick() - tickstart < CHASSIS_CONTROL_TIMEOUT)	
			{
#ifdef MECANUM_CHASSIS
				switch(app.packet.buffer[0])
				{
					case 0:
						angle = 0;
						speed = 0;	
						break;
					
					case 1:
						angle = 90;
						speed = 60;	
						break;
					
					case 2:
						angle = 45;
						speed = 60;	
						break;
					
					case 3:
						angle = 0;
						speed = 60;	
						break;
					
					case 4:
						angle = 315;
						speed = 60;	
						break;
					
					case 5:
						angle = 270;
						speed = 60;	
						break;
					
					case 6:
						angle = 225;
						speed = 60;	
						break;
					
					case 7:
						angle = 180;
						speed = 60;	
						break;	
					
					case 8:
						angle = 135;
						speed = 60;	
						break;					
				}
				
				switch(app.packet.buffer[1])
				{
					case 0: 
						rot = 0;
						break;
					
					case 1: 
						rot = 50;
						break;	

					case 2: 
						rot = -50;
						break;				
				}
#endif
			
#ifdef DIFFERENTIAL_CHASSIS
				switch(app.packet.buffer[0])
				{
					case 0:
						speed = 0.0f;
						break;
					
					case 3:
						speed = 80.0f;
						break;	

					case 7:
						speed = -80.0f;
						break;				
				}
					
				switch(app.packet.buffer[1])
				{
					case 0:
						rot = 0.0f;
						break;
						
					case 1:
						rot = 50.0f;
						break;
						
					case 2:
						rot = -50.0f;
						break;
				}
#endif
			}
			else
			{
#ifdef MECANUM_CHASSIS
				rot = 0;
				angle = 0;
				speed = 0;	
#endif

#ifdef DIFFERENTIAL_CHASSIS	
				rot = 0;
				speed = 0;
				app.status = CMD_FUNC_NULL;
#endif		
			}
			
#ifdef MECANUM_CHASSIS
			mecanum_chassis_run(angle, speed, rot, 0);
#endif
		
#ifdef DIFFERENTIAL_CHASSIS
			differentials_chassis_run(speed, rot);
#endif				
			break;
					
		case CMD_SERVOS_RESET:
			robot_arm_reset(1000);
			x_value = DEFAULT_X;
			y_value = DEFAULT_Y;
			z_value = DEFAULT_Z;
			app.status = CMD_FUNC_NULL;
			break;

        default:
            break;
    }
}

void stop_app_receive()
{
	DMA_UR3R_STA = 0x00;
}

void start_app_receive()
{
	DMA_UR3R_STA = 0xa1;
}



void Uart3_isr(void) interrupt 17
{
	uint8_t rx_byte_h = 0;
	uint8_t rx_byte_l = 0;
	uint32_t rx_buf_size = 0;
	
	/* 超时中断 */
	if(UR3TOSR & 0x01)
	{
		/* 清除超时中断标志位 */
		UR3TOSR = 0x80;
		rx_byte_h = DMA_UR3R_DONEH;
		rx_byte_l = DMA_UR3R_DONE;
		/*更新写入的数据大小*/
		rx_buf_size = ((uint16_t)rx_byte_h << 8) | rx_byte_l;
		rb_write(&app.rb, uart3_rx_buff, rx_buf_size);
		DMA_UR3R_CR = 0;
		DMA_UR3R_STA = 0x00;
		/* 重新打开接收 */
		DMA_UR3R_CR = 0xa1;
	}
}

void uart3_tx_dma_isr(void) interrupt 54
{
	DMA_UR3T_STA = 0x00;
	tx3_state = 0;
}


