#ifndef RSLIDAR_JSON_CONFIG_H_
#define RSLIDAR_JSON_CONFIG_H_

#include <vector>
#include "json/json.h"
#include "rs_driver/driver/driver_param.hpp"

class RsliadrJsonConfig {
public:
    RsliadrJsonConfig(std::string &config_path) {
        m_config_path_ = config_path;
        m_config_.resize(0);
    }

    ~RsliadrJsonConfig() {};

    bool getRSDriverParam(robosense::lidar::RSDriverParam &param, int id) {
        int sn = getSN(id);
        if (sn == -1) {
            LOG(INFO) << id << " false";
            return false;
        }
        // TODO json校验
        param.lidar_type = strToLidarType(m_config_["etc"][sn]["lidar_type"].asString());
        param.input_type = strToInputType(m_config_["etc"][sn]["input_type"].asString());

        param.input_param.msop_port = id;
        param.input_param.difop_port = m_config_["etc"][sn]["input_param"]["difop_port"].asUInt();
        param.input_param.host_address = m_config_["etc"][sn]["input_param"]["host_address"].asString();
        param.input_param.group_address = m_config_["etc"][sn]["input_param"]["group_address"].asString();
        param.input_param.pcap_path = m_config_["etc"][sn]["input_param"]["pcap_path"].asString();
        param.input_param.pcap_rate = m_config_["etc"][sn]["input_param"]["pcap_rate"].asFloat();
        param.input_param.pcap_repeat = m_config_["etc"][sn]["input_param"]["pcap_repeat"].asBool();
        param.input_param.use_vlan = m_config_["etc"][sn]["input_param"]["use_vlan"].asBool();
        param.input_param.user_layer_bytes = m_config_["etc"][sn]["input_param"]["user_layer_bytes"].asUInt();
        param.input_param.tail_layer_bytes = m_config_["etc"][sn]["input_param"]["tail_layer_bytes"].asUInt();

        param.decoder_param.wait_for_difop = m_config_["etc"][sn]["decoder_param"]["wait_for_difop"].asBool();
        param.decoder_param.min_distance = m_config_["etc"][sn]["decoder_param"]["min_distance"].asFloat();
        param.decoder_param.max_distance = m_config_["etc"][sn]["decoder_param"]["max_distance"].asFloat();

        float value = m_config_["etc"][sn]["decoder_param"]["start_angle"].asFloat();
        if (value < 0 ) {
            value += 360;
        } else if (value > 360) {
            value -= 360;
        }
        param.decoder_param.start_angle = value;
        value = m_config_["etc"][sn]["decoder_param"]["end_angle"].asFloat();
        if (value < 0 ) {
            value += 360;
        } else if (value > 360) {
            value -= 360;
        }
        param.decoder_param.end_angle = value;

        param.decoder_param.use_lidar_clock = m_config_["etc"][sn]["decoder_param"]["use_lidar_clock"].asBool();
        param.decoder_param.dense_points = m_config_["etc"][sn]["decoder_param"]["dense_points"].asBool();
        param.decoder_param.config_from_file = m_config_["etc"][sn]["decoder_param"]["config_from_file"].asBool();
        param.decoder_param.angle_path = m_config_["etc"][sn]["decoder_param"]["angle_path"].asString();
        param.decoder_param.split_frame_mode = (SplitFrameMode) m_config_["etc"][sn]["decoder_param"]["split_frame_mode"].asInt();
        param.decoder_param.split_angle = m_config_["etc"][sn]["decoder_param"]["split_angle"].asFloat();
        param.decoder_param.num_blks_split = m_config_["etc"][sn]["decoder_param"]["num_blks_split"].asUInt();

        getRSTransformParam(param.decoder_param.transform_param, sn);

        return true;
    }

    bool getRSTransformParam(robosense::lidar::RSTransformParam &param, int id) {
        int sn = getSN(id);
        if (sn == -1) {
            return false;
        }

        param.x = m_config_["etc"][sn]["transform"]["x"].asFloat();
        param.y = m_config_["etc"][sn]["transform"]["y"].asFloat();
        param.z = m_config_["etc"][sn]["transform"]["z"].asFloat();
        param.roll = m_config_["etc"][sn]["transform"]["roll"].asFloat();
        param.pitch = m_config_["etc"][sn]["transform"]["pitch"].asFloat();
        param.yaw = m_config_["etc"][sn]["transform"]["yaw"].asFloat();

        return true;
    }

    bool getMatrix4dParam(Eigen::Matrix4d &param, int id) {
        int sn = getSN(id);
        if (sn == -1) {
            return false;
        }

        float r, p, y, cx, cy, cz;
        r = m_config_["etc"][sn]["calib"]["r"].asFloat();
        p = m_config_["etc"][sn]["calib"]["p"].asFloat();
        y = m_config_["etc"][sn]["calib"]["y"].asFloat();
        cx = m_config_["etc"][sn]["calib"]["cx"].asFloat();
        cy = m_config_["etc"][sn]["calib"]["cy"].asFloat();
        cz = m_config_["etc"][sn]["calib"]["cz"].asFloat();
        Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(r * M_PI / 180.0, Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(p * M_PI / 180.0, Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(y * M_PI / 180.0, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d trans(cx, cy, cz);
        param << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() *
                 rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;

        return true;
    }

    bool getLidarIdList(std::vector<int> &list) {
        if (!ReadJsonFile(m_config_path_, m_config_)) {
            return false;
        }

        if (!m_config_.isMember("etc") || !m_config_["etc"].isArray()) {
            return false;
        }

        for (int i = 0; i < m_config_["etc"].size(); i++) {
                if (!m_config_["etc"][i]["enable"].asBool()) {
                    continue;
                }
                list.emplace_back(m_config_["etc"][i]["input_param"]["msop_port"].asUInt());
        }
        return true;
    }

    bool setConfigPath(std::string path) {
        m_config_path_ = path;
        m_config_.resize(0);
        return ReadJsonFile(m_config_path_, m_config_);
    }

private:
    int getSN(const int &id) {
        if (!ReadJsonFile(m_config_path_, m_config_)) {
            return -1;
        }

        if (!m_config_.isMember("etc") || !m_config_["etc"].isArray()) {
            return -1;
        }

        for (int i = 0; i < m_config_["etc"].size(); i++) {
            if(m_config_["etc"][i]["input_param"]["msop_port"].asUInt() == id) {
                if (!m_config_["etc"][i]["enable"].asBool()) {
                    return -1;
                }
                return i;
            }
        }

        return -1;
    }
private:
    Json::Value m_config_;
    std::string m_config_path_;
};

#endif