/**
  * @project Project
  * @brief   TODO: add a brief
  * @author  lz
  * @data    2023/4/28
 **/
#include "alg_manager.h"

bool VelodyneManager::run_measure_form(const std::string &path) {
    // 初始化glog日志
    initGlog();

    // 初始化配置信息
    if (!initMeasureConfig(path)) {
        LOG(ERROR) << "Init measure config form " << path << " error!";
        return false;
    }

    // 初始化mqtt配置
    m_mqtt_client = new CloudDataMqttAsync;
    m_mqtt_client->init();

    // 初始化rabbitmq
    m_detect_result_rabbitmq = new DetectResultRabbitmq;
    Error_manager ret;
    if ((ret = m_detect_result_rabbitmq->rabbitmq_init()) != SUCCESS) {
        LOG(ERROR) << ret.to_string();
        exit(EXIT_FAILURE);
    }

    // 根据配置信息启动所有入口的线程
    for (auto &measure: m_measure_config->measures()) {
        std::thread t(&VelodyneManager::thread_func, this, std::ref(measure), std::ref(*m_mqtt_client));
        t.detach();
    }

    return true;
}

bool VelodyneManager::initGlog() {
    FLAGS_colorlogtostderr = true;   // log信息区分颜色
    FLAGS_logtostderr = true;           // 允许log信息打印到屏幕
    google::SetStderrLogging(google::GLOG_INFO);  // 输出log的最低等级是 INFO (可以设置为WARNING或者更高)
    google::InitGoogleLogging("");  // 用当前可执行程序初始化glog
    return true;
}

bool VelodyneManager::initMeasureConfig(const std::string &path) {
    m_measure_config = new MeasureConfig;
    Json::Value config;
    ReadJsonFile(path, config);
    google::protobuf::util::Status ret = google::protobuf::util::JsonStringToMessage(config.toStyledString(),
                                                                                     m_measure_config);
    LOG(INFO) << m_measure_config->DebugString();
    if (!ret.ok()) {
        LOG(ERROR) << "init config from proto error";
        return false;
    }
    return true;
}

void VelodyneManager::thread_func(const MeasureLidar &config, CloudDataMqttAsync &mqtt_client) {
    DetectWheel detect;
    detect.init(config.region());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    LOG(INFO) << "------------------------------";
    while (true) {
        // 采集该雷达监听的雷达下的所有数据并合并在一个点云数据中

#if NO_MQTT_GET_CLOUD
        if (!getLocalPointCloud("../etc/data/cloud_cut.txt", cloud)) {
            LOG(WARNING) << "get local point cloud error, please Re-check the ../etc/test_car.txt file's data again.";
        }
#else
        getTopicsLidarData(config, cloud);
#endif

        // 对点云数据进行检测，获取车辆信息
        switch (detect.detect(cloud).get_error_code()) {
            case VELODYNE_REGION_EMPTY_CLOUD:
                continue;
            default:
                break;
        }

        // 将车辆检测结果进行转换
        measure_statu statu;
        detect.upStatus(cloud);
        packMeasureInfo(detect, statu);

        if (statu.info().border_statu()) {
            LOG(INFO) << statu.DebugString();
        } else {
            LOG(INFO) << "detect okk.";
            LOG(INFO) << statu.DebugString();
        }
        // send mqtt message
        mqtt_client.SendMessage(m_measure_config->sendtopics() + config.measuretopic(),
                                (void *) statu.DebugString().c_str(),
                                statu.DebugString().length());

        // send rabbitmq message
        m_detect_result_rabbitmq->encapsulate_msg(statu.DebugString(), atoi(config.measuretopic().c_str()), "statu_ex",
                                                  "measure_" + config.measuretopic() + "_statu_port", 10000);
    }
}

bool VelodyneManager::getTopicsLidarData(const MeasureLidar &config, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    for (auto &topic: config.lidartopic()) {
        m_mqtt_client->getCloudData(m_measure_config->lidartopics() + topic, cloud);
    }

    if (cloud->empty()) {
        LOG(WARNING) << "Can't get any cloud data.";
        return false;
    }

    return true;
}

bool VelodyneManager::packMeasureInfo(DetectWheel &detect, measure_statu &measure) {
    DetectResult result;
    if (Error_code::FAILED == detect.getDetectResult(result).get_error_code()) {
        return false;
    }

    // 直接参数
    auto info = measure.mutable_info();
    info->set_theta(result.theta());
    info->set_length(0.0f);  // not need
    info->set_width(result.width());
    info->set_height(0.0f);  // not need
    info->set_wheelbase(result.wheel_base());
    info->set_front_theta(result.front_theta());

    // TODO:中心点需要转换
    info->set_cx(result.cx());
    info->set_cy(result.cy());

    info->set_ground_status(detect.groundStatus());  // 地面雷达状态
    info->set_border_statu(detect.borderStatus());    // 超界状态

    // is stop 暂未使用
    if (info->border_statu() == 0) {
        info->set_is_stop(1);    // 暂时未使用
    } else {
        info->set_is_stop(2);    // 暂时未使用
    }

    return true;
}

bool VelodyneManager::getLocalPointCloud(const std::string &path, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    cloud->clear();
    ReadTxtCloud(path, cloud);
    Eigen::Rotation2D<double> rotation(180 * M_PI / 180);
    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
    for (auto &pt: cloud->points) {
        const Eigen::Matrix<double, 2, 1> point(double(pt.x), double(pt.y));
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * (point);
        pt.x = tanslate_point[0];
        pt.y = tanslate_point[1];
    }
    return !cloud->empty();
}