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

#include "dispatch_coordinates.h"
#include "../tool/proto_tool.h"


Dispatch_coordinates::Dispatch_coordinates()
{


}
Dispatch_coordinates::~Dispatch_coordinates()
{

}

//初始化 调度的坐标系 模块。如下三选一
Error_manager Dispatch_coordinates::dispatch_coordinates_init()
{
	return  dispatch_coordinates_init_from_protobuf(DISPATCH_COORDINATES_PARAMETER_PATH);
}

//初始化 调度的坐标系 模块。从文件读取
Error_manager Dispatch_coordinates::dispatch_coordinates_init_from_protobuf(std::string prototxt_path)
{
	Dispatch_proto::Dispatch_coordinates_parameter_all t_dispatch_coordinates_parameter_all;
	if(!  proto_tool::read_proto_param(prototxt_path,t_dispatch_coordinates_parameter_all) )
	{
		return Error_manager(DISPATCH_COORDINATES_READ_PROTOBUF_ERROR,MINOR_ERROR,
							 "Dispatch_coordinates read_proto_param  failed");
	}

	return dispatch_coordinates_init_from_protobuf(t_dispatch_coordinates_parameter_all);
}

//初始化 调度的坐标系 模块。从protobuf读取
Error_manager Dispatch_coordinates::dispatch_coordinates_init_from_protobuf(Dispatch_proto::Dispatch_coordinates_parameter_all& dispatch_coordinates_parameter_all)
{
	LOG(INFO) << " ---Dispatch_coordinates::dispatch_coordinates_init_from_protobuf() run--- "<< this;
	Error_manager t_error;
	int size;

	size = dispatch_coordinates_parameter_all.packspace_coordinates_parameters_size();
	for (int i = 0; i < size; ++i)
	{
		Point3D_tool::Point3D t_point3d;
		int id = dispatch_coordinates_parameter_all.packspace_coordinates_parameters(i).id();
		t_point3d.x = dispatch_coordinates_parameter_all.packspace_coordinates_parameters(i).x();
		t_point3d.y = dispatch_coordinates_parameter_all.packspace_coordinates_parameters(i).y();
		t_point3d.z = dispatch_coordinates_parameter_all.packspace_coordinates_parameters(i).z();
		m_packspace_coordinates[id] = t_point3d;
	}

	size = dispatch_coordinates_parameter_all.passageway_coordinates_parameters_size();
	for (int i = 0; i < size; ++i)
	{
		Point3D_tool::Point3D t_point3d;
		int id = dispatch_coordinates_parameter_all.passageway_coordinates_parameters(i).id();
		t_point3d.x = dispatch_coordinates_parameter_all.passageway_coordinates_parameters(i).x();
		t_point3d.y = dispatch_coordinates_parameter_all.passageway_coordinates_parameters(i).y();
		t_point3d.z = dispatch_coordinates_parameter_all.passageway_coordinates_parameters(i).z();
		m_passageway_coordinates[id] = t_point3d;
	}

	size = dispatch_coordinates_parameter_all.carrier_coordinates_parameters_size();
	for (int i = 0; i < size; ++i)
	{
		Point3D_tool::Point3D t_point3d;
		int id = dispatch_coordinates_parameter_all.carrier_coordinates_parameters(i).id();
		t_point3d.x = dispatch_coordinates_parameter_all.carrier_coordinates_parameters(i).x();
		t_point3d.y = dispatch_coordinates_parameter_all.carrier_coordinates_parameters(i).y();
		t_point3d.z = dispatch_coordinates_parameter_all.carrier_coordinates_parameters(i).z();
		m_carrier_coordinates[id] = t_point3d;
	}

	size = dispatch_coordinates_parameter_all.catcher_coordinates_parameters_size();
	for (int i = 0; i < size; ++i)
	{
		Point3D_tool::Point3D t_point3d;
		int id = dispatch_coordinates_parameter_all.catcher_coordinates_parameters(i).id();
		t_point3d.x = dispatch_coordinates_parameter_all.catcher_coordinates_parameters(i).x();
		t_point3d.y = dispatch_coordinates_parameter_all.catcher_coordinates_parameters(i).y();
		t_point3d.z = dispatch_coordinates_parameter_all.catcher_coordinates_parameters(i).z();
		m_catcher_coordinates[id] = t_point3d;
	}


	m_carrier_box.x_min = dispatch_coordinates_parameter_all.carrier_box_parameters().x_min();
	m_carrier_box.x_max = dispatch_coordinates_parameter_all.carrier_box_parameters().x_max();
	m_carrier_box.y_min = dispatch_coordinates_parameter_all.carrier_box_parameters().y_min();
	m_carrier_box.y_max = dispatch_coordinates_parameter_all.carrier_box_parameters().y_max();
	m_carrier_box.z_min = dispatch_coordinates_parameter_all.carrier_box_parameters().z_min();
	m_carrier_box.z_max = dispatch_coordinates_parameter_all.carrier_box_parameters().z_max();

	m_catcher_box.x_min = dispatch_coordinates_parameter_all.catcher_box_parameters().x_min();
	m_catcher_box.x_max = dispatch_coordinates_parameter_all.catcher_box_parameters().x_max();
	m_catcher_box.y_min = dispatch_coordinates_parameter_all.catcher_box_parameters().y_min();
	m_catcher_box.y_max = dispatch_coordinates_parameter_all.catcher_box_parameters().y_max();
	m_catcher_box.z_min = dispatch_coordinates_parameter_all.catcher_box_parameters().z_min();
	m_catcher_box.z_max = dispatch_coordinates_parameter_all.catcher_box_parameters().z_max();

	m_catcher_b_min = dispatch_coordinates_parameter_all.catcher_b_min();
	m_catcher_b_max = dispatch_coordinates_parameter_all.catcher_b_max();
	m_catcher_d1_min = dispatch_coordinates_parameter_all.catcher_d1_min();
	m_catcher_d1_max = dispatch_coordinates_parameter_all.catcher_d1_max();	
	m_catcher_d2_min = dispatch_coordinates_parameter_all.catcher_d2_min();
	m_catcher_d2_max = dispatch_coordinates_parameter_all.catcher_d2_max();

	std::cout << " huli test :::: " << " m_packspace_coordinates.size() = " << m_packspace_coordinates.size() << std::endl;
	std::cout << " huli test :::: " << " m_passageway_coordinates.size() = " << m_passageway_coordinates.size() << std::endl;
	std::cout << " huli test :::: " << " m_carrier_coordinates.size() = " << m_carrier_coordinates.size() << std::endl;
	std::cout << " huli test :::: " << " m_catcher_coordinates.size() = " << m_catcher_coordinates.size() << std::endl;

	m_default_wheelbase = dispatch_coordinates_parameter_all.default_wheelbase();

	m_catcher_1th_floor_z = dispatch_coordinates_parameter_all.catcher_1th_floor_z();
	m_catcher_2th_floor_z = dispatch_coordinates_parameter_all.catcher_2th_floor_z();
	m_catcher_3th_floor_z = dispatch_coordinates_parameter_all.catcher_3th_floor_z();
	m_catcher_4th_floor_z = dispatch_coordinates_parameter_all.catcher_4th_floor_z();

	m_carrier_1th_floor_z = dispatch_coordinates_parameter_all.carrier_1th_floor_z();
	m_carrier_2th_floor_z = dispatch_coordinates_parameter_all.carrier_2th_floor_z();
	m_carrier_3th_floor_z = dispatch_coordinates_parameter_all.carrier_3th_floor_z();
	m_carrier_4th_floor_z = dispatch_coordinates_parameter_all.carrier_4th_floor_z();

	m_catcher_d1_d2_distance = dispatch_coordinates_parameter_all.catcher_d1_d2_distance();
	m_catcher_wheel_base_limit = dispatch_coordinates_parameter_all.catcher_wheel_base_limit();

	m_carrier_y_y1_distance = dispatch_coordinates_parameter_all.carrier_y_y1_distance();
	m_carrier_default_y1_back = dispatch_coordinates_parameter_all.carrier_default_y1_back();
	m_carrier_default_y1_leave = dispatch_coordinates_parameter_all.carrier_default_y1_leave();
	m_carrier_default_y_back = dispatch_coordinates_parameter_all.carrier_default_y_back();
	m_carrier_default_y_leave = dispatch_coordinates_parameter_all.carrier_default_y_leave();

	m_carrier_default_x_left = dispatch_coordinates_parameter_all.carrier_default_x_left();
	m_carrier_default_x_right = dispatch_coordinates_parameter_all.carrier_default_x_right();
	return Error_code::SUCCESS;
}