//
// Created by huli on 2020/7/28.
//

#include "motor_driver.h"
#include "../motor/motor_communication.h"

Motor_driver::Motor_driver()
{
	m_motor_device_id = -1;
	m_motor_driver_status = MOTOR_DRIVER_UNKNOW;
	m_can_device_id = -1;
	m_can_port_id = -1;

	m_can_send_id = 0;
	m_can_send_extern_flag = false;
	m_control_byte = CONTROL_STOP;
	m_target_velocity = 0;

	m_can_receive_id = 0;
	m_can_receive_extern_flag = false;
	m_status_byte = STATUS_UNKNOW;
	m_actual_velocity = 0;
	m_receive_time = std::chrono::system_clock::now();
}

Motor_driver::~Motor_driver()
{
	motor_driver_uninit();
}

Error_manager Motor_driver::motor_driver_init(int motor_device_id, int can_device_id, int can_port_id)
{
	m_motor_device_id = motor_device_id;
	m_motor_driver_status = MOTOR_DRIVER_UNKNOW; //初始化之后,电机驱动器的状态不是ready, 必须唤醒通信之后,才能进入待机.
	m_can_device_id = can_device_id;
	m_can_port_id = can_port_id;

	//发送id为0, 就是广播消息, 唤醒can总线上面的所有的电机驱动器
	m_can_send_id = 0;
	m_can_send_extern_flag = false;
	m_control_byte = CONTROL_INIT;
	m_target_velocity = 0;

	m_can_receive_id = MOTOR_DRIVER_CAN_RECEIVE_MESSAGE_BASE + m_motor_device_id;
	m_can_receive_extern_flag = false;
	m_status_byte = STATUS_UNKNOW;
	m_actual_velocity = 0;

	return Error_code::SUCCESS;
}

Error_manager Motor_driver::motor_driver_uninit()
{
	m_motor_driver_status = MOTOR_DRIVER_UNKNOW;

	m_control_byte = CONTROL_STOP;
	m_target_velocity = 0;

	return Error_code::SUCCESS;
}

//刷新状态
Error_manager Motor_driver::update_status()
{
	//根据 接受消息和时间, 刷新状态
	if ( std::chrono::system_clock::now() - m_receive_time > std::chrono::milliseconds(MOTOR_DRIVER_CAN_COMMUNICATION_TIME_OUT_MS) )
	{
		m_motor_driver_status = MOTOR_DRIVER_COMMUNICATION_FAULT;
		return Error_manager(Error_code::MOTOR_DRIVER_COMMUNICATION_ERROR, Error_level::MINOR_ERROR,
							" Motor_driver::update_status error ");
	}
	else if ( m_status_byte == STATUS_UNKNOW || m_status_byte == STATUS_FAULT)
	{
		m_motor_driver_status = MOTOR_DRIVER_DEVICE_FAULT;
		return Error_manager(Error_code::MOTOR_DRIVER_DEVICE_ERROR, Error_level::MINOR_ERROR,
							 " Motor_driver::update_status error ");
	}
	else if ( m_status_byte == STATUS_READY )
	{
		m_motor_driver_status = MOTOR_DRIVER_READY;
		return Error_code::SUCCESS;
	}
	else if ( m_status_byte == STATUS_RUN )
	{
		m_motor_driver_status = MOTOR_DRIVER_RUN;
		return Error_code::SUCCESS;
	}
	return Error_code::SUCCESS;
}

//检查状态
Error_manager Motor_driver::check_status()
{
	return update_status();
}

//检查消息是否有效, 判断这条消息是不是给我的.
Error_manager Motor_driver::check_msg(Can_message* p_msg)
{
	if ( p_msg->can_device_id == m_can_device_id
		 && p_msg->can_port_id == m_can_port_id
		 && p_msg->can_object.ID == m_can_receive_id
		 && p_msg->can_object.ExternFlag == m_can_receive_extern_flag)
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
							" INVALID_MESSAGE error ");
	}
}

//处理消息
Error_manager Motor_driver::execute_msg(Can_message* p_msg)
{
//	Error_manager t_error;
	if ( p_msg->can_device_id == m_can_device_id
		 && p_msg->can_port_id == m_can_port_id
		 && p_msg->can_object.ID == m_can_receive_id
		 && p_msg->can_object.ExternFlag == m_can_receive_extern_flag)
	{
		//按照通信协议,解析can消息, 2~3字节是频率实际值, (前低后高) 需要转化为速度实际值
		m_status_byte = (Motor_driver::Status_byte)(p_msg->can_object.Data[0]);
		short t_frequency= (int)(p_msg->can_object.Data[3])<<8 | (int)(p_msg->can_object.Data[2]);
		//这里需要 频率和速度的转化公式
		float temp = ((float)t_frequency)/2605.0f;
		m_actual_velocity = temp;
		m_receive_time = std::chrono::system_clock::now();

		//这里不用刷新状态, 保证在使用之前刷新状态就可以了
		//		update_status();

		//处理任务, 电机驱动器的核心控制流程就在这里
		return execute_task();
	}
	else
	{
		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
							 " INVALID_MESSAGE error ");
	}

	return Error_code::SUCCESS;
}

