#include "wanji_716N_device.h"


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

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

// 初始化
Error_manager Wanji_716N_lidar_device::init(const std::string& t_ip, int t_port,
                                            Point2D_tool::Polar_coordinates_box t_polar_coordinates_box,
                                            Point2D_tool::Point2D_box t_point2D_box,
                                            Point2D_tool::Point2D_transform t_point2D_transform) {
    Error_manager t_error;
    LOG(INFO) << " Wanji_716N_lidar_device::init " << t_ip << this;

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


    t_point2D_transform.m00 = 1;
    t_point2D_transform.m01 = 0;
    t_point2D_transform.m02 = 0;
    t_point2D_transform.m10 = 0;
    t_point2D_transform.m11 = 1;
    t_point2D_transform.m12 = 0;

    //初始化通信协议
    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;
    }

    //初始化雷达通信模块

    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_716N_lidar_device::execute_thread_fun, this);

    m_wanji_lidar_device_status = E_READY;

    return t_error;
}

//反初始化
Error_manager Wanji_716N_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 = nullptr;
    }

    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_716N_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_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
                             "  Wanji_716N_lidar_device::check_status() error "};
    } else {
        return {Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
                             " Wanji_716N_lidar_device::check_status() error "};
    }
    return Error_code::SUCCESS;
}

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

//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
Error_manager
Wanji_716N_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 == nullptr || p_cloud.get() == nullptr) {
        return {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_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
                         " Wanji_716N_lidar_device::get_new_cloud_and_wait error "};
}

//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
Error_manager
Wanji_716N_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 == nullptr || p_cloud.get() == nullptr) {
        return {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_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
                             " Wanji_716N_lidar_device::get_current_cloud error "};
    }
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
Error_manager Wanji_716N_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 == nullptr || p_cloud.get() == nullptr) {
        return {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_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
                             " Wanji_716N_lidar_device::get_last_cloud error "};
    }
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
//注:函数内部不加锁, 由外部统一加锁.
Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                                      std::chrono::system_clock::time_point command_time) {
    if (p_cloud.get() == nullptr) {
        return {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 * 2)) {
        Error_manager code = m_protocol.get_scan_points(p_cloud);
        // LOG(WARNING) << " !!!!!!!!!!!!! wj scan cloud: "<<p_cloud->size();
        return code;
    } else {
        //    LOG(WARNING) << "get cloud failed..... "<<m_protocol.get_scan_time().time_since_epoch().count()<<", "<<command_time.time_since_epoch().count();
        return {Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
                             " Wanji_716N_lidar_device::get_last_cloud error "};
    }
}

Wanji_716N_lidar_device::Wanji_716N_device_status Wanji_716N_lidar_device::get_status() {
    updata_status();
    return m_wanji_lidar_device_status;
}

void Wanji_716N_lidar_device::updata_status() {
    Async_Client::Communication_status communication_status = m_async_client.get_status();
    // Wj_716N_lidar_protocol::Wanji_716N_device_status wanji_lidar_protocol_status = m_protocol.get_status();

    if (communication_status == Async_Client::Communication_status::E_FAULT)// ||
        //  wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_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_716N_lidar_protocol::Wanji_716N_device_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;
    }
}