#include "cloudlocalize.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>

#define ACCEPT_SCORE 0.45
#define MIN_MOVEMENT 0.6
#define MIN_ROTATION 0.003

namespace juefx{

CloudLocalize::Ptr cloudLocalizeInstance;
CloudLocalize::CloudLocalize()
{
    VoxelMapMatcherOption option;
    option.option.filter_resolution = 0.5;
    option.option.cloud_range = 80;
    option.fast_option.accepted_score = ACCEPT_SCORE;
    option.fast_option.accepted_low_score = 0.55;
    matcher_.reset(new VoxelMapMatcher(option));
    matcher_->SetMapPath(Parameter::Instance()->GetMapPath());
}

void CloudLocalize::Reset()
{
    cloudLocalizeInstance.reset();
}

CloudLocalize::Ptr CloudLocalize::Instance()
{
    if(!cloudLocalizeInstance){
        LOG(INFO) << "new cloudLocalizeInstance";
        cloudLocalizeInstance.reset(new CloudLocalize());
    }
    return cloudLocalizeInstance;
}

void CloudLocalize::LoadMap()
{
    matcher_->LoadMap();
}

void CloudLocalize::LoadArea(const Vector3d &pose){
    auto start = std::chrono::high_resolution_clock::now();
    matcher_->loadArea(pose);
    auto end = std::chrono::high_resolution_clock::now();
    auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end - start).count() * 1000;
    LOG(INFO) << "Position " << pose.transpose() << " loaded, time_used " << time_used;
}

bool CloudLocalize::FindMapPose(
        const FilterPoseInfo &filterPoseInfo, Isometry3d &mapPose, bool isCurrPos)
{
    if(isCurrPos){
        currFilterPose = filterPoseInfo.filterPose;
    }
    if(State::Instance()->HasState(INIT_MATCHING)){
        if(LastMatchedSuccess){
            mapPose = guessFilter2map * filterPoseInfo.filterPose;
        }else{
            if(!FindReferencePose(filterPoseInfo.hasGnss, filterPoseInfo.blh, mapPose)){
                return false;
            }
        }
    }else if(State::Instance()->HasState(INIT_MATCHED)){
        mapPose = Parameter::Instance()->GetFilter2Map() * filterPoseInfo.filterPose;
    }else{
        return false;
    }
    return true;
}

bool CloudLocalize::IsMapLoaded(const Vector3d &pose)
{
    return matcher_->IsMapLoaded(pose);
}

