#pragma once

#include <eigen3/Eigen/Dense>

namespace oh_my_loam {

class Pose3D {
 public:
  Pose3D() {
    q_.setIdentity();
    p_.setZero();
  };

  Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
      : q_(q), p_(p) {}

  Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
      : q_(r_mat), p_(p) {}

  Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}

  Pose3D Inv() const {
    auto q_inv = q_.inverse();
    auto p_inv = q_inv * p_;
    return {q_inv, p_inv};
  }

  Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const {
    return q_ * vec + p_;
  }

  Eigen::Vector3d operator*(const Eigen::Vector3d& vec) const {
    return Transform(vec);
  }

  template <typename PointT>
  PointT Transform(const PointT& pt) const {
    PointT pt_n = pt;
    Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z));
    pt_n.x = vec.x;
    pt_n.y = vec.y;
    pt_n.z = vec.z;
    return pt_n;
  }

  template <typename PointT>
  PointT operator*(const PointT& vec) const {
    return Transform<PointT>(vec);
  }

  Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
    return vec + p_;
  }

  Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }

  // Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
  Pose3D InterPolate(const Pose3D& pose_to, double t) const {
    auto q_interp = q_.slerp(t, pose_to.q_);
    auto p_interp = (pose_to.p_ - p_) * t;
    return {q_interp, p_interp};
  }

  std::string ToString() const;

  Eigen::Quaterniond q() const { return q_; }
  Eigen::Vector3d p() const { return p_; }

 protected:
  Eigen::Quaterniond q_;  // orientation
  Eigen::Vector3d p_;     // position
};

Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);

Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);

using Trans3D = Pose3D;

}  // namespace oh_my_loam