//
// Created by huli on 2021/3/22.
//

#include "dispatch_process.h"
#include "../system/system_communication.h"
#include "../dispatch/dispatch_manager.h"

Dispatch_process::Dispatch_process()
{
	m_dispatch_process_status = DISPATCH_PROCESS_STATUS_UNKNOW;
	m_dispatch_process_type = DISPATCH_PROCESS_TYPE_UNKNOW;
	m_dispatch_source = 0;
	m_dispatch_destination = 0;

	m_dispatch_process_catcher_motion = CATCHER_MOTION_UNKNOW;
	m_dispatch_process_carrier_motion = CARRIER_MOTION_UNKNOW;
}

Dispatch_process::~Dispatch_process()
{
	Dispatch_process_uninit();
}

//初始化, 就把主控发送的请求传入即可.
Error_manager Dispatch_process::Dispatch_process_init(message::Dispatch_request_msg dispatch_request_msg)
{
	if ( dispatch_request_msg.base_info().has_timeout_ms() )
	{
		m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
	}
	else
	{
		m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
	}
	m_command_key = dispatch_request_msg.command_key();
	m_start_time = std::chrono::system_clock::now();

	if ( dispatch_request_msg.dispatch_motion_direction() == message::E_STORE_CAR )
	{
		m_dispatch_process_type = DISPATCH_PROCESS_STORE;
		m_dispatch_source = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
		m_dispatch_destination = dispatch_request_msg.parkspace_info().parkspace_id() + PARKSPACE_ID_BASE;
		Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
		Common_data::copy_data(m_car_measure_information, dispatch_request_msg.locate_information());
	}
	else if( dispatch_request_msg.dispatch_motion_direction() == message::E_PICKUP_CAR )
	{
		m_dispatch_process_type = DISPATCH_PROCESS_PICKUP;
		m_dispatch_source = dispatch_request_msg.parkspace_info().parkspace_id() + PARKSPACE_ID_BASE;
		m_dispatch_destination = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
		Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
	}
	else
	{
		m_dispatch_process_type = DISPATCH_PROCESS_TYPE_UNKNOW;
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							 "  Dispatch_process::Dispatch_process_init ERROR ");
	}

	m_dispatch_request_msg = dispatch_request_msg;
	m_dispatch_process_status = DISPATCH_PROCESS_CREATED;
	return Error_code::SUCCESS;
}

//反初始化
Error_manager Dispatch_process::Dispatch_process_uninit()
{
	return Error_code::SUCCESS;
}

void Dispatch_process::Main()
{
	Error_manager t_error;

	//主流程, 循环执行
	while ( std::chrono::system_clock::now() - m_start_time < std::chrono::milliseconds(m_timeout_ms) )
	{
		std::this_thread::sleep_for(std::chrono::microseconds(1));

		switch ( m_dispatch_process_status )
		{
			case DISPATCH_PROCESS_CREATED:
			{
				//检查调度请求
				m_result = check_dispatch_request_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//发送调度总计划
				m_result = send_dispatch_plan_request_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				m_dispatch_process_status = DISPATCH_PROCESS_READY;
				break;
			}
			case DISPATCH_PROCESS_READY:
			{
				//等待控制指令
				m_result = wait_dispatch_control_request_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					//不成功, 就表示没有新的指令, 那么什么都不做, 原地待命
				}
				else
				{
					//流程正常, 就去连接设备
					m_dispatch_process_status = DISPATCH_PROCESS_CONNECT_DEVICE;
					break;
				}

				//等待调度总计划答复
				m_result = wait_dispatch_plan_response_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					//不成功, 就表示没有总计划答复, 那么什么都不做, 原地待命
				}
				else
				{
					//流程正常, 就进入完成状态,
					m_dispatch_process_status = DISPATCH_PROCESS_OVER;
					break;
				}
				break;
			}
			case DISPATCH_PROCESS_CONNECT_DEVICE:
			{
				//连接调度设备
				m_result = connect_dispatch_device();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//流程正常, 就进入工作状态,
				m_dispatch_process_status = DISPATCH_PROCESS_WORKING;
				break;

				break;
			}
			case DISPATCH_PROCESS_WORKING:
			{

				break;
			}
			case DISPATCH_PROCESS_RESPONSE:
			{
				break;
			}
			case DISPATCH_PROCESS_OVER:
			{
				//发送调度答复, 发给主控的
				m_result = send_dispatch_response_msg();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}
				//流程正常, 就进入等待状态, 等待调度控制发送动作指令
				m_dispatch_process_status = DISPATCH_PROCESS_RELEASE;
				break;
			}
			case DISPATCH_PROCESS_RELEASE:
			{
				//通知调度管理, 释放资源,
				m_result = release_resource();
				if ( m_result !=Error_code::SUCCESS)
				{
					m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
					break;
				}

				//在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源
				return;
				break;
			}
			case DISPATCH_PROCESS_FAULT:
			{
				break;
			}
			default:
			{

				break;
			}
		}
	}

	//任务超时

	return;
}


