#include "Kinematics.h"
#include "robot_arm.h"
#include "w25q64.h"
#include "stdlib.h"
#include "string.h"
#include "math.h"
#include "buzzer.h"
#include "serial_servos.h"
#include "global.h"
#include "intrins.h"

KinematicsObjectTypeDef  kinematics;
RobotArmHandleTypeDef robot_arm;

void serial_servos_delay(void)	//@40MHz
{
	unsigned long edata i;
	_nop_();
	_nop_();
	_nop_();
	i = 9998UL;
	while (i) i--;
}

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

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

static void theta2servo(KinematicsObjectTypeDef* self, uint32_t time)
{
	uint8_t i;
	float target_angle[4];

	switch(SERVO_TYPE)
	{
		case 1:
			target_angle[0] = 90.0f + self->knot[0].theta;
			target_angle[1] = 180.0f - self->knot[1].theta;
			target_angle[2] = 90.0f + self->knot[2].theta;
			target_angle[3] = 90.0f + self->knot[3].theta;

			for (i = 0; i < 4; i++)
			{	
				pwm_servo_angle_set(&pwm_servos[i], target_angle[i], time);
			}	
			break;
		
		case 2:
			target_angle[0] = self->knot[0].theta;
			target_angle[1] = 90.0f - self->knot[1].theta;
			target_angle[2] = self->knot[2].theta;
			target_angle[3] = self->knot[3].theta;
			for (i = 0; i < 4; i++)
			{	
				serial_servo_set_position(&serial_servo_controller, 6 - i, 500 + (int)(SERIAL_ANGLE_FACTOR * target_angle[i]), time);
				serial_servos_delay();
			}
			break;
	}
}

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));
}

/* 
 * 6号舵机 左+ 右-
 * 5号舵机 前+ 后-
 * 4号舵机 前- 后+
 * 3号舵机 前- 后+
 */
//bool result1_state, result2_state;
uint8_t robot_arm_coordinate_set(float target_x,
								 float target_y,
								 float target_z,
								 float pitch,
								 uint32_t time)
{
	uint8_t i, result1_state, result2_state;
	
	KinematicsObjectTypeDef kinematics_result1;
	KinematicsObjectTypeDef kinematics_result2;	
	VectorObjectTypeDef vector;

	vector.x = target_x;
	vector.y = target_y;
	vector.z = target_z;
	
	result1_state = set_pitch_range(&kinematics_result1, &vector, pitch, MIN_PITCH);
	result2_state = set_pitch_range(&kinematics_result2, &vector, pitch, MAX_PITCH);
	
	if (result1_state == 1)
	{
		kinematics.alpha = kinematics_result1.alpha;
		kinematics.vector.x = kinematics_result1.vector.x;
		kinematics.vector.y = kinematics_result1.vector.y;
		kinematics.vector.z = kinematics_result1.vector.z;
		for (i = 0; i< 4; i++)
		{
			kinematics.knot[i].theta = kinematics_result1.knot[i].theta;
		}
		
		if (result2_state == 1)
		{
			if (fabs(kinematics_result2.alpha - pitch) < fabs(kinematics_result1.alpha - pitch))
			{
				kinematics.alpha = kinematics_result2.alpha;
				kinematics.vector.x = kinematics_result2.vector.x;
				kinematics.vector.y = kinematics_result2.vector.y;
				kinematics.vector.z = kinematics_result2.vector.z;
				for (i = 0; i< 4; i++)
				{
					kinematics.knot[i].theta = kinematics_result2.knot[i].theta;
				}			
			}
		}
	}
	else
	{
		if (result2_state == 1)
		{
			kinematics.alpha = kinematics_result2.alpha;
			kinematics.vector.x = kinematics_result2.vector.x;
			kinematics.vector.y = kinematics_result2.vector.y;
			kinematics.vector.z = kinematics_result2.vector.z;
			for (i = 0; i< 4; i++)
			{
				kinematics.knot[i].theta = kinematics_result2.knot[i].theta;
			}
		}
		else
		{
			return 0;
		}
	}
	result1_state = 0;
	result2_state = 0;
	theta2servo(&kinematics, time);
	return 1;
}

