#include "Motion.hpp"
#include "cvutils.hpp"

#include "src/TrajectoryDBAccessList.h"

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

#include <boost/graph/connected_components.hpp>
#include <boost/config.hpp>

#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>

using namespace std;
using namespace cv;

/******************** FeatureTrajectory ********************/

FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const Mat& homography)
  : lost(false) {
  positions = TrajectoryPoint2fPtr(new TrajectoryPoint2f());
  velocities = TrajectoryPoint2fPtr(new TrajectoryPoint2f());
  addPoint(frameNum, p, homography);
}

FeatureTrajectory::FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities)
  : lost(false) {
  positions = _positions;
  velocities = _velocities;
}

FeatureTrajectory::FeatureTrajectory(const int& id, TrajectoryDBAccessList<Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) {
  bool success = trajectoryDB.read(positions, id, positionsTableName);
  if (!success)
    cout << "problem loading positions" << endl;
  success = trajectoryDB.read(velocities, id, velocitiesTableName);
  if (!success)
    cout << "problem loading velocities" << endl;
  // take advantage to request first and last instant from database
  trajectoryDB.timeInterval(firstInstant, lastInstant, id);
}

bool FeatureTrajectory::isDisplacementSmall(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::isMotionSmooth(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;
}

bool FeatureTrajectory::minMaxSimilarity(const FeatureTrajectory& ft, const int& firstInstant, const int& lastInstant, float connectionDistance, float segmentationDistance) {
  float minDistance = norm(positions->getPointAtInstant(firstInstant)-ft.positions->getPointAtInstant(firstInstant));
  float maxDistance = minDistance;
  bool connected = minDistance <= connectionDistance;
  int t=firstInstant+1;
  while (t <= lastInstant && connected) {
    float distance = norm(positions->getPointAtInstant(t)-ft.positions->getPointAtInstant(t));
    if (distance < minDistance)
      minDistance = distance;
    else if (distance > maxDistance)
      maxDistance = distance;
    connected = connected && (maxDistance-minDistance < segmentationDistance);
    t++;
  }

  return connected;
}

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

/******************** FeatureGraph ********************/

void FeatureGraph::addFeature(const FeatureTrajectoryPtr& ft) {
  UndirectedGraph::vertex_descriptor newVertex = add_vertex(graph);
  graph[newVertex].feature = ft;
  for (boost::graph_traits<UndirectedGraph>::vertex_iterator vi = vertices(graph).first; 
       vi!=vertices(graph).second; ++vi) { // vi pointer to vertex_descriptor
    FeatureTrajectoryPtr ft2 = graph[*vi].feature;
    int lastInstant = static_cast<int>(min(ft->getLastInstant(), ft2->getLastInstant()));
    int firstInstant = static_cast<int>(max(ft->getFirstInstant(), ft2->getFirstInstant()));
    if (lastInstant-firstInstant > static_cast<int>(minFeatureTime)) { // equivalent to lastInstant-firstInstant+1 >= minFeatureTime
      if (ft->minMaxSimilarity(*ft2, firstInstant, lastInstant, connectionDistance, segmentationDistance)) {
	UndirectedGraph::edge_descriptor e;
	bool unused;
	tie(e, unused) = add_edge(newVertex, *vi, graph);
	// no need to add measures to graph[e] (edge properties)
      }
    }
  }
}

void FeatureGraph::connectedComponents(const int& lastInstant) {
  typedef boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
  vector<vertices_size_type> components(num_vertices(graph));
  // int num = connected_components(graph, &components[0]);
  // // todo change the type of the component map http://www.boost.org/doc/libs/1_48_0/libs/graph/doc/connected_components.html
  // cout << "Total number of components: " << num << endl;

  // for (unsigned int i = 0; i < components.size(); ++i)
  //     cout << "Vertex " << i <<" is in component " << components[i] << endl;
  // cout << endl;
}

string FeatureGraph::informationString(void) {
  stringstream ss;
  ss << num_vertices(graph) << " vertices, " << num_edges(graph) << " edges";
  return ss.str();
}