/*
 * testTupleConfig.cpp
 *
 *  Created on: Jan 13, 2010
 *      Author: richard
 */

#include <CppUnitLite/TestHarness.h>
#include <stdexcept>

#define GTSAM_MAGIC_KEY

#include <Pose2.h>
#include <Point2.h>

#include "Vector.h"
#include "Key.h"
#include "VectorConfig.h"
#include "TupleConfig-inl.h"

using namespace gtsam;
using namespace std;

typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;

/* ************************************************************************* */
TEST( PairConfig, insert_equals1 )
{
  Pose2 x1(1,2,3), x2(6,7,8);
  Point2 l1(4,5), l2(9,10);

  Config expected;
  expected.insert(1, x1);
  expected.insert(2, x2);
  expected.insert(1, l1);
  expected.insert(2, l2);

  Config actual;
  actual.insert(1, x1);
  actual.insert(2, x2);
  actual.insert(1, l1);
  actual.insert(2, l2);

  CHECK(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( PairConfig, insert_equals2 )
{
  Pose2 x1(1,2,3), x2(6,7,8);
  Point2 l1(4,5), l2(9,10);

  Config cfg1;
  cfg1.insert(1, x1);
  cfg1.insert(2, x2);
  cfg1.insert(1, l1);
  cfg1.insert(2, l2);

  Config cfg2;
  cfg2.insert(1, x1);
  cfg2.insert(2, x2);
  cfg2.insert(1, l1);

  CHECK(!cfg1.equals(cfg2));

  cfg2.insert(2, Point2(9,11));

  CHECK(!cfg1.equals(cfg2));
}

///* ************************************************************************* */
//TEST( PairConfig, insert_duplicate )
//{
//  Pose2 x1(1,2,3), x2(6,7,8);
//  Point2 l1(4,5), l2(9,10);
//
//  Config cfg1;
//  cfg1.insert(1, x1);
//  cfg1.insert(2, x2);
//  cfg1.insert(1, l1);
//  cfg1.insert(2, l2);
//  cfg1.insert(2, l1);
//
//  CHECK(assert_equal(l2, cfg1[PointKey(2)]));
//  CHECK(cfg1.size() == 4);
//  CHECK(cfg1.dim() == 10);
//}


/* ************************************************************************* */
TEST( PairConfig, size_dim )
{
  Pose2 x1(1,2,3), x2(6,7,8);
  Point2 l1(4,5), l2(9,10);

  Config cfg1;
  cfg1.insert(1, x1);
  cfg1.insert(2, x2);
  cfg1.insert(1, l1);
  cfg1.insert(2, l2);

  CHECK(cfg1.size() == 4);
  CHECK(cfg1.dim() == 10);
}

/* ************************************************************************* */
TEST(PairConfig, at)
{
  Pose2 x1(1,2,3), x2(6,7,8);
  Point2 l1(4,5), l2(9,10);

  Config cfg1;
  cfg1.insert(1, x1);
  cfg1.insert(2, x2);
  cfg1.insert(1, l1);
  cfg1.insert(2, l2);

  CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
  CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
  CHECK(assert_equal(l1, cfg1[PointKey(1)]));
  CHECK(assert_equal(l2, cfg1[PointKey(2)]));

  bool caught = false;
  try {
    cfg1[PoseKey(3)];
  } catch(invalid_argument e) {
    caught = true;
  }
  CHECK(caught);

  caught = false;
  try {
    cfg1[PointKey(3)];
  } catch(invalid_argument e) {
    caught = true;
  }
  CHECK(caught);
}

/* ************************************************************************* */
TEST(PairConfig, expmap)
{
  Pose2 x1(1,2,3), x2(6,7,8);
  Point2 l1(4,5), l2(9,10);

  Config cfg1;
  cfg1.insert(1, x1);
  cfg1.insert(2, x2);
  cfg1.insert(1, l1);
  cfg1.insert(2, l2);

  VectorConfig increment;
  increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
  increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
  increment.insert("l1", Vector_(2, 1.0, 1.1));
  increment.insert("l2", Vector_(2, 1.3, 1.4));

  Config expected;
  expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
  expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
  expected.insert(1, Point2(5.0, 6.1));
  expected.insert(2, Point2(10.3, 11.4));

  CHECK(assert_equal(expected, expmap(cfg1, increment)));
}

/* ************************************************************************* */

// some key types
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Vector, 'L'> LamKey;

// some config types
typedef LieConfig<PoseKey, Pose2> PoseConfig;
typedef LieConfig<PointKey, Point2> PointConfig;
typedef LieConfig<LamKey, Vector> LamConfig;

// some TupleConfig types
typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
typedef TupleConfig<PoseConfig, TupleConfig<PointConfig, TupleConfigEnd<LamConfig> > > ConfigB;

/* ************************************************************************* */
TEST(TupleConfig, create_insert) {
	// create some tuple configs
	ConfigA configA;
	ConfigB configB;

	PoseKey x1(1);
	PointKey l1(1);
	LamKey L1(1);
	Pose2 pose1(1.0, 2.0, 0.3);
	Point2 point1(2.0, 3.0);
	Vector lam1 = Vector_(1, 2.3);

	// Insert
	configA.insert(x1, pose1);
	configA.insert(l1, point1);

	configB.insert(x1, pose1);
	configB.insert(l1, point1);
	configB.insert(L1, lam1);

	// bracket operator - FAILS on config types after first one
	CHECK(assert_equal(configA[x1], pose1));
//	CHECK(assert_equal(configA[l1], point1));
	CHECK(assert_equal(configB[x1], pose1));
//	CHECK(assert_equal(configB[l1], point1));
//	CHECK(assert_equal(configB[L1], lam1));

	// exists
	CHECK(configA.exists(x1));
	CHECK(configA.exists(l1));
	CHECK(configB.exists(x1));
	CHECK(configB.exists(l1));
	CHECK(configB.exists(L1));

	// at - access function - FAILS as with bracket operator
	CHECK(assert_equal(configA.at(x1), pose1));
//	CHECK(assert_equal(configA.at(l1), point1));
	CHECK(assert_equal(configB.at(x1), pose1));
//	CHECK(assert_equal(configB.at(l1), point1));
//	CHECK(assert_equal(configB.at(L1), lam1));
}


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