/**
 * Matlab toolbox interface definition for gtsam_unstable
 */

// specify the classes from gtsam we are using
virtual class gtsam::Value;
virtual class gtsam::LieScalar;
virtual class gtsam::Point2;
virtual class gtsam::Rot2;
virtual class gtsam::Pose2;
virtual class gtsam::Point3;
virtual class gtsam::Rot3;
virtual class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::NonlinearFactor;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
class gtsam::Values;
class gtsam::KeySet;
class gtsam::KeyVector;

namespace gtsam {

#include <gtsam_unstable/base/Dummy.h>
class Dummy {
  Dummy();
  void print(string s) const;
  unsigned char dummyTwoVar(unsigned char a) const;
};

#include <gtsam_unstable/dynamics/PoseRTV.h>
virtual class PoseRTV : gtsam::Value {
  PoseRTV();
  PoseRTV(Vector rtv);
  PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
  PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel);
  PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel);
  PoseRTV(const gtsam::Pose3& pose);
  PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);

  // testable
  bool equals(const gtsam::PoseRTV& other, double tol) const;
  void print(string s) const;

  // access
  gtsam::Point3 translation() const;
  gtsam::Rot3 rotation() const;
  gtsam::Point3 velocity() const;
  gtsam::Pose3 pose() const;

  // Vector interfaces
  Vector vector() const;
  Vector translationVec() const;
  Vector velocityVec() const;

  // manifold/Lie
  static size_t Dim();
  size_t dim() const;
  gtsam::PoseRTV retract(Vector v) const;
  Vector localCoordinates(const gtsam::PoseRTV& p) const;
  static gtsam::PoseRTV Expmap(Vector v);
  static Vector Logmap(const gtsam::PoseRTV& p);
  gtsam::PoseRTV inverse() const;
  gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
  gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;

  // measurement
  double range(const gtsam::PoseRTV& other) const;
  gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const;

  // IMU/dynamics
  gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
  gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
  gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
  Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
  gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
  Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
};

#include <gtsam_unstable/geometry/Pose3Upright.h>
virtual class Pose3Upright : gtsam::Value {
  Pose3Upright();
  Pose3Upright(const gtsam::Pose3Upright& x);
  Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
  Pose3Upright(double x, double y, double z, double theta);
  Pose3Upright(const gtsam::Pose2& pose, double z);

  void print(string s) const;
  bool equals(const gtsam::Pose3Upright& pose, double tol) const;

  double x() const;
  double y() const;
  double z() const;
  double theta() const;

  gtsam::Point2 translation2() const;
  gtsam::Point3 translation() const;
  gtsam::Rot2 rotation2() const;
  gtsam::Rot3 rotation() const;
  gtsam::Pose2 pose2() const;
  gtsam::Pose3 pose() const;

  size_t dim() const;
  gtsam::Pose3Upright retract(Vector v) const;
  Vector localCoordinates(const gtsam::Pose3Upright& p2) const;

  static gtsam::Pose3Upright identity();
  gtsam::Pose3Upright inverse() const;
  gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
  gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;

  static gtsam::Pose3Upright Expmap(Vector xi);
  static Vector Logmap(const gtsam::Pose3Upright& p);
}; // \class Pose3Upright

#include <gtsam_unstable/geometry/BearingS2.h>
virtual class BearingS2 : gtsam::Value {
  BearingS2();
  BearingS2(double azimuth, double elevation);
  BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);

  gtsam::Rot2 azimuth() const;
  gtsam::Rot2 elevation() const;

  static gtsam::BearingS2 fromDownwardsObservation(const gtsam::Pose3& A, const gtsam::Point3& B);
  static gtsam::BearingS2 fromForwardObservation(const gtsam::Pose3& A, const gtsam::Point3& B);

  void print(string s) const;
  bool equals(const gtsam::BearingS2& x, double tol) const;

  size_t dim() const;
  gtsam::BearingS2 retract(Vector v) const;
  Vector localCoordinates(const gtsam::BearingS2& p2) const;
};

// std::vector<gtsam::Point2>
class Point2Vector
{
  //Capacity
  size_t size() const;
  size_t max_size() const;
  void resize(size_t sz);
  size_t capacity() const;
  bool empty() const;
  void reserve(size_t n);

