//
// Created by zx on 2021/5/11.
//
#pragma once

#include "defines.hpp"
#include <fstream>
#include <fcntl.h>

static bool string2point(const std::string &str, pcl::PointXYZ &point) {
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3];
    for (int i = 0; i < 3; ++i) {
        if (iss >> value) {
            data[i] = value;
        } else {
            return false;
        }
    }
    point.x = data[0];
    point.y = data[1];
    point.z = data[2];
    return true;
}

static bool ReadTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    DLOG(INFO) << "Load point data form file: " << file;
    std::ifstream fin(file.c_str());
    if (fin.bad()) {
        DLOG(WARNING) << "File " << file << " don't exist.";
        return false;
    }
    const int line_length = 64;
    char str[line_length] = {0};
    while (fin.getline(str, line_length)) {
        pcl::PointXYZ point{};
        if (string2point(str, point)) {
            cloud->push_back(point);
        }
    }
    DLOG(INFO) << "Load point complete, get point number: " << cloud->size();
    return true;
}

static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    DLOG(INFO) << "begain push point data to " << file;
    std::fstream fileTxtCloud;
    fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
    for (auto &point: *cloud) {
        fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
    }
    fileTxtCloud.close();
    DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
    return true;
}

static bool WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
    DLOG(INFO) << "begain push point data to " << file;
    std::fstream fileTxtCloud;
    fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
    for (auto &point: *cloud) {
        fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
    }
    fileTxtCloud.close();
    DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
    return true;
}

#include <Eigen/Core>

// 计算均方误差
static bool computer_var(std::vector<double> data, double &var) {
    if (data.empty())
        return false;
    Eigen::VectorXd dis_vec(data.size());
    for (int i = 0; i < data.size(); ++i) {
        dis_vec[i] = data[i];
    }
    double mean = dis_vec.mean();
    Eigen::VectorXd mean_vec(data.size());
    Eigen::VectorXd mat = dis_vec - (mean_vec.setOnes() * mean);
    Eigen::MatrixXd result = (mat.transpose()) * mat;
    var = sqrt(result(0) / double(data.size()));
    return true;
}

#include <opencv2/opencv.hpp>

static double distance(cv::Point2f p1, cv::Point2f p2) {
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

#include <google/protobuf/message.h>
#include <google/protobuf/text_format.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>

//读取protobuf 配置文件，转化为protobuf参数形式
//input：	prototxt_path ：prototxt文件路径
//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter) {
    int fd = open(prototxt_path.c_str(), O_RDONLY);
    if (fd == -1) return false;
    google::protobuf::io::FileInputStream *input = new google::protobuf::io::FileInputStream(fd);
    bool ret = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
    delete input;
    close(fd);
    return ret;
}