pose2d.h

#pragma once
#undef __ARM_NEON__
#undef __ARM_NEON
#include <Eigen/Dense>
 
#include <cmath>
#include <iostream>
#include <vector>
 
#include "core/utils/math/geometry/rotation2d.h"
#include "core/utils/math/geometry/transform2d.h"
#include "core/utils/math/geometry/translation2d.h"
#include "core/utils/math/geometry/twist2d.h"

class Pose2d {
  public:
    constexpr Pose2d() : m_translation(Translation2d()), m_rotation(Rotation2d()) {}

    Pose2d(const Translation2d &translation, const Rotation2d &rotation);

    Pose2d(const double &x, const double &y, const Rotation2d &rotation);

    Pose2d(const double &x, const double &y, const double &radians);

    Pose2d(const Translation2d &translation, const double &radians);

    Pose2d(const Eigen::Vector3d &pose_vector);

    Translation2d translation() const;

    double x() const;

    double y() const;

    Rotation2d rotation() const;
    
    void setRotationRad(double rotRad);

    void setRotationDeg(double rotDeg);

    bool operator==(const Pose2d other) const;

    Pose2d operator*(const double &scalar) const;

    Pose2d operator/(const double &scalar) const;

    Pose2d operator+(const Transform2d &transform) const;

    Transform2d operator-(const Pose2d &other) const;

    friend std::ostream &operator<<(std::ostream &os, const Pose2d &pose);

    Pose2d relative_to(const Pose2d &other) const;

    Pose2d transform_by(const Transform2d &transform) const;

    Pose2d exp(const Twist2d &twist) const;

    Twist2d log(const Pose2d &end_pose) const;
 
  private:
    Translation2d m_translation;
    Rotation2d m_rotation;
};

Pose2d wrapped_mean(const std::vector<Pose2d> &list);