#ifndef FEATURE_HPP
#define FEATURE_HPP

#include "src/Trajectory.h"

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

template<typename T> class TrajectoryDBAccess;

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);

  unsigned int length(void) const { return positions->size();}

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

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

  /// 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

protected:
  bool lost;
  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;

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

/// Class to group features
class FeatureGraph {
public:
  FeatureGraph(float _minDistance, float _maxDistance) : minDistance (_minDistance), maxDistance(_maxDistance) {}

protected:
  struct FeatureConnection {
    float minDistance;
    float maxDistance;
  };
  
  struct VertexInformation {
    boost::shared_ptr<FeatureTrajectory> feature;
  };

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

  float minDistance;
  float maxDistance;

  UndirectedGraph graph;

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

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

#endif