  //Element access
  gtsam::Point2 at(size_t n) const;
  gtsam::Point2 front() const;
  gtsam::Point2 back() const;

  //Modifiers
  void assign(size_t n, const gtsam::Point2& u);
  void push_back(const gtsam::Point2& x);
  void pop_back();
};

#include <gtsam_unstable/geometry/SimWall2D.h>
class SimWall2D {
  SimWall2D();
  SimWall2D(const gtsam::Point2& a, const gtsam::Point2& b);
  SimWall2D(double ax, double ay, double bx, double by);

  void print(string s) const;
  bool equals(const gtsam::SimWall2D& other, double tol) const;

  gtsam::Point2 a() const;
  gtsam::Point2 b() const;

  gtsam::SimWall2D scale(double s) const;
  double length() const;
  gtsam::Point2 midpoint() const;

  bool intersects(const gtsam::SimWall2D& wall) const;
  //   bool intersects(const gtsam::SimWall2D& wall, boost::optional<gtsam::Point2&> pt=boost::none) const;

  gtsam::Point2 norm() const;
  gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const;
};

#include <gtsam_unstable/geometry/SimPolygon2D.h>
class SimPolygon2D {
   static void seedGenerator(size_t seed);
   static gtsam::SimPolygon2D createTriangle(const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::Point2& pC);
   static gtsam::SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width);

   static gtsam::SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len,
       double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys);
   static gtsam::SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len,
       double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys);

   gtsam::Point2 landmark(size_t i) const;
   size_t size() const;
   gtsam::Point2Vector vertices() const;

   bool equals(const gtsam::SimPolygon2D& p, double tol) const;
   void print(string s) const;

   gtsam::SimWall2DVector walls() const;
   bool contains(const gtsam::Point2& p) const;
   bool overlaps(const gtsam::SimPolygon2D& p) const;

   static bool anyContains(const gtsam::Point2& p, const gtsam::SimPolygon2DVector& obstacles);
   static bool anyOverlaps(const gtsam::SimPolygon2D& p, const gtsam::SimPolygon2DVector& obstacles);
   static bool insideBox(double s, const gtsam::Point2& p);
   static bool nearExisting(const gtsam::Point2Vector& S,
       const gtsam::Point2& p, double threshold);

   static gtsam::Point2 randomPoint2(double s);
   static gtsam::Rot2 randomAngle();
   static double randomDistance(double mu, double sigma);
   static double randomDistance(double mu, double sigma, double min_dist);
   static gtsam::Point2 randomBoundedPoint2(double boundary_size,
       const gtsam::Point2Vector& landmarks, double min_landmark_dist);
   static gtsam::Point2 randomBoundedPoint2(double boundary_size,
       const gtsam::Point2Vector& landmarks,
       const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist);
   static gtsam::Point2 randomBoundedPoint2(double boundary_size,
       const gtsam::SimPolygon2DVector& obstacles);
   static gtsam::Point2 randomBoundedPoint2(
       const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
       const gtsam::Point2Vector& landmarks,
       const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist);
   static gtsam::Pose2 randomFreePose(double boundary_size, const gtsam::SimPolygon2DVector& obstacles);
 };

 // std::vector<gtsam::SimWall2D>
 class SimWall2DVector
 {
   //Capacity
   size_t size() const;
   size_t max_size() const;
   void resize(size_t sz);
   size_t capacity() const;
   bool empty() const;
   void reserve(size_t n);

   //Element access
   gtsam::SimWall2D at(size_t n) const;
   gtsam::SimWall2D front() const;
   gtsam::SimWall2D back() const;

   //Modifiers
   void assign(size_t n, const gtsam::SimWall2D& u);
   void push_back(const gtsam::SimWall2D& x);
   void pop_back();
 };

 // std::vector<gtsam::SimPolygon2D>
 class SimPolygon2DVector
 {
   //Capacity
   size_t size() const;
   size_t max_size() const;
   void resize(size_t sz);
   size_t capacity() const;
   bool empty() const;
   void reserve(size_t n);

   //Element access
   gtsam::SimPolygon2D at(size_t n) const;
   gtsam::SimPolygon2D front() const;
   gtsam::SimPolygon2D back() const;

   //Modifiers
   void assign(size_t n, const gtsam::SimPolygon2D& u);
   void push_back(const gtsam::SimPolygon2D& x);
   void pop_back();
 };