//检查调度请求
Error_manager Dispatch_process::check_dispatch_request_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	return Error_code::SUCCESS;
}

//发送调度总计划
Error_manager Dispatch_process::send_dispatch_plan_request_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	m_dispatch_plan_request_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_plan_request_msg);
	m_dispatch_plan_request_msg.mutable_base_info()->set_timeout_ms(m_timeout_ms);
	m_dispatch_plan_request_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_mamager);
	m_dispatch_plan_request_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
	m_dispatch_plan_request_msg.set_command_key(m_command_key);

	m_dispatch_plan_request_msg.set_dispatch_task_type((message::Dispatch_task_type)m_dispatch_process_type);
	m_dispatch_plan_request_msg.set_dispatch_source(m_dispatch_source);
	m_dispatch_plan_request_msg.set_dispatch_destination(m_dispatch_destination);
	//这里不写错误码

	std::string t_msg = m_dispatch_plan_request_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg);
	return Error_code::SUCCESS;
}


//等待控制指令
Error_manager Dispatch_process::wait_dispatch_control_request_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	//key不相等 就表示 收到了新的控制指令
	if ( m_dispatch_control_request_msg.command_key() == m_dispatch_control_response_msg.command_key() )
	{
		return Error_code::NODATA;
	}
	else
	{
		return Error_code::SUCCESS;
	}
}



