#ifndef _ODOM_H_
#define _ODOM_H_

#include <deque>
#include <mutex>

#include <Eigen/Dense>

#include "common/common.h"

namespace hlzn_slam {

namespace odom {

class odom {
    public:
        struct odometry {
            common::Rigid2f odom;
            common::Time time;
        };

        void addOdometry(const float x, const float y, const float yaw);

        bool extrapolaOdom(const common::Time& start_time, 
                           const common::Time& end_time, 
                           common::Rigid2f& T);
        bool extrapolaOdom(const common::Rigid2f& start_odom, const common::Time& end_time, 
                            common::Rigid2f& T);
    private:
        std::deque<odometry> odometry_;

        std::mutex mutex;
        #define LOCK() std::lock_guard<std::mutex> lockGuard(mutex);
};

}
}

#endif