//
// 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;
//	return Error_code::SUCCESS;
//}

//调度地面雷达 执行状态消息
Error_manager Dispatch_ground_lidar::execute_for_ground_status_msg_new(measure_info &t_measure_info)
{
	std::unique_lock<std::mutex> t_lock2(m_lock);
	m_measure_info = t_measure_info;
	m_ground_status_msg_updata_time = std::chrono::system_clock::now();
	m_ground_status_msg_updata_flag = true;

	//胡力 新增 接受雷达雷达 通信周期 超过1秒就打印
	int time_key = 10+m_ground_lidar_id;
	Time_tool::get_instance_references().time_end(time_key);
	if ( Time_tool::get_instance_references().timetool_map.find(time_key)!=Time_tool::get_instance_references().timetool_map.end() )
	{
	    if ( Time_tool::get_instance_references().timetool_map[time_key].t_time_difference >= std::chrono::milliseconds(1000) )
	    {
			Time_tool::get_instance_references().cout_time_millisecond(time_key);
			double dieoutTime=(double)Time_tool::get_instance_references().timetool_map[time_key].t_time_difference.count()/1000000;
			LOG(INFO) << "计时器:"<<time_key<<" 计时的时间为：" <<dieoutTime<<" 毫秒" << " --- " << this;
	    }
	}
	Time_tool::get_instance_references().time_start(time_key);
	//胡力 新增 接受雷达雷达 通信周期 超过1秒就打印

	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);

				check_measure_info(m_measure_info);


				//将nnxx的protobuf 转化为 snap7的DB块,  把店面雷达数据转发给plc
//				int t_inlet_id = m_ground_status_msg.mutable_id_struct()->terminal_id() % 2;
				int t_inlet_id = m_ground_lidar_id;
				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();
//					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle();
//					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();


					if ( m_measure_info.ground_status() == 0 ) //ground_status  :ok 1,nothing 2,noise  3,border
					{
                        p_response_data->m_response_car_center_x = m_measure_info.cx();
                        p_response_data->m_response_car_center_y = m_measure_info.cy();
                        p_response_data->m_response_car_angle = m_measure_info.theta();
                        p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
                        p_response_data->m_response_car_length = m_measure_info.length();
                        p_response_data->m_response_car_width = m_measure_info.width();
                        p_response_data->m_response_car_height = m_measure_info.height();
                        p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
                        p_response_data->m_response_car_wheel_width = m_measure_info.width();
                        p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
                        p_response_data->m_response_uniformed_car_y = m_measure_info.cy();

						p_response_data->m_response_locate_correct = 1;	//locate_correct  0=无效, 1=有效,
						p_response_data->m_response_ground_status = m_measure_info.ground_status();	//0  ok 1,nothing 2,noise  3,border
						p_response_data->m_response_border_status = m_measure_info.border_statu();	//超界状态描述, 按位运算

					}
					else
                    {
                        p_response_data->m_response_car_center_x = 0;
                        p_response_data->m_response_car_center_y = 0;
                        p_response_data->m_response_car_angle = 0;
                        p_response_data->m_response_car_front_theta = 0;
                        p_response_data->m_response_car_length = 0;
                        p_response_data->m_response_car_width = 0;
                        p_response_data->m_response_car_height = 0;
                        p_response_data->m_response_car_wheel_base = 0;
                        p_response_data->m_response_car_wheel_width = 0;
                        p_response_data->m_response_uniformed_car_x = 0;
                        p_response_data->m_response_uniformed_car_y = 0;

                        p_response_data->m_response_locate_correct = 0;
						p_response_data->m_response_ground_status = m_measure_info.ground_status();	//0  ok 1,nothing 2,noise  3,border
						p_response_data->m_response_border_status = m_measure_info.border_statu();	//超界状态描述, 按位运算

					}

//                    LOG(WARNING) << "------------------------------------------------------------ "<<std::endl;
//                    LOG(WARNING) << "m_measure_info = " << m_measure_info.ground_status() <<std::endl;
//                    LOG(WARNING) << "m_measure_info = " << m_measure_info.DebugString() <<std::endl;
//                    LOG(WARNING) << "m_measure_info.wheelbase() = " << m_measure_info.wheelbase() <<std::endl;
//                    LOG(WARNING) << "m_measure_info.ground_status() = " << m_measure_info.ground_status() <<std::endl;
//                    LOG(WARNING) << "p_response_data->m_response_locate_correct = " << p_response_data->m_response_locate_correct <<std::endl;
//                    printf("p_response_data->m_response_locate_correct = %d, \n", p_response_data->m_response_locate_correct);
//                    LOG(WARNING) << "-++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ "<<std::endl;

                }
				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();
//					p_response_data->m_response_car_angle = m_ground_status_msg.locate_information_realtime().locate_angle();
//					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();

                    if ( m_measure_info.ground_status() == 0 ) //ground_status  :ok 1,nothing 2,noise  3,border
                    {
                        p_response_data->m_response_car_center_x = m_measure_info.cx();
                        p_response_data->m_response_car_center_y = m_measure_info.cy();
                        p_response_data->m_response_car_angle = m_measure_info.theta();
                        p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
                        p_response_data->m_response_car_length = m_measure_info.length();
                        p_response_data->m_response_car_width = m_measure_info.width();
                        p_response_data->m_response_car_height = m_measure_info.height();
                        p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
                        p_response_data->m_response_car_wheel_width = m_measure_info.width();
                        p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
                        p_response_data->m_response_uniformed_car_y = m_measure_info.cy();

                        p_response_data->m_response_locate_correct = 1;	//locate_correct  0=无效, 1=有效,
						p_response_data->m_response_ground_status = m_measure_info.ground_status();	//0  ok 1,nothing 2,noise  3,border
						p_response_data->m_response_border_status = m_measure_info.border_statu();	//超界状态描述, 按位运算

					}
                    else
                    {
                        p_response_data->m_response_car_center_x = 0;
                        p_response_data->m_response_car_center_y = 0;
                        p_response_data->m_response_car_angle = 0;
                        p_response_data->m_response_car_front_theta = 0;
                        p_response_data->m_response_car_length = 0;
                        p_response_data->m_response_car_width = 0;
                        p_response_data->m_response_car_height = 0;
                        p_response_data->m_response_car_wheel_base = 0;
                        p_response_data->m_response_car_wheel_width = 0;
                        p_response_data->m_response_uniformed_car_x = 0;
                        p_response_data->m_response_uniformed_car_y = 0;

                        p_response_data->m_response_locate_correct = 0;
						p_response_data->m_response_ground_status = m_measure_info.ground_status();	//0  ok 1,nothing 2,noise  3,border
						p_response_data->m_response_border_status = m_measure_info.border_statu();	//超界状态描述, 按位运算

					}
				}

				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;
				}
			}
		}

	}
}

//检查感测消息
Error_manager Dispatch_ground_lidar::check_measure_info(measure_info &t_measure_info)
{
	if ( t_measure_info.theta() > 10 ||
	t_measure_info.theta() < -10)
	{
	    LOG(INFO) << " 地面雷达数据异常, t_measure_info =  " << t_measure_info.DebugString().c_str()  << " --- " ;
	}

	return Error_code::SUCCESS;
}