void robot_arm_offset_read(int8_t* value)
{
	uint8_t i;
	uint8_t read_value[6];
	
	switch(SERVO_TYPE)
	{
		case 1:
			w25q64_read(SERVOS_OFFSET_BASE_ADDRESS, read_value, sizeof(read_value));
			for(i = 0; i < 6; i++)
			{
				value[i] = (int8_t)read_value[i];
				robot_arm.servo_offset[i] = (int8_t)read_value[i];
				pwm_servo_offset_set(&pwm_servos[5 - i], (int)robot_arm.servo_offset[i]);
			}
			break;
		
		case 2:
			for(i = 0; i < 6; i++)
			{
				while(!serial_servo_read_deviation(&serial_servo_controller, i + 1, &value[i]))
				{
					serial_servos_delay();
				}
				robot_arm.servo_offset[i] = value[i];
				serial_servos_delay();
			}
			break;
	}
}

void robot_arm_offset_set(uint8_t id, int8_t value)
{
	id = id > 6 ? 6 : id < 1 ? 1 : id;
	value = value > 100 ? 100 : value < -100 ? -100 : value;
	robot_arm.servo_offset[id - 1] = value;
	
	switch(SERVO_TYPE)
	{
		case 1:
			pwm_servo_offset_set(&pwm_servos[6 - id], (int)value);
			break;
		
		case 2:
			serial_servo_set_deviation(&serial_servo_controller, id, (int)value);
			serial_servos_delay();
			break;
	}
}

void robot_arm_offset_save()
{
	uint8_t i;
	uint8_t _data[6];
	
	switch(SERVO_TYPE)
	{
		case 1:
			for(i = 0; i < 6; i++)
			{
				_data[i] = (uint8_t)pwm_servos[5 - i].offset;
			}
			
			w25q64_erase_sector(SERVOS_OFFSET_BASE_ADDRESS);
			w25q64_write(SERVOS_OFFSET_BASE_ADDRESS, (const uint8_t*)_data, sizeof(_data));
			break;
		
		case 2:
			for(i = 0; i < 6; i++)
			{
				serial_servo_save_deviation(&serial_servo_controller, i + 1);
				serial_servos_delay();;
			}
			break;
	}
}

/*
 * 1号舵机 张开- 闭合+
 */
void robot_arm_claw_set(float open_angle, uint32_t open_angle_time)
{
	float target_open_angle;
	
	open_angle = open_angle > MAX_OPEN_ANGLE ? MAX_OPEN_ANGLE : \
				(open_angle < MIN_OPEN_ANGLE ? MIN_OPEN_ANGLE : open_angle);

	switch(SERVO_TYPE)
	{
		case 1:
			target_open_angle = 90.0f - open_angle;
			pwm_servo_angle_set(&pwm_servos[5], target_open_angle, open_angle_time);
			break;
		
		case 2:
			target_open_angle = open_angle;
			serial_servo_set_position(&serial_servo_controller, 1, 700 - (int)(5.555555555555556f * target_open_angle), open_angle_time);
			break;
	}
} 

/*
 * 2号舵机 右转- 左转+
 */
void robot_arm_roll_set(float rotation_angle, uint32_t rotation_angle_time)
{
	float target_rotation_angle;
	rotation_angle = rotation_angle > MAX_ROTATION_ANGLE ? MAX_ROTATION_ANGLE : \
					(rotation_angle < MIN_ROTATION_ANGLE ? MIN_ROTATION_ANGLE : rotation_angle);

	switch(SERVO_TYPE)
	{
		case 1:
			target_rotation_angle = 90.0f - rotation_angle;
			pwm_servo_angle_set(&pwm_servos[4], target_rotation_angle, rotation_angle_time);
			break;
		
		case 2:
			target_rotation_angle = rotation_angle;
			serial_servo_set_position(&serial_servo_controller, 2, 500 - (int)(SERIAL_ANGLE_FACTOR * target_rotation_angle), rotation_angle_time);
			break;
	}
}

void robot_arm_knot_run(uint8_t id, int target_duty, uint32_t time)
{
	id = id > 6 ? 6 : id < 1 ? 1 : id;
	
	switch(SERVO_TYPE)
	{
		case 1:
			if(id == 1)
			{
				/* 防止爪子舵机堵转 */
				target_duty = target_duty > 1500 ? 1500 : target_duty;	
			}
			pwm_servo_duty_set(&pwm_servos[6 - id], target_duty, time);
			break;
			
		case 2:
			if(id == 1)
			{
				target_duty = target_duty > 700 ? 700 : target_duty;	
			}
			serial_servo_set_position(&serial_servo_controller, id, (int)target_duty, time);
			serial_servos_delay();
			break;
	}
}

