#include "Map.h"
#include "Frame.h"
#include "map_builder/commons.h"
#include "file_system/file_path.h"
#include "pcl_conversions/pcl_conversions.h"
#include <atomic>
#include <filesystem>
#include <ios>
#include <memory>
#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>

namespace Dobot {

LoopPair GetLoopInfo(const Frame& source_frame, const Frame& target_frame) {

    // std::string output_folder_path("/tmp");
    // output_folder_path += "/" + std::to_string(source_frame.getId());

    // std::filesystem::create_directory(output_folder_path);

    LoopPair loop_pair;
    loop_pair.score = 0.;
    loop_pair.source_id = -1;
    loop_pair.target_id = -1;

    pcl::IterativeClosestPoint<Frame::PointType, Frame::PointType> pcl_icp;

    Frame::CloudType::Ptr target_raw = target_frame.getCloud();
    Frame::CloudType::Ptr source_raw = source_frame.getCloud();

    float leaf_size = 0.03f; // 体素边长（单位：米），例如 1cm

    // 对 source 降采样
    pcl::VoxelGrid<CloudType::PointType> voxel_filter_source;
    voxel_filter_source.setInputCloud(source_raw);
    voxel_filter_source.setLeafSize(leaf_size, leaf_size, leaf_size);
    CloudType::Ptr source_filtered(new CloudType);
    voxel_filter_source.filter(*source_filtered);

    // 对 target 降采样
    pcl::VoxelGrid<CloudType::PointType> voxel_filter_target;
    voxel_filter_target.setInputCloud(target_raw);
    voxel_filter_target.setLeafSize(leaf_size, leaf_size, leaf_size);
    CloudType::Ptr target_filtered(new CloudType);
    voxel_filter_target.filter(*target_filtered);

    Frame::CloudType::Ptr align_cloud(new Frame::CloudType);

    Eigen::Matrix4d T_map_source = source_frame.getPose();
    Eigen::Matrix4d T_map_target = target_frame.getPose();

    // 计算 source -> target 的初始变换
    Eigen::Matrix4d T_target_source_initial = T_map_target.inverse() * T_map_source;
    Eigen::Matrix4d final_pose;

    LOG(INFO) << "source_filtered : " << source_filtered->points.size()
              << " target: " << target_filtered->points.size();

    pcl_icp.setInputSource(source_filtered);
    pcl_icp.setInputTarget(target_filtered);

    // pcl_icp.computeTransformation(T_target_source_initial, final_pose);
    pcl_icp.align(*align_cloud, T_target_source_initial.cast<float>());

    CloudType::Ptr source_in_target(new CloudType);

    // pcl::transformPointCloud(*source_filtered, *source_in_target,
    //                          T_target_source_initial);

    // pcl::io::savePCDFileBinary(output_folder_path + "/target.pcd",
    //                            *target_filtered);
    // pcl::io::savePCDFileBinary(output_folder_path + "/source.pcd",
    //                            *source_filtered);
    // pcl::io::savePCDFileBinary(output_folder_path + "/align.pcd", *align_cloud);
    // pcl::io::savePCDFileBinary(output_folder_path + "/targe_in_source.pcd",
    //                            *source_in_target);

    bool converged = pcl_icp.hasConverged();
    float fitness_score = pcl_icp.getFitnessScore(); // 越小越好

    LOG(INFO) << "icp converted: " << std::boolalpha << converged
              << " socre: " << fitness_score;

    if (!pcl_icp.hasConverged() || pcl_icp.getFitnessScore() > 0.55) {
        return loop_pair;
    }

    loop_pair.target_id = target_frame.getId();
    loop_pair.source_id = source_frame.getId();

    Eigen::Matrix4f loop_transform = pcl_icp.getFinalTransformation();

    loop_pair.score = pcl_icp.getFitnessScore();

    Eigen::Matrix3d r_refined =
        loop_transform.block<3, 3>(0, 0).cast<double>() *
        source_frame.getRotation();

    // Eigen::Vector3d t_refined =
    //     loop_transform.block<3, 3>(0, 0).cast<double>() *
    //         source_frame.getTranslatioin() +
    //     loop_transform.block<3, 1>(0, 3).cast<double>();
    loop_pair.r_offset = loop_transform.block<3, 3>(0, 0).cast<double>();
    loop_pair.t_offset = loop_transform.block<3, 1>(0, 3).cast<double>();

    return loop_pair;
}

const std::vector<Frame::Ptr> Map::getFramesVec() const {

    std::vector<Frame::Ptr> frame_vec;

    frame_vec.reserve(frames_.size());

    for (const auto& kv : frames_) {
        frame_vec.push_back(kv.second);
    }

    std::sort(frame_vec.begin(), frame_vec.end(),
              [](const Frame::Ptr &a, const Frame::Ptr &b) {
                return a->getId() < b->getId();
              });

    return frame_vec;
}

std::vector<Eigen::Vector3d> Map::GetKFTrajectory3dof() {

    std::vector<Eigen::Vector3d> trajectory;

    auto frame_vecs = getFramesVec();

    for (const auto &frame : frame_vecs) {

        trajectory.emplace_back(frame->getTranslatioin());

    }

    return trajectory;

}

Map2::PointCloudMap::Ptr Map::toPointCloudMap() {
    auto pc_map = std::make_shared<Map2::PointCloudMap>();

    CloudType::Ptr pointcloud_all_in_world(new CloudType);

    LOG(INFO) << "Frame size: " << frames_.size();
    for (const auto &kv : frames_) {
        auto frame = kv.second;

        const auto pointcloud = frame->getCloud();

        const Eigen::Matrix4d pose = frame->getPose();

        CloudType::Ptr pointcloud_in_world(new CloudType);

        pcl::transformPointCloud(*pointcloud, *pointcloud_in_world, pose);

        // 合并所有帧的点云
        *pointcloud_all_in_world += *pointcloud_in_world;
    }

    pcl::VoxelGrid<PointType> voxel_filter;
    CloudType::Ptr filtered_cloud(new CloudType);
    voxel_filter.setInputCloud(pointcloud_all_in_world);


    // 设置体素大小（单位：米）—— 核心参数！
    voxel_filter.setLeafSize(0.030f, 0.030f, 0.030f); // 10cm 的立方体体素

    // 执行滤波
    voxel_filter.filter(*filtered_cloud);
    LOG(INFO) << "Original point count: " << pointcloud_all_in_world->points.size();

    *pc_map->getCloud() = *filtered_cloud;

    LOG(INFO) << "Filtered point count: " << pc_map->getCloud()->points.size();

    return pc_map;
}

Map2::SCMap::Ptr Map::toSCMap() {

    Map2::SCMap::Ptr sc_map = std::make_shared<Map2::SCMap>();

    CHECK(0);

    return sc_map;
}

}