bool CloudLocalize::MatchMap(
        const PointCloudJ::Ptr pcloud,
        GuessPose &guessPose,
        MatchResult &result)
{
    auto cloudTime = stod(pcloud->header.frame_id);
    auto frameRpy = RotationQuaternionToEulerVector(
                Quaterniond(guessPose.map_point.linear()));

    DeterminePrecision(guessPose.rotation);
    guessPose.precision_type = Precision;

    LOG(INFO) << setprecision(15) << "cloudTime: " << setprecision(15) << cloudTime
              << " framePose: {" << guessPose.map_point.translation().transpose()
              << "} frameRpy: {" << frameRpy.transpose()
              << "} guessPose.movement: {" << guessPose.movement
              << "} guessPose.rotation: {" << guessPose.rotation
              << "} guessPose.precision_type: {" << (uint16_t)guessPose.precision_type << "}";
    if(guessPose.movement > 5){
        LOG(WARNING) << "guessPose.movement too large!";
        return false;
    }
    if(guessPose.movement < MIN_MOVEMENT){
        guessPose.movement = MIN_MOVEMENT;
    }
    if(guessPose.rotation < MIN_ROTATION){
        guessPose.rotation = MIN_ROTATION;
    }
    GnssPoint finalGnssPose;
    Isometry3f finalPose;
    float score;
    string process_name = "4.2 MatchPointCloud by " + to_string((uint16_t)Precision);
    Timer::Evaluate(
        [&, this]() {
        score = matcher_->MatchPointCloud(pcloud, guessPose, finalGnssPose, finalPose);
        },
        process_name);

    CheckMatchResult(score);
    if(score < 0){
        return false;
    }
    if(State::Instance()->HasState(INIT_MATCHING)){
        if(InitMatched){
            Isometry3d filter2map = finalPose.cast<double>() * currFilterPose.inverse();
            Parameter::Instance()->SetFilter2Map(filter2map);
            State::Instance()->RemoveState(INIT_MATCHING);
            State::Instance()->AddState(INIT_MATCHED);
            auto rpy = RotationQuaternionToEulerVector(
                        Quaterniond(filter2map.linear()));
            std::ofstream ofs(Parameter::Instance()->GetBaseDir() + "/local2map.txt", ios::out);
            ofs << filter2map.translation().x() << ", " << filter2map.translation().y() <<  ", "
                << filter2map.translation().z() << ", " << rpy.x() <<  ", "
                << rpy.y() << ", " << rpy.z();
            ofs.close();
            LOG(INFO) << "-----------------------------------------------------";
            LOG(INFO) << "First Match! filter2map: " << filter2map.translation().transpose()
                      << " rotation: " << rpy.transpose();
        }else{
            guessFilter2map = finalPose.cast<double>() * currFilterPose.inverse();
            LOG(INFO) << setprecision(15) << "guessFilter2map update: " << cloudTime
                      << " translation: " << guessFilter2map.translation().transpose()
                      << " rotation: " << RotationQuaternionToEulerVector(
                             Quaterniond(guessFilter2map.linear())).transpose();
        }
    }else{
        if(MATCH_PRECISION_HIGH == guessPose.precision_type
                || MATCH_PRECISION_FULL == guessPose.precision_type){
            result.state = HIGH_PRECISION;
        }else if(MATCH_PRECISION_FAST == guessPose.precision_type){
            result.state = FAST_PRECISION;
        }
        result.state = MatchPrecisionState(result.state + CheckCloud(pcloud));
    }
    PointCloudJ finalCloud;
    pcl::transformPointCloud(*pcloud, finalCloud, finalPose.cast<float>());
    if(finalCloud.size()){
        pcl::io::savePCDFileBinary(Parameter::Instance()->GetBaseDir() + "/result/pcd/" +
                                   to_string(cloudTime) + ".pcd", finalCloud);
    }
    MatchedPose = finalPose.cast<double>();
    result.finalPose = finalPose;
    return true;
}

void CloudLocalize::DeterminePrecision(float rotation)
{
    if(State::Instance()->HasState(INIT_MATCHING)){
        if(!LastMatchedSuccess){
            Precision = MATCH_PRECISION_INIT;
        }else{
            if(InitFastMatched){
                Precision = MATCH_PRECISION_HIGH;
            }else if(Precision < MATCH_PRECISION_FAST){
                Precision++;
            }
        }
    }else if(State::Instance()->HasState(INIT_MATCHED)){
        if(false == LastMatchedSuccess || 9 == fastPassCnt){
            Precision = MATCH_PRECISION_HIGH;
            if(rotation >= 0.015){
                Precision = MATCH_PRECISION_FULL;
            }
        }else{
            Precision = MATCH_PRECISION_FAST;
        }
    }
}

void CloudLocalize::CheckMatchResult(float score)
{
    if(score < 0){
        LastMatchedSuccess = false;
        fastPassCnt = 0;
    }else{
        LastMatchedSuccess = true;
        if(MATCH_PRECISION_FAST == Precision){
            fastPassCnt++;
        }else{
            fastPassCnt = 0;
        }
        if(State::Instance()->HasState(INIT_MATCHING)){
            if(InitFastMatched){
                InitMatched = true;
            }else if(fastPassCnt >= 3){
                InitFastMatched = true;
            }
        }
    }
}

bool CloudLocalize::FindReferencePose(
        bool hasGnss, const Vector3d &blh, Isometry3d &mapPose)
{
    DCHECK(State::Instance()->HasState(INIT_MATCHING));
    if(hasGnss){
        return PoseGuesser::Instance()->GetMapPoseByBlh(blh, mapPose) < 3;
    }else{
        mapPose = Parameter::Instance()->GetInitPose();
    }
    return true;
}

int CloudLocalize::CheckCloud(const PointCloudJ::Ptr &cloud)
{
    PointCloudJ::Ptr points_filtered;
    points_filtered.reset(new PointCloudJ);
    pcl::VoxelGrid<PointJ> grid;
    grid.setLeafSize(0.5, 0.5, 0.5);
    grid.setInputCloud(cloud);
    grid.filter(*points_filtered);
    if(points_filtered->size() < 600){
        return 1;
    }
    return 0;
}

} // end of namespace