#include "rslidar_manager.h"

Error_manager RslidarManager::init() {
    RSDriverParam param;
    LOG(INFO) << "load etc file form: " << m_config_path;
    m_config = new RsliadrJsonConfig(m_config_path);
    std::vector<int> list;
    m_config->getLidarIdList(list);

    for (auto id:list) {
        auto *lidar = new RslidarDevice();
        if (m_config->getRSDriverParam(param, id)) {
            if (lidar->init(param) == SUCCESS) {
                m_lidars_map_.insert(std::pair<int, RslidarDevice*>(id, lidar));
                LOG(INFO) << id;
            }
        }
    }

    return SUCCESS;
}

size_t RslidarManager::getLidarNumber() {
    return m_lidars_map_.size();
}

bool RslidarManager::getIdList(std::vector<int> &list) {
    return m_config->getLidarIdList(list);
}

bool RslidarManager::updateRSTransformParam(int id) {
    RSTransformParam param;
    if(!m_config->getRSTransformParam(param, id)) {
        return false;
    }

    m_lidars_map_.at(id)->updateRSTransformParam(param);
    return true;
}

RslidarDevice *RslidarManager::getRslidar(int id) {
    return m_lidars_map_.at(id);
}