#include <iostream>
#include <sys/resource.h>

#include <glog/logging.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include "localize/voxel_map_matcher.h"
using namespace std;

#define MIN_MOVEMENT 0.6
#define MIN_ROTATION 0.003
#define prepare_result_cloud(scan, pose, result) \
    result->points.clear();                      \
    for (auto &p : scan->points)                 \
    {                                            \
        Eigen::Vector3f v(p.x, p.y, p.z);        \
        Eigen::Vector3f new_v = pose * v;        \
        PointJ new_p;                            \
        new_p.x = new_v.x();                     \
        new_p.y = new_v.y();                     \
        new_p.z = new_v.z();                     \
        new_p.intensity = p.intensity;           \
        result->points.push_back(new_p);         \
    }

#define init_map_matcher_data()                               \
    PointCloudJ::Ptr scan(new PointCloudJ);                   \
    PointCloudJ::Ptr result(new PointCloudJ);                 \
    pcl::io::loadPCDFile("/home/juefx/Documents/data/test_data/final/ring100.pcd", *scan);                 \
    GnssPoint init_pose;                                      \
    init_pose.B = 31.2763319199 - 0.000002;                   \
    init_pose.L = 121.6587757024 - 0.000002;                  \
    init_pose.H = 16.424827 + 0.05;                           \
                                                              \
    Eigen::Vector3d gnss_rpy(0.765478, 1.597311, 249.582748); \
    gnss_rpy[1] = -gnss_rpy[1];                               \
    gnss_rpy[2] = 90 - gnss_rpy[2];                           \
    init_pose.rpy = gnss_rpy * M_PI / 180;                    \
    GnssPoint blh_final_pose;                                 \
    Eigen::Isometry3f final_pose;

#define init_big_map_matcher_data()                                  \
    std::string base_dir = "/home/wdw/Downloads/329546499/329546499.stream"; \
    PointCloudJ::Ptr raw_scan(new PointCloudJ);                      \
    PointCloudJ::Ptr scan(new PointCloudJ);                          \
    PointCloudJ::Ptr result(new PointCloudJ);                        \
    pcl::io::loadPCDFile("/home/wdw/Downloads/pcd(3)/1683748706.956934_base.pcd", *raw_scan);            \
    Eigen::Isometry3d mapPose;   \
    mapPose.translation() = Eigen::Vector3d(-21.0299381284723, 0266.663172243435, 029.9718818825938);  \
    Eigen::Vector3d mapPoseRpy(0.0154573707048036, 0.0165835359033508, 002.47772534260029);   \
    mapPose.linear() = Eigen::Matrix3d(       \
        Eigen::AngleAxisd(mapPoseRpy[2], Eigen::Vector3d::UnitZ())    \
        * Eigen::AngleAxisd(mapPoseRpy[1], Eigen::Vector3d::UnitY())    \
        * Eigen::AngleAxisd(mapPoseRpy[0], Eigen::Vector3d::UnitX()));    \
    GnssPoint blh_final_pose;                                        \
    float Movement = 0.139191344380379;                                        \
    float Rotation = 0.0263764802366495;                                        \
    int Precision = 3;                                        \
    Eigen::Isometry3f final_pose;

void test_ceres_map_matcher()
{
    init_map_matcher_data();

    juefx::MapMatcherOption map_option;
    juefx::CeresVoxelMatcherOption vmap_option;
    map_option.filter_resolution = 0.5;
    map_option.cloud_range = 80;

    juefx::CeresVoxelMapMatcher vmap_matcher(map_option, vmap_option);
    vmap_matcher.SetMapPath(".");
//    vmap_matcher.SetMapPath("/home/juefx/localize_data/2220/");
    while (!vmap_matcher.IsMapLoaded(init_pose))
        sleep(1);
    vmap_matcher.MatchPointCloud(scan, init_pose, blh_final_pose, final_pose);
    prepare_result_cloud(scan, final_pose, result);
    pcl::io::savePCDFileBinary("2result_voxel.pcd", *result);
//    pcl::io::savePCDFileBinary("/home/juefx/localize_data/2220/2result.pcd", *result);
}