void robot_arm_knot_stop(uint8_t id)
{
	id = id > 6 ? 6 : id < 1 ? 1 : id;
	
	switch(SERVO_TYPE)
	{
		case 1:
			pwm_servo_duty_set(&pwm_servos[6 - id], pwm_servos[6 - id].current_duty, 0);
			break;
		
		case 2:
			serial_servo_stop(&serial_servo_controller, id);
			serial_servos_delay();
			break;
	}
}

uint8_t robot_arm_knot_is_finish(uint8_t id, int target_duty)
{
	int position;
	id = id > 6 ? 6 : id < 1 ? 1 : id;
	
	switch(SERVO_TYPE)
	{
		case 1:
			return pwm_servos[6 - id].current_duty == target_duty;
		
		case 2:
			while(!serial_servo_read_position(&serial_servo_controller, id, &position))
			{
				serial_servos_delay();
			}
			serial_servos_delay();
			return abs(target_duty - position) < 15 ? 1 : 0;
		
		default:
			return 0;
	}
}

int robot_arm_get_knot_current_duty(uint8_t id)
{
	int position;
	id = id > 6 ? 6 : id < 1 ? 1 : id;
	
	switch(SERVO_TYPE)
	{
		case 1:
			return pwm_servos[6 - id].current_duty;
		
		case 2:
			while(!serial_servo_read_position(&serial_servo_controller, id, &position))
			{
				serial_servos_delay();
				serial_servos_delay();
			}
			serial_servos_delay();
			return position;
		
		default:
			return 0;
	}
}

void action_group_erase()
{
	/* 将所有动作组的动作帧数量设置为0，即代表将所有动作组擦除 */
    memset(robot_arm.action_group._sum, 0, sizeof(robot_arm.action_group._sum));
    w25q64_erase_sector(ACTION_FRAME_SUM_BASE_ADDRESS);
    w25q64_write(ACTION_FRAME_SUM_BASE_ADDRESS, (const uint8_t*)robot_arm.action_group._sum, sizeof(robot_arm.action_group._sum));
}


static uint8_t robot_arm_flash_init()
{
	uint8_t i;
	uint8_t read_logo[8];
	uint8_t offset_val[6];
	static const uint8_t logo[] = "Hiwonder";
	
	memset(offset_val, 0, sizeof(offset_val));
	
	w25q64_init();
	w25q64_read(LOGO_BASE_ADDRESS, read_logo, sizeof(read_logo));
//	w25q64_read(ACTION_FRAME_SUM_BASE_ADDRESS, robot_arm.action_group._sum, sizeof(robot_arm.action_group._sum));
	for (i = 0; i < sizeof(read_logo); i++)
	{
		if (read_logo[i] != logo[i])
		{
			w25q64_erase_sector(LOGO_BASE_ADDRESS);
			w25q64_write(LOGO_BASE_ADDRESS, logo, sizeof(logo));
			w25q64_erase_sector(SERVOS_OFFSET_BASE_ADDRESS);	
			w25q64_write(SERVOS_OFFSET_BASE_ADDRESS, offset_val, sizeof(offset_val));
			action_group_erase();
		}
	}
	w25q64_read(LOGO_BASE_ADDRESS, read_logo, sizeof(read_logo));
	for (i = 0; i < sizeof(read_logo); i++)
	{
		if (read_logo[i] != logo[i])
		{
			return 0;

		}
	}
	return 1;
}

/* 一个的动作帧的数组内容
 * 控制的舵机数量：frame[0]
 * 运行时间：frame[1] + frame[2] << 8
 * 舵机id：frame[3 + i * 3]:
 * 舵机脉宽：frame[4 + i * 3] + frame[5 + i * 3] << 8
 * 0-动作帧运行失败 1-动作帧运行完成 
 */
