#include "Motion.hpp"
#include "cvutils.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, const Mat& homography) {
  addPoint(frameNum, p, homography);
}

bool FeatureTrajectory::smallDisplacement(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const {
  bool result = false;
  unsigned int nPositions = positions.size();
  if (nPositions > nDisplacements) {
    float disp = 0;
    for (unsigned int i=0; i<nDisplacements; i++)
      disp += displacementDistances[nPositions-2-i];
    result = disp < minTotalFeatureDisplacement;
  }
  return result;
}

bool FeatureTrajectory::motionSmooth(const int& accelerationBound, const int& deviationBound) const {
  bool result = true;
  unsigned int nPositions = positions.size();
  if (nPositions >= 3) {
    float ratio;
    if (displacementDistances[nPositions-2] > displacementDistances[nPositions-3])
      ratio = displacementDistances[nPositions-2] / displacementDistances[nPositions-3];
    else
      ratio = displacementDistances[nPositions-3] / displacementDistances[nPositions-2];

    float cosine = scalarProduct(velocities[nPositions-3],velocities[nPositions-2]) / (displacementDistances[nPositions-3] * displacementDistances[nPositions-2]);
    
    result &= (ratio < accelerationBound) & (cosine > deviationBound);
  }
  return result;
}

void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p, const Mat& homography) {
  Point2f pp = p;
  if (!homography.empty())
    pp = project(p, homography);
  positions.add(frameNum, pp);
  computeMotionData(frameNum);
  assert(positions.size() == displacementDistances.size()+1);
  assert(positions.size() == velocities.size()+1);
}

void FeatureTrajectory::shorten(void) { 
  positions.pop_back(); 
  velocities.pop_back(); 
  displacementDistances.pop_back();
}

void FeatureTrajectory::write(TrajectoryDBAccess<Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) const {
  trajectoryDB.write(positions, positionsTableName);
  trajectoryDB.write(velocities, velocitiesTableName);
}

#ifdef USE_OPENCV
/// \todo add option for anti-aliased drawing, thickness
void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const {
  Point2f p1, p2;
  if (!homography.empty())
    p1 = project(positions[0], homography);
  else
    p1 = positions[0];
  for (unsigned int i=1; i<positions.size(); i++) {
    if (!homography.empty())
      p2 = project(positions[i], homography);
    else
      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 >= 2) {
    Point2f displacement = positions[nPositions-1] - positions[nPositions-2];
    //if (nPositions == 2) // duplicate first displacement so that positions and velocities have the same length
    //velocities.add(frameNum-1, displacement);
    velocities.add(frameNum, displacement);
    float dist = norm(displacement);
    displacementDistances.push_back(dist);
  }
}