#include "Feature.hpp"

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"

#include "src/TrajectoryDBAccessList.h"

using namespace std;
using namespace cv;

FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p) {
  addPoint(frameNum, p);
}

void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
  positions.add(frameNum, p);
  computeMotionData(frameNum);
}

void FeatureTrajectory::write(TrajectoryDBAccess<Point2f>& trajectoryDB) const {
  /// \todo save velocities
  trajectoryDB.write(positions);
}

#ifdef USE_OPENCV
/// \todo add option for anti-aliased drawing, thickness
void FeatureTrajectory::draw(Mat& img, const Scalar& color) const {
  Point2f p1 = positions[0];
  for (unsigned int i=1; i<positions.size(); i++) {
    Point2f p2 = positions[i];
    line(img, p1, p2, color, 1);
    p1 = p2;
  }
}
#endif

// protected

void FeatureTrajectory::computeMotionData(const int& frameNum) {
  unsigned int nPositions = positions.size();
  if (nPositions >= 3) {
    Point2f displacement = positions[nPositions-1] - positions[nPositions-2];
    velocities.add(frameNum, displacement);
    float dist = norm(displacement);
    displacementDistances.push_back(dist);
  } else if (nPositions == 2) {
    Point2f displacement = positions[1] - positions[0];
    velocities.add(frameNum-1, displacement);
    velocities.add(frameNum, displacement);
    float dist = norm(displacement);
    displacementDistances.push_back(dist);
    displacementDistances.push_back(dist);
  }
}