static uint8_t action_frame_run(uint8_t action_group_index, uint8_t frame_index)
{
	uint8_t i;
	uint8_t control_servos_sum;
	uint8_t frame[ACTION_FRAME_SIZE];

	uint32_t ag_addr_offset;
	uint32_t af_addr_offset;
	
	static uint8_t finish_state = 0;
	static uint8_t set_id[6] = {0};
	static uint16_t set_duty[6] = {0};
	static uint32_t action_tick;
	switch(robot_arm.action_group.frame.status)
	{

		case ACTION_FRAME_START:
			ag_addr_offset = action_group_index * ACTION_GROUP_SIZE;
			af_addr_offset = frame_index * ACTION_FRAME_SIZE;
			w25q64_read(ACTION_GROUP_BASE_ADDRESS + ag_addr_offset + af_addr_offset, frame, sizeof(frame));
			control_servos_sum = frame[0];

			if (control_servos_sum > MAX_SERVOS_NUM)
			{
				return ACTION_FRAME_START;
			}
			
			robot_arm.action_group.frame.time = BYTE_TO_HW(frame[2], frame[1]);

			for (i = 0; i < control_servos_sum; i++)
			{
				set_id[i] = frame[3 + i * 3];
				set_duty[i] = (uint16_t)BYTE_TO_HW(frame[5 + i * 3], frame[4 + i * 3]);
				robot_arm_knot_run(set_id[i], (int)set_duty[i], robot_arm.action_group.frame.time);
			}
			
			action_tick = get_tick();
			robot_arm.action_group.frame.status = ACTION_FRAME_RUNNING;
			break;
		
		case ACTION_FRAME_RUNNING:
			if(get_tick() - action_tick >= robot_arm.action_group.frame.time)
			{
				robot_arm.action_group.frame.index++;
				robot_arm.action_group.frame.status = ACTION_FRAME_IDLE;			
			}
			break;
		
		case ACTION_FRAME_IDLE:
			break;
		
		default:
			break;
	}
	
	return robot_arm.action_group.frame.status;
}

uint8_t action_group_run(uint8_t action_group_index, uint16_t running_times)
{
	uint8_t state = 0;
	
	switch (robot_arm.action_group.status)
	{
		case ACTION_GROUP_START:
			robot_arm.action_group.running_times = running_times;
			robot_arm.action_group.index = action_group_index;
			robot_arm.action_group.frame.index = 0;
			w25q64_read(ACTION_FRAME_SUM_BASE_ADDRESS + action_group_index,
						&robot_arm.action_group.sum, 1);
			/* 如果该动作组的动作帧数量大于0，则说明已经下载过动作 */
			robot_arm.action_group.status = robot_arm.action_group.sum > 0 ? ACTION_GROUP_RUNNING : ACTION_GROUP_IDLE;
			break;

		case ACTION_GROUP_RUNNING:
			if(action_frame_run(robot_arm.action_group.index, robot_arm.action_group.frame.index) == ACTION_FRAME_IDLE)	
			{
				if (robot_arm.action_group.frame.index == robot_arm.action_group.sum)
				{
					robot_arm.action_group.status = ACTION_GROUP_END_PERIOD;
					robot_arm.action_group.frame.index = 0;
				}
				else
				{
					robot_arm.action_group.frame.status = ACTION_FRAME_START;
				}
			}	
			break;

		case ACTION_GROUP_END_PERIOD:
			if(robot_arm.action_group.running_times == 1)
			{
				robot_arm.action_group.status = ACTION_GROUP_IDLE;
			}
			else
			{
				--robot_arm.action_group.running_times;
				robot_arm.action_group.status = ACTION_GROUP_RUNNING;
			}
			break;

		case ACTION_GROUP_IDLE:
			state = 1;
			break;
		
		default:
			break;
	}
	return state;
}

void action_group_reset()
{
	robot_arm.action_group.status = ACTION_GROUP_START;
	robot_arm.action_group.frame.status = ACTION_FRAME_START;
}

void action_group_stop()
{
	uint8_t i;
	for (i = 0; i < MAX_SERVOS_NUM; i++)
	{
		robot_arm_knot_stop(i);
	}
	robot_arm.action_group.status = ACTION_GROUP_IDLE;
	robot_arm.action_group.frame.status = ACTION_FRAME_START;
}

