#ifndef MOTION_HPP
#define MOTION_HPP

#include "src/Trajectory.h"

#include <boost/shared_ptr.hpp>
#include <boost/graph/adjacency_list.hpp>

template<typename T> class TrajectoryDBAccess;
template<typename T> class TrajectoryDBAccessList;

typedef boost::shared_ptr<TrajectoryPoint2f> TrajectoryPoint2fPtr;

/** Class for feature data
    positions, velocities and other statistics to evaluate their quality
    before saving. */
class FeatureTrajectory {
public:
  FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography);

  FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities);

  /** loads from database
   can be made generic for different list and blob */
  FeatureTrajectory(const int& id, TrajectoryDBAccessList<cv::Point2f>& trajectoryDB, const std::string& positionsTableName, const std::string& velocitiesTableName);

  unsigned int length(void) const { return positions->size();} // cautious if not continuous: max-min+1

  void setId(const unsigned int& id) { positions->setId(id);velocities->setId(id);}

  void setLost(void) { lost = true;}
  bool isLost(void) { return lost;}

  unsigned int getFirstInstant(void) {return firstInstant;}
  unsigned int getLastInstant(void) {return lastInstant;}

  /// indicates whether the sum of the last nDisplacements displacements has been inferior to minFeatureDisplacement
  bool isDisplacementSmall(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const;

  /// indicates whether the last two displacements are smooth (limited acceleration and angle)
  bool isMotionSmooth(const int& accelerationBound, const int& deviationBound) const;

  void addPoint(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography);

  void shorten(void);

  void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB, const std::string& positionsTableName, const std::string& velocitiesTableName) const;

#ifdef USE_OPENCV
  void draw(cv::Mat& img, const cv::Mat& homography, const cv::Scalar& color) const;
#endif

  friend std::stringstream& operator<<(std::stringstream& out, const FeatureTrajectory& ft);

protected:
  bool lost; /// \todo remove
  /// first frame number
  unsigned int firstInstant;
  /// last frame number
  unsigned int lastInstant;

  TrajectoryPoint2fPtr positions;
  /** one fewer velocity than position
      v_n = p_n+1 - p_n*/
  TrajectoryPoint2fPtr velocities;
  
  /// norms of velocities for feature constraints, one fewer positions than positions
  std::vector<float> displacementDistances;

  void computeMotionData(const int& frameNum);
};

typedef boost::shared_ptr<FeatureTrajectory> FeatureTrajectoryPtr;

// inlined
inline std::stringstream& operator<<(std::stringstream& out, const FeatureTrajectory& ft) {
  out << *(ft.positions);
  out << "\n";
  out << *(ft.velocities);
  return out;
}

// class MovingObject {}
// roadUserType, group of features

/** Class to group features: Beymer et al. 99/Saunier and Sayed 06
    \todo create various graph types with different parameters, that accept different feature distances or ways to connect and segment features */
class FeatureGraph {
public:
  //FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {}
  FeatureGraph(float _connectionDistance, float _segmentationDistance) : connectionDistance (_connectionDistance), segmentationDistance(_segmentationDistance) {}

  void addFeature(const FeatureTrajectoryPtr& ft);

  // add vertex, includes adding links to current vertices
  // find connected components, check if old enough, if so, remove

protected:
  struct FeatureConnection {
    float minDistance;
    float maxDistance;
  };
  
  struct VertexInformation {
    FeatureTrajectoryPtr feature;
  };

  typedef boost::adjacency_list <boost::listS, boost::listS, boost::undirectedS, VertexInformation, FeatureConnection> UndirectedGraph;

  float connectionDistance;
  float segmentationDistance;
  // float minDistance;
  // float maxDistance;

  UndirectedGraph graph;

  //std::vector<UndirectedGraph::vertex_descriptor> currentVertices, lostVertices;
};

// inlined implementations
// inline FeatureGraph::FeatureGraph(float _minDistance, float _maxDistance)

#endif