//
// 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;
	m_ground_status_msg_updata_flag = false;

}

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


//调度地面雷达 初始化
Error_manager Dispatch_ground_lidar::dispatch_ground_lidar_init(int plc_id, int ground_lidar_id)
{
	m_plc_id =	plc_id;
	m_ground_lidar_id = ground_lidar_id;
	mp_execute_thread = nullptr ;
	m_ground_status_msg_updata_time = std::chrono::system_clock::now();
	m_ground_status_msg_updata_flag = false;

	//	线程默认开启
	m_execute_condition.reset(false, true, false);
	mp_execute_thread = new std::thread(&Dispatch_ground_lidar::execute_thread_fun, this);

	m_dispatch_ground_lidar_status = DISPATCH_GROUND_LIDAR_READY;

	return Error_code::SUCCESS;
}
//调度地面雷达 反初始化
Error_manager Dispatch_ground_lidar::dispatch_ground_lidar_uninit()
{
	if (mp_execute_thread)
	{
		m_execute_condition.kill_all();
	}
	if (mp_execute_thread)
	{
		mp_execute_thread->join();
		delete mp_execute_thread;
		mp_execute_thread = NULL;
	}

	m_dispatch_ground_lidar_status = Dispatch_ground_lidar::DISPATCH_GROUND_LIDAR_UNKNOW;

	return Error_code::SUCCESS;
}
//调度地面雷达 执行状态消息
Error_manager Dispatch_ground_lidar::execute_for_ground_status_msg(message::Ground_status_msg &ground_status_msg)
{
	std::unique_lock<std::mutex> t_lock2(m_lock);
	m_ground_status_msg = ground_status_msg;
	m_ground_status_msg_updata_time = std::chrono::system_clock::now();
	m_ground_status_msg_updata_flag = true;

//	std::cout << " huli test :::: " << " ---------------------------------------------------- = " << std::endl;
//	std::cout << " huli test :::: " << " m_ground_status_msg = " << m_ground_status_msg.DebugString() << std::endl;

	return Error_code::SUCCESS;
}

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





//执行外界任务的执行函数
void Dispatch_ground_lidar::execute_thread_fun()
{
	LOG(INFO) << " Dispatch_ground_lidar::execute_thread_fun() start " << this;
	Error_manager t_error;

	while (m_execute_condition.is_alive())
	{
		m_execute_condition.wait();
		if (m_execute_condition.is_alive())
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
//			std::this_thread::sleep_for(std::chrono::seconds(1));

			std::this_thread::yield();


			if (m_ground_status_msg_updata_flag)
			{

				std::unique_lock<std::mutex> t_lock(Dispatch_communication::get_instance_references().m_data_lock);
				std::unique_lock<std::mutex> t_lock2(m_lock);

				//将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
				int t_inlet_id = m_ground_status_msg.terminal_id() % 2;
				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 != 2)
				{
					p_response_data->m_response_heartbeat = 1-p_response_data->m_response_heartbeat;
					p_response_data->m_response_communication_mode = p_request->m_request_communication_mode;
					p_response_data->m_response_refresh_command = p_request->m_request_refresh_command;

					p_response_data->m_response_car_center_x = m_ground_status_msg.locate_information_realtime().locate_x();
					p_response_data->m_response_car_center_y = m_ground_status_msg.locate_information_realtime().locate_y();
#ifdef MEASURE_TO_PLC_CORRECTION
					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle()-90+0.3;
#endif
#ifndef MEASURE_TO_PLC_CORRECTION
					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle();
#endif
					p_response_data->m_response_car_front_theta = m_ground_status_msg.locate_information_realtime().locate_front_theta();
					p_response_data->m_response_car_length = m_ground_status_msg.locate_information_realtime().locate_length();
					p_response_data->m_response_car_width = m_ground_status_msg.locate_information_realtime().locate_width();
					p_response_data->m_response_car_height = m_ground_status_msg.locate_information_realtime().locate_height();
					p_response_data->m_response_car_wheel_base = m_ground_status_msg.locate_information_realtime().locate_wheel_base();
					p_response_data->m_response_car_wheel_width = m_ground_status_msg.locate_information_realtime().locate_wheel_width();
					p_response_data->m_response_uniformed_car_x = m_ground_status_msg.locate_information_realtime().uniformed_car_x();
					p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
					p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
				}
				else if (p_response_data->m_response_refresh_command == p_request->m_request_refresh_command)
				{
					p_response_data->m_response_heartbeat = 1-p_response_data->m_response_heartbeat;
					p_response_data->m_response_communication_mode = p_request->m_request_communication_mode;
					p_response_data->m_response_refresh_command = p_request->m_request_refresh_command;
				}
				else
				{
					p_response_data->m_response_heartbeat = 1-p_response_data->m_response_heartbeat;
					p_response_data->m_response_communication_mode = p_request->m_request_communication_mode;
					p_response_data->m_response_refresh_command = p_request->m_request_refresh_command;

					p_response_data->m_response_car_center_x = m_ground_status_msg.locate_information_realtime().locate_x();
					p_response_data->m_response_car_center_y = m_ground_status_msg.locate_information_realtime().locate_y();
#ifdef MEASURE_TO_PLC_CORRECTION
					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle()-90+0.3;
#endif
#ifndef MEASURE_TO_PLC_CORRECTION
					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle();
#endif
					p_response_data->m_response_car_front_theta = m_ground_status_msg.locate_information_realtime().locate_front_theta();
					p_response_data->m_response_car_length = m_ground_status_msg.locate_information_realtime().locate_length();
					p_response_data->m_response_car_width = m_ground_status_msg.locate_information_realtime().locate_width();
					p_response_data->m_response_car_height = m_ground_status_msg.locate_information_realtime().locate_height();
					p_response_data->m_response_car_wheel_base = m_ground_status_msg.locate_information_realtime().locate_wheel_base();
					p_response_data->m_response_car_wheel_width = m_ground_status_msg.locate_information_realtime().locate_wheel_width();
					p_response_data->m_response_uniformed_car_x = m_ground_status_msg.locate_information_realtime().uniformed_car_x();
					p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
					p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
				}

				m_ground_status_msg_updata_flag = false;
			}


			//判断超时
			{
				std::unique_lock<std::mutex> t_lock3(m_lock);
				if (std::chrono::system_clock::now() - m_ground_status_msg_updata_time > std::chrono::seconds(5))
				{
					m_dispatch_ground_lidar_status = DISPATCH_GROUND_LIDAR_DISCONNECT;
				}
				else
				{
					m_dispatch_ground_lidar_status = DISPATCH_GROUND_LIDAR_READY;
				}
			}
		}

	}
}