//*************************************************************************
// slam
//*************************************************************************

namespace gtsam {

#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/navigation/ImuBias.h>

// ######

#include <gtsam/slam/BetweenFactor.h>
template <T = {double, Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
               gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
               gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
  BetweenFactor(size_t key1, size_t key2, const T& relativePose,
                const gtsam::noiseModel::Base* noiseModel);
  T measured() const;

  // enabling serialization functionality
  void serialize() const;
};

#include <gtsam/slam/ProjectionFactor.h>
template <POSE, LANDMARK, CALIBRATION>
virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
  GenericProjectionFactor(const gtsam::Point2& measured,
                          const gtsam::noiseModel::Base* noiseModel,
                          size_t poseKey, size_t pointKey,
                          const CALIBRATION* k);
  GenericProjectionFactor(const gtsam::Point2& measured,
                          const gtsam::noiseModel::Base* noiseModel,
                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
                          const POSE& body_P_sensor);

  GenericProjectionFactor(const gtsam::Point2& measured,
                          const gtsam::noiseModel::Base* noiseModel,
                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
                          bool throwCheirality, bool verboseCheirality);
  GenericProjectionFactor(const gtsam::Point2& measured,
                          const gtsam::noiseModel::Base* noiseModel,
                          size_t poseKey, size_t pointKey, const CALIBRATION* k,
                          bool throwCheirality, bool verboseCheirality,
                          const POSE& body_P_sensor);

  gtsam::Point2 measured() const;
  CALIBRATION* calibration() const;
  bool verboseCheirality() const;
  bool throwCheirality() const;

  // enabling serialization functionality
  void serialize() const;
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                       gtsam::Cal3_S2>
    GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                       gtsam::Cal3DS2>
    GenericProjectionFactorCal3DS2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                       gtsam::Cal3Fisheye>
    GenericProjectionFactorCal3Fisheye;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                       gtsam::Cal3Unified>
    GenericProjectionFactorCal3Unified;

#include <gtsam/slam/GeneralSFMFactor.h>
template <CAMERA, LANDMARK>
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
  GeneralSFMFactor(const gtsam::Point2& measured,
                   const gtsam::noiseModel::Base* model, size_t cameraKey,
                   size_t landmarkKey);
  gtsam::Point2 measured() const;
};
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>,
                                gtsam::Point3>
    GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>,
                                gtsam::Point3>
    GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                                gtsam::Point3>
    GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                                gtsam::Point3>
    GeneralSFMFactorCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
                                gtsam::Point3>
    GeneralSFMFactorCal3Unified;

template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
                         gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
  GeneralSFMFactor2(const gtsam::Point2& measured,
                    const gtsam::noiseModel::Base* model, size_t poseKey,
                    size_t landmarkKey, size_t calibKey);
  gtsam::Point2 measured() const;

  // enabling serialization functionality
  void serialize() const;
};

#include <gtsam/slam/SmartProjectionFactor.h>

/// Linearization mode: what factor to linearize to
enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD };

/// How to manage degeneracy
enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };

class SmartProjectionParams {
  SmartProjectionParams();
  void setLinearizationMode(gtsam::LinearizationMode linMode);
  void setDegeneracyMode(gtsam::DegeneracyMode degMode);
  void setRankTolerance(double rankTol);
  void setEnableEPI(bool enableEPI);
  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
};

#include <gtsam/slam/SmartProjectionPoseFactor.h>
template <CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
                            const CALIBRATION* K);
  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
                            const CALIBRATION* K,
                            const gtsam::Pose3& body_P_sensor);
  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
                            const CALIBRATION* K,
                            const gtsam::SmartProjectionParams& params);
  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
                            const CALIBRATION* K,
                            const gtsam::Pose3& body_P_sensor,
                            const gtsam::SmartProjectionParams& params);

  void add(const gtsam::Point2& measured_i, size_t poseKey_i);

  // enabling serialization functionality
  void serialize() const;
};

typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
    SmartProjectionPose3Factor;

#include <gtsam/slam/StereoFactor.h>
template <POSE, LANDMARK>
virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
  GenericStereoFactor(const gtsam::StereoPoint2& measured,
                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
  gtsam::StereoPoint2 measured() const;
  gtsam::Cal3_S2Stereo* calibration() const;

  // enabling serialization functionality
  void serialize() const;
};
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
    GenericStereoFactor3D;

