#include "Motion.hpp"
#include "testutils.hpp"

#include "opencv2/core/core.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <iostream>

using namespace std;
using namespace cv;

BOOST_AUTO_TEST_SUITE(test_graph)

BOOST_AUTO_TEST_CASE(graph_add_connected_components) {
  FeatureGraph featureGraph(5, 1, 5 , 1.); // (float _connectionDistance, float _segmentationDistance, unsigned int _minFeatureTime, float _minNFeaturesPerGroup)
  unsigned int lastInstant = 20;
  FeatureTrajectoryPtr ft1 = createFeatureTrajectory(1, 10, lastInstant, Point2f(1,1), Point2f(0.5, 0.));
  FeatureTrajectoryPtr ft2 = createFeatureTrajectory(2, 10, lastInstant, Point2f(1.1,1), Point2f(0.5, 0.));

  featureGraph.addFeature(ft1);
  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 1);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 0);

  featureGraph.addFeature(ft2);
  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 2);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 1);

  featureGraph.connectedComponents(lastInstant);
  vector<vector<unsigned int> > components = featureGraph.getFeatureGroups();
  BOOST_CHECK_EQUAL(components.size(), 0);
  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 2);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 1);

  featureGraph.connectedComponents(lastInstant+1);
  components = featureGraph.getFeatureGroups();
  BOOST_CHECK_EQUAL(components.size(), 1);
  BOOST_CHECK_EQUAL(components[0].size(), 2);
  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 0);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 0);

  // test connection distance
  featureGraph.addFeature(ft1);
  featureGraph.addFeature(ft2);
  FeatureTrajectoryPtr ft3 = createFeatureTrajectory(3, 10, lastInstant, Point2f(6.05,1), Point2f(0.5, 0.)); // connected to ft2 only
  featureGraph.addFeature(ft3);
  FeatureTrajectoryPtr ft4 = createFeatureTrajectory(4, 10, lastInstant, Point2f(11.1,1), Point2f(0.5, 0.)); // not connected
  featureGraph.addFeature(ft4);

  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 4);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 2);

  featureGraph.connectedComponents(lastInstant+1);
  components = featureGraph.getFeatureGroups();
  BOOST_CHECK_EQUAL(components.size(), 2);
  BOOST_CHECK_EQUAL(components[0].size(), 3);
  BOOST_CHECK_EQUAL(components[1].size(), 1);
  BOOST_CHECK_EQUAL(featureGraph.getNVertices(), 0);
  BOOST_CHECK_EQUAL(featureGraph.getNEdges(), 0);
}

BOOST_AUTO_TEST_SUITE_END()