//连接调度设备
Error_manager Dispatch_process::connect_dispatch_device()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	Error_manager t_error;

	//机器手的配置 准备工作
	if ( m_dispatch_control_request_msg.dispatch_device_type() >= message::Dispatch_device_type::ROBOT_1 &&
		 m_dispatch_control_request_msg.dispatch_device_type() <= message::Dispatch_device_type::ROBOT_2 )
	{
		//找到对应的设备
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::ROBOT_1 )
		{
			mp_catcher = Dispatch_manager::get_instance_references().m_catcher_map[0];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::ROBOT_2 )
		{
			mp_catcher = Dispatch_manager::get_instance_references().m_catcher_map[1];
		}

		//检查设备状态
		if ( mp_catcher->check_status() == Error_code::SUCCESS &&
			 mp_catcher->m_actual_device_status == Dispatch_device_base::DEVICE_READY &&
			 mp_catcher->m_actual_load_status == Dispatch_device_base::NO_CAR)
		{
			//创建任务单
			mp_catcher_task = std::shared_ptr<Task_Base>(new Catcher_task);
			mp_catcher_task->task_init(NULL,std::chrono::milliseconds(15000));
			Catcher_task * tp_catcher_task = (Catcher_task *)mp_catcher_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_catcher_task->m_request_key = "";
			tp_catcher_task->m_request_x = mp_catcher->m_actual_x;
			tp_catcher_task->m_request_y = mp_catcher->m_actual_y;
			tp_catcher_task->m_request_b = mp_catcher->m_actual_b;
			tp_catcher_task->m_request_z = mp_catcher->m_actual_z;
			tp_catcher_task->m_request_d1 = mp_catcher->m_actual_d1;
			tp_catcher_task->m_request_d2 = mp_catcher->m_actual_d2;
			tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
			tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)mp_catcher->m_actual_clamp_motion1;

			t_error = mp_catcher->execute_task(mp_catcher_task, Dispatch_device_base::E_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
		}
		else
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
								 " Dispatch_process::excute_robot_catch_car_from_inlet device_status error ");
		}

		//设置起点
		if ( m_dispatch_control_request_msg.dispatch_source() == 0 )
		{
			m_source_coordinates.x = 0;
			m_source_coordinates.y = 0;
			m_source_coordinates.z = 0;
		}
		else if ( m_dispatch_control_request_msg.dispatch_source() > 0 && m_dispatch_control_request_msg.dispatch_source() <= 165)
		{
			m_source_coordinates = Dispatch_coordinates::get_instance_references().m_packspace_coordinates[m_dispatch_control_request_msg.dispatch_source()];
		}
		else
		{
			m_source_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[m_dispatch_control_request_msg.dispatch_source()];
		}

		//设置终点
		if ( m_dispatch_control_request_msg.dispatch_destination() == 0 )
		{
			m_destination_coordinates.x = 0;
			m_destination_coordinates.y = 0;
			m_destination_coordinates.z = 0;
		}
		else if ( m_dispatch_control_request_msg.dispatch_destination() > 0 && m_dispatch_control_request_msg.dispatch_destination() <= 165)
		{
			m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_packspace_coordinates[m_dispatch_control_request_msg.dispatch_destination()];
		}
		else
		{
			m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[m_dispatch_control_request_msg.dispatch_destination()];
		}
	}
		//搬运器的配置 准备工作
	else if ( m_dispatch_control_request_msg.dispatch_device_type() >= message::Dispatch_device_type::CARRIER_1 &&
			  m_dispatch_control_request_msg.dispatch_device_type() <= message::Dispatch_device_type::CARRIER_3 )
	{
		//找到对应的设备
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_1 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[0];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_2 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[1];
		}
		if ( m_dispatch_control_request_msg.dispatch_device_type() == message::Dispatch_device_type::CARRIER_3 )
		{
			mp_carrier = Dispatch_manager::get_instance_references().m_carrier_map[1];
		}

		//检查设备状态
		if ( mp_carrier->check_status() == Error_code::SUCCESS &&
			 mp_carrier->m_actual_device_status == Dispatch_device_base::DEVICE_READY &&
			 mp_carrier->m_actual_load_status == Dispatch_device_base::NO_CAR)
		{
			//创建任务单
			mp_carrier_task = std::shared_ptr<Task_Base>(new Carrier_task);
			mp_carrier_task->task_init(NULL,std::chrono::milliseconds(15000));
			Carrier_task * tp_carrier_task = (Carrier_task *)mp_carrier_task.get();

			//第一次发送 空的唯一码, 可以和设备建立联系
			tp_carrier_task->m_request_key = "";
			tp_carrier_task->m_request_x = mp_carrier->m_actual_x;
			tp_carrier_task->m_request_y = mp_carrier->m_actual_y;
			tp_carrier_task->m_request_z = mp_carrier->m_actual_z;
			tp_carrier_task->m_request_y1 = mp_carrier->m_actual_y1;
			tp_carrier_task->m_request_y2 = mp_carrier->m_actual_y2;
			tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)mp_carrier->m_actual_clamp_motion1;
			tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)mp_carrier->m_actual_joint_motion_x1;
			tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION;
			tp_carrier_task->m_request_space_id = 0;
			tp_carrier_task->m_request_floor_id = 0;
			tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;

			t_error = mp_carrier->execute_task(mp_catcher_task, Dispatch_device_base::E_ONE_LEVEL);
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
		}
		else
		{
			return Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
								 " Dispatch_process::excute_robot_catch_car_from_inlet device_status error ");
		}

		//设置起点
		if ( m_dispatch_control_request_msg.dispatch_source() == 0 )
		{
			m_source_coordinates.x = 0;
			m_source_coordinates.y = 0;
			m_source_coordinates.z = 0;
		}
		else if ( m_dispatch_control_request_msg.dispatch_source() > 0 && m_dispatch_control_request_msg.dispatch_source() <= 165)
		{
			m_source_coordinates = Dispatch_coordinates::get_instance_references().m_packspace_coordinates[m_dispatch_control_request_msg.dispatch_source()];
		}
		else
		{
			m_source_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates[m_dispatch_control_request_msg.dispatch_source()];
		}

		//设置终点
		if ( m_dispatch_control_request_msg.dispatch_destination() == 0 )
		{
			m_destination_coordinates.x = 0;
			m_destination_coordinates.y = 0;
			m_destination_coordinates.z = 0;
		}
		else if ( m_dispatch_control_request_msg.dispatch_destination() > 0 && m_dispatch_control_request_msg.dispatch_destination() <= 165)
		{
			m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_packspace_coordinates[m_dispatch_control_request_msg.dispatch_destination()];
		}
		else
		{
			m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates[m_dispatch_control_request_msg.dispatch_destination()];
		}
	}

	return Error_code::SUCCESS;
}

