//
// Created by xtkuang on 2025/7/7.
//

#ifndef SE3_H
#define SE3_H

#pragma once

#include <eigen3/Eigen/Core>
#include <optional>

#include "so3.h"

namespace cmvr::math {

class SE3;
class se3;
class se3v;

class se3 {
 public:
  using MatrixType = Eigen::Matrix4d;

 private:
  se3() = default;
};

class se3v {
 public:
  using MatrixType = Eigen::Vector<double, 6>;

 private:
  se3v() = default;
};

class SE3 {
 public:
  using MatrixType = Eigen::Matrix4d;

  static MatrixType Identity();

  static MatrixType T(const Eigen::Vector3d& p);

  static MatrixType T(const SO3::MatrixType& R, const Eigen::Vector3d& p = Eigen::Vector3d::Zero());

  static MatrixType Inverse(const MatrixType& T);

  static SO3::MatrixType GetRotation(const MatrixType& T);

  static Eigen::Vector3d GetPosition(const MatrixType& T);

  static MatrixType Exp(const se3v::MatrixType& S, double angle = 1.);

  static MatrixType Exp(so3v::MatrixType w, Eigen::Vector3d v, double angle = 1.);

  static se3v::MatrixType Log(const MatrixType& T);

  static typename Eigen::Matrix<double, 6, 6> Ad(const MatrixType& T);

  static se3v::MatrixType Ad(const MatrixType& T, const se3v::MatrixType& S);

  static Eigen::Matrix<double, 6, 6> InvAd(const MatrixType& T);

  static se3v::MatrixType InvAd(const MatrixType& T, const se3v::MatrixType& S);

  static typename Eigen::Matrix<double, 6, 6> ad(const se3v::MatrixType& S);

  static Eigen::Matrix<double, 6, 6> adTranspose(const se3v::MatrixType& S);

  static se3v::MatrixType ad(const se3v::MatrixType& S1, const se3v::MatrixType& S2);

  static se3v::MatrixType adTranspose(const se3v::MatrixType& S1, const se3v::MatrixType& S2);

  static Eigen::Vector3d Multiply(const MatrixType& T, const Eigen::Vector3d& p);

  template <typename Container, typename = std::enable_if_t<std::is_same_v<typename Container::value_type, MatrixType>>>
  static std::optional<MatrixType> Average(const Container& matrices, double eps, int max_iter = -1);

  static se3v::MatrixType Vec(const se3::MatrixType& s);

  static se3::MatrixType Hat(const se3v::MatrixType& v);

 private:
  SE3() = default;
};

}  // namespace rb::math

#endif //SE3_H