#include "wanji_lidar_device.h"



// 万集雷达封装类构造函数
Wanji_lidar_device::Wanji_lidar_device()
{
	m_wanji_lidar_device_status = E_UNKNOWN;
	mp_wanji_lidar_scan_task = NULL;
}

// 万集雷达封装类析构函数
Wanji_lidar_device::~Wanji_lidar_device()
{
	uninit();
}

// 初始化
Error_manager Wanji_lidar_device::init(wj::wjLidarParams params)
{
	Error_manager t_error;
	LOG(INFO) << " Wanji_lidar_device::init "<< this;

	//唤醒队列
	m_communication_data_queue.wake_queue();

	m_lidar_id = params.lidar_id();

	//控制参数
	Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;	//极坐标的限定范围
	Point2D_tool::Point2D_box			t_point2D_box;				//平面坐标的限定范围
	Point2D_tool::Point2D_transform 	t_point2D_transform;		//平面坐标的转换矩阵

	t_polar_coordinates_box.angle_min = params.angle_min();
	t_polar_coordinates_box.angle_max = params.angle_max();
	t_polar_coordinates_box.distance_min = params.range_min();
	t_polar_coordinates_box.distance_max = params.range_max();

	t_point2D_box.x_min = params.scan_limit().minx();
	t_point2D_box.x_max = params.scan_limit().maxx();
	t_point2D_box.y_min = params.scan_limit().miny();
	t_point2D_box.y_max = params.scan_limit().maxy();

	t_point2D_transform.m00 = params.transform().m00();
	t_point2D_transform.m01 = params.transform().m01();
	t_point2D_transform.m02 = params.transform().m02();
	t_point2D_transform.m10 = params.transform().m10();
	t_point2D_transform.m11 = params.transform().m11();
	t_point2D_transform.m12 = params.transform().m12();

	//初始化通信协议
	t_error = m_protocol.init(&m_communication_data_queue, t_polar_coordinates_box, t_point2D_box, t_point2D_transform);
	if (t_error != SUCCESS)
	{
		return t_error;
	}

	//初始化雷达通信模块
	std::string	t_ip = params.net_config().ip_address();
	int t_port = params.net_config().port();

	t_error = m_async_client.init(t_ip, t_port, &m_communication_data_queue);
	if ( t_error != SUCCESS )
	{
		return t_error;
	}


	//启动 内部线程。默认wait。
	m_execute_condition.reset(false, false, false);
	mp_execute_thread = new std::thread(&Wanji_lidar_device::execute_thread_fun, this);

	m_wanji_lidar_device_status = E_READY;

	return t_error;
}

//反初始化
Error_manager Wanji_lidar_device::uninit()
{
//终止队列，防止 wait_and_pop 阻塞线程。
	m_communication_data_queue.termination_queue();

	//关闭线程
	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_async_client.uninit();
	m_protocol.uninit();

//清空队列
	m_communication_data_queue.clear_and_delete();

	if ( m_wanji_lidar_device_status != E_FAULT  )
	{
		m_wanji_lidar_device_status = E_UNKNOWN;
	}
	return Error_code::SUCCESS;
}


