/*
 * @Description: 
 * @Author: yct
 * @Date: 2021-07-23 16:39:04
 * @LastEditTime: 2021-07-28 16:29:13
 * @LastEditors: yct
 */
#include "velodyne_manager.h"
#include "../tool/proto_tool.h"

//初始化 雷达 管理模块。如下三选一
Error_manager Velodyne_manager::velodyne_manager_init()
{
    return velodyne_manager_init_from_protobuf(VELODYNE_MANAGER_PARAMETER_PATH);
}

//初始化 雷达 管理模块。从文件读取
Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(std::string prototxt_path)
{
    velodyne::velodyneManagerParams t_velo_params;
    if (!proto_tool::read_proto_param(prototxt_path, t_velo_params))
    {
        return Error_manager(WJ_MANAGER_READ_PROTOBUF_ERROR, MINOR_ERROR, "velodyne_manager read_proto_param  failed");
    }

    return velodyne_manager_init_from_protobuf(t_velo_params);
}

//初始化 雷达 管理模块。从protobuf读取
Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(velodyne::velodyneManagerParams & velodyne_parameters)
{
    LOG(INFO) << " ---velodyne_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	if ( m_velodyne_manager_status != E_UNKNOWN)
	{
		return Error_manager(Error_code::WJ_MANAGER_INIT_ERROR, Error_level::MINOR_ERROR,
							 " velodyne_manager::velodyne_manager_init_from_protobuf init repeat error ");
	}

	//创建雷达设备
	int t_velodyne_lidar_size = velodyne_parameters.velodyne_lidars_size();
	for(int i=0;i<t_velodyne_lidar_size;++i)
	{
		Velodyne_lidar_device* p_velodyne_lidar_device(new Velodyne_lidar_device);
		t_error = p_velodyne_lidar_device->init(velodyne_parameters.velodyne_lidars(i));
//		LOG(WARNING) << wanji_parameters.wj_lidar(i).DebugString();
		if ( t_error != Error_code::SUCCESS )
		{
			delete(p_velodyne_lidar_device);
			return t_error;
		}
		m_velodyne_lidar_device_map[i] = (p_velodyne_lidar_device);
	}

	//创建预测算法
	int t_ground_region_size = velodyne_parameters.region_size;
	for(int i=0;i<t_ground_region_size;++i)
	{
		Region_worker* p_region_worker(new Region_worker);
		Point2D_tool::Point2D_box t_point2d_box;
		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
		t_point2d_box.y_max = wanji_parameters.regions(i).maxy();
		// changed by yct, 传入终端index,仅普爱使用
		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection, i);
		if ( t_error != Error_code::SUCCESS )
		{
			delete(p_region_worker);
			return t_error;
		}
		m_region_worker_map[i] = p_region_worker;
	}



	//启动 收集点云的线程。默认 。
	m_collect_cloud_condition.reset(false, false, false);
	mp_collect_cloud_thread = new std::thread(&Wanji_manager::collect_cloud_thread_fun, this);

	//启动 执行的线程。默认 。
	m_execute_condition.reset(false, false, false);
	mp_execute_thread = new std::thread(&Wanji_manager::execute_thread_fun, this);

	m_wanji_manager_status = E_READY;

	return Error_code::SUCCESS;
}

// 反初始化
Error_manager Velodyne_manager::Velodyne_manager_uninit()
{
    
}