#include <gtsam/slam/PoseTranslationPrior.h>
template <POSE>
virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
  PoseTranslationPrior(size_t key, const POSE& pose_z,
                       const gtsam::noiseModel::Base* noiseModel);
  POSE::Translation measured() const;

  // enabling serialization functionality
  void serialize() const;
};

typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;

#include <gtsam/slam/PoseRotationPrior.h>
template <POSE>
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
  PoseRotationPrior(size_t key, const POSE& pose_z,
                    const gtsam::noiseModel::Base* noiseModel);
  POSE::Rotation measured() const;
};

typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;

#include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
  EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
                        const gtsam::Point2& pB,
                        const gtsam::noiseModel::Base* noiseModel);
  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                gtsam::DefaultKeyFormatter) const;
  bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
  Vector evaluateError(const gtsam::EssentialMatrix& E) const;
};

#include <gtsam/slam/EssentialMatrixConstraint.h>
virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
  EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,
                            const gtsam::noiseModel::Base *model);
  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                gtsam::DefaultKeyFormatter) const;
  bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const;
  Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const;
  const gtsam::EssentialMatrix& measured() const;
};

#include <gtsam/slam/dataset.h>

class SfmTrack {
  SfmTrack();
  SfmTrack(const gtsam::Point3& pt);
  const Point3& point3() const;

  double r;
  double g;
  double b;

  std::vector<pair<size_t, gtsam::Point2>> measurements;

  size_t number_measurements() const;
  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
  pair<size_t, size_t> siftIndex(size_t idx) const;
  void add_measurement(size_t idx, const gtsam::Point2& m);

  // enabling serialization functionality
  void serialize() const;

  // enabling function to compare objects
  bool equals(const gtsam::SfmTrack& expected, double tol) const;
};

class SfmData {
  SfmData();
  size_t number_cameras() const;
  size_t number_tracks() const;
  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
  gtsam::SfmTrack track(size_t idx) const;
  void add_track(const gtsam::SfmTrack& t);
  void add_camera(const gtsam::SfmCamera& cam);

  // enabling serialization functionality
  void serialize() const;

  // enabling function to compare objects
  bool equals(const gtsam::SfmData& expected, double tol) const;
};

gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);

pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
    bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
    bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
    string filename, gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
void save2D(const gtsam::NonlinearFactorGraph& graph,
            const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
            string filename);

// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose2]
class BetweenFactorPose2s {
  BetweenFactorPose2s();
  size_t size() const;
  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
};
gtsam::BetweenFactorPose2s parse2DFactors(string filename);

// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose3]
class BetweenFactorPose3s {
  BetweenFactorPose3s();
  size_t size() const;
  gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
  void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);

pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);

pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
                                                           bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
              const gtsam::Values& estimate, string filename);

#include <gtsam/slam/InitializePose3.h>
class InitializePose3 {
  static gtsam::Values computeOrientationsChordal(
      const gtsam::NonlinearFactorGraph& pose3Graph);
  static gtsam::Values computeOrientationsGradient(
      const gtsam::NonlinearFactorGraph& pose3Graph,
      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
  static gtsam::Values computeOrientationsGradient(
      const gtsam::NonlinearFactorGraph& pose3Graph,
      const gtsam::Values& givenGuess);
  static gtsam::NonlinearFactorGraph buildPose3graph(
      const gtsam::NonlinearFactorGraph& graph);
  static gtsam::Values initializeOrientations(
      const gtsam::NonlinearFactorGraph& graph);
  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
                                  const gtsam::Values& givenGuess,
                                  bool useGradient);
  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
};

#include <gtsam/slam/KarcherMeanFactor-inl.h>
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
               gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
  KarcherMeanFactor(const gtsam::KeyVector& keys);
};

template <T = {gtsam::Rot3}>
const T FindKarcherMean<T>(const std::vector<T>& rotations);

#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
                                                size_t d);

template <T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
  FrobeniusFactor(size_t key1, size_t key2);
  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);

  Vector evaluateError(const T& R1, const T& R2);
};

template <T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
                         gtsam::noiseModel::Base* model);

  Vector evaluateError(const T& R1, const T& R2);
};
  
#include <gtsam/slam/lago.h>
namespace lago {
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
  gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
}
  
}  // namespace gtsam