//对外的接口函数，负责接受并处理任务单，
Error_manager Wanji_lidar_device::execute_task(Task_Base* p_wanji_lidar_scan_task)
{
	LOG(INFO) << " ---Laser_manager::execute_task run---  "<< this;
	Error_manager t_error;
	Error_manager t_result;

	//检查指针
	if (p_wanji_lidar_scan_task == NULL) {
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Wanji_lidar_device::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_wanji_lidar_scan_task->get_task_type() != WANJI_LIDAR_SCAN)
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 " task type error ！= WANJI_LIDAR_SCAN ");
	}

	//检查接收方的状态
	t_error = check_status();
	if ( t_error != SUCCESS )
	{
		t_result.compare_and_cover_error(t_error);
	}
	else
	{
		//接受任务，并将任务的状态改为TASK_SIGNED已签收
		mp_wanji_lidar_scan_task = (Wanji_lidar_scan_task *) p_wanji_lidar_scan_task;
		mp_wanji_lidar_scan_task->set_task_statu(TASK_SIGNED);

		//检查消息内容是否正确，
		//检查三维点云指针
		if (mp_wanji_lidar_scan_task->get_task_point_cloud().get() == NULL)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_point_cloud is null");
			t_result.compare_and_cover_error(t_error);
		}
		else if ( mp_wanji_lidar_scan_task->get_task_cloud_lock() == NULL )
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_cloud_lock is null");
			t_result.compare_and_cover_error(t_error);
		}
		else
		{
			m_wanji_lidar_device_status = E_BUSY;
			//启动雷达管理模块，的核心工作线程
			m_execute_condition.notify_all(true);
			//通知 thread_work 子线程启动。

			//将任务的状态改为 TASK_WORKING 处理中
			mp_wanji_lidar_scan_task->set_task_statu(TASK_WORKING);
		}

		//return 之前要填充任务单里面的错误码.
		if (t_result != Error_code::SUCCESS)
		{
			//忽略轻微故障
			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
			{
				//将任务的状态改为 TASK_ERROR 结束错误
				mp_wanji_lidar_scan_task->set_task_statu(TASK_ERROR);
			}
			//返回错误码
			mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_result);
		}
	}

	return t_result;
}

//检查雷达状态,是否正常运行
Error_manager Wanji_lidar_device::check_status()
{
	updata_status();
	if ( m_wanji_lidar_device_status == E_READY )
	{
		return Error_code::SUCCESS;
	}
	else if(m_wanji_lidar_device_status == E_BUSY)
	{
	    return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
	    					"  Wanji_lidar_device::check_status() error ");
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Wanji_lidar_device::check_status() error ");
	}
	return Error_code::SUCCESS;
}
//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
Error_manager Wanji_lidar_device::end_task()
{
	m_execute_condition.notify_all(false);

	if ( m_wanji_lidar_device_status == E_BUSY)
	{
		m_wanji_lidar_device_status = E_READY;
	}
	//else 状态不变

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_wanji_lidar_scan_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_wanji_lidar_scan_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
		{
			//强制改为TASK_OVER，不管它当前在做什么。
			mp_wanji_lidar_scan_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//强制改为 TASK_ERROR，不管它当前在做什么。
			mp_wanji_lidar_scan_task->set_task_statu(TASK_ERROR);
		}
	}

	return Error_code::SUCCESS;
}
//取消任务单，由发送方提前取消任务单
Error_manager Wanji_lidar_device::cancel_task(Task_Base* p_wanji_lidar_scan_task)
{
	if ( p_wanji_lidar_scan_task == mp_wanji_lidar_scan_task )
	{
		m_execute_condition.notify_all(false);

		if ( m_wanji_lidar_device_status == E_BUSY)
		{
			m_wanji_lidar_device_status = E_READY;
		}
		//else 状态不变

		//强制改为 TASK_DEAD，不管它当前在做什么。
		mp_wanji_lidar_scan_task->set_task_statu(TASK_DEAD);

		return Error_code::SUCCESS;
	}
	else
	{
	    return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
	    					" Wanji_lidar_device::cancel_task PARAMRTER ERROR ");
	}

}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Wanji_lidar_device::is_ready()
{
	updata_status();
	return m_wanji_lidar_device_status == E_READY;
}



//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
Error_manager Wanji_lidar_device::get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
												   std::chrono::system_clock::time_point command_time, int timeout_ms)
{
	if ( p_mutex == NULL || p_cloud.get() == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					"  POINTER IS NULL ");
	}
	//判断是否超时
	while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
	{
		//获取指令时间之后的点云, 如果没有就会循环
		if( m_protocol.get_scan_time() > command_time  )
		{
			std::unique_lock<std::mutex> lck(*p_mutex);
			Error_manager code = m_protocol.get_scan_points(p_cloud);
			return code;
		}
		//else 等待下一次数据更新

		//等1ms
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}
	//超时退出就报错
	return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
						 " Wanji_lidar_device::get_new_cloud_and_wait error ");
}

