#ifndef AMR_CONTROL_POSE_H
#define AMR_CONTROL_POSE_H
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <unistd.h>
#include <vector>
#include <fstream>
#include <math.h>
#include <cmath>

namespace vda_5050
{
  class Pose {
  private:
    double x_, y_, yaw_;
    double v_max_, accuracy_;

    void modifyYaw(void) {
      while (yaw_ < -M_PI)
        yaw_ += 2.0 * M_PI;
      while (yaw_ > M_PI)
        yaw_ -= 2.0 * M_PI;
    }

  public:
    Pose() :
      x_(0.0), y_(0.0), yaw_(0.0), v_max_(0.0), accuracy_(0.0) {
    };

    Pose(double x, double y, double yaw) :
      x_(x), y_(y), yaw_(yaw), v_max_(0.0), accuracy_(0.0) {
    };

    Pose(double x, double y, double yaw, double v_max, double accuracy) :
      x_(x), y_(y), yaw_(yaw), v_max_(v_max), accuracy_(accuracy) {
    };

    ~Pose() {};

    inline void setX(double x) { x_ = x; }
    inline void setY(double y) { y_ = y; }
    inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); }
    inline void setVmax(double v_max) { v_max_ = v_max; }
    inline void setAccuracy(double accuracy) { accuracy_ = accuracy; }
    inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); }
    inline void setPose(double x, double y, double yaw, double v_max, double accuracy)
    {
      x_ = x, y_ = y, yaw_ = yaw, v_max_ = v_max, accuracy_ = accuracy, modifyYaw();
    }
    inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); }

    inline double getX(void) { return x_; }
    inline double getY(void) { return y_; }
    inline double getYaw(void) { return yaw_; }
    inline double getVmax(void) { return v_max_; }
    inline double getAccuracy(void) { return accuracy_; }
    inline Pose getPose(void) { return Pose(x_, y_, yaw_, v_max_, accuracy_); }
    bool SavePoseAsFile(const std::string& file_name);
    bool LoadPoseFromFile(const std::string& file_name);

  }; // class Pose

}

#endif