#include "serial_servos.h"
#include "AI8051U.h"
#include "string.h"
#include "AI8051U_Uart.h"
#include "led.h"
#include "global.h"

SerialServoControllerTypeDef serial_servo_controller;

uint8_t xdata serial_servo_buff[128] = {0};	

static uint8_t tx4_state = 0;

/**
 * @brief 校验和计算函数
 * 
 * @param  buf      需要计算校验和的数组
 * @return uint8_t  校验和计算结构
 */
static uint8_t serial_servo_checksum(const uint8_t buf[])
{
	int i;
    uint16_t temp = 0;
    for (i = 2; i < buf[3] + 2; ++i) {
        temp += buf[i];
    }
    return (uint8_t)(~temp);
}

/**
 * @brief 自动填充数据帧的帧头、ID、命令字段
 * 
 * @param  frame      指向SerialServoCmdTypeDef类型的指针
 * @param  servo_id   舵机id号
 * @param  cmd        控制命令
 *   @arg  SERIAL_SERVO_MOVE_TIME_WRITE         设置舵机位置与运行时间
 *   @arg  SERIAL_SERVO_POS_READ                读取舵机位置
 *   @arg  SERIAL_SERVO_ID_WRITE                设置舵机ID
 *   @arg  SERIAL_SERVO_ID_READ                 读取舵机ID
 *   @arg  SERIAL_SERVO_ANGLE_OFFSET_ADJUST     设置舵机偏差
 *   @arg  SERIAL_SERVO_ANGLE_OFFSET_WRITE      保存舵机偏差
 *   @arg  SERIAL_SERVO_ANGLE_OFFSET_READ       读取舵机偏差
 *   @arg  SERIAL_SERVO_ANGLE_LIMIT_WRITE       设置舵机角度范围
 *   @arg  SERIAL_SERVO_ANGLE_LIMIT_READ        读取舵机角度范围
 *   @arg  SERIAL_SERVO_VIN_LIMIT_WRITE         设置舵机电压范围
 *   @arg  SERIAL_SERVO_VIN_LIMIT_READ          读取舵机电压范围
 *   @arg  SERIAL_SERVO_VIN_READ                读取当前舵机电压
 *   @arg  SERIAL_SERVO_TEMP_MAX_LIMIT_WRITE    设置舵机温度范围
 *   @arg  SERIAL_SERVO_TEMP_MAX_LIMIT_READ     读取舵机温度范围
 *   @arg  SERIAL_SERVO_TEMP_READ               读取当前舵机温度
 *   @arg  SERIAL_SERVO_LOAD_OR_UNLOAD_WRITE    设置舵机状态(上电/掉电)
 *   @arg  SERIAL_SERVO_LOAD_OR_UNLOAD_READ     读取舵机状态(上电/掉电)
 *   @arg  SERIAL_SERVO_MOVE_STOP               停止舵机运行
 */
static void cmd_frame_init(SerialServoCmdTypeDef *frame, int servo_id, int cmd)
{
    frame->header_1 = SERIAL_SERVO_FRAME_HEADER;
    frame->header_2 = SERIAL_SERVO_FRAME_HEADER;
    frame->servo_id = servo_id;
    frame->command = cmd;
}

/**
 * @brief 自动填充数据帧的数据长度、校验值字段
 * 
 * @param  frame      指向SerialServoCmdTypeDef类型的指针
 * @param  args_num   发送的数据个数
 */

static void cmd_frame_complete(SerialServoCmdTypeDef *frame, int args_num)
{
    frame->length = args_num + 3;
    frame->args[args_num] = serial_servo_checksum((uint8_t*)frame);
//	frame->args[args_num + 1] = 0x00;
}