void test_fast_map_matcher()
{
    init_map_matcher_data();

    juefx::MapMatcherOption map_option;
    juefx::FastVoxelMatcherOption fmap_option;
    map_option.filter_resolution = 0.5;
    map_option.cloud_range = 80;

    juefx::FastVoxelMapMatcher fmap_matcher(map_option, fmap_option);
//    fmap_matcher.SetMapPath(".");
    fmap_matcher.SetMapPath("/home/juefx/Documents/data/test_data/stream");
    fmap_matcher.LoadMap();
//    fmap_matcher.IsMapLoaded(init_pose);
//    while (!fmap_matcher.IsMapLoaded(init_pose))
//        sleep(1);
//    LOG(INFO) << "scan->size(): " << scan->size();
//    fmap_matcher.MatchPointCloud(scan, init_pose, blh_final_pose, final_pose);
//    LOG(INFO) << 333;
//    prepare_result_cloud(scan, final_pose, result);
//    LOG(INFO) << 444;
//    pcl::io::savePCDFileBinary("2result_fast1.pcd", *result);

    GuessPose guess_pose;
    guess_pose.use_gnss = true;
    guess_pose.gnss_point = init_pose;
    guess_pose.movement = 1.2;
    guess_pose.rotation = 0.017;
    guess_pose.precision_type = MATCH_PRECISION_FAST;
    fmap_matcher.MatchPointCloud(scan, guess_pose, blh_final_pose, final_pose);
    prepare_result_cloud(scan, final_pose, result);
//    pcl::io::savePCDFileBinary("2result_fast2.pcd", *result);
    pcl::io::savePCDFileBinary("/home/juefx/Documents/data/test_data/2result.pcd", *result);
}

void test_onemesh_voxel_map_matcher()
{
    init_big_map_matcher_data();

    juefx::VoxelMapMatcherOption vmap_option;
    vmap_option.option.filter_resolution = 1.2;
    vmap_option.option.cloud_range = 80;
    vmap_option.fast_option.accepted_score = 0.35;
//    vmap_option.fast_option.Yaw_search_window *= vmap_option.fast_option.init_Yaw_scale;
//    vmap_option.fast_option.XY_search_window *= vmap_option.fast_option.init_XYZ_scale;
//    vmap_option.fast_option.Z_search_window *= vmap_option.fast_option.init_XYZ_scale;

    juefx::VoxelMapMatcher vmap_matcher(vmap_option);
//    vmap_matcher.SetMapPath("./final");
    vmap_matcher.SetMapPath(base_dir/* + "stream"*/);
    vmap_matcher.loadArea(mapPose.translation());
//    vmap_matcher.IsMapLoaded(init_pose);
//    while (!vmap_matcher.IsMapLoaded(init_pose))
//        sleep(1);

//    Eigen::Matrix4d map2base;
//    auto mapPoseInv = mapPose.inverse();
//    map2base.block(0, 3, 3, 1) = mapPoseInv.translation();
//    map2base.block(0, 0, 3, 3) = mapPoseInv.linear();
//    pcl::transformPointCloud(*raw_scan, *raw_scan, map2base.cast<float>());

//    Eigen::Matrix4f imu2base = Eigen::Matrix4f::Identity();
//    imu2base.block(0, 0, 3, 3) = Eigen::Matrix3f(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
//    pcl::transformPointCloud(*raw_scan, *raw_scan, imu2base);

    PointCloudJ mapCloud;
    pcl::transformPointCloud(*raw_scan, mapCloud, mapPose.cast<float>());
    pcl::io::savePCDFileBinary("/home/wdw/Downloads/test/mapCloud.pcd", mapCloud);
    GuessPose guess_pose;
    guess_pose.use_gnss = false;
//    guess_pose.gnss_point = init_pose;
    guess_pose.map_point = mapPose;
    guess_pose.movement = Movement;
    guess_pose.rotation = Rotation;
    guess_pose.precision_type = Precision;
    if(guess_pose.movement < MIN_MOVEMENT){
        guess_pose.movement = MIN_MOVEMENT;
    }
    if(guess_pose.rotation < MIN_ROTATION){
        guess_pose.rotation = MIN_ROTATION;
    }
//    guess_pose.movement = 2;
//    guess_pose.rotation = 0.2;
    vmap_matcher.MatchPointCloud(raw_scan, guess_pose, blh_final_pose, final_pose);
    prepare_result_cloud(raw_scan, final_pose, result);
//    pcl::io::savePCDFileBinary("result_voxel.pcd", *result);
//    pcl::io::savePCDFileBinary(base_dir + "/result/2result.pcd", *result);

    rusage usage;
    getrusage(RUSAGE_SELF, &usage);
    LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
}

int main(int argc, char *argv[])
{
    //test_ceres_map_matcher();
//    test_fast_map_matcher();
    test_onemesh_voxel_map_matcher();

    return 0;
}