// Nonlinear factors from gtsam, for our Value types
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::PoseRTV}>
virtual class PriorFactor : gtsam::NonlinearFactor {
  PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
};


#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::PoseRTV}>
virtual class BetweenFactor : gtsam::NonlinearFactor {
  BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
};


#include <gtsam/slam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NonlinearFactor {
  RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
};

typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;


#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::PoseRTV}>
virtual class NonlinearEquality : gtsam::NonlinearFactor {
  // Constructor - forces exact evaluation
  NonlinearEquality(size_t j, const T& feasible);
  // Constructor - allows inexact evaluation
  NonlinearEquality(size_t j, const T& feasible, double error_gain);
};

#include <gtsam_unstable/dynamics/IMUFactor.h>
template<POSE = {gtsam::PoseRTV}>
virtual class IMUFactor : gtsam::NonlinearFactor {
  /** Standard constructor */
  IMUFactor(Vector accel, Vector gyro,
    double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);

  /** Full IMU vector specification */
  IMUFactor(Vector imu_vector,
    double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);

  Vector gyro() const;
  Vector accel() const;
  Vector z() const;
  size_t key1() const;
  size_t key2() const;
};

#include <gtsam_unstable/dynamics/FullIMUFactor.h>
template<POSE = {gtsam::PoseRTV}>
virtual class FullIMUFactor : gtsam::NonlinearFactor {
  /** Standard constructor */
  FullIMUFactor(Vector accel, Vector gyro,
    double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);

  /** Single IMU vector - imu = [accel, gyro] */
  FullIMUFactor(Vector imu,
    double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model);

  Vector gyro() const;
  Vector accel() const;
  Vector z() const;
  size_t key1() const;
  size_t key2() const;
};


#include <gtsam_unstable/dynamics/DynamicsPriors.h>
virtual class DHeightPrior : gtsam::NonlinearFactor {
  DHeightPrior(size_t key, double height, const gtsam::noiseModel::Base* model);
};


virtual class DRollPrior : gtsam::NonlinearFactor {
  /** allows for explicit roll parameterization - uses canonical coordinate */
  DRollPrior(size_t key, double wx, const gtsam::noiseModel::Base* model);
  /** Forces roll to zero */
  DRollPrior(size_t key, const gtsam::noiseModel::Base* model);
};


virtual class VelocityPrior : gtsam::NonlinearFactor {
  VelocityPrior(size_t key, Vector vel, const gtsam::noiseModel::Base* model);
};


virtual class DGroundConstraint : gtsam::NonlinearFactor {
  // Primary constructor allows for variable height of the "floor"
  DGroundConstraint(size_t key, double height, const gtsam::noiseModel::Base* model);
  // Fully specify vector - use only for debugging
  DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
};

#include <gtsam/base/LieScalar.h>

#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
  /** Standard constructor */
  VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt);

  Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const;
};

//*************************************************************************
// nonlinear
//*************************************************************************
#include <gtsam_unstable/nonlinear/summarization.h>
gtsam::GaussianFactorGraph* summarizeGraphSequential(
    const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
gtsam::GaussianFactorGraph* summarizeGraphSequential(
    const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);

pair<gtsam::GaussianFactorGraph,gtsam::Ordering>
partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
    const gtsam::KeySet& overlap_keys);

//*************************************************************************
// slam
//*************************************************************************
#include <gtsam/geometry/Pose2.h>

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

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

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

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

#include <gtsam_unstable/slam/RelativeElevationFactor.h>
virtual class RelativeElevationFactor: gtsam::NonlinearFactor {
  RelativeElevationFactor();
  RelativeElevationFactor(size_t poseKey, size_t pointKey, double measured,
      const gtsam::noiseModel::Base* model);

  double measured() const;
  void print(string s) const;
};

#include <gtsam_unstable/slam/DummyFactor.h>
virtual class DummyFactor : gtsam::NonlinearFactor {
  DummyFactor(size_t key1, size_t dim1, size_t key2, size_t dim2);
};

} //\namespace gtsam