static int serial_servo_rx_handler(SerialServoControllerTypeDef *self, uint8_t rx_byte)
{
    switch (self->rx_state)
	{
        case SERIAL_SERVO_RECV_STARTBYTE_1: 
            self->rx_state = SERIAL_SERVO_FRAME_HEADER == rx_byte ? SERIAL_SERVO_RECV_STARTBYTE_2 : SERIAL_SERVO_RECV_STARTBYTE_1;
			if(self->rx_state == SERIAL_SERVO_RECV_STARTBYTE_1)
			{
				self->it_state = SERIAL_SERVO_READ_DATA_ERROR;
			}
            self->rx_frame.header_1 = SERIAL_SERVO_FRAME_HEADER;
            return -1;

        case SERIAL_SERVO_RECV_STARTBYTE_2: 
            self->rx_state = SERIAL_SERVO_FRAME_HEADER == rx_byte ? SERIAL_SERVO_RECV_SERVO_ID : SERIAL_SERVO_RECV_STARTBYTE_1;
			if(self->rx_state == SERIAL_SERVO_RECV_STARTBYTE_1)
			{
				self->it_state = SERIAL_SERVO_READ_DATA_ERROR;
			}
            self->rx_frame.header_2 = SERIAL_SERVO_FRAME_HEADER;
            return -2;

        case SERIAL_SERVO_RECV_SERVO_ID: 
            self->rx_frame.servo_id = rx_byte;
			self->rx_state = SERIAL_SERVO_RECV_LENGTH;
            return 1;

        case SERIAL_SERVO_RECV_LENGTH: 
			/* 包长度超过允许长度 */
            if(rx_byte > 7) 
			{
                self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1; 
				self->it_state = SERIAL_SERVO_READ_DATA_ERROR;
                return -3;
            }
            self->rx_frame.length = rx_byte;
            self->rx_state = SERIAL_SERVO_RECV_COMMAND;
            return 2;

        case SERIAL_SERVO_RECV_COMMAND: 
            self->rx_frame.command = rx_byte;
            self->rx_args_index = 0;
            /* 若没有参数则直接进入校验字段 */
            self->rx_state = self->rx_frame.length == 6 ? SERIAL_SERVO_RECV_CHECKSUM : SERIAL_SERVO_RECV_ARGUMENTS; 
            return 3;

        case SERIAL_SERVO_RECV_ARGUMENTS: 
            self->rx_frame.args[self->rx_args_index++] = rx_byte;
            if (self->rx_args_index + 3 == self->rx_frame.length) 
			{
                self->rx_state = SERIAL_SERVO_RECV_CHECKSUM;
            }
            return 4;

        case SERIAL_SERVO_RECV_CHECKSUM: 
            if(serial_servo_checksum((uint8_t*)&self->rx_frame) != rx_byte) 
			{
                self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1;
				self->it_state = SERIAL_SERVO_READ_DATA_ERROR;
                return -99;
            } 
			else 
			{
                self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1;
                return 0;
            }

        default: 
            self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1;
			self->it_state = SERIAL_SERVO_READ_DATA_ERROR;
            return -100;
    }
}

static void serial_servo_write_pin(uint8_t new_state)
{
	/* 高电平进入写模式，低电平进入读模式 */
	P20 = new_state;
}

/**
 * @brief 读写总线舵机数据
 * 
 * @param  self       指向SerialServoCmdTypeDef类型的指针
 * @param  frame      指向SerialServoCmdTypeDef类型的指针
 * @param  tx_only    读写标志符
 * @return int8_t     1-已完成数据帧发送 0-已完成数据帧读取
 */

void uart4_send_buffer(SerialServoCmdTypeDef* self, uint16_t len)
{
	uint16_t data_len;
	uint16_t arry_num;
	if(tx4_state == 0)
	{
		data_len = 3 + len - 1;

		if(len > Uart4_Tx)
		{
			return;
		}
		DMA_UR4T_AMT = (uint8_t)data_len;
		DMA_UR4T_AMTH = (uint8_t)(data_len >> 8);
		
		uart4_tx_buff[0] = self->header_1;
		uart4_tx_buff[1] = self->header_2;
		uart4_tx_buff[2] = self->servo_id;
		uart4_tx_buff[3] = self->length;
		uart4_tx_buff[4] = self->command;
		for(arry_num = 0; arry_num < len - 2; arry_num++)
		{
			uart4_tx_buff[5 + arry_num] = self->args[arry_num];
		}
		DMA_UR4T_CR = 0xc0;		/* 使能UR4T_DMA功能 */
		tx4_state = 1;			/* 等待数据发送完成 */
	}
}

static int8_t serial_write_and_read(SerialServoControllerTypeDef *self, SerialServoCmdTypeDef *frame, uint8_t tx_only)
{

    int8_t ret = -1;	
	switch(self->it_state)
	{
		case SERIAL_SERVO_WRITE_DATA_READY:
			serial_servo_controller.it_state = SERIAL_SERVO_WRITE_DATA;
			/* 进入写模式 */
			serial_servo_write_pin(1);
			memcpy(&self->tx_frame, frame, sizeof(SerialServoCmdTypeDef));
			self->tx_byte_index = 0;
			self->tx_only = tx_only;
			self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1;
			uart4_send_buffer(&self->tx_frame, self->tx_frame.length);
			ret = 1;
			break;
		
		case SERIAL_SERVO_READ_DATA_FINISH:
			self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
			ret = 0;
			break;
		
		case SERIAL_SERVO_READ_DATA_ERROR:
			self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
			break;
			
		default:
			break;
	}
	return ret;
}

void serial_servo_set_id(SerialServoControllerTypeDef *self, uint8_t old_id, uint8_t new_id)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, old_id, SERIAL_SERVO_ID_WRITE);
    frame.args[0] = new_id;
    cmd_frame_complete(&frame, 1);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_id(SerialServoControllerTypeDef *self, uint8_t servo_id, uint8_t *ret_servo_id)
{
	SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ID_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
        *ret_servo_id = self->rx_frame.args[0];
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
        return 1;
    }
    return 0;
}