//检查机器手任务单
Error_manager Dispatch_process::check_catcher_task()
{
	if ( mp_catcher_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_catcher_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_catcher_task = (Catcher_task *)mp_catcher_task.get();

		if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_catcher_task->get_task_error_manager();
		}
		else if ( tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_catcher_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}
//检查搬运器任务单
Error_manager Dispatch_process::check_carrier_task()
{
	if ( mp_carrier_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_carrier_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_carrier_task = (Catcher_task *)mp_carrier_task.get();

		if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_carrier_task->get_task_error_manager();
		}
		else if ( tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_carrier_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}
//检查通道口任务单
Error_manager Dispatch_process::check_passageway_task()
{
	if ( mp_passageway_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::check_passageway_task  POINTER IS NULL ");
	}
	else
	{
		Catcher_task * tp_passageway_task = (Catcher_task *)mp_passageway_task.get();

		if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
		{
			return Error_code::SUCCESS;
		}
		else if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
		{
			return tp_passageway_task->get_task_error_manager();
		}
		else if ( tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING &&
				  tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED &&
				  tp_passageway_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
		{
			return Error_code::NODATA;
		}
	}
}

//执行调度控制指令, 并根据完成情况给答复
Error_manager Dispatch_process::excute_dispatch_control()
{

	return Error_code::SUCCESS;
}

//机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
Error_manager Dispatch_process::excute_robot_catch_car_from_inlet()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	Error_manager t_error;

	//设备的动作也使用外部的Main()的线程来循环
	static int s_step = 0;
	Catcher_task * tp_catcher_task = NULL;

	if ( mp_catcher.get() == NULL || mp_catcher_task.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Dispatch_process::excute_robot_catch_car_from_inlet()  POINTER IS NULL ");
	}
	else
	{
		tp_catcher_task = (Catcher_task *)mp_catcher_task.get();
	}

	if ( s_step == 0 )
	{
		t_error = check_catcher_task();
		if ( t_error == Error_code::SUCCESS )
		{
			s_step++;//成功完成之后, 并不返回, 直接进入下一步
		}
		else if( t_error == Error_code::NODATA )
		{
		    //什么也不做, 继续等待任务完成, 此时Main函数会一直循环.
		}
		else
		{
			s_step = 0;
			return t_error;
		}
	}
	if ( s_step == 1 )
	{
		std::unique_lock<std::mutex> t_lock1(m_lock);
		std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
		char t_key[50] = {0};
		sprintf(t_key, "%s+%d+%d", m_dispatch_control_request_msg.command_key().c_str(), m_dispatch_control_request_msg.dispatch_task_type(), s_step);
		tp_catcher_task->m_respons_key = t_key;
		//机器手z轴移到最高点.
		tp_catcher_task->m_request_z = Dispatch_coordinates::get_instance_references().m_catcher_4th_floor_z;
	}
}
//机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
Error_manager Dispatch_process::excute_robot_put_car_to_carrier()
{

}

//执行通道口动作
Error_manager Dispatch_process::excute_passageway_motion()
{
	return Error_code::SUCCESS;
}

//等待调度总计划答复
Error_manager Dispatch_process::wait_dispatch_plan_response_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	//key 相等 就表示 收到了总计划答复
	if ( m_dispatch_plan_request_msg.command_key() == m_dispatch_plan_response_msg.command_key() )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_code::NODATA;
	}
}

//发送调度答复, 发给主控的
Error_manager Dispatch_process::send_dispatch_response_msg()
{
	std::unique_lock<std::mutex> t_lock(m_lock);
	m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg);
	m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_timeout_ms);
	m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_mamager);
	m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
	m_dispatch_response_msg.set_command_key(m_command_key);

	m_dispatch_response_msg.mutable_error_manager()->CopyFrom(m_dispatch_plan_response_msg.error_manager());

	std::string t_msg = m_dispatch_response_msg.SerializeAsString();
	System_communication::get_instance_references().encapsulate_msg(t_msg);
	return Error_code::SUCCESS;
}

//通知调度管理, 释放资源,
Error_manager Dispatch_process::release_resource()
{
	return Dispatch_manager::get_instance_references().release_dispatch_process(m_command_key);
}