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

#include <vector>

#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Permutation.h>

#include <gtsam/CppUnitLite/TestHarness.h>

using namespace gtsam;
using namespace std;

TEST(VectorValues, standard) {
  Vector v1 = Vector_(3, 1.0,2.0,3.0);
  Vector v2 = Vector_(2, 4.0,5.0);
  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);

  vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
  VectorValues combined(dims);
  combined[0] = v1;
  combined[1] = v2;
  combined[2] = v3;

  CHECK(assert_equal(combined[0], v1))
  CHECK(assert_equal(combined[1], v2))
  CHECK(assert_equal(combined[2], v3))

  VectorValues incremental;
  incremental.reserve(3, 9);
  incremental.push_back_preallocated(v1);
  incremental.push_back_preallocated(v2);
  incremental.push_back_preallocated(v3);

  CHECK(assert_equal(incremental[0], v1))
  CHECK(assert_equal(incremental[1], v2))
  CHECK(assert_equal(incremental[2], v3))
}

TEST(VectorValues, permuted_combined) {
  Vector v1 = Vector_(3, 1.0,2.0,3.0);
  Vector v2 = Vector_(2, 4.0,5.0);
  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);

  vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
  VectorValues combined(dims);
  combined[0] = v1;
  combined[1] = v2;
  combined[2] = v3;

  Permutation perm1(3);
  perm1[0] = 1;
  perm1[1] = 2;
  perm1[2] = 0;

  Permutation perm2(3);
  perm2[0] = 1;
  perm2[1] = 2;
  perm2[2] = 0;

  Permuted<VectorValues> permuted1(combined);
  CHECK(assert_equal(v1, permuted1[0]))
  CHECK(assert_equal(v2, permuted1[1]))
  CHECK(assert_equal(v3, permuted1[2]))

  permuted1.permute(perm1);
  CHECK(assert_equal(v1, permuted1[2]))
  CHECK(assert_equal(v2, permuted1[0]))
  CHECK(assert_equal(v3, permuted1[1]))

  permuted1.permute(perm2);
  CHECK(assert_equal(v1, permuted1[1]))
  CHECK(assert_equal(v2, permuted1[2]))
  CHECK(assert_equal(v3, permuted1[0]))

  Permuted<VectorValues> permuted2(perm1, combined);
  CHECK(assert_equal(v1, permuted2[2]))
  CHECK(assert_equal(v2, permuted2[0]))
  CHECK(assert_equal(v3, permuted2[1]))

  permuted2.permute(perm2);
  CHECK(assert_equal(v1, permuted2[1]))
  CHECK(assert_equal(v2, permuted2[2]))
  CHECK(assert_equal(v3, permuted2[0]))

}

TEST(VectorValues, permuted_incremental) {
  Vector v1 = Vector_(3, 1.0,2.0,3.0);
  Vector v2 = Vector_(2, 4.0,5.0);
  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);

  VectorValues incremental;
  incremental.reserve(3, 9);
  incremental.push_back_preallocated(v1);
  incremental.push_back_preallocated(v2);
  incremental.push_back_preallocated(v3);

  Permutation perm1(3);
  perm1[0] = 1;
  perm1[1] = 2;
  perm1[2] = 0;

  Permutation perm2(3);
  perm2[0] = 1;
  perm2[1] = 2;
  perm2[2] = 0;

  Permuted<VectorValues> permuted1(incremental);
  CHECK(assert_equal(v1, permuted1[0]))
  CHECK(assert_equal(v2, permuted1[1]))
  CHECK(assert_equal(v3, permuted1[2]))

  permuted1.permute(perm1);
  CHECK(assert_equal(v1, permuted1[2]))
  CHECK(assert_equal(v2, permuted1[0]))
  CHECK(assert_equal(v3, permuted1[1]))

  permuted1.permute(perm2);
  CHECK(assert_equal(v1, permuted1[1]))
  CHECK(assert_equal(v2, permuted1[2]))
  CHECK(assert_equal(v3, permuted1[0]))

  Permuted<VectorValues> permuted2(perm1, incremental);
  CHECK(assert_equal(v1, permuted2[2]))
  CHECK(assert_equal(v2, permuted2[0]))
  CHECK(assert_equal(v3, permuted2[1]))

  permuted2.permute(perm2);
  CHECK(assert_equal(v1, permuted2[1]))
  CHECK(assert_equal(v2, permuted2[2]))
  CHECK(assert_equal(v3, permuted2[0]))

}

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