void serial_servo_set_position(SerialServoControllerTypeDef *self, uint8_t servo_id, int position, uint32_t duration)
{
    SerialServoCmdTypeDef frame;
    position = position > 1000 ? 1000 : position;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_MOVE_TIME_WRITE);
    frame.args[0] = GET_LOW_BYTE(position);
    frame.args[1] = GET_HIGH_BYTE(position);
    frame.args[2] = GET_LOW_BYTE(duration);
    frame.args[3] = GET_HIGH_BYTE(duration);
    cmd_frame_complete(&frame, 4);
    serial_write_and_read(self, &frame, 1);
}


uint8_t serial_servo_read_position(SerialServoControllerTypeDef *self, uint8_t servo_id, int *position)
{
	SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_POS_READ);
    cmd_frame_complete(&frame, 0);
    if (0 == serial_write_and_read(self, &frame, 0)) 
	{
        *position = (int)BYTE_TO_HW(self->rx_frame.args[1], self->rx_frame.args[0]);
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
        return 1;
    }
    return 0;
}

void serial_servo_set_deviation(SerialServoControllerTypeDef *self, uint8_t servo_id, int new_deviation)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ANGLE_OFFSET_ADJUST);
    frame.args[0] = (uint8_t)((int8_t) new_deviation);
    cmd_frame_complete(&frame, 1);
    serial_write_and_read(self, &frame, 1);
}

void serial_servo_save_deviation(SerialServoControllerTypeDef *self, uint8_t servo_id)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ANGLE_OFFSET_WRITE);
    cmd_frame_complete(&frame, 0);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_deviation(SerialServoControllerTypeDef *self, uint8_t servo_id, int8_t *deviation)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ANGLE_OFFSET_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
        *deviation = (int8_t)(self->rx_frame.args[0]);
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
        return 1;
    }
    return 0;
}

void serial_servo_set_angle_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint32_t limit_l, uint32_t limit_h)
{
	uint32_t real_limit_l;
	uint32_t real_limit_h;
	SerialServoCmdTypeDef frame;
	
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ANGLE_LIMIT_WRITE);
	
    limit_l = limit_l > 1000 ? 1000 : limit_l;
	limit_h = limit_h > 1000 ? 1000 : limit_h;
	
	real_limit_l = limit_l > limit_h ? limit_h : limit_l;
	real_limit_h = limit_l > limit_h ? limit_l : limit_h;
    frame.args[0] = GET_LOW_BYTE(real_limit_l);
    frame.args[1] = GET_HIGH_BYTE(real_limit_l);
    frame.args[2] = GET_LOW_BYTE(real_limit_h);
    frame.args[3] = GET_HIGH_BYTE(real_limit_h);
    cmd_frame_complete(&frame, 4);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_angle_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint16_t limit[2])
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_ANGLE_LIMIT_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
		limit[0] = *((uint16_t*)(&self->rx_frame.args[0]));
		limit[1] = *((uint16_t*)(&self->rx_frame.args[2]));
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;			
        return 1;
    }
    return 0;
}

void serial_servo_set_temp_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint32_t limit)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_TEMP_MAX_LIMIT_WRITE);
    frame.args[0] = limit > 100 ? 100 : (uint8_t)limit;
    cmd_frame_complete(&frame, 1);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_temp_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint8_t *limit)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_TEMP_MAX_LIMIT_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0)) 
	{
        *limit = (uint8_t)(self->rx_frame.args[0]);
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;	
        return 1;
    }
    return 0;
}

uint8_t serial_servo_read_temp(SerialServoControllerTypeDef *self, uint8_t servo_id, uint8_t *temp)
{
    SerialServoCmdTypeDef frame;	
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_TEMP_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
        *temp = (uint8_t)(self->rx_frame.args[0]);
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;	
        return 1;
    }
    return 0;
}

void serial_servo_set_vin_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint32_t limit_l, uint32_t limit_h)
{
	uint32_t real_limit_l;
	uint32_t real_limit_h;
    SerialServoCmdTypeDef frame;
	
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_VIN_LIMIT_WRITE);
    limit_l = limit_l < 4500 ? 4500 : limit_l;
    limit_h = limit_h > 14000 ? 14000 : limit_h;
	real_limit_l  = limit_l > limit_h ? limit_h : limit_l;
	real_limit_h = limit_l > limit_h ? limit_l : limit_h;
    frame.args[0] = GET_LOW_BYTE(real_limit_l);
    frame.args[1] = GET_HIGH_BYTE(real_limit_l);
    frame.args[2] = GET_LOW_BYTE(real_limit_h);
    frame.args[3] = GET_HIGH_BYTE(real_limit_h);
    cmd_frame_complete(&frame, 4);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_vin_limit(SerialServoControllerTypeDef *self, uint8_t servo_id, uint16_t limit[2])
{
    SerialServoCmdTypeDef frame;	
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_VIN_LIMIT_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
        limit[0] = *((uint16_t*)(&self->rx_frame.args[0]));
		limit[1] = *((uint16_t*)(&self->rx_frame.args[2]));
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;	
        return 1;
    }
    return 0;
}

