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

#include "opencv2/core/core.hpp"

#include "catch.hpp"

#include <iostream>

using namespace std;
using namespace cv;

TEST_CASE("graph/connected_components", "test graph 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);
  REQUIRE(featureGraph.getNVertices() == 1);
  REQUIRE(featureGraph.getNEdges() == 0);

  featureGraph.addFeature(ft2);
  REQUIRE(featureGraph.getNVertices() == 2);
  REQUIRE(featureGraph.getNEdges() == 1);

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

  featureGraph.connectedComponents(lastInstant+1);
  components = featureGraph.getFeatureGroups();
  REQUIRE(components.size() == 1);
  REQUIRE(components[0].size() == 2);
  REQUIRE(featureGraph.getNVertices() == 0);
  REQUIRE(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);

  REQUIRE(featureGraph.getNVertices() == 4);
  REQUIRE(featureGraph.getNEdges() == 2);

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