//处理任务, 电机驱动器的核心控制流程就在这里
Error_manager Motor_driver::execute_task()
{
	Error_manager t_error;

//	std::cout << "----------huli test----------------" << std::endl;
//	printf("m_can_send_id = 0x%x, m_control_byte = 0x%x, m_target_velocity = 0x%x, \n",(int)m_can_send_id,(int)m_control_byte, (int)m_target_velocity);
//	printf("m_can_receive_id = 0x%x, m_status_byte = 0x%x, m_actual_velocity = 0x%x, \n",(int)m_can_receive_id,(int)m_status_byte, (int)m_actual_velocity);

	t_error = update_status();
	if(t_error != MOTOR_DRIVER_COMMUNICATION_ERROR)
	{
		switch ( m_status_byte )
		{
		    case STATUS_UNKNOW:
		    {
		        ;
		        break;
		    }
		    case STATUS_READY:
		    {
		        if ( m_control_byte == CONTROL_INIT )
		        {//从发送唤醒信息改为正常控制
					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
					m_can_send_extern_flag = false;
					m_control_byte = CONTROL_RUN;
					m_target_velocity = 0;
		        }
		        //正常控制
		        break;
		    }
			case STATUS_RUN:
			{
				if ( m_control_byte == CONTROL_INIT )
				{//从发送唤醒信息改为正常控制
					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
					m_can_send_extern_flag = false;
					m_control_byte = CONTROL_RUN;
					m_target_velocity = 0;
				}
				//正常控制
				break;
			}
			case STATUS_FAULT:
			{
				if ( m_control_byte == CONTROL_INIT )
				{//从发送唤醒信息改为正常控制
					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
					m_can_send_extern_flag = false;
					m_control_byte = CONTROL_STOP;
					m_target_velocity = 0;
				}
				else
				{
					//故障状态, 速度锁定为0
					m_target_velocity = 0;
				}
				break;
			}
		    default:
		    {

		        break;
		    }
		}
	}
	return t_error;
}

//定时封装发送消息,
Error_manager Motor_driver::encapsulate_send_data()
{
	return Error_code::SUCCESS;
//	return encapsulate_send_data_once();
}

//发送can消息, 触发式, 调用本函数就发送一条.
Error_manager Motor_driver::encapsulate_send_data_once()
{
	//定时发送
	Can_message t_msg;
	t_msg.can_device_id = m_can_device_id;
	t_msg.can_port_id = m_can_port_id;
	t_msg.can_object.ID = m_can_send_id;
	t_msg.can_object.SendType = 0;
	t_msg.can_object.RemoteFlag = 0;
	t_msg.can_object.ExternFlag = 0;
	t_msg.can_object.DataLen = 4;

	//按照通信协议,解析can消息, 2~3字节是频率实际值, (前低后高)
	t_msg.can_object.Data[0] = m_control_byte;
	t_msg.can_object.Data[1] = 0;
	//这里需要 频率和速度的转化公式
	float temp = m_target_velocity*2605;

	short t_frequency = (short)temp;
	t_msg.can_object.Data[2] = t_frequency & 0xff;
	t_msg.can_object.Data[3] = t_frequency >> 8;

	return Motor_communication::get_instance_references().encapsulate_msg(&t_msg);
}


Motor_driver::Motor_driver_status Motor_driver::get_motor_driver_status()
{
	update_status();
	return m_motor_driver_status;
}

Error_manager Motor_driver::set_target_velocity(float target_velocity)
{
	Error_manager t_error = update_status();
	if (  t_error== SUCCESS )
	{
		m_control_byte = CONTROL_RUN;
	    m_target_velocity = target_velocity;
		return encapsulate_send_data_once();
	}
	else
	{
		return t_error;
	}
}

float Motor_driver::get_actual_velocity()
{
	return m_actual_velocity;
}

//复位
Error_manager Motor_driver::motor_driver_reset()
{
	m_control_byte = CONTROL_RESET;
	m_actual_velocity = 0;
	return encapsulate_send_data_once();
}

//急停
Error_manager 		Motor_driver::motor_driver_stop()
{
	m_control_byte = CONTROL_STOP;
	m_actual_velocity = 0;
	return encapsulate_send_data_once();
}