uint8_t serial_servo_read_vin(SerialServoControllerTypeDef *self, uint8_t servo_id, uint16_t *vin)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_VIN_READ);
    cmd_frame_complete(&frame, 0);
    if(0 == serial_write_and_read(self, &frame, 0))
	{
        *vin = ((uint32_t) * ((uint16_t*)self->rx_frame.args));
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;	
        return 1;
    }
    return 0;
}

void serial_servo_stop(SerialServoControllerTypeDef *self, uint8_t servo_id)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_MOVE_STOP);
    cmd_frame_complete(&frame, 0);
    serial_write_and_read(self, &frame, 1);
}

void serial_servo_load_unload(SerialServoControllerTypeDef *self, uint8_t servo_id, uint8_t load)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_LOAD_OR_UNLOAD_WRITE);
    frame.args[0] = load;
    cmd_frame_complete(&frame, 1);
    serial_write_and_read(self, &frame, 1);
}

uint8_t serial_servo_read_load_unload(SerialServoControllerTypeDef *self, uint8_t servo_id, uint8_t* load_unload)
{
    SerialServoCmdTypeDef frame;
    cmd_frame_init(&frame, servo_id, SERIAL_SERVO_LOAD_OR_UNLOAD_READ);
    cmd_frame_complete(&frame, 0);
    if (0 == serial_write_and_read(self, &frame, 0))
	{
        *load_unload = (uint8_t)(self->rx_frame.args[0]);
		memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));
		memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));
		self->it_state = SERIAL_SERVO_WRITE_DATA_READY;	
        return 1;
    }
    return 0;
}

static void serial_servo_controller_object_init(SerialServoControllerTypeDef *self)
{
    self->rx_args_index = 0;
	self->it_state = SERIAL_SERVO_WRITE_DATA_READY;
    self->rx_state = SERIAL_SERVO_RECV_STARTBYTE_1;
    memset(&self->rx_frame, 0, sizeof(SerialServoCmdTypeDef));

    self->tx_only = 1;
    self->tx_byte_index = 0;
    memset(&self->tx_frame, 0, sizeof(SerialServoCmdTypeDef));

}

void serial_servo_init()
{
	serial_servo_controller_object_init(&serial_servo_controller);

	/* 写入模式, 只有在带接收过程的指令才会打开接收 */
	P2M0 |= 0x01; 
	P2M1 &= ~0x01; 
	P2SR &= ~0x01; /* 转换速度最快 */

    serial_servo_write_pin(0);
	uart4_init(115200);
}


//void Uart4_isr(void) interrupt 18
//{
//	uint8_t i;
//	uint8_t rx_byte_h = 0;
//	uint8_t rx_byte_l = 0;

//	/* 超时中断 */
//	if(UR4TOSR & 0x01)
//	{	
//		/* 清除超时中断标志位 */
//		UR4TOSR = 0x80;
//		rx_byte_h = DMA_UR4R_DONEH;
//		rx_byte_l = DMA_UR4R_DONE;
//		/*更新写入的数据大小*/
////		rx_buf_size = ((uint16_t)rx_byte_h << 8) | rx_byte_l;

//		for(i = 0; i < Uart4_Rx; i++)
//		{
//			if (0 == serial_servo_rx_handler(&serial_servo_controller, uart4_rx_buff[i]))
//			{
//				serial_servo_controller.it_state = SERIAL_SERVO_READ_DATA_FINISH;
//				led_off(0);
//				break;
//			}
//			if(serial_servo_controller.it_state == SERIAL_SERVO_READ_DATA_ERROR)
//			{
//				break;
//			}
//		}	
//	
//		DMA_UR4R_STA = 0x00;
//		DMA_UR4R_CR = 0;
//		
//		/* 重新打开接收 */
//		DMA_UR4R_CR = 0xa1;
//	}
//}
	
void uart4_tx_dma_isr(void) interrupt 56
{
	DMA_UR4T_STA = 0x00;
	tx4_state = 0;
	if(serial_servo_controller.tx_only == 1)
	{
		serial_servo_controller.it_state = SERIAL_SERVO_WRITE_DATA_READY;
	}
	else
	{
		serial_servo_write_pin(0);
		serial_servo_controller.it_state = SERIAL_SERVO_READ_DATA;
	}
}
	