#include "colmap/geometry/pose_prior.h"

#include "colmap/util/logging.h"

#include "pycolmap/helpers.h"
#include "pycolmap/pybind11_extension.h"

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

using namespace colmap;
using namespace pybind11::literals;
namespace py = pybind11;

void BindPosePrior(py::module& m) {
  using PosePriorCoordinateSystem = PosePrior::CoordinateSystem;
  py::enum_<PosePriorCoordinateSystem> PyCoordinateSystem(
      m, "PosePriorCoordinateSystem");
  PyCoordinateSystem.value("UNDEFINED", PosePriorCoordinateSystem::UNDEFINED)
      .value("WGS84", PosePriorCoordinateSystem::WGS84)
      .value("CARTESIAN", PosePriorCoordinateSystem::CARTESIAN);
  AddStringToEnumConstructor(PyCoordinateSystem);

  py::classh_ext<PosePrior> PyPosePrior(m, "PosePrior");
  PyPosePrior.def(py::init<>())
      .def_readwrite("pose_prior_id", &PosePrior::pose_prior_id)
      .def_readwrite("corr_data_id", &PosePrior::corr_data_id)
      .def_readwrite("position", &PosePrior::position)
      .def_readwrite("position_covariance", &PosePrior::position_covariance)
      .def_readwrite("coordinate_system", &PosePrior::coordinate_system)
      .def_readwrite("gravity", &PosePrior::gravity)
      .def("has_position", &PosePrior::HasPosition)
      .def("has_position_cov", &PosePrior::HasPositionCov)
      .def("has_gravity", &PosePrior::HasGravity);
  DefDeprecation(PyPosePrior, "is_valid", "has_position");
  DefDeprecation(PyPosePrior, "is_covariance_valid", "has_position_cov");
  MakeDataclass(PyPosePrior);

  m.def("compute_rot90_from_gravity",
        &ComputeRot90FromGravity,
        "gravity"_a,
        "Compute number of 90 deg CCW rotations to make sensor upright.");
}