//uint8_t write_frame[8][ACTION_FRAME_SIZE] = {0};
//uint8_t read_frame[8][ACTION_FRAME_SIZE] = {0};
void action_group_save(uint8_t action_group_index, 
					   uint8_t frame_num,
					   uint8_t frame_index,
					   uint8_t* _pdata,
					   uint16_t _size)
{	
	uint8_t i;
	uint32_t ag_addr_offset;
	uint32_t af_addr_offset;
	uint32_t page_offset;
	uint32_t write_addr;
	uint16_t remaining_space;
	
	robot_arm.action_group.index = action_group_index;
	robot_arm.action_group.frame.index = frame_index;
	ag_addr_offset = action_group_index * ACTION_GROUP_SIZE;	/* 动作组偏移地址 */
	af_addr_offset = frame_index * ACTION_FRAME_SIZE;			/* 动作帧偏移地址 */
	page_offset = af_addr_offset % 256; // 正确计算页偏移
	write_addr = ACTION_GROUP_BASE_ADDRESS + ag_addr_offset + af_addr_offset;
	remaining_space = 256 - page_offset;
	
    /* 擦除动作组空间（首帧） */
    if (frame_index == 0) 
	{
        for (i = 0; i < 2; i++)
		{
            w25q64_erase_sector(ACTION_GROUP_BASE_ADDRESS + ag_addr_offset + (i * 4096));
        }
    }

    /* 跨页写入处理 */
    if (remaining_space < ACTION_FRAME_SIZE)
	{
        w25q64_write(write_addr, _pdata, remaining_space);
        w25q64_write(write_addr + remaining_space, _pdata + remaining_space, ACTION_FRAME_SIZE - remaining_space);
    } 
	else 
	{
		w25q64_write(write_addr, _pdata, ACTION_FRAME_SIZE);
    }

//	memcpy(write_frame[frame_index], _pdata, _size);
//	w25q64_read(ACTION_GROUP_BASE_ADDRESS + ag_addr_offset + af_addr_offset, read_frame[frame_index], sizeof(read_frame[frame_index]));
		 
	if ((robot_arm.action_group.frame.index + 1) == frame_num)
	{
		/* 如果写入的是最后一帧，此时需要更新一下flash中对应动作组的动作帧总数 */
		w25q64_read(ACTION_FRAME_SUM_BASE_ADDRESS,robot_arm.action_group._sum, sizeof(robot_arm.action_group._sum));
		robot_arm.action_group._sum[robot_arm.action_group.index] = frame_num;
		w25q64_erase_sector(ACTION_FRAME_SUM_BASE_ADDRESS);
		w25q64_write(ACTION_FRAME_SUM_BASE_ADDRESS, (const uint8_t*)robot_arm.action_group._sum, sizeof(robot_arm.action_group._sum));
	}
}

void robot_arm_reset(uint32_t time)
{
	switch(SERVO_TYPE)
	{
		case 1:
			pwm_servo_duty_set(&pwm_servos[0], PWM_SERVO6_RESET_DUTY, time);
			pwm_servo_duty_set(&pwm_servos[1], PWM_SERVO5_RESET_DUTY, time);
			pwm_servo_duty_set(&pwm_servos[2], PWM_SERVO4_RESET_DUTY, time);
			pwm_servo_duty_set(&pwm_servos[3], PWM_SERVO3_RESET_DUTY, time);
			pwm_servo_duty_set(&pwm_servos[4], PWM_SERVO2_RESET_DUTY, time);
			pwm_servo_duty_set(&pwm_servos[5], PWM_SERVO1_RESET_DUTY, time);
			break;
		
		case 2:
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 1, SERIAL_SERVO1_RESET_DUTY, time);
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 2, SERIAL_SERVO2_RESET_DUTY, time);
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 3, SERIAL_SERVO3_RESET_DUTY, time);
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 4, SERIAL_SERVO4_RESET_DUTY, time);
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 5, SERIAL_SERVO5_RESET_DUTY, time);
			serial_servos_delay();
			serial_servo_set_position(&serial_servo_controller, 6, SERIAL_SERVO6_RESET_DUTY, time);
			serial_servos_delay();
			break;
	}
}

uint8_t robot_arm_init(void)
{
	switch(SERVO_TYPE)
	{
		case 1:
			pwm_servos_init();
			break;
		
		case 2:
			serial_servo_init();
			break;
	}

	kinematics_init(&kinematics);
	memset(&robot_arm, 0, sizeof(RobotArmHandleTypeDef));
	if(robot_arm_flash_init() == 0)
	{
		return 0;
	}
	return 1;
}



