/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testVectorValues.cpp
 * @author  Richard Roberts
 * @date    Sep 16, 2010
 */

#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/map.hpp>

using namespace std;
using namespace boost::assign;
using boost::adaptors::map_keys;
using namespace gtsam;

/* ************************************************************************* */
TEST(VectorValues, basics)
{
  // Tests insert(), size(), dim(), exists(), vector()

  // insert
  VectorValues actual;
  actual.insert(0, (Vector(1) << 1));
  actual.insert(1, (Vector(2) << 2, 3));
  actual.insert(5, (Vector(2) << 6, 7));
  actual.insert(2, (Vector(2) << 4, 5));

  // Check dimensions
  LONGS_EQUAL(4, actual.size());
  LONGS_EQUAL(1, actual.dim(0));
  LONGS_EQUAL(2, actual.dim(1));
  LONGS_EQUAL(2, actual.dim(2));
  LONGS_EQUAL(2, actual.dim(5));

  // Logic
  EXPECT(actual.exists(0));
  EXPECT(actual.exists(1));
  EXPECT(actual.exists(2));
  EXPECT(!actual.exists(3));
  EXPECT(!actual.exists(4));
  EXPECT(actual.exists(5));
  EXPECT(!actual.exists(6));

  // Check values
  EXPECT(assert_equal((Vector(1) << 1), actual[0]));
  EXPECT(assert_equal((Vector(2) << 2, 3), actual[1]));
  EXPECT(assert_equal((Vector(2) << 4, 5), actual[2]));
  EXPECT(assert_equal((Vector(2) << 6, 7), actual[5]));
  FastVector<Key> keys = list_of(0)(1)(2)(5);
  EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7), actual.vector(keys)));

  // Check exceptions
  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
  CHECK_EXCEPTION(actual.dim(3), out_of_range);
}

/* ************************************************************************* */
TEST(VectorValues, combine)
{
  VectorValues expected;
  expected.insert(0, (Vector(1) << 1));
  expected.insert(1, (Vector(2) << 2, 3));
  expected.insert(5, (Vector(2) << 6, 7));
  expected.insert(2, (Vector(2) << 4, 5));

  VectorValues first;
  first.insert(0, (Vector(1) << 1));
  first.insert(1, (Vector(2) << 2, 3));

  VectorValues second;
  second.insert(5, (Vector(2) << 6, 7));
  second.insert(2, (Vector(2) << 4, 5));

  VectorValues actual(first, second);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(VectorValues, subvector)
{
  VectorValues init;
  init.insert(10, (Vector(1) << 1));
  init.insert(11, (Vector(2) << 2, 3));
  init.insert(12, (Vector(2) << 4, 5));
  init.insert(13, (Vector(2) << 6, 7));

  std::vector<Key> keys;
  keys += 10, 12, 13;
  Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7);
  EXPECT(assert_equal(expSubVector, init.vector(keys)));
}

/* ************************************************************************* */
TEST(VectorValues, LinearAlgebra)
{
  VectorValues test1;
  test1.insert(0, (Vector(1) << 1));
  test1.insert(1, (Vector(2) << 2, 3));
  test1.insert(5, (Vector(2) << 6, 7));
  test1.insert(2, (Vector(2) << 4, 5));

  VectorValues test2;
  test2.insert(0, (Vector(1) << 6));
  test2.insert(1, (Vector(2) << 1, 6));
  test2.insert(5, (Vector(2) << 4, 3));
  test2.insert(2, (Vector(2) << 1, 8));

  // Dot product
  double dotExpected = test1.vector().dot(test2.vector());
  double dotActual = test1.dot(test2);
  DOUBLES_EQUAL(dotExpected, dotActual, 1e-10);

  // Norm
  double normExpected = test1.vector().norm();
  double normActual = test1.norm();
  DOUBLES_EQUAL(normExpected, normActual, 1e-10);

  // Squared norm
  double sqNormExpected = test1.vector().norm();
  double sqNormActual = test1.norm();
  DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10);

  // Addition
  Vector sumExpected = test1.vector() + test2.vector();
  VectorValues sumActual = test1 + test2;
  EXPECT(sumActual.hasSameStructure(test1));
  EXPECT(assert_equal(sumExpected, sumActual.vector()));
  Vector sum2Expected = sumActual.vector() + test1.vector();
  VectorValues sum2Actual = sumActual;
  sum2Actual += test1;
  EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));

  // Add to empty
  VectorValues sumActual3;
  sumActual3.addInPlace_(test1);
  sumActual3.addInPlace_(test2);
  EXPECT(assert_equal(sumExpected, sumActual3.vector()));

  // Subtraction
  Vector difExpected = test1.vector() - test2.vector();
  VectorValues difActual = test1 - test2;
  EXPECT(difActual.hasSameStructure(test1));
  EXPECT(assert_equal(difExpected, difActual.vector()));

  // Scaling
  Vector scalExpected = test1.vector() * 5.0;
  VectorValues scalActual = test1;
  scalActual *= 5.0;
  EXPECT(assert_equal(scalExpected, scalActual.vector()));
  VectorValues scal2Actual = 5.0 * test1;
  EXPECT(assert_equal(scalExpected, scal2Actual.vector()));
}

/* ************************************************************************* */
TEST(VectorValues, convert)
{
  Vector x(7);
  x << 1, 2, 3, 4, 5, 6, 7;

  VectorValues expected;
  expected.insert(0, (Vector(1) << 1));
  expected.insert(1, (Vector(2) << 2, 3));
  expected.insert(2, (Vector(2) << 4, 5));
  expected.insert(5, (Vector(2) << 6, 7));

  std::map<Key,size_t> dims;
  dims.insert(make_pair(0,1));
  dims.insert(make_pair(1,2));
  dims.insert(make_pair(2,2));
  dims.insert(make_pair(5,2));
  VectorValues actual(x,dims);
  EXPECT(assert_equal(expected, actual));

  // Test other direction, note vector() is not guaranteed to give right result
  FastVector<Key> keys = list_of(0)(1)(2)(5);
  EXPECT(assert_equal(x, actual.vector(keys)));

  // Test version with dims argument
  EXPECT(assert_equal(x, actual.vector(dims)));
}

/* ************************************************************************* */
TEST(VectorValues, vector_sub)
{
  VectorValues vv;
  vv.insert(0, (Vector(1) << 1));
  vv.insert(1, (Vector(2) << 2, 3));
  vv.insert(2, (Vector(2) << 4, 5));
  vv.insert(5, (Vector(2) << 6, 7));
  vv.insert(7, (Vector(2) << 8, 9));

  std::map<Key,size_t> dims;
  dims.insert(make_pair(0,1));
  dims.insert(make_pair(5,2));

  Vector expected(3);
  expected << 1, 6, 7;

  // Test FastVector version
  FastVector<Key> keys = list_of(0)(5);
  EXPECT(assert_equal(expected, vv.vector(keys)));

  // Test version with dims argument
  EXPECT(assert_equal(expected, vv.vector(dims)));
}

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