//
// Created by huli on 2021/9/25.
//

#include "dispatch_ground_lidar.h"

Dispatch_ground_lidar::Dispatch_ground_lidar()
{
	m_dispatch_ground_lidar_status = DISPATCH_GROUND_LIDAR_UNKNOW;
	m_plc_id = 0;
	m_ground_lidar_id = 0;
}

Dispatch_ground_lidar::~Dispatch_ground_lidar()
{
	dispatch_plc_uninit();
}


//调度地面雷达 初始化
Error_manager Dispatch_ground_lidar::dispatch_plc_init(int plc_id, int ground_lidar_id)
{
	m_plc_id =	plc_id;
	m_ground_lidar_id = ground_lidar_id;

	return Error_code::SUCCESS;
}
//调度地面雷达 反初始化
Error_manager Dispatch_ground_lidar::dispatch_plc_uninit()
{
	return Error_code::SUCCESS;
}
//调度地面雷达 执行状态消息
Error_manager Dispatch_ground_lidar::execute_for_ground_status_msg(message::Ground_status_msg &ground_status_msg)
{
	//将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
	int t_inlet_id = ground_status_msg.terminal_id() %2;
	std::unique_lock<std::mutex> t_lock(Dispatch_communication::get_instance_references().m_data_lock);
	Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_data * p_response_data = & Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_data[t_inlet_id];
	Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_key * p_response_key = & Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_key[t_inlet_id];
	Dispatch_communication::Ground_lidar_request_from_plc_to_manager * p_request = & Dispatch_communication::get_instance_references().m_ground_lidar_request_from_plc_to_manager[t_inlet_id];

	if ( p_request->m_request_communication_mode == 0 )
	{
		p_response_key->m_response_heartbeat++;
		p_response_key->m_response_communication_mode = p_request->m_request_communication_mode;
		p_response_key->m_response_refresh_command = p_request->m_request_refresh_command;

		p_response_data->m_response_car_center_x = ground_status_msg.locate_information_realtime().locate_x();
		p_response_data->m_response_car_center_y = ground_status_msg.locate_information_realtime().locate_y();
		p_response_data->m_response_car_angle = ground_status_msg.locate_information_realtime().locate_angle();
		p_response_data->m_response_car_front_theta = ground_status_msg.locate_information_realtime().locate_front_theta();
		p_response_data->m_response_car_length = ground_status_msg.locate_information_realtime().locate_length();
		p_response_data->m_response_car_width = ground_status_msg.locate_information_realtime().locate_width();
		p_response_data->m_response_car_height = ground_status_msg.locate_information_realtime().locate_height();
		p_response_data->m_response_car_wheel_base = ground_status_msg.locate_information_realtime().locate_wheel_base();
		p_response_data->m_response_car_wheel_width = ground_status_msg.locate_information_realtime().locate_wheel_width();
		p_response_data->m_response_locate_correct = ground_status_msg.locate_information_realtime().locate_correct();
	}
	else if ( p_response_key->m_response_refresh_command == p_request->m_request_refresh_command )
	{
		p_response_key->m_response_heartbeat++;
		p_response_key->m_response_communication_mode = p_request->m_request_communication_mode;
		p_response_key->m_response_refresh_command = p_request->m_request_refresh_command;
	}
	else
	{
		p_response_key->m_response_heartbeat++;
		p_response_key->m_response_communication_mode = p_request->m_request_communication_mode;
		p_response_key->m_response_refresh_command = p_request->m_request_refresh_command;

		p_response_data->m_response_car_center_x = ground_status_msg.locate_information_realtime().locate_x();
		p_response_data->m_response_car_center_y = ground_status_msg.locate_information_realtime().locate_y();
		p_response_data->m_response_car_angle = ground_status_msg.locate_information_realtime().locate_angle();
		p_response_data->m_response_car_front_theta = ground_status_msg.locate_information_realtime().locate_front_theta();
		p_response_data->m_response_car_length = ground_status_msg.locate_information_realtime().locate_length();
		p_response_data->m_response_car_width = ground_status_msg.locate_information_realtime().locate_width();
		p_response_data->m_response_car_height = ground_status_msg.locate_information_realtime().locate_height();
		p_response_data->m_response_car_wheel_base = ground_status_msg.locate_information_realtime().locate_wheel_base();
		p_response_data->m_response_car_wheel_width = ground_status_msg.locate_information_realtime().locate_wheel_width();
		p_response_data->m_response_locate_correct = ground_status_msg.locate_information_realtime().locate_correct();
	}

}

Dispatch_ground_lidar::Dispatch_ground_lidar_status Dispatch_ground_lidar::get_dispatch_ground_lidar_status()
{
	return m_dispatch_ground_lidar_status;
}