#include "map_matcher_interface.h"

#include "carto/transform/transform.h"

#include "utils/timer.h"
#include "utils/util.hpp"
#include "utils/voxel_filter.h"
#include "voxel_map/voxel_map_interface.h"

namespace juefx
{

MapMatcherInterface::~MapMatcherInterface() {
    if (map_ != NULL) delete map_;
}

bool MapMatcherInterface::SetMapPath(std::string path_name) {
    map_ = new VoxelMap();
    return map_->SetMapPath(path_name);
}

bool MapMatcherInterface::LoadMap() {
    bool ret = false;
    if (map_ != NULL) {
        ret = map_->LoadMap();
        LOG(INFO) << "Map matcher map loaded, resolution: " << map_->GetResolution(VOXEL_RESOLUTION_1X);
    }
    return ret;
}

bool MapMatcherInterface::loadArea(const Eigen::Vector3d &pose){
    if (map_ != NULL) {
        map_->loadArea(pose);
        return true;
    }
    return false;
}

bool MapMatcherInterface::IsMapLoaded(const GnssPoint blh_pose) {
    return map_ != NULL && map_->IsMapLoaded(blh_pose);
}

bool MapMatcherInterface::IsMapLoaded(const Eigen::Vector3d &pose)
{
    return map_ != NULL && map_->IsMapLoaded(pose);
}

double MapMatcherInterface::AreaAvgGridCnt(const Eigen::Vector3d& pose){
    if(map_ != NULL){
        return map_->AreaAvgGridCnt(pose);
    }
    return 0;
}

float MapMatcherInterface::MatchPointCloud(const PointCloudJ::Ptr pcloud, const GuessPose guess_pose,
                                           GnssPoint &final_pose, Eigen::Isometry3f &local_pose)
{
    TIMER_START(start);

    Eigen::Isometry3d pose_in_map;
//    if (guess_pose.use_gnss) {
//        const auto &init_pose = guess_pose.gnss_point;
//        if (map_ == NULL || !map_->GuessPosition(init_pose, pose_in_map)) {
//            char buf[1024];
//            snprintf(buf, 1023, "{ blh: %.8f %.8f %.3f, euler: %.5f %.5f %.5f }",
//                        init_pose.B, init_pose.L, init_pose.H,
//                        init_pose.rpy.x(), init_pose.rpy.y(), init_pose.rpy.z());
//            LOG(INFO) << "Match scan with map, init pose: " << buf << ", guess position failed.";
//            return -1;
//        }
//        pose_in_map.linear() = euler_rpy_2_matrix(init_pose.rpy);

//#ifdef VOXEL_MAP_DEBUG
//        char buf1[1024];
//        Eigen::Vector3d trans3 = pose_in_map.translation();
//        Eigen::Vector3d euler3 = matrix_2_euler_rpy(pose_in_map.rotation());
//        snprintf(buf1, 1023, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }",
//                    trans3.x(), trans3.y(), trans3.z(), euler3.x(), euler3.y(), euler3.z());
//        std::cout << "GuessPosition() return: " << buf1 << std::endl;
//#endif
//    } else {
//        pose_in_map = guess_pose.map_point;
//    }

    pose_in_map = guess_pose.map_point;
    VectorCloud vector_cloud;
//    fix_num_filter(pcloud, vector_cloud, guess_pose.movement, options_.cloud_range);
    vector_voxel_filter(pcloud, vector_cloud, options_.filter_resolution, options_.cloud_range);
#ifdef VOXEL_MAP_DEBUG
    {
        TIMER_END(start, seconds);
        printf("Prepare finished, cost %.3f sec.\n", seconds);
    }
#endif

    Rigid3d predict_pose(pose_in_map.translation(),
                         Eigen::Quaterniond(pose_in_map.rotation()));
    Rigid3d estimate_pose;
    float score = InnerMatch(vector_cloud, guess_pose, predict_pose, estimate_pose);

    if (score >= 0) {
        map_->ConfirmPosition(estimate_pose.translation(), final_pose);
        rigid3d_to_isometry3f(estimate_pose, local_pose);
        final_pose.rpy =
            cartographer::transform::RotationQuaternionToEulerVector(estimate_pose.rotation());

        char buf[1024];
#ifdef VOXEL_MAP_DEBUG
        Rigid3d delta_pose = predict_pose.inverse() * estimate_pose; // Trans from estimate to predict
        auto delta_rpy = cartographer::transform::RotationQuaternionToEulerVector(delta_pose.rotation());
        snprintf(buf, 1023, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }",
                 delta_pose.translation().x(), delta_pose.translation().y(), delta_pose.translation().z(),
                 delta_rpy.x(), delta_rpy.y(), delta_rpy.z());
        printf("match scan delta: %s\n", buf);
#endif
        snprintf(buf, 1023, "{ pos: %.3f %.3f %.3f, euler: %.5f %.5f %.5f }",
                 estimate_pose.translation().x(), estimate_pose.translation().y(), estimate_pose.translation().z(),
                 final_pose.rpy.x(), final_pose.rpy.y(), final_pose.rpy.z());
        TIMER_END(start, seconds);
        LOG(INFO) << "Match scan with map, type: " << (uint16_t)guess_pose.precision_type 
                  << " score: " << score << ", transform: " << buf << ", cost " << seconds << " sec.";
    }
    else {
        final_pose = guess_pose.gnss_point;
        local_pose = pose_in_map.cast<float>();
        TIMER_END(start, seconds);
        LOG(INFO) << "Match scan with map FAILED, type: " << (uint16_t)guess_pose.precision_type
                  << " score: " << score << ", cost " << seconds << " sec.";
    }
    return score;
}

} // namespace juefx