#include "lidar_manager.h"

Error_manager ClampLidarManager::Init(const std::string &file) {
    Error_manager ret = loadProtobufFile(file, m_config);
    if (ret != SUCCESS) {
        LOG(WARNING) << ret.to_string();
        return ret;
    } else {
        LOG(INFO) << "\n======================================== "
                     "Load Clamp Lida Config Success "
                     "========================================\n";
    }

    for (auto &device: m_config.devices()) {
        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;
        Point2D_tool::Point2D_transform t_point2D_transform;

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

        Point2D_tool::Point2D_box t_point2D_box;
        t_point2D_box.x_min = device.scan_box_limit().minx();
        t_point2D_box.x_max = device.scan_box_limit().maxx();
        t_point2D_box.y_min = device.scan_box_limit().miny();
        t_point2D_box.y_max = device.scan_box_limit().maxy();

        auto *wj = new Wanji_716N_lidar_device();
        Error_manager code = wj->init(device.ip_address(),
                                      device.port(),
                                      t_polar_coordinates_box,
                                      t_point2D_box,
                                      t_point2D_transform);
        if (code != SUCCESS) {
            LOG(ERROR) << code.get_error_description();
            return code;
        } else {
            mp_wj_device.insert(std::pair<std::string, Wanji_716N_lidar_device*>(device.ip_address(), wj));
//            clamps[i].safety_statu.set_timeout(0.3);
        }
    }
    LOG(INFO) << "\n======================================== "
                 "Clamp Lida Config Init End "
                 "========================================\n";

    for (auto &device: mp_wj_device) {
        std::thread(&ClampLidarManager::thread_func, this, device.first);
    }
    return {};
}

void ClampLidarManager::thread_func(const std::string &ip_address) {

//    while (true) {
//        clamp->mutex.lock();
//        Safety_status safety_t;
//        if (run(&(clamp->lidar), &(clamp->detector), &safety_t)) {
//            clamp->safety_statu = safety_t;
//        }
//        clamp->mutex.unlock();
//
//        usleep(100 * 1000);
//    }
}