//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
Error_manager Wanji_lidar_device::get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::system_clock::time_point command_time)
{
	if ( p_mutex == NULL || p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
	if( m_protocol.get_scan_time() > command_time )
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		Error_manager code = m_protocol.get_scan_points(p_cloud);
		return code;
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
Error_manager Wanji_lidar_device::get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::system_clock::time_point command_time)
{
	if ( p_mutex == NULL || p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		Error_manager code = m_protocol.get_scan_points(p_cloud);
		return code;
	}
	else
	{
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_lidar_device::get_last_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
//注:函数内部不加锁, 由外部统一加锁.
Error_manager Wanji_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
												 std::chrono::system_clock::time_point command_time)
{
	if ( p_cloud.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
	{
		Error_manager code = m_protocol.get_scan_points(p_cloud);
//		LOG(WARNING) << p_cloud->size();
		return code;
	}
	else
	{
//        LOG(WARNING) << "get cloud failed.....";
		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " Wanji_lidar_device::get_last_cloud error ");
	}
}

Wanji_lidar_device::Wanji_lidar_device_status Wanji_lidar_device::get_status()
{
	updata_status();
	return m_wanji_lidar_device_status;
}

void Wanji_lidar_device::updata_status()
{
	Async_Client::Communication_status communication_status = m_async_client.get_status();
	Wj_716_lidar_protocol::Wanji_lidar_protocol_status wanji_lidar_protocol_status = m_protocol.get_status();

	if ( communication_status == Async_Client::Communication_status::E_FAULT ||
		 wanji_lidar_protocol_status == Wj_716_lidar_protocol::Wanji_lidar_protocol_status::E_FAULT)
	{
		m_wanji_lidar_device_status = E_FAULT;
	}
	if ( communication_status == Async_Client::Communication_status::E_DISCONNECT )
	{
		m_wanji_lidar_device_status = E_DISCONNECT;
	}
	else if ( communication_status == Async_Client::Communication_status::E_READY &&
			  wanji_lidar_protocol_status == Wj_716_lidar_protocol::Wanji_lidar_protocol_status::E_READY )
	{
		if ( m_execute_condition.get_pass_ever() )
		{
			m_wanji_lidar_device_status  = E_BUSY;
		}
		else
		{
			m_wanji_lidar_device_status = E_READY;
		}
	}
	else
	{
		m_wanji_lidar_device_status = E_UNKNOWN;
	}
}


//mp_laser_manager_thread 线程执行函数，
//thread_work 内部线程负责获取点云结果
void Wanji_lidar_device::execute_thread_fun()
{
	LOG(INFO) << " Wanji_lidar_device::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::yield();

			if ( mp_wanji_lidar_scan_task == NULL )
			{
				//没有任务就忽略, 直接关闭线程
				m_wanji_lidar_device_status = E_READY;
				m_execute_condition.notify_all(false);
			}
			else
			{
				if ( mp_wanji_lidar_scan_task->is_over_time() )
				{
					t_error.error_manager_reset(TASK_TIMEOVER, MINOR_ERROR, "Wanji_lidar_device::get_cloud_thread() TASK_TIMEOVER");
					mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
					//故障结束任务
					end_task();
				}
				else
				{
					//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
					//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
					if( m_protocol.get_scan_time() >
						mp_wanji_lidar_scan_task->get_command_time() - std::chrono::milliseconds(m_protocol.get_scan_cycle()) )
					{
						std::mutex *tp_mutex = mp_wanji_lidar_scan_task->get_task_cloud_lock();
						pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = mp_wanji_lidar_scan_task->get_task_point_cloud();
						{
							std::unique_lock<std::mutex> lck(*tp_mutex);
							t_error = m_protocol.get_scan_points(tp_cloud);
						}
						mp_wanji_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
						//正常结束任务
						end_task();
					}
					else
					{
						//等1ms
						std::this_thread::sleep_for(std::chrono::milliseconds(1));
					}
				}
			}
		}
	}
	LOG(INFO) << " Wanji_lidar_device::execute_thread_fun() end :"<<this;
	return;
}