/*
 * testGenericGraph.cpp
 *
 *   Created on: Nov 23, 2010
 *       Author: nikai
 *  Description: unit tests for generic graph
 */

#include <map>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>

#include <gtsam_unstable/partition/GenericGraph.h>

using namespace std;
using namespace gtsam;
using namespace gtsam::partition;

/* ************************************************************************* */
/**
 *      l7               l9
 *     / | \            / |
 *   x1 -x2-x3 - l8 - x4- x5-x6
 */
TEST ( GenerciGraph, findIslands )
{
	GenericGraph2D graph;
	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));

	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;

	WorkSpace workspace(10); // from 0 to 9
	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
	LONGS_EQUAL(2, islands.size());
	vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
	vector<size_t> island2; island2 += 4, 5, 6, 9;
	CHECK(island1 == islands.front());
	CHECK(island2 == islands.back());
}

/* ************************************************************************* */
/**
 *         l7    l8
 *     / /  |  X  |  \
 *   x1 -x2-x3   x4- x5-x6
 */
TEST( GenerciGraph, findIslands2 )
{
	GenericGraph2D graph;
	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));

	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;

	WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
	LONGS_EQUAL(1, islands.size());
	vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
	CHECK(island1 == islands.front());
}

/* ************************************************************************* */
/**
 *     x1 - l5
 *     x2 - x3 - x4 - l6
 */
TEST ( GenerciGraph, findIslands3 )
{
	GenericGraph2D graph;
	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));

	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;

	WorkSpace workspace(7); // from 0 to 9
	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
	LONGS_EQUAL(2, islands.size());
	vector<size_t> island1; island1 += 1, 5;
	vector<size_t> island2; island2 += 2, 3, 4, 6;
	CHECK(island1 == islands.front());
	CHECK(island2 == islands.back());
}

/* ************************************************************************* */
/**
 *     x3 - l4 - x7
 */
TEST ( GenerciGraph, findIslands4 )
{
	GenericGraph2D graph;
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
	std::vector<size_t> keys; keys += 3, 4, 7;

	WorkSpace workspace(8); // from 0 to 7
	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
	LONGS_EQUAL(2, islands.size());
	vector<size_t> island1; island1 += 3, 4;
	vector<size_t> island2; island2 += 7;
	CHECK(island1 == islands.front());
	CHECK(island2 == islands.back());
}

/* ************************************************************************* */
/**
 *     x1 - l5 - x2
 *      | /    \ |
 *     x3        x4
 */
TEST ( GenerciGraph, findIslands5 )
{
	GenericGraph2D graph;
	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));

	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));

	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;

	WorkSpace workspace(6); // from 0 to 5
	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
	LONGS_EQUAL(2, islands.size());
	vector<size_t> island1; island1 += 1, 3, 5;
	vector<size_t> island2; island2 += 2, 4;
	CHECK(island1 == islands.front());
	CHECK(island2 == islands.back());
}

/* ************************************************************************* */
/**
 *     l3  l4  l5  l6
 *       \ | /  \  /
 *         x1    x2
 */
TEST ( GenerciGraph, reduceGenericGraph )
{
	GenericGraph3D graph;
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
	graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
	graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));

	std::vector<size_t> cameraKeys, landmarkKeys;
	cameraKeys.push_back(1);
	cameraKeys.push_back(2);
	landmarkKeys.push_back(3);
	landmarkKeys.push_back(4);
	landmarkKeys.push_back(5);
	landmarkKeys.push_back(6);

	std::vector<int> dictionary;
	dictionary.resize(7, -1); // from 0 to 6
	dictionary[1] = 0;
	dictionary[2] = 1;

	GenericGraph3D reduced;
	std::map<size_t, vector<size_t> > cameraToLandmarks;
	reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
	LONGS_EQUAL(1, reduced.size());
	LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
}

/* ************************************************************************* */
/**
 *     l3  l4  l5  l6
 *       \ | /  \  /
 *         x1    x2 - x7
 */
TEST ( GenericGraph, reduceGenericGraph2 )
{
	GenericGraph3D graph;
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
	graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
	graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
	graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
	graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));

	std::vector<size_t> cameraKeys, landmarkKeys;
	cameraKeys.push_back(1);
	cameraKeys.push_back(2);
	cameraKeys.push_back(7);
	landmarkKeys.push_back(3);
	landmarkKeys.push_back(4);
	landmarkKeys.push_back(5);
	landmarkKeys.push_back(6);

	std::vector<int> dictionary;
	dictionary.resize(8, -1); // from 0 to 7
	dictionary[1] = 0;
	dictionary[2] = 1;
	dictionary[7] = 6;

	GenericGraph3D reduced;
	std::map<size_t, vector<size_t> > cameraToLandmarks;
	reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
	LONGS_EQUAL(2, reduced.size());
	LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
	LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
}

/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera )
{
	std::set<size_t> cameras1;   cameras1 += 1, 2, 3, 4, 5;
	std::set<size_t> cameras2;   cameras2 += 8, 7, 6, 5;
	bool actual = hasCommonCamera(cameras1, cameras2);
	CHECK(actual);
}

/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera2 )
{
	std::set<size_t> cameras1;   cameras1 += 1, 3, 5, 7;
	std::set<size_t> cameras2;   cameras2 += 2, 4, 6, 8, 10;
	bool actual = hasCommonCamera(cameras1, cameras2);
	CHECK(!actual);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */