#include "odom/odom.h"

namespace hlzn_slam {

namespace odom {

void odom::addOdometry(const float x, const float y, const float yaw)
{
    LOCK()
    odometry odom;

    odom.odom = common::Rigid2f(common::Rigid2f::Vector({x, y}), yaw);
    odom.time = std::chrono::system_clock::now();

    if (!odometry_.empty()) {
        std::chrono::duration<float> dur = odom.time  - odometry_.front().time;

        if (dur.count() > 1.0) {
            odometry_.pop_front();
        }
    }
    odometry_.push_back(odom);
}

bool odom::extrapolaOdom(const common::Time& start_time, 
                         const common::Time& end_time, 
                         common::Rigid2f& T) {             
    LOCK()
    if (odometry_.empty()) return false;
    
    odometry start_odom, end_odom;
    float start_close_minmal = 1e5, end_close_minmal = 1e5;
    
    // 在里程计序列中找到与时间戳最紧密的两帧数据
    for (const odometry& odom : odometry_) {
        std::chrono::duration<float> start_close = start_time  - odom.time;
        std::chrono::duration<float> end_close = end_time  - odom.time;
        
        if (start_close_minmal > fabs(start_close.count())) {
            start_odom = odom;
            start_close_minmal = fabs(start_close.count());
        }
        if (end_close_minmal > fabs(end_close.count())) {
            end_odom = odom;
            end_close_minmal = fabs(end_close.count());
        }
    }
    // 300 ms
    // std::cout<<" "<<end_close_minmal<<" "<<start_close_minmal<<std::endl;
    if (end_close_minmal > 3e-1 || start_close_minmal > 3e-1) {
        return false;
    }
    common::Rigid2f::Vector translation = start_odom.odom.rotation().inverse() * end_odom.odom.translation() - 
                                             start_odom.odom.rotation().inverse() * start_odom.odom.translation();
    Eigen::Rotation2D<float> rotation = start_odom.odom.rotation().cast<float>().inverse() * end_odom.odom.rotation().cast<float>();

    T = common::Rigid2f(translation, rotation.cast<float>());

    return true;
}

bool odom::extrapolaOdom(const common::Rigid2f& start_odom, const common::Time& end_time, 
                                common::Rigid2f& T) {
    LOCK()
    if (odometry_.empty()) return false;
    
    odometry end_odom;
    float end_close_minmal = 1e5;
    
    // 在里程计序列中找到与时间戳最紧密的两帧数据
    for (const odometry& odom : odometry_) {
        std::chrono::duration<float> end_close = end_time  - odom.time;
        
        if (end_close_minmal > fabs(end_close.count())) {
            end_odom = odom;
            end_close_minmal = fabs(end_close.count());
        }
    }
    // 300 ms
    // std::cout<<" "<<end_close_minmal<<" "<<close_start_elapsed_seconds<<std::endl;
    if (end_close_minmal > 3e-1) {
        return false;
    }
    common::Rigid2f::Vector translation = start_odom.rotation().inverse() * end_odom.odom.translation() - 
                                                start_odom.rotation().inverse() * start_odom.translation();
    Eigen::Rotation2D<float> rotation = start_odom.rotation().cast<float>().inverse() * end_odom.odom.rotation().cast<float>();

    T = common::Rigid2f(translation, rotation.cast<float>());

    return true;
}

}
}