/**
 * @file tests/math_test.cpp
 * @author Ryan Curtin
 *
 * Tests for everything in the math:: namespace.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core/math/clamp.hpp>
#include <mlpack/core/math/random.hpp>
#include <mlpack/core/math/range.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace math;

BOOST_AUTO_TEST_SUITE(MathTest);

/**
 * Verify that the empty constructor creates an empty range.
 */
BOOST_AUTO_TEST_CASE(RangeEmptyConstructor)
{
  Range x = Range();

  // Just verify that it is empty.
  BOOST_REQUIRE_GT(x.Lo(), x.Hi());
}

/**
 * Verify that the point constructor correctly creates a range that is just a
 * point.
 */
BOOST_AUTO_TEST_CASE(RangePointConstructor)
{
  Range x(10.0);

  BOOST_REQUIRE_CLOSE(x.Lo(), x.Hi(), 1e-25);
  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);
  BOOST_REQUIRE_CLOSE(x.Lo(), 10.0, 1e-25);
  BOOST_REQUIRE_CLOSE(x.Hi(), 10.0, 1e-25);
}

/**
 * Verify that the range constructor correctly creates the range.
 */
BOOST_AUTO_TEST_CASE(RangeConstructor)
{
  Range x(0.5, 5.5);

  BOOST_REQUIRE_CLOSE(x.Lo(), 0.5, 1e-25);
  BOOST_REQUIRE_CLOSE(x.Hi(), 5.5, 1e-25);
}

/**
 * Test that we get the width correct.
 */
BOOST_AUTO_TEST_CASE(RangeWidth)
{
  Range x(0.0, 10.0);

  BOOST_REQUIRE_CLOSE(x.Width(), 10.0, 1e-20);

  // Make it empty.
  x.Hi() = 0.0;

  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);

  // Make it negative.
  x.Hi() = -2.0;

  BOOST_REQUIRE_SMALL(x.Width(), 1e-5);

  // Just one more test.
  x.Lo() = -5.2;
  x.Hi() = 5.2;

  BOOST_REQUIRE_CLOSE(x.Width(), 10.4, 1e-5);
}

/**
 * Test that we get the midpoint correct.
 */
BOOST_AUTO_TEST_CASE(RangeMidpoint)
{
  Range x(0.0, 10.0);

  BOOST_REQUIRE_CLOSE(x.Mid(), 5.0, 1e-5);

  x.Lo() = -5.0;

  BOOST_REQUIRE_CLOSE(x.Mid(), 2.5, 1e-5);
}

/**
 * Test that we can expand to include other ranges correctly.
 */
BOOST_AUTO_TEST_CASE(RangeIncludeOther)
{
  // We need to test both |= and |.
  // We have three cases: non-overlapping; overlapping; equivalent, and then a
  // couple permutations (switch left with right and make sure it still works).
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  Range z(0.0, 2.0); // Used for operator|=().
  Range w;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  // Switch operator precedence.
  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  // Now make them overlapping.
  x = Range(0.0, 3.5);
  y = Range(3.0, 4.0);

  z = x;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  // Switch operator precedence.
  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  // Now the equivalent case.
  x = Range(0.0, 2.0);
  y = Range(0.0, 2.0);

  z = x;
  z |= y;
  w = x | y;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);

  z = y;
  z |= x;
  w = y | x;

  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);
}

/**
 * Test that we can 'and' ranges correctly.
 */
BOOST_AUTO_TEST_CASE(RangeIntersectOther)
{
  // We need to test both &= and &.
  // We have three cases: non-overlapping, overlapping; equivalent, and then a
  // couple permutations (switch left with right and make sure it still works).
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  Range z(0.0, 2.0);
  Range w;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_SMALL(z.Width(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Width(), 1e-5);

  // Reverse operator precedence.
  z = y;
  z &= x;
  w = y & x;

  BOOST_REQUIRE_SMALL(z.Width(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Width(), 1e-5);

  // Now make them overlapping.
  x = Range(0.0, 3.5);
  y = Range(3.0, 4.0);

  z = x;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 3.5, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 3.5, 1e-5);

  // Reverse operator precedence.
  z = y;
  z &= x;
  w = y & x;

  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 3.5, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 3.5, 1e-5);

  // Now make them equivalent.
  x = Range(2.0, 4.0);
  y = Range(2.0, 4.0);

  z = x;
  z &= y;
  w = x & y;

  BOOST_REQUIRE_CLOSE(z.Lo(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);
}

/**
 * Test multiplication of a range with a double.
 */
BOOST_AUTO_TEST_CASE(RangeMultiply)
{
  // We need to test both * and *=, as well as both cases of *.
  // We'll try with a couple of numbers: -1, 0, 2.
  // And we'll have a couple of cases for bounds: strictly less than zero;
  // including zero; and strictly greater than zero.
  //
  // So, nine total cases.
  Range x(-5.0, -3.0);
  Range y(-5.0, -3.0);
  Range z;
  Range w;

  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 5.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), -6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), -6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), -6.0, 1e-5);

  x = Range(-2.0, 2.0);
  y = x;

  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -2.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 2.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -4.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 4.0, 1e-5);

  x = Range(3.0, 5.0);

  y = x;
  y *= -1.0;
  z = x * -1.0;
  w = -1.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), -3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), -3.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), -5.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), -3.0, 1e-5);

  y = x;
  y *= 0.0;
  z = x * 0.0;
  w = 0.0 * x;

  BOOST_REQUIRE_SMALL(y.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(y.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(z.Hi(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Lo(), 1e-5);
  BOOST_REQUIRE_SMALL(w.Hi(), 1e-5);

  y = x;
  y *= 2.0;
  z = x * 2.0;
  w = 2.0 * x;

  BOOST_REQUIRE_CLOSE(y.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(y.Hi(), 10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(z.Hi(), 10.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Lo(), 6.0, 1e-5);
  BOOST_REQUIRE_CLOSE(w.Hi(), 10.0, 1e-5);
}

/**
 * Test equality operator.
 */
BOOST_AUTO_TEST_CASE(RangeEquality)
{
  // Three cases: non-overlapping, overlapping, equivalent.  We should also
  // consider empty ranges, which are not necessarily equal...
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  // These are odd calls, but we don't want to use operator!= here.
  BOOST_REQUIRE_EQUAL((x == y), false);
  BOOST_REQUIRE_EQUAL((y == x), false);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x == y), false);
  BOOST_REQUIRE_EQUAL((y == x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x == y), true);
  BOOST_REQUIRE_EQUAL((y == x), true);

  x = Range(1.0, -1.0); // Empty.
  y = Range(1.0, -1.0); // Also empty.

  BOOST_REQUIRE_EQUAL((x == y), true);
  BOOST_REQUIRE_EQUAL((y == x), true);

  // No need to test what it does if the empty ranges are different "ranges"
  // because we are not forcing behavior for that.
}

/**
 * Test inequality operator.
 */
BOOST_AUTO_TEST_CASE(RangeInequality)
{
  // We will use the same three cases as the RangeEquality test.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  // Again, odd calls, but we want to force use of operator!=.
  BOOST_REQUIRE_EQUAL((x != y), true);
  BOOST_REQUIRE_EQUAL((y != x), true);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x != y), true);
  BOOST_REQUIRE_EQUAL((y != x), true);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x != y), false);
  BOOST_REQUIRE_EQUAL((y != x), false);

  x = Range(1.0, -1.0); // Empty.
  y = Range(1.0, -1.0); // Also empty.

  BOOST_REQUIRE_EQUAL((x != y), false);
  BOOST_REQUIRE_EQUAL((y != x), false);
}

/**
 * Test strict less-than operator.
 */
BOOST_AUTO_TEST_CASE(RangeStrictLessThan)
{
  // Three cases: non-overlapping, overlapping, and equivalent.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  BOOST_REQUIRE_EQUAL((x < y), true);
  BOOST_REQUIRE_EQUAL((y < x), false);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x < y), false);
  BOOST_REQUIRE_EQUAL((y < x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x < y), false);
  BOOST_REQUIRE_EQUAL((y < x), false);
}

/**
 * Test strict greater-than operator.
 */
BOOST_AUTO_TEST_CASE(RangeStrictGreaterThan)
{
  // Three cases: non-overlapping, overlapping, and equivalent.
  Range x(0.0, 2.0);
  Range y(3.0, 5.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), true);

  y = Range(1.0, 3.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), false);

  y = Range(0.0, 2.0);

  BOOST_REQUIRE_EQUAL((x > y), false);
  BOOST_REQUIRE_EQUAL((y > x), false);
}

/**
 * Test the Contains() operator.
 */
BOOST_AUTO_TEST_CASE(RangeContains)
{
  // We have three Range cases: strictly less than 0; overlapping 0; and
  // strictly greater than 0.  Then the numbers we check can be the same three
  // cases, including one greater than and one less than the range.  This should
  // be about 15 total cases.
  Range x(-2.0, -1.0);

  BOOST_REQUIRE(!x.Contains(-3.0));
  BOOST_REQUIRE(x.Contains(-2.0));
  BOOST_REQUIRE(x.Contains(-1.5));
  BOOST_REQUIRE(x.Contains(-1.0));
  BOOST_REQUIRE(!x.Contains(-0.5));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(1.0));

  x = Range(-1.0, 1.0);

  BOOST_REQUIRE(!x.Contains(-2.0));
  BOOST_REQUIRE(x.Contains(-1.0));
  BOOST_REQUIRE(x.Contains(0.0));
  BOOST_REQUIRE(x.Contains(1.0));
  BOOST_REQUIRE(!x.Contains(2.0));

  x = Range(1.0, 2.0);

  BOOST_REQUIRE(!x.Contains(-1.0));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(0.5));
  BOOST_REQUIRE(x.Contains(1.0));
  BOOST_REQUIRE(x.Contains(1.5));
  BOOST_REQUIRE(x.Contains(2.0));
  BOOST_REQUIRE(!x.Contains(2.5));

  // Now let's try it on an empty range.
  x = Range();

  BOOST_REQUIRE(!x.Contains(-10.0));
  BOOST_REQUIRE(!x.Contains(0.0));
  BOOST_REQUIRE(!x.Contains(10.0));

  // And an infinite range.
  x = Range(-DBL_MAX, DBL_MAX);

  BOOST_REQUIRE(x.Contains(-10.0));
  BOOST_REQUIRE(x.Contains(0.0));
  BOOST_REQUIRE(x.Contains(10.0));
}

/**
 * Test that Range::Contains() works on other Ranges.  It should return false
 * unless the ranges overlap at all.
 */
BOOST_AUTO_TEST_CASE(RangeContainsRange)
{
  // Empty ranges should not contain each other.
  Range a;
  Range b;

  BOOST_REQUIRE_EQUAL(a.Contains(b), false);
  BOOST_REQUIRE_EQUAL(b.Contains(a), false);

  // Completely disparate ranges.
  a = Range(-5.0, -3.0);
  b = Range(3.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), false);
  BOOST_REQUIRE_EQUAL(b.Contains(a), false);

  // Overlapping at the end-point; this is containment of the end point.
  a = Range(-5.0, 0.0);
  b = Range(0.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Partially overlapping.
  a = Range(-5.0, 2.0);
  b = Range(-2.0, 5.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // One range encloses the other.
  a = Range(-5.0, 5.0);
  b = Range(-3.0, 3.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Identical ranges.
  a = Range(-3.0, 3.0);
  b = Range(-3.0, 3.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);

  // Single-point ranges.
  a = Range(0.0, 0.0);
  b = Range(0.0, 0.0);

  BOOST_REQUIRE_EQUAL(a.Contains(b), true);
  BOOST_REQUIRE_EQUAL(b.Contains(a), true);
}

/**
 * Make sure shuffling data works.
 */
BOOST_AUTO_TEST_CASE(ShuffleTest)
{
  arma::mat data(3, 10, arma::fill::zeros);
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
  }

  arma::mat outputData;
  arma::Row<size_t> outputLabels;

  ShuffleData(data, labels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_SMALL(outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL(outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], 1);
}

/**
 * Make sure shuffling sparse data works.
 */
BOOST_AUTO_TEST_CASE(SparseShuffleTest)
{
  arma::sp_mat data(3, 10);
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
  }
  // This appears to be a necessary workaround for an Armadillo 8 bug.
  data *= 1.0;

  arma::sp_mat outputData;
  arma::Row<size_t> outputLabels;

  ShuffleData(data, labels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_SMALL((double) outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL((double) outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], 1);
}

/**
 * Make sure shuffling cubes works.
 */
BOOST_AUTO_TEST_CASE(CubeShuffleTest)
{
  arma::cube data(3, 10, 5, arma::fill::zeros);
  arma::cube labels(1, 10, 5);
  for (size_t i = 0; i < labels.n_slices; ++i)
  {
    for (size_t j = 0; j < labels.n_cols; ++j)
    {
      data(0, j, i) = i;
      data(1, j, i) = j;
      labels(0, j, i) = j + i;
    }
  }

  arma::cube outputData, outputLabels;

  ShuffleData(data, labels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputData.n_slices, data.n_slices);
  BOOST_REQUIRE_EQUAL(outputLabels.n_rows, labels.n_rows);
  BOOST_REQUIRE_EQUAL(outputLabels.n_cols, labels.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_slices, labels.n_slices);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    for (size_t s = 0; s < data.n_slices; ++s)
    {
      BOOST_REQUIRE_EQUAL(data(0, i, s) + data(1, i, s), labels(0, i, s));
      BOOST_REQUIRE_SMALL(data(2, i, s), 1e-5);
      counts[data(1, i, s)]++;
    }
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], data.n_slices);
}

/**
 * Make sure shuffling data with weights works.
 */
BOOST_AUTO_TEST_CASE(ShuffleWeightsTest)
{
  arma::mat data(3, 10, arma::fill::zeros);
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
    weights[i] = i;
  }

  arma::mat outputData;
  arma::Row<size_t> outputLabels;
  arma::rowvec outputWeights;

  ShuffleData(data, labels, weights, outputData, outputLabels, outputWeights);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);
  BOOST_REQUIRE_EQUAL(outputWeights.n_elem, weights.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  arma::Row<size_t> weightCounts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), (size_t) outputWeights[i]);
    BOOST_REQUIRE_SMALL(outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL(outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
    weightCounts[(size_t) outputWeights[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL(counts[i], 1);
    BOOST_REQUIRE_EQUAL(weightCounts[i], 1);
  }
}

/**
 * Make sure shuffling sparse data with weights works.
 */
BOOST_AUTO_TEST_CASE(SparseShuffleWeightsTest)
{
  arma::sp_mat data(3, 10);
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
    weights[i] = i;
  }
  // This appears to be a necessary workaround for an Armadillo 8 bug.
  data *= 1.0;

  arma::sp_mat outputData;
  arma::Row<size_t> outputLabels;
  arma::rowvec outputWeights;

  ShuffleData(data, labels, weights, outputData, outputLabels, outputWeights);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);
  BOOST_REQUIRE_EQUAL(outputWeights.n_elem, weights.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  arma::Row<size_t> weightCounts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), (size_t) outputWeights[i]);
    BOOST_REQUIRE_SMALL((double) outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL((double) outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
    weightCounts[(size_t) outputWeights[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL(counts[i], 1);
    BOOST_REQUIRE_EQUAL(weightCounts[i], 1);
  }
}

/**
 * Make sure shuffling data works when the same matrices are given as input and
 * output.
 */
BOOST_AUTO_TEST_CASE(InplaceShuffleTest)
{
  arma::mat data(3, 10, arma::fill::zeros);
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
  }

  arma::mat outputData(data);
  arma::Row<size_t> outputLabels(labels);

  ShuffleData(outputData, outputLabels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_SMALL(outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL(outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], 1);
}

/**
 * Make sure shuffling sparse data works when the input and output matrices are
 * the same.
 */
BOOST_AUTO_TEST_CASE(InplaceSparseShuffleTest)
{
  arma::sp_mat data(3, 10);
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
  }

  arma::sp_mat outputData(data);
  arma::Row<size_t> outputLabels(labels);

  ShuffleData(outputData, outputLabels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_SMALL((double) outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL((double) outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], 1);
}

/**
 * Make sure shuffling cubes works when the input and output cubes are the same.
 */
BOOST_AUTO_TEST_CASE(InplaceCubeShuffleTest)
{
  arma::cube data(3, 10, 5, arma::fill::zeros);
  arma::cube labels(1, 10, 5);
  for (size_t i = 0; i < labels.n_slices; ++i)
  {
    for (size_t j = 0; j < labels.n_cols; ++j)
    {
      data(0, j, i) = i;
      data(1, j, i) = j;
      labels(0, j, i) = j + i;
    }
  }

  arma::cube outputData(data), outputLabels(labels);

  ShuffleData(outputData, outputLabels, outputData, outputLabels);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputData.n_slices, data.n_slices);
  BOOST_REQUIRE_EQUAL(outputLabels.n_rows, labels.n_rows);
  BOOST_REQUIRE_EQUAL(outputLabels.n_cols, labels.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_slices, labels.n_slices);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    for (size_t s = 0; s < data.n_slices; ++s)
    {
      BOOST_REQUIRE_EQUAL(data(0, i, s) + data(1, i, s), labels(0, i, s));
      BOOST_REQUIRE_SMALL(data(2, i, s), 1e-5);
      counts[data(1, i, s)]++;
    }
  }

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(counts[i], data.n_slices);
}

/**
 * Make sure shuffling data with weights works when the same matrices are given
 * as input and output.
 */
BOOST_AUTO_TEST_CASE(InplaceShuffleWeightsTest)
{
  arma::mat data(3, 10, arma::fill::zeros);
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
    weights[i] = i;
  }

  arma::mat outputData(data);
  arma::Row<size_t> outputLabels(labels);
  arma::rowvec outputWeights(weights);

  ShuffleData(outputData, outputLabels, outputWeights, outputData, outputLabels,
      outputWeights);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);
  BOOST_REQUIRE_EQUAL(outputWeights.n_elem, weights.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  arma::Row<size_t> weightCounts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), (size_t) outputWeights[i]);
    BOOST_REQUIRE_SMALL(outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL(outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
    weightCounts[(size_t) outputWeights[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL(counts[i], 1);
    BOOST_REQUIRE_EQUAL(weightCounts[i], 1);
  }
}

/**
 * Make sure shuffling sparse data with weights works when the input and output
 * matrices are the same.
 */
BOOST_AUTO_TEST_CASE(InplaceSparseShuffleWeightsTest)
{
  arma::sp_mat data(3, 10);
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  for (size_t i = 0; i < 10; ++i)
  {
    data(0, i) = i;
    labels[i] = i;
    weights[i] = i;
  }

  arma::sp_mat outputData(data);
  arma::Row<size_t> outputLabels(labels);
  arma::rowvec outputWeights(weights);

  ShuffleData(outputData, outputLabels, outputWeights, outputData, outputLabels,
      outputWeights);

  BOOST_REQUIRE_EQUAL(outputData.n_rows, data.n_rows);
  BOOST_REQUIRE_EQUAL(outputData.n_cols, data.n_cols);
  BOOST_REQUIRE_EQUAL(outputLabels.n_elem, labels.n_elem);
  BOOST_REQUIRE_EQUAL(outputWeights.n_elem, weights.n_elem);

  // Make sure we only have each point once.
  arma::Row<size_t> counts(10, arma::fill::zeros);
  arma::Row<size_t> weightCounts(10, arma::fill::zeros);
  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), outputLabels[i]);
    BOOST_REQUIRE_EQUAL((size_t) outputData(0, i), (size_t) outputWeights[i]);
    BOOST_REQUIRE_SMALL((double) outputData(1, i), 1e-5);
    BOOST_REQUIRE_SMALL((double) outputData(2, i), 1e-5);
    counts[outputLabels[i]]++;
    weightCounts[(size_t) outputWeights[i]]++;
  }

  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_EQUAL(counts[i], 1);
    BOOST_REQUIRE_EQUAL(weightCounts[i], 1);
  }
}

BOOST_AUTO_TEST_SUITE_END();


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

#include <mitkTestingMacros.h>
#include <mitkLogMacros.h>
#include <mitkCameraCalibrationFacade.h>
#include <mitkOpenCVFileIOUtils.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

/**
 * Test for stereo trianglulation and projection. Start with a realistic set
 * of 3D points defined relative to the left lens. Project them to screen space,
 * then Triangulate them back to world space
 * pair, triangulate them to lens space, then project them back to onscreen
 * coordinates, they should match
 */

int mitkReprojectionTest ( int argc, char * argv[] )
{

  std::string calibrationDirectory = "";
  double pixelNoise = 0.0;
  bool quantize = false;
  double featureDepth = 50;
  bool cropNonVisiblePoints = true;
  double screenWidth = 1980;
  double screenHeight = 540;
  double cropValue = std::numeric_limits<double>::quiet_NaN();

  bool ok;
  while ( argc > 1 )
  {
    ok = false;
    if (( ok == false ) && (strcmp(argv[1],"--calibration") == 0 ))
    {
      argc--;
      argv++;
      calibrationDirectory = argv[1];
      MITK_INFO << "Loading calibration from " << calibrationDirectory;
      argc--;
      argv++;
      ok=true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--pixelNoise") == 0 ))
    {
      argc--;
      argv++;
      pixelNoise = atof(argv[1]);
      argc--;
      argv++;
      ok =true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--quantize") == 0 ))
    {
      argc--;
      argv++;
      quantize=true;
      ok=true;
    }
    if (( ok == false ) && ( strcmp (argv[1],"--featureDepth") == 0 ))
    {
      argc--;
      argv++;
      featureDepth = atof(argv[1]);
      argc--;
      argv++;
      ok=true;
    }
    if ( ok == false )
    {
      MITK_ERROR << "Failed to parse arguments";
      return EXIT_FAILURE;
    }
  }

  MITK_TEST_BEGIN("mitkReprojectionTest");
  cv::Mat leftCameraPositionToFocalPointUnitVector = cv::Mat(1,3,CV_64FC1);
  cv::Mat leftCameraIntrinsic = cv::Mat(3,3,CV_64FC1);
  cv::Mat leftCameraDistortion = cv::Mat(1,4,CV_64FC1);
  cv::Mat rightCameraIntrinsic = cv::Mat(3,3,CV_64FC1);
  cv::Mat rightCameraDistortion = cv::Mat(1,4,CV_64FC1);
  cv::Mat rightToLeftRotationMatrix = cv::Mat(3,3,CV_64FC1);
  cv::Mat rightToLeftTranslationVector = cv::Mat(3,1,CV_64FC1);
  cv::Mat leftCameraToTracker = cv::Mat(4,4,CV_64FC1);

  mitk::LoadStereoCameraParametersFromDirectory (calibrationDirectory,
     &leftCameraIntrinsic,&leftCameraDistortion,&rightCameraIntrinsic,
     &rightCameraDistortion,&rightToLeftRotationMatrix,
     &rightToLeftTranslationVector,&leftCameraToTracker);

  // Section to output reprojected image size
  cv::Point2d topLeft = cv::Point2d (0,0);
  cv::Point2d bottomRight = cv::Point2d (screenWidth, screenHeight);
  cv::Point2d undistortedTopLeft;
  cv::Point2d undistortedBottomRight;
  mitk::UndistortPoint ( topLeft, leftCameraIntrinsic, leftCameraDistortion, undistortedTopLeft);
  mitk::UndistortPoint ( bottomRight, leftCameraIntrinsic, leftCameraDistortion, undistortedBottomRight);

  cv::Point3d reProjectedTopLeft = mitk::ReProjectPoint (undistortedTopLeft, leftCameraIntrinsic);
  cv::Point3d reProjectedBottomRight = mitk::ReProjectPoint (undistortedBottomRight, leftCameraIntrinsic);

  reProjectedTopLeft.x *= featureDepth;
  reProjectedTopLeft.y *= featureDepth;
  reProjectedTopLeft.z *= featureDepth;

  reProjectedBottomRight.x *= featureDepth;
  reProjectedBottomRight.y *= featureDepth;
  reProjectedBottomRight.z *= featureDepth;

  double diagonalSize = sqrt (
       pow ( (reProjectedTopLeft.x - reProjectedBottomRight.x),2.0) +
       pow ( (reProjectedTopLeft.y - reProjectedBottomRight.y),2.0) );

  MITK_INFO << "Top Left = " << reProjectedTopLeft;
  MITK_INFO << "Bottom Right = " << reProjectedBottomRight;
  MITK_INFO << "Length of Diagonal = " << diagonalSize;
  // End of reprojected image size bit

  CvMat* outputLeftCameraWorldPointsIn3D = NULL;
  CvMat* outputLeftCameraWorldNormalsIn3D = NULL ;
  CvMat* output2DPointsLeft = NULL ;
  CvMat* output2DPointsRight = NULL;

  CvMat* outputTrimmedLeftCameraWorldPointsIn3D = NULL;
  CvMat* outputTrimmedLeftCameraWorldNormalsIn3D = NULL ;
  CvMat* outputTrimmed2DPointsLeft = NULL ;
  CvMat* outputTrimmed2DPointsRight = NULL;

  int numberOfPoints = 2601;
  cv::Mat leftCameraWorldPoints = cv::Mat (numberOfPoints,3,CV_64FC1);
  cv::Mat leftCameraWorldNormals = cv::Mat (numberOfPoints,3,CV_64FC1);

  CvMat* leftCameraTriangulatedWorldPoints_m1 = cvCreateMat (numberOfPoints,3,CV_64FC1);
  cv::Mat leftScreenPoints = cv::Mat (numberOfPoints,2,CV_64FC1);
  cv::Mat rightScreenPoints = cv::Mat (numberOfPoints,2,CV_64FC1);

  CvMat* leftCameraTriangulatedWorldPoints_trimmed_m1 = cvCreateMat (numberOfPoints,3,CV_64FC1);
  cv::Mat leftTrimmedScreenPoints = cv::Mat (numberOfPoints,2,CV_64FC1);
  cv::Mat rightTrimmedScreenPoints = cv::Mat (numberOfPoints,2,CV_64FC1);

  for ( int row = 0 ; row < 51 ; row ++ )
  {
    for ( int col = 0 ; col < 51 ; col ++ )
    {
      leftCameraWorldPoints.at<double>(row * 51 + col, 0) = -25 + (col);
      leftCameraWorldPoints.at<double>(row * 51 + col, 1) = -25 + (row);
      leftCameraWorldPoints.at<double>(row * 51 + col, 2) = featureDepth;
      leftCameraWorldNormals.at<double>(row*51 + col, 0 ) = 0;
      leftCameraWorldNormals.at<double>(row*51 + col, 1 ) = 0;
      leftCameraWorldNormals.at<double>(row*51 + col, 2 ) = -1.0;
    }
  }
  leftCameraPositionToFocalPointUnitVector.at<double>(0,0)=0;
  leftCameraPositionToFocalPointUnitVector.at<double>(0,1)=0;
  leftCameraPositionToFocalPointUnitVector.at<double>(0,2)=1.0;

  std::vector<int> Points = mitk::ProjectVisible3DWorldPointsToStereo2D
    ( leftCameraWorldPoints,leftCameraWorldNormals,
      leftCameraPositionToFocalPointUnitVector,
      leftCameraIntrinsic,leftCameraDistortion,
      rightCameraIntrinsic,rightCameraDistortion,
      rightToLeftRotationMatrix,rightToLeftTranslationVector,
      outputLeftCameraWorldPointsIn3D,
      outputLeftCameraWorldNormalsIn3D,
      output2DPointsLeft,
      output2DPointsRight);

  bool cropUndistortedPointsToScreen = true;
  double cropValueInf = std::numeric_limits<double>::infinity();
  std::vector<int> trimmedPoints = mitk::ProjectVisible3DWorldPointsToStereo2D
    ( leftCameraWorldPoints,leftCameraWorldNormals,
      leftCameraPositionToFocalPointUnitVector,
      leftCameraIntrinsic,leftCameraDistortion,
      rightCameraIntrinsic,rightCameraDistortion,
      rightToLeftRotationMatrix,rightToLeftTranslationVector,
      outputTrimmedLeftCameraWorldPointsIn3D,
      outputTrimmedLeftCameraWorldNormalsIn3D,
      outputTrimmed2DPointsLeft,
      outputTrimmed2DPointsRight,
      cropUndistortedPointsToScreen,
      0,screenWidth,
      0,screenHeight,cropValueInf
      );

  boost::mt19937 rng;
  boost::normal_distribution<> nd(0.0,pixelNoise);
  boost::variate_generator<boost::mt19937& , boost::normal_distribution<> > var_nor (rng,nd);
  for ( int i = 0 ; i < numberOfPoints ; i ++ )
  {
    CV_MAT_ELEM (*output2DPointsLeft ,double,i,0) += var_nor();
    CV_MAT_ELEM (*output2DPointsLeft ,double,i,1) += var_nor();
    CV_MAT_ELEM (*output2DPointsRight ,double,i,0) += var_nor();
    CV_MAT_ELEM (*output2DPointsRight ,double,i,1) += var_nor();

    CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,0) += var_nor();
    CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,1) += var_nor();
    CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,0) += var_nor();
    CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,1) += var_nor();
  }

  if ( quantize )
  {
    for ( int i = 0 ; i < numberOfPoints ; i ++ )
    {
      CV_MAT_ELEM (*output2DPointsLeft ,double,i,0) =
        floor ( CV_MAT_ELEM (*output2DPointsLeft ,double,i,0) + 0.5 );
      CV_MAT_ELEM (*output2DPointsLeft ,double,i,1) =
        floor ( CV_MAT_ELEM (*output2DPointsLeft ,double,i,1) + 0.5 );
      CV_MAT_ELEM (*output2DPointsRight ,double,i,0) =
        floor ( CV_MAT_ELEM (*output2DPointsRight ,double,i,0) + 0.5 );
      CV_MAT_ELEM (*output2DPointsRight ,double,i,1) =
        floor ( CV_MAT_ELEM (*output2DPointsRight ,double,i,1) + 0.5 );

      CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,0) =
        floor ( CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,0) + 0.5 );
      CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,1) =
        floor ( CV_MAT_ELEM (*outputTrimmed2DPointsLeft ,double,i,1) + 0.5 );
      CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,0) =
        floor ( CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,0) + 0.5 );
      CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,1) =
        floor ( CV_MAT_ELEM (*outputTrimmed2DPointsRight ,double,i,1) + 0.5 );
    }
  }

  mitk::UndistortPoints(output2DPointsLeft,
      leftCameraIntrinsic,leftCameraDistortion,
      leftScreenPoints,
      cropNonVisiblePoints,
      0.0 , screenWidth, 0.0, screenHeight, cropValue);

  mitk::UndistortPoints(output2DPointsRight,
      rightCameraIntrinsic,rightCameraDistortion,
      rightScreenPoints,
      cropNonVisiblePoints,
      0.0 , screenWidth, 0.0, screenHeight, cropValue);

  mitk::UndistortPoints(outputTrimmed2DPointsLeft,
      leftCameraIntrinsic,leftCameraDistortion,
      leftTrimmedScreenPoints,
      cropNonVisiblePoints,
      0.0 , screenWidth, 0.0, screenHeight, cropValue);

  mitk::UndistortPoints(outputTrimmed2DPointsRight,
      rightCameraIntrinsic,rightCameraDistortion,
      rightTrimmedScreenPoints,
      cropNonVisiblePoints,
      0.0 , screenWidth, 0.0, screenHeight, cropValue);

 //check it with the c Wrapper function
  cv::Mat leftCameraTranslationVector = cv::Mat (1,3,CV_64FC1);
  cv::Mat leftCameraRotationVector = cv::Mat (1,3,CV_64FC1);
  cv::Mat rightCameraTranslationVector = cv::Mat (1,3,CV_64FC1);
  cv::Mat rightCameraRotationVector = cv::Mat (1,3,CV_64FC1);

  for ( int i = 0 ; i < 3 ; i ++ )
  {
    leftCameraTranslationVector.at<double>(0,i) = 0.0;
    leftCameraRotationVector.at<double>(0,i) = 0.0;
  }
  rightCameraTranslationVector = rightToLeftTranslationVector * -1;
  cv::Rodrigues ( rightToLeftRotationMatrix.inv(), rightCameraRotationVector  );

  MITK_DEBUG << leftCameraTranslationVector;
  MITK_DEBUG << leftCameraRotationVector;
  MITK_DEBUG << rightCameraTranslationVector;
  MITK_DEBUG << rightCameraRotationVector;

  CvMat leftScreenPointsMat = leftScreenPoints;// cvCreateMat(numberOfPoints,2,CV_64FC1;
  CvMat rightScreenPointsMat= rightScreenPoints;
  CvMat leftTrimmedScreenPointsMat = leftTrimmedScreenPoints;// cvCreateMat(numberOfPoints,2,CV_64FC1;
  CvMat rightTrimmedScreenPointsMat= rightTrimmedScreenPoints;
  CvMat leftCameraIntrinsicMat= leftCameraIntrinsic;
  CvMat leftCameraRotationVectorMat= leftCameraRotationVector;
  CvMat leftCameraTranslationVectorMat= leftCameraTranslationVector;
  CvMat rightCameraIntrinsicMat = rightCameraIntrinsic;
  CvMat rightCameraRotationVectorMat = rightCameraRotationVector;
  CvMat rightCameraTranslationVectorMat= rightCameraTranslationVector;

  mitk::CStyleTriangulatePointPairsUsingSVD(
    leftScreenPointsMat,
    rightScreenPointsMat,
    leftCameraIntrinsicMat,
    leftCameraRotationVectorMat,
    leftCameraTranslationVectorMat,
    rightCameraIntrinsicMat,
    rightCameraRotationVectorMat,
    rightCameraTranslationVectorMat,
    *leftCameraTriangulatedWorldPoints_m1);

  mitk::CStyleTriangulatePointPairsUsingSVD(
    leftTrimmedScreenPointsMat,
    rightTrimmedScreenPointsMat,
    leftCameraIntrinsicMat,
    leftCameraRotationVectorMat,
    leftCameraTranslationVectorMat,
    rightCameraIntrinsicMat,
    rightCameraRotationVectorMat,
    rightCameraTranslationVectorMat,
    *leftCameraTriangulatedWorldPoints_trimmed_m1);

  std::vector < std::pair<cv::Point2d, cv::Point2d> > inputUndistortedPoints;
  for ( int i = 0 ; i < numberOfPoints ; i ++ )
  {
    std::pair <cv::Point2d, cv::Point2d > pointPair;
    pointPair.first.x = leftScreenPoints.at<double>(i,0);
    pointPair.first.y = leftScreenPoints.at<double>(i,1);
    pointPair.second.x = rightScreenPoints.at<double>(i,0);
    pointPair.second.y = rightScreenPoints.at<double>(i,1);
    inputUndistortedPoints.push_back(pointPair);
  }
  std::vector < std::pair<cv::Point2d, cv::Point2d> > inputTrimmedUndistortedPoints;
  for ( int i = 0 ; i < numberOfPoints ; i ++ )
  {
    std::pair <cv::Point2d, cv::Point2d > pointPair;
    pointPair.first.x = leftTrimmedScreenPoints.at<double>(i,0);
    pointPair.first.y = leftTrimmedScreenPoints.at<double>(i,1);
    pointPair.second.x = rightTrimmedScreenPoints.at<double>(i,0);
    pointPair.second.y = rightTrimmedScreenPoints.at<double>(i,1);
    inputTrimmedUndistortedPoints.push_back(pointPair);
  }
  std::vector <std::pair < cv::Point3d, double > > leftCameraTriangulatedWorldPoints_m2 =
    mitk::TriangulatePointPairsUsingGeometry(
        inputUndistortedPoints,
        leftCameraIntrinsic,
        rightCameraIntrinsic,
        rightToLeftRotationMatrix,
        rightToLeftTranslationVector,
        100.0 // don't know tolerance allowable yet.
        );

   std::vector <std::pair < cv::Point3d, double > > leftCameraTriangulatedWorldPoints_trimmed_m2 =
    mitk::TriangulatePointPairsUsingGeometry(
        inputTrimmedUndistortedPoints,
        leftCameraIntrinsic,
        rightCameraIntrinsic,
        rightToLeftRotationMatrix,
        rightToLeftTranslationVector,
        100.0 // don't know tolerance allowable yet.
        );

 MITK_INFO << "size of triangulated point vector = " <<  leftCameraTriangulatedWorldPoints_m2.size();
 MITK_INFO << "size of trimmed triangulated point vector = " <<  leftCameraTriangulatedWorldPoints_trimmed_m2.size();
  //mitk::TriangulatePointPairsUsingGeometry does not ensure that the indexes in inputUndistortedPoints
  //are preserved in  leftCameraTriangulatedWorldPoints_m2, let's add a NaN to the end and
  //build a look up table
  cv::Point3d nanPoint = cv::Point3d(std::numeric_limits<double>::quiet_NaN(),
      std::numeric_limits<double>::quiet_NaN(),std::numeric_limits<double>::quiet_NaN());
  leftCameraTriangulatedWorldPoints_m2.push_back(std::pair < cv::Point3d, double > (nanPoint, std::numeric_limits<double>::infinity()));
  leftCameraTriangulatedWorldPoints_trimmed_m2.push_back(std::pair < cv::Point3d, double > (nanPoint, std::numeric_limits<double>::infinity()));

  unsigned int leftCameraTriangulatedWorldPoints_Counter = 0;
  unsigned int leftTrimmedCameraTriangulatedWorldPoints_Counter = 0;
  std::vector < unsigned int > leftCameraTriangulatedWorldPoints_LookUpVector;
  std::vector < unsigned int > leftTrimmedCameraTriangulatedWorldPoints_LookUpVector;
  for ( int i = 0 ; i < numberOfPoints ; i ++ )
  {
    if ( ( ! boost::math::isnan(inputUndistortedPoints[i].first.x) ) &&
          ( ! boost::math::isnan(inputUndistortedPoints[i].second.x) ) )
    {
      leftCameraTriangulatedWorldPoints_LookUpVector.push_back(leftCameraTriangulatedWorldPoints_Counter);
      leftCameraTriangulatedWorldPoints_Counter++;
    }
    else
    {
      leftCameraTriangulatedWorldPoints_LookUpVector.push_back(leftCameraTriangulatedWorldPoints_m2.size()-1);
    }
    if ( ( ! boost::math::isnan(inputTrimmedUndistortedPoints[i].first.x) ) &&
          ( ! boost::math::isnan(inputTrimmedUndistortedPoints[i].second.x) ) )
    {
      leftTrimmedCameraTriangulatedWorldPoints_LookUpVector.push_back(leftTrimmedCameraTriangulatedWorldPoints_Counter);
      leftTrimmedCameraTriangulatedWorldPoints_Counter++;
    }
    else
    {
      leftTrimmedCameraTriangulatedWorldPoints_LookUpVector.push_back(leftCameraTriangulatedWorldPoints_trimmed_m2.size()-1);
    }

  }

  for ( int row = 0 ; row < 51 ; row += 25 )
  {
    for ( int col = 0 ; col < 51 ; col += 25 )
    {
      MITK_INFO << "(" << row << "," << col <<  ") [" << leftCameraWorldPoints.at<double>(row*51 + col,0) << ","
        << leftCameraWorldPoints.at<double>(row*51 + col,1) << ","
        << leftCameraWorldPoints.at<double>(row*51 + col,2) << "] => ("
        << CV_MAT_ELEM (*output2DPointsLeft ,double, row*51 + col,0) << ","
        << CV_MAT_ELEM (*output2DPointsLeft, double,row*51 + col,1) << ") ("
        << CV_MAT_ELEM (*output2DPointsRight,double,row*51 + col,0) << ","
        << CV_MAT_ELEM (*output2DPointsRight,double,row*51 + col,1) << ") => "
        << " ["
        << CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, row* 51 + col, 0) << ","
        << CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, row* 51 + col, 1) << ","
        << CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, row* 51 + col, 2) << "] "
        << leftCameraTriangulatedWorldPoints_m2[
        leftCameraTriangulatedWorldPoints_LookUpVector[row*51 + col]].first;
    }
  }

  double xErrorMean_m1 = 0.0;
  double yErrorMean_m1 = 0.0;
  double zErrorMean_m1 = 0.0;
  double xErrorMean_m2 = 0.0;
  double yErrorMean_m2 = 0.0;
  double zErrorMean_m2 = 0.0;
  double errorRMS_m1 = 0.0;
  double errorRMS_m2 = 0.0;

  int goodPoints_m1 = 0;
  int goodPoints_m2 = 0;

  double xErrorMean_trimmed_m1 = 0.0;
  double yErrorMean_trimmed_m1 = 0.0;
  double zErrorMean_trimmed_m1 = 0.0;
  double xErrorMean_trimmed_m2 = 0.0;
  double yErrorMean_trimmed_m2 = 0.0;
  double zErrorMean_trimmed_m2 = 0.0;
  double errorRMS_trimmed_m1 = 0.0;
  double errorRMS_trimmed_m2 = 0.0;

  int goodPoints_trimmed_m1 = 0;
  int goodPoints_trimmed_m2 = 0;

  for ( int i = 0 ; i < numberOfPoints ; i ++ )
  {
    double xError_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, i, 0) -
      leftCameraWorldPoints.at<double>(i,0);
    double yError_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, i, 1) -
      leftCameraWorldPoints.at<double>(i,1);
    double zError_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_m1,double, i, 2) -
      leftCameraWorldPoints.at<double>(i,2);
    double xError_m2 = leftCameraTriangulatedWorldPoints_m2[leftCameraTriangulatedWorldPoints_LookUpVector[i]].first.x -
      leftCameraWorldPoints.at<double>(i,0);
    double yError_m2 = leftCameraTriangulatedWorldPoints_m2[leftCameraTriangulatedWorldPoints_LookUpVector[i]].first.y -
      leftCameraWorldPoints.at<double>(i,1);
    double zError_m2 = leftCameraTriangulatedWorldPoints_m2[leftCameraTriangulatedWorldPoints_LookUpVector[i]].first.z -
      leftCameraWorldPoints.at<double>(i,2);

    double error_m1 = (xError_m1 * xError_m1 + yError_m1 * yError_m1 + zError_m1 * zError_m1);
    double error_m2 = (xError_m2 * xError_m2 + yError_m2 * yError_m2 + zError_m2 * zError_m2);

    if ( ! boost::math::isnan(error_m1) )
    {
      xErrorMean_m1 += xError_m1;
      yErrorMean_m1 += yError_m1;
      zErrorMean_m1 += zError_m1;
      errorRMS_m1 += error_m1;
      goodPoints_m1++;
    }
    if ( ! boost::math::isnan(error_m2 ))
    {
      xErrorMean_m2 += xError_m2;
      yErrorMean_m2 += yError_m2;
      zErrorMean_m2 += zError_m2;
      errorRMS_m2 += error_m2;
      goodPoints_m2++;
    }

    double xError_trimmed_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_trimmed_m1,double, i, 0) -
      leftCameraWorldPoints.at<double>(i,0);
    double yError_trimmed_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_trimmed_m1,double, i, 1) -
      leftCameraWorldPoints.at<double>(i,1);
    double zError_trimmed_m1 = CV_MAT_ELEM (*leftCameraTriangulatedWorldPoints_trimmed_m1,double, i, 2) -
      leftCameraWorldPoints.at<double>(i,2);
    double xError_trimmed_m2 = leftCameraTriangulatedWorldPoints_trimmed_m2[leftTrimmedCameraTriangulatedWorldPoints_LookUpVector[i]].first.x -
      leftCameraWorldPoints.at<double>(i,0);
    double yError_trimmed_m2 = leftCameraTriangulatedWorldPoints_trimmed_m2[leftTrimmedCameraTriangulatedWorldPoints_LookUpVector[i]].first.y -
      leftCameraWorldPoints.at<double>(i,1);
    double zError_trimmed_m2 = leftCameraTriangulatedWorldPoints_trimmed_m2[leftTrimmedCameraTriangulatedWorldPoints_LookUpVector[i]].first.z -
      leftCameraWorldPoints.at<double>(i,2);

    double error_trimmed_m1 = (xError_trimmed_m1 * xError_trimmed_m1 + yError_trimmed_m1 * yError_trimmed_m1 + zError_trimmed_m1 * zError_trimmed_m1);
    double error_trimmed_m2 = (xError_trimmed_m2 * xError_trimmed_m2 + yError_trimmed_m2 * yError_trimmed_m2 + zError_trimmed_m2 * zError_trimmed_m2);

    if ( ! boost::math::isnan(error_trimmed_m1) )
    {
      xErrorMean_trimmed_m1 += xError_trimmed_m1;
      yErrorMean_trimmed_m1 += yError_trimmed_m1;
      zErrorMean_trimmed_m1 += zError_trimmed_m1;
      errorRMS_trimmed_m1 += error_trimmed_m1;
      goodPoints_trimmed_m1++;
    }
    if ( ! boost::math::isnan(error_trimmed_m2 ))
    {
      xErrorMean_trimmed_m2 += xError_trimmed_m2;
      yErrorMean_trimmed_m2 += yError_trimmed_m2;
      zErrorMean_trimmed_m2 += zError_trimmed_m2;
      errorRMS_trimmed_m2 += error_trimmed_m2;
      goodPoints_trimmed_m2++;
    }

  }
  MITK_INFO << "Method 1 Dumped " << numberOfPoints - goodPoints_m1 << " off screen points";
  MITK_INFO << "Method 2 Dumped " << numberOfPoints - goodPoints_m2 << " off screen points";
  xErrorMean_m1 /= goodPoints_m1;
  yErrorMean_m1 /= goodPoints_m1;
  zErrorMean_m1 /= goodPoints_m1;
  xErrorMean_m2 /= goodPoints_m2;
  yErrorMean_m2 /= goodPoints_m2;
  zErrorMean_m2 /= goodPoints_m2;
  errorRMS_m1 = sqrt(errorRMS_m1/goodPoints_m1);
  errorRMS_m2 = sqrt(errorRMS_m2/goodPoints_m2);

  MITK_INFO << "Mean x error c wrapper = " <<  xErrorMean_m1;
  MITK_INFO << "Mean y error c wrapper = " <<  yErrorMean_m1;
  MITK_INFO << "Mean z error c wrapper = " <<  zErrorMean_m1;
  MITK_INFO << "RMS error c wrapper = " <<  errorRMS_m1;
  MITK_INFO << "Mean x error c++ wrapper = " <<  xErrorMean_m2;
  MITK_INFO << "Mean y error c++ wrapper = " <<  yErrorMean_m2;
  MITK_INFO << "Mean z error c++ wrapper = " <<  zErrorMean_m2;
  MITK_INFO << "RMS error c++ wrapper = " <<  errorRMS_m2;

  MITK_TEST_CONDITION (fabs(xErrorMean_m1) < 0.5, "Testing x error mean value for c wrapper method");
  MITK_TEST_CONDITION (fabs(yErrorMean_m1) < 0.5, "Testing y error mean value for c wrapper method");
  MITK_TEST_CONDITION (fabs(zErrorMean_m1) < 0.5, "Testing z error mean value for c wrapper method");
  MITK_TEST_CONDITION (errorRMS_m1 < 2.0 , "Testing RMS error value for c method");
  MITK_TEST_CONDITION (fabs(xErrorMean_m2) < 1e-3, "Testing x error mean value for c++ method");
  MITK_TEST_CONDITION (fabs(yErrorMean_m2) < 1e-3, "Testing y error mean value for c++ method");
  MITK_TEST_CONDITION (fabs(zErrorMean_m2) < 1e-3, "Testing z error mean value for c++ method");
  MITK_TEST_CONDITION (errorRMS_m2 < 2e-3, "Testing RMS error value for c++ method");

  MITK_INFO << "Method 1 Trimmed Dumped " << numberOfPoints - goodPoints_trimmed_m1 << " off screen points";
  MITK_INFO << "Method 2 Trimmed Dumped " << numberOfPoints - goodPoints_trimmed_m2 << " off screen points";
  xErrorMean_trimmed_m1 /= goodPoints_trimmed_m1;
  yErrorMean_trimmed_m1 /= goodPoints_trimmed_m1;
  zErrorMean_trimmed_m1 /= goodPoints_trimmed_m1;
  xErrorMean_trimmed_m2 /= goodPoints_trimmed_m2;
  yErrorMean_trimmed_m2 /= goodPoints_trimmed_m2;
  zErrorMean_trimmed_m2 /= goodPoints_trimmed_m2;
  errorRMS_trimmed_m1 = sqrt(errorRMS_trimmed_m1/goodPoints_trimmed_m1);
  errorRMS_trimmed_m2 = sqrt(errorRMS_trimmed_m2/goodPoints_trimmed_m2);

  MITK_INFO << "Mean x error trimmed c wrapper = " <<  xErrorMean_trimmed_m1;
  MITK_INFO << "Mean y error trimmed c wrapper = " <<  yErrorMean_trimmed_m1;
  MITK_INFO << "Mean z error trimmed c wrapper = " <<  zErrorMean_trimmed_m1;
  MITK_INFO << "RMS error trimmed c wrapper = " <<  errorRMS_trimmed_m1;
  MITK_INFO << "Mean x error trimmed c++ wrapper = " <<  xErrorMean_trimmed_m2;
  MITK_INFO << "Mean y error trimmed c++ wrapper = " <<  yErrorMean_trimmed_m2;
  MITK_INFO << "Mean z error trimmed c++ wrapper = " <<  zErrorMean_trimmed_m2;
  MITK_INFO << "RMS error trimmed c++ wrapper = " <<  errorRMS_trimmed_m2;
  MITK_TEST_CONDITION (fabs(xErrorMean_trimmed_m1) < 0.5, "Testing x error mean value for c wrapper method");
  MITK_TEST_CONDITION (fabs(yErrorMean_trimmed_m1) < 0.5, "Testing y error mean value for c wrapper method");
  MITK_TEST_CONDITION (fabs(zErrorMean_trimmed_m1) < 0.5, "Testing z error mean value for c wrapper method");
  MITK_TEST_CONDITION (errorRMS_trimmed_m1 < 2.0, "Testing RMS error value for c method");
  MITK_TEST_CONDITION (fabs(xErrorMean_trimmed_m2) < 1e-3, "Testing x error mean value for c++ method");
  MITK_TEST_CONDITION (fabs(yErrorMean_trimmed_m2) < 1e-3, "Testing y error mean value for c++ method");
  MITK_TEST_CONDITION (fabs(zErrorMean_trimmed_m2) < 1e-3, "Testing z error mean value for c++ method");
  MITK_TEST_CONDITION (errorRMS_trimmed_m2 < 2e-3, "Testing RMS error value for c++ method");

  MITK_TEST_END();
}


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_GNOMONIC_SPHEROID_HPP
#define BOOST_GEOMETRY_FORMULAS_GNOMONIC_SPHEROID_HPP

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Gnomonic projection on spheroid (ellipsoid of revolution).
\author See
- Charles F.F Karney, Algorithms for geodesics, 2011
https://arxiv.org/pdf/1109.4448.pdf
*/
template <
    typename CT,
    template <typename, bool, bool, bool, bool ,bool> class Inverse,
    template <typename, bool, bool, bool, bool> class Direct
>
class gnomonic_spheroid
{
    typedef Inverse<CT, false, true, true, true, true> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

    typedef Direct<CT, false, false, true, true> direct_quantities_type;
    typedef Direct<CT, true, false, false, false> direct_coordinates_type;
    typedef typename direct_coordinates_type::result_type direct_result;

public:
    template <typename Spheroid>
    static inline bool forward(CT const& lon0, CT const& lat0,
                               CT const& lon, CT const& lat,
                               CT & x, CT & y,
                               Spheroid const& spheroid)
    {
        inverse_result i_res = inverse_type::apply(lon0, lat0, lon, lat, spheroid);
        CT const& m = i_res.reduced_length;
        CT const& M = i_res.geodesic_scale;

        if (math::smaller_or_equals(M, CT(0)))
        {
            return false;
        }

        CT rho = m / M;
        x = sin(i_res.azimuth) * rho;
        y = cos(i_res.azimuth) * rho;

        return true;
    }

    template <typename Spheroid>
    static inline bool inverse(CT const& lon0, CT const& lat0,
                               CT const& x, CT const& y,
                               CT & lon, CT & lat,
                               Spheroid const& spheroid)
    {
        CT const a = get_radius<0>(spheroid);
        CT const ds_threshold = a * std::numeric_limits<CT>::epsilon(); // TODO: 0 for non-fundamental type

        CT const azimuth = atan2(x, y);
        CT const rho = math::sqrt(math::sqr(x) + math::sqr(y)); // use hypot?
        CT distance = a * atan(rho / a);

        bool found = false;
        for (int i = 0 ; i < 10 ; ++i)
        {
            direct_result d_res = direct_quantities_type::apply(lon0, lat0, distance, azimuth, spheroid);
            CT const& m = d_res.reduced_length;
            CT const& M = d_res.geodesic_scale;

            if (math::smaller_or_equals(M, CT(0)))
            {
                // found = false;
                return found;
            }

            CT const drho = m / M - rho; // rho = m / M
            CT const ds = drho * math::sqr(M); // drho/ds = 1/M^2
            distance -= ds;

            // ds_threshold may be 0
            if (math::abs(ds) <= ds_threshold)
            {
                found = true;
                break;
            }
        }

        if (found)
        {
            direct_result d_res = direct_coordinates_type::apply(lon0, lat0, distance, azimuth, spheroid);
            lon = d_res.lon2;
            lat = d_res.lat2;
        }

        return found;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_GNOMONIC_SPHEROID_HPP


#ifndef PYTHONIC_INCLUDE_NUMPY_COSH_HPP
#define PYTHONIC_INCLUDE_NUMPY_COSH_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/cosh.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    auto cosh(T const &v) -> decltype(boost::simd::cosh(v))
    {
      return boost::simd::cosh(v);
    }
    template <class T>
    std::complex<T> cosh(std::complex<T> const &v)
    {
      return std::cosh(v);
    }
  }
#define NUMPY_NARY_FUNC_NAME cosh
#define NUMPY_NARY_FUNC_SYM wrapper::cosh
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_CORRECT_FMA_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_CORRECT_FMA_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/correct_fma.hpp>
#include <boost/simd/include/functions/scalar/two_prod.hpp>
#include <boost/simd/include/functions/scalar/two_add.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::correct_fma_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)(scalar_< arithmetic_<A0> >)(scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return a0*a1+a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::correct_fma_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)(scalar_< floating_<A0> >)(scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
        A0 p, rp, s, rs;
        two_prod(a0, a1, p, rp);
        two_add(p, a2, s, rs);
        return s+(rp+rs);
//         return ::fma(a0, a1, a2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::correct_fma_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)(scalar_< single_<A0> >)(scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return static_cast<float>(static_cast<double>(a0)*static_cast<double>(a1)+static_cast<double>(a2));
    }
  };
} } }

#endif


#pragma once

#include <vector>

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

namespace Aronda
{

using Angle_t = boost::units::quantity<boost::units::si::plane_angle>;

struct GameSquare
{
    double inner_radius;
    double outter_radius;
    Angle_t min_angle;
    Angle_t max_angle;
};

using GameSquareContainer_t = std::vector<GameSquare>;

GameSquareContainer_t buildSquares(const double outter_radius);

const auto PI = 3.14159265359 * boost::units::si::radians;
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/experimental/inflation/interpolatedyoyoptionletstripper.hpp>
#include <ql/experimental/inflation/kinterpolatedyoyoptionletvolatilitysurface.hpp>
#include <ql/instruments/makeyoyinflationcapfloor.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <qle/termstructures/interpolatedyoycapfloortermpricesurface.hpp>
#include <qle/termstructures/yoyinflationoptionletvolstripper.hpp>
#include <qle/termstructures/kinterpolatedyoyoptionletvolatilitysurface.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;
using boost::shared_ptr;

namespace QuantExt {

YoYInflationOptionletVolStripper::YoYInflationOptionletVolStripper(
    const boost::shared_ptr<CapFloorTermVolSurface>& volSurface, const boost::shared_ptr<YoYInflationIndex>& index,
    const Handle<YieldTermStructure>& nominalTs, VolatilityType type, Real displacement)
    : volSurface_(volSurface), yoyIndex_(index), nominalTs_(nominalTs), type_(type), displacement_(displacement) {

    performCalculations();
}

void YoYInflationOptionletVolStripper::performCalculations() {

    Calendar cal = yoyIndex_->yoyInflationTermStructure()->calendar();
    Period obsLag = yoyIndex_->yoyInflationTermStructure()->observationLag();
    Size settDays = volSurface_->settlementDays();
    DayCounter dc = yoyIndex_->yoyInflationTermStructure()->dayCounter();
    BusinessDayConvention bdc = volSurface_->businessDayConvention();

    Frequency frequency = yoyIndex_->frequency();

    std::vector<Rate> strikes = volSurface_->strikes();
    std::vector<Period> terms = volSurface_->optionTenors();
    std::vector<Time> times = volSurface_->optionTimes();

    std::vector<Period> optionletTerms;
    optionletTerms.push_back(terms.front());
    while (optionletTerms.back() != terms.back()) {
        optionletTerms.push_back(optionletTerms.back() + Period(1, Years));
    }

    Matrix cPrice(strikes.size(), strikes.size() == 0 ? 0 : optionletTerms.size(), Null<Real>()),
        fPrice(strikes.size(), strikes.size() == 0 ? 0 : optionletTerms.size(), Null<Real>());

    // populate cPrice
    for (Size i = 0; i < optionletTerms.size(); i++) {
        for (Size j = 0; j < strikes.size(); j++) {
            Time t = volSurface_->timeFromReference(volSurface_->optionDateFromTenor(optionletTerms[i]));

            Real vol = volSurface_->volatility(t, strikes[j]);
            boost::shared_ptr<ConstantYoYOptionletVolatility> ovs(
                new ConstantYoYOptionletVolatility(vol, settDays, cal, bdc, dc, obsLag, frequency, false, -1.0, 3.0));
            boost::shared_ptr<PricingEngine> pe;
            Handle<QuantLib::YoYOptionletVolatilitySurface> hovs(ovs);
            if (type_ == ShiftedLognormal) {
                if (displacement_ == 0.0) {
                    pe = boost::make_shared<YoYInflationBlackCapFloorEngine>(yoyIndex_, hovs);
                } else {
                    pe = boost::make_shared<YoYInflationUnitDisplacedBlackCapFloorEngine>(yoyIndex_, hovs);
                }
            } else if (type_ == Normal) {
                pe = boost::make_shared<YoYInflationBachelierCapFloorEngine>(yoyIndex_, hovs);
            } else {
                QL_FAIL("unknown volatility type: " << type_);
            }
            // calculate the cap price
            YoYInflationCapFloor cap =
                YoYInflationCapFloor(MakeYoYInflationCapFloor(YoYInflationCapFloor::Cap, optionletTerms[i].length(),
                                                              cal, yoyIndex_, obsLag, strikes[j])
                                         .withPricingEngine(pe)
                                         .withNominal(10000));
            cPrice[j][i] = cap.NPV();
            // floor price
            YoYInflationCapFloor floor =
                YoYInflationCapFloor(MakeYoYInflationCapFloor(YoYInflationCapFloor::Floor, optionletTerms[i].length(),
                                                              cal, yoyIndex_, obsLag, strikes[j])
                                         .withPricingEngine(pe)
                                         .withNominal(10000));
            fPrice[j][i] = floor.NPV();
        }
    }

    Rate baseRate = yoyIndex_->yoyInflationTermStructure()->baseRate();
    QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> ys(settDays, obsLag, yoyIndex_, baseRate,
                                                                           nominalTs_, dc, cal, bdc, strikes, strikes,
                                                                           optionletTerms, cPrice, fPrice);

    boost::shared_ptr<QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> > yoySurface =
        boost::make_shared<QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> >(ys);

    boost::shared_ptr<InterpolatedYoYOptionletStripper<Linear> > yoyStripper =
        boost::make_shared<InterpolatedYoYOptionletStripper<Linear> >();

    // Create an empty volatlity surface to pass to the engine
    boost::shared_ptr<QuantLib::YoYOptionletVolatilitySurface> ovs =
        boost::dynamic_pointer_cast<QuantLib::YoYOptionletVolatilitySurface>(
            boost::make_shared<QuantLib::ConstantYoYOptionletVolatility>(0.0, settDays, cal, bdc, dc, obsLag, frequency,
                                                                         yoySurface->indexIsInterpolated()));
    Handle<QuantLib::YoYOptionletVolatilitySurface> hovs(ovs);

    boost::shared_ptr<YoYInflationBachelierCapFloorEngine> cfEngine =
        boost::make_shared<YoYInflationBachelierCapFloorEngine>(yoyIndex_, hovs);

    boost::shared_ptr<QuantExt::StrikeInterpolatedYoYOptionletVolatilitySurface<Linear> > interpVolSurface =
      boost::make_shared<QuantExt::StrikeInterpolatedYoYOptionletVolatilitySurface<Linear> >(settDays, cal, bdc, dc, obsLag,
                                                                                yoySurface, cfEngine, yoyStripper, 0);

    boost::shared_ptr<QuantExt::YoYOptionletVolatilitySurface> newSurface =
        boost::make_shared<QuantExt::YoYOptionletVolatilitySurface>(interpVolSurface, type_, displacement_);
    yoyOptionletVolSurface_ = newSurface;
}

} // namespace QuantExt


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef PAIRWISE_STATISTIC_VARIABLE_HPP_
#define PAIRWISE_STATISTIC_VARIABLE_HPP_

#include "clotho/genetics/pairwise_statistic_def.hpp"

#include "clotho/powerset/variable_subset.hpp"

#include "clotho/utility/popcount.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>

namespace accum=boost::accumulators;

template < class E, class B, class BM, class EK >
struct pairwise_statistic< clotho::powersets::variable_subset< E, B, BM, EK > > {
    typedef clotho::powersets::variable_subset< E, B, BM, EK > sequence_type;
    typedef accum::accumulator_set< double, accum::stats< accum::tag::weighted_mean, accum::tag::weighted_variance, accum::tag::weighted_sum, accum::tag::count >, double > accum_type;

    typedef B   block_type;

    pairwise_statistic( sequence_type & b, double w = 1.0 ) :
        _base( &b )
        , base_weight(w)
        , m_global_diff( NULL )
        , m_global_int( NULL )
        , m_global_un( NULL )
        , m_tech_dup(0) {
    }

    pairwise_statistic( sequence_type & b, accum_type & d, accum_type & i, accum_type & u, double w = 1.0 ) :
        _base( &b )
        , base_weight(w)
        , m_global_diff( &d )
        , m_global_int( &i )
        , m_global_un( &u )
        , m_tech_dup(0) {
    }

    void update( sequence_type & seq, double w = 1.0 ) {
        typename sequence_type::data_citerator sit = _base->begin(), sit2 = seq.begin();

        unsigned int _diff = 0, _int = 0;

        while( true ) {
            if( sit == _base->end() ) {
                while( sit2 != seq.end() ) {
                    block_type b = *sit2;
                    ++sit2;
                    unsigned int res = popcount( b );
                    _diff += res;
//                    _un += res;
                }
                break;
            }

            if( sit2 == seq.end() ) {
                while( sit != _base->end() ) {
                    block_type b = *sit;
                    ++sit;
                    unsigned int res = popcount( b );
                    _diff += res;
//                    _un += res;
                }
                break;
            }

            block_type b0 = *sit, b1 = *sit2;
            ++sit;
            ++sit2;
            _diff += popcount( b0 ^ b1 );   // symmetric difference
            _int += popcount( b0 & b1 );    // intersection
//            _un += popcount( b0 | b1 );
        }
        unsigned int _un = _diff + _int;    // union = symmetric difference + intersection

        double _w = (base_weight * w);

        m_acc_diff( (double)_diff, accum::weight = _w );
        m_acc_int( (double)_int, accum::weight = _w );
        m_acc_un( (double)_un, accum::weight = _w );

        if( !_diff ) ++m_tech_dup;

        if( m_global_int ) {
            (*m_global_int)( (double) _int, accum::weight = w );
            (*m_global_diff)( (double) _diff, accum::weight = w);
            (*m_global_un)( (double) _un, accum::weight = w );
        }
    }

    double count() {
        return accum::count( m_acc_diff );
    }

    double difference_sum() {
        return accum::weighted_sum( m_acc_diff );
    }

    double difference_mean() {
        return accum::weighted_mean( m_acc_diff );
    }

    double difference_variance() {
        return accum::weighted_variance( m_acc_diff );
    }

    double intersection_sum() {
        return accum::weighted_sum( m_acc_int );
    }

    double intersection_mean() {
        return accum::weighted_mean( m_acc_int );
    }

    double intersection_variance() {
        return accum::weighted_variance( m_acc_int );
    }

    double union_sum() {
        return accum::weighted_sum( m_acc_un );
    }

    double union_mean() {
        return accum::weighted_mean( m_acc_un );
    }

    double union_variance() {
        return accum::weighted_variance( m_acc_un );
    }

    sequence_type * _base;
    double          base_weight;

    accum_type   m_acc_diff, m_acc_int, m_acc_un;
    accum_type * m_global_diff, * m_global_int, * m_global_un;
    unsigned int m_tech_dup;
};

#endif  // PAIRWISE_STATISTIC_VARIABLE_HPP_


// Component
#include "RosConversionHelper.hpp"
#include "TrajectorySolver.hpp"

// Ros
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/shared_ptr.hpp>
#include <unsupported/Eigen/Splines>

// Standard
#include <utility>

namespace local_planner
{

TrajectorySolver::TrajectorySolver(const TrajectoryConfig& cfg) :
    m_cfg{cfg}
{}

TrajectorySolver::~TrajectorySolver() = default;

autonomy_msgs::Trajectory::ConstPtr TrajectorySolver::calculateTrajectory(const std::vector<Point>& path, const ros::Time& now_s)
{
    m_dist_based_traj.cmds.clear();
    m_dist_based_traj.cmds.reserve(path.size());
    m_dist_based_traj.execution_times.clear();
    m_dist_based_traj.execution_times.reserve(path.size());
    m_time_based_traj.cmds.clear();
    m_time_based_traj.execution_times.clear();

    calculateDistanceBasedTrajectory(path);

    calculateTimeBasedTrajectory(now_s);

    return boost::make_shared<autonomy_msgs::Trajectory>(m_time_based_traj);
}

void TrajectorySolver::calculateDistanceBasedTrajectory(const std::vector<Point>& path)
{
    if (path.size() < 2U)
    {
        return;
    }

    Point previous_point = path.front();
    float64_t total_path_dist_m{0.0};

    for (const auto& pt : path)
    {
        if (pt == previous_point)
        {
            continue;
        }

        total_path_dist_m += std::sqrt(std::pow(pt.getX() - previous_point.getX(), 2U) + std::pow(pt.getY() - previous_point.getY(), 2U));

        previous_point = pt;
    }

    const float64_t end_speed_mps = m_data.getGoalPose()->speed_mps;
    geometry_msgs::Twist current_state = m_data.getLocalPose()->twist.twist;
    float64_t current_heading_r = RosConversionHelper::quaternionMsgToYawR(m_data.getLocalPose()->pose.pose.orientation);
    correctAngle(current_heading_r);
    float64_t dist_traveled_m{0.0};
    float64_t total_time_s{0.0};

    for (std::vector<Point>::const_iterator path_it = path.cbegin(); path_it != path.cend() - 1U; ++path_it)
    {
        const std::vector<Point>::const_iterator next_it = path_it + 1U;
        const float64_t dx_m = next_it->getX() - path_it->getX();
        const float64_t dy_m = next_it->getY() - path_it->getY();
        const float64_t dp_m = std::sqrt(std::pow(dx_m, 2U) + std::pow(dy_m, 2U));
        dist_traveled_m += dp_m;

        const float64_t cur_vel_x =  current_state.linear.x*std::cos(current_heading_r) + current_state.linear.y*std::sin(current_heading_r);
        const float64_t cur_vel_y = -current_state.linear.x*std::sin(current_heading_r) + current_state.linear.y*std::cos(current_heading_r);
        const float64_t cur_speed_mps = std::sqrt(std::pow(cur_vel_x, 2U) + std::pow(cur_vel_y, 2U));

        const float64_t dist_left_m = total_path_dist_m - dist_traveled_m;

        bool needs_to_slow{false};

        if (cur_speed_mps > end_speed_mps)
        {
            const float64_t time_to_goal_speed = (cur_speed_mps - end_speed_mps)/(m_cfg.getMaxAccelMps2());
            const float64_t dist_to_goal_speed = cur_speed_mps*time_to_goal_speed - m_cfg.getMaxAccelMps2()*std::pow(time_to_goal_speed, 2U)/2.0;

            needs_to_slow = (dist_to_goal_speed >= dist_left_m);
        }

        const float64_t accel_mps2 = needs_to_slow ? -m_cfg.getMaxAccelMps2() : m_cfg.getMaxAccelMps2();

        float64_t dt_s{0.0};

        const float64_t vf = std::sqrt(std::pow(cur_speed_mps, 2U) + 2.0*dp_m*accel_mps2);
        dt_s = (vf - cur_speed_mps)/accel_mps2;

        if (std::signbit(dt_s) == true)
        {
            return;
        }

        const float64_t next_speed_mps = cur_speed_mps + accel_mps2*dt_s;

        if (next_speed_mps > m_cfg.getMaxSpeedMps())
        {
            const float64_t dt_to_max_vel_s = (m_cfg.getMaxSpeedMps() - cur_speed_mps)/m_cfg.getMaxAccelMps2();
            const float64_t dp_to_max_vel_m = cur_speed_mps*dt_to_max_vel_s + m_cfg.getMaxAccelMps2()*std::pow(dt_to_max_vel_s, 2U)/2.0;
            const float64_t dp_to_next_pt_m = dp_m - dp_to_max_vel_m;
            const float64_t dt_to_next_pt_s = dp_to_next_pt_m/m_cfg.getMaxSpeedMps();
            dt_s = dt_to_max_vel_s + dt_to_next_pt_s;
        }

        const float64_t commanded_speed_mps = std::min(next_speed_mps, m_cfg.getMaxSpeedMps());

        float64_t velocity_heading_r  = std::atan2(dy_m, dx_m);
        correctAngle(velocity_heading_r);
        float64_t actual_vs_velocity_heading_diff_r = velocity_heading_r - current_heading_r;
        correctAngle(actual_vs_velocity_heading_diff_r);

        const float64_t lon_vel_mps = commanded_speed_mps*std::cos(actual_vs_velocity_heading_diff_r);
        const float64_t lat_vel_mps = commanded_speed_mps*std::sin(actual_vs_velocity_heading_diff_r);

        current_state.linear.x = lon_vel_mps;
        current_state.linear.y = lat_vel_mps;

        float64_t d_heading_r = velocity_heading_r - current_heading_r;
        correctAngle(d_heading_r);

        if (d_heading_r > M_PI)
        {
            d_heading_r -= 2.0*M_PI;
        }

        float64_t yaw_rate_rps = d_heading_r/dt_s;
        yaw_rate_rps = yaw_rate_rps < -m_cfg.getMaxYawRateRps() ? -m_cfg.getMaxYawRateRps() : yaw_rate_rps;
        yaw_rate_rps = yaw_rate_rps >  m_cfg.getMaxYawRateRps() ?  m_cfg.getMaxYawRateRps() : yaw_rate_rps;

        current_heading_r += yaw_rate_rps*dt_s;

        correctAngle(current_heading_r);

        current_state.angular.z = yaw_rate_rps;

        m_dist_based_traj.cmds.emplace_back(current_state);

        m_dist_based_traj.execution_times.emplace_back(ros::Time(total_time_s));

        total_time_s += dt_s;
    }
}

void TrajectorySolver::calculateTimeBasedTrajectory(const ros::Time& now_s)
{
    if (m_dist_based_traj.cmds.size() != m_dist_based_traj.execution_times.size() || m_dist_based_traj.cmds.size() == 0U)
    {
        ROS_ERROR_STREAM("Bad command formed in distance based trajectory calculation");

        return;
    }

    using Spline1d        = Eigen::Spline<float64_t, 2>;
    using Spline1dFitting = Eigen::SplineFitting<Spline1d>;

    Eigen::MatrixXd x_vel_points(2, m_dist_based_traj.execution_times.size());
    Eigen::MatrixXd y_vel_points(2, m_dist_based_traj.execution_times.size());
    Eigen::MatrixXd yaw_rate_points(2, m_dist_based_traj.execution_times.size());

    float64_t prev_time_s = m_dist_based_traj.execution_times.front().toSec();
    float64_t total_traj_time_s = 0.0;

    for (std::size_t cmd_it = 0U; cmd_it < m_dist_based_traj.execution_times.size(); ++cmd_it)
    {
        const float64_t current_time_s = m_dist_based_traj.execution_times[cmd_it].toSec();
        total_traj_time_s += current_time_s - prev_time_s;
        prev_time_s = current_time_s;

        x_vel_points(0, cmd_it)    = current_time_s;
        y_vel_points(0, cmd_it)    = current_time_s;
        yaw_rate_points(0, cmd_it) = current_time_s;
    }

    for (std::size_t cmd_it = 0U; cmd_it < m_dist_based_traj.cmds.size(); ++cmd_it)
    {
        x_vel_points(1, cmd_it)    = m_dist_based_traj.cmds[cmd_it].linear.x;
        y_vel_points(1, cmd_it)    = m_dist_based_traj.cmds[cmd_it].linear.y;
        yaw_rate_points(1, cmd_it) = m_dist_based_traj.cmds[cmd_it].angular.z;
    }

    const Spline1d x_vel_spline    = Spline1dFitting::Interpolate(x_vel_points,    3U);
    const Spline1d y_vel_spline    = Spline1dFitting::Interpolate(y_vel_points,    3U);
    const Spline1d yaw_rate_spline = Spline1dFitting::Interpolate(yaw_rate_points, 3U);

    const float64_t spline_discretization = (m_cfg.getTimeStepMs()/1000.0)/total_traj_time_s;
    const std::size_t num_spline_pts = static_cast<std::size_t>(1.0/spline_discretization);

    for (std::size_t spline_it = 0U; spline_it < num_spline_pts; ++spline_it)
    {
        Eigen::MatrixXd x_vel_pt    = x_vel_spline(static_cast<float64_t>(spline_it)*spline_discretization);
        Eigen::MatrixXd y_vel_pt    = y_vel_spline(static_cast<float64_t>(spline_it)*spline_discretization);
        Eigen::MatrixXd yaw_rate_pt = yaw_rate_spline(static_cast<float64_t>(spline_it)*spline_discretization);

        geometry_msgs::Twist current_state;
        current_state.linear.x  = x_vel_pt(1);
        current_state.linear.y  = y_vel_pt(1);
        current_state.angular.z = yaw_rate_pt(1);

        m_time_based_traj.cmds.emplace_back(current_state);
        m_time_based_traj.execution_times.emplace_back(now_s + ros::Duration(x_vel_pt(0)));
    }
}

void TrajectorySolver::correctAngle(float64_t& angle)
{
    if (angle > 2.0*M_PI)
    {
        angle -= 2.0*M_PI;
    }
    if (angle < 0.0)
    {
        angle += 2*M_PI;
    }
}

} // namespace local_planner


// this must define the same symbol as the main module file (numpy requirement)
#define PY_ARRAY_UNIQUE_SYMBOL andres_graph_PyArray_API
#define NO_IMPORT_ARRAY

#include <Python.h>
#include <boost/python.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "andres/graph/grid-graph.hxx"
#include "graph_exporter.hxx"

namespace bp = boost::python;
namespace agraph = andres::graph;

agraph::GridGraph<2> * makeGridGraph2D(const size_t sx, const size_t sy){
#ifndef NO_CXX11
    return new agraph::GridGraph<2>({sx,sy});
#else
    agraph::GridGraph<2>::VertexCoordinate shape;
    shape[0] = sx;
    shape[1] = sy;
    return new agraph::GridGraph<2>(shape);
#endif
}

agraph::GridGraph<3> * makeGridGraph3D(const size_t sx, const size_t sy, const size_t sz){
#ifndef NO_CXX11
    return new agraph::GridGraph<3>({sx,sy,sz});
#else
    agraph::GridGraph<3>::VertexCoordinate shape;
    shape[0] = sx;
    shape[1] = sy;
    shape[2] = sz;
    return new agraph::GridGraph<3>(shape);
#endif
}

void exportGridGraph2d(){

   bp::class_<agraph::GridGraph<2> > ("GridGraph2D",bp::init<>())
        .def(GraphExporter<agraph::GridGraph<2> >("GridGraph2D"))
        .def("__init__",bp::make_constructor(&makeGridGraph2D))
   ;
}

void exportGridGraph3d(){

   bp::class_<agraph::GridGraph<3> > ("GridGraph3D",bp::init<>())
        .def(GraphExporter<agraph::GridGraph<3> >("GridGraph3D"))
        .def("__init__",bp::make_constructor(&makeGridGraph3D))
   ;
}


/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include "PolyACDDecrypter.h"

#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/vec_ZZ.h>
#include <json/json.h>
#include <sstream>

PolyACDDecrypter::PolyACDDecrypter() {
}

PolyACDDecrypter::~PolyACDDecrypter() {
}

void PolyACDDecrypter::readSecretsFromJSON(std::string& json){
	Json::Value root;   // will contains the root value after parsing.
	Json::Reader reader;
	bool parsingSuccessful = reader.parse(json,root);
	if (parsingSuccessful){
		std::istringstream k1buf(root["k1"].asString());
		k1buf >> k1;
		std::istringstream k2buf(root["k2"].asString());
		k2buf >> k2;
	}
};

NTL::ZZX PolyACDDecrypter::decrypt(NTL::ZZX& ciphertext){
	NTL::ZZ c_l = LeadCoeff(ciphertext);
	NTL::ZZ m_n = c_l/k1;
	NTL::ZZX c;
	long d = deg(ciphertext)+1;
	c.SetLength(d);
	for (long i=0; i<d-1; i++){
		SetCoeff(c,i,coeff(ciphertext,i));
	}
	SetCoeff(c,d,m_n);
	SetCoeff(c,d-1,c_l-k1*m_n);
	NTL::ZZX m = c/k2;
	return m;
};


#pragma once
#ifndef OPENGM_INFERENCE_SAT_HXX
#define OPENGM_INFERENCE_SAT_HXX

#include <iostream>
#include <vector>

#include <boost/config.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/foreach.hpp>

#include "opengm/inference/inference.hxx"
#include "opengm/operations/and.hxx"
#include "opengm/operations/or.hxx"
#include "opengm/inference/visitors/visitors.hxx"

namespace opengm {

   /// 2-SAT solver
   ///
   /// \ingroup inference
   template<class GM>
   class SAT : Inference<GM, opengm::Or> {
   public:
      typedef opengm::Or AccumulationType;
      typedef GM GraphicalModelType;
      OPENGM_GM_TYPE_TYPEDEFS;

      struct Parameter {};

      SAT(const GraphicalModelType&, const Parameter& = Parameter());
      std::string name() const;
      const GraphicalModelType& graphicalModel() const;
      InferenceTermination infer();
      template<class VISITOR>
      InferenceTermination infer(VISITOR &);
      virtual void reset();
      ValueType value() const;
      typedef visitors::VerboseVisitor<SAT<GM> >        VerboseVisitorType;
      typedef visitors::TimingVisitor<SAT<GM> >         TimingVisitorType;
		typedef visitors::EmptyVisitor<SAT<GM> >          EmptyVisitorType;
   private:
      const GraphicalModelType& gm_;
      std::vector<int> component_;
   };

   template<class GM>
   inline SAT<GM>::SAT
   (
      const GraphicalModelType& gm,
      const Parameter& para
   )
   :  gm_(gm)
   {
      if(!NO_DEBUG) {
         OPENGM_ASSERT(gm_.factorOrder() <= 2);
         OPENGM_ASSERT(typeid(OperatorType) == typeid(opengm::And));
         for(size_t i=0; i<gm_.numberOfVariables();++i) {
            OPENGM_ASSERT(gm_.numberOfLabels(i) == 2);
         }
      }
   }
   template<class GM>
   void
   inline SAT<GM>::reset()
   {
   }

   template<class GM>
   inline std::string
   SAT<GM>::name() const
   {
      return "2Sat";
   }

   template<class GM>
   inline const GM&
   SAT<GM>::graphicalModel() const
   {
      return gm_;
   }

   template<class GM>
   InferenceTermination
   SAT<GM>::infer() {
      EmptyVisitorType v;
      return infer(v);
   }

   template<class GM>
   template<class VISITOR>
   InferenceTermination
   SAT<GM>::infer
   (
      VISITOR & visitor
   ) {
      visitor.begin(*this);
      typedef std::pair<int, int> clause;
      typedef boost::adjacency_list<> Graph; // properties of our graph. by default: oriented graph
      // build graph
      Graph g(gm_.numberOfVariables() * 2);
      for(size_t f=0; f<gm_.numberOfFactors(); ++f) {
         if(gm_[f].numberOfVariables() != 2) {
            throw RuntimeError("This implementation of the 2-SAT solver supports only factors of order 2.");
         }
         std::vector<size_t> vec(2);
         for(vec[0]=0; vec[0]<2; ++vec[0]) {
            for(vec[1]=0; vec[1]<2; ++vec[1]) {
               if(!gm_[f](vec.begin())) {
                  const int  v1=gm_[f].variableIndex(0)+(1-vec[0])*gm_.numberOfVariables();
                  const int nv1=gm_[f].variableIndex(0)+(0+vec[0])*gm_.numberOfVariables();
                  const int  v2=gm_[f].variableIndex(1)+(1-vec[1])*gm_.numberOfVariables();
                  const int nv2=gm_[f].variableIndex(1)+(0+vec[1])*gm_.numberOfVariables();
                  boost::add_edge(nv1,v2,g);
                  boost::add_edge(nv2,v1,g);
               }
            }
         }
      }
      component_.resize(num_vertices(g));
      strong_components(g, make_iterator_property_map(component_.begin(), get(boost::vertex_index, g)));
      visitor.end(*this);
      return NORMAL;
   }

   template<class GM>
   inline typename GM::ValueType
   SAT<GM>::value() const
   {
      bool satisfied = true;
      for(IndexType i=0; i<gm_.numberOfVariables(); i++) {
         if(component_[i] == component_[i+gm_.numberOfVariables()]) {
            satisfied = false;
         }
      }
      return satisfied;
   }

} // namespace opengm

#endif // #ifndef OPENGM_INFERENCE_SAT_HXX


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/orderable.hpp>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto int_constants = tuple_c<int, -10, -2, 0, 1, 3, 4>;

    //////////////////////////////////////////////////////////////////////////
    // Comparable and Orderable
    //////////////////////////////////////////////////////////////////////////
    {
        // operators
        static_assert(has_operator<IntegralConstant<int>, decltype(equal)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(not_equal)>{}, "");

        static_assert(has_operator<IntegralConstant<int>, decltype(less)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(less_equal)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(greater)>{}, "");
        static_assert(has_operator<IntegralConstant<int>, decltype(greater_equal)>{}, "");

        // laws
        test::TestComparable<IntegralConstant<int>>{int_constants};
        test::TestOrderable<IntegralConstant<int>>{int_constants};
    }
}


#pragma once

// ros
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <image_transport/camera_subscriber.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <std_msgs/msg/header.hpp>
#include <cv_bridge/cv_bridge.h>

// apriltag
#include <apriltag.h>
#include "common/homography.h"
#include <tf2_ros/transform_broadcaster.h>
#include <Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
#include <apriltag_msgs/msg/april_tag_detection.hpp>

class AprilTagNode : public rclcpp::Node {
public:
    AprilTagNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions());

    ~AprilTagNode() override;

private:
    typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Mat3;

    apriltag_family_t* tf;
    apriltag_detector_t* const td;
    std::string tag_family;
    double tag_edge_size;
    int max_hamming;
    std::unordered_map<int, std::string> tag_frames;
    std::unordered_map<int, double> tag_sizes;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    bool remove_duplicates_ = true;
    bool z_up ;
    // function pointer for tag family creation / destruction
    static const std::map<std::string, apriltag_family_t *(*)(void)> tag_create;
    const static std::map<std::string, void (*)(apriltag_family_t*)> tag_destroy;

    image_transport::CameraSubscriber sub_cam;
    const rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_detections;

    void onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img,
                const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci);

    void getPose(const matd_t& H, geometry_msgs::msg::Transform& t, const double size) const;
    void addObjectPoints(double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints) const;
    void addImagePoints (apriltag_detection_t *detection, std::vector<cv::Point2d >& imagePoints) const;
    Eigen::Matrix4d getRelativeTransform(
        std::vector<cv::Point3d > objectPoints,
        std::vector<cv::Point2d > imagePoints,
        double fx, double fy, double cx, double cy) const;

    geometry_msgs::msg::TransformStamped makeTagPose(
        const Eigen::Matrix4d& transform,
        const Eigen::Quaternion<double> rot_quaternion,
        const std_msgs::msg::Header& header);

    static int idComparison (const void* first, const void* second);
    void removeDuplicates(zarray_t* detections_ );

};


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2022 Ilia Shirobokov <i.shirobokov@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE plonk_constraint_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/mnt4.hpp>
#include <nil/crypto3/algebra/curves/pallas.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/pallas.hpp>
#include <nil/crypto3/algebra/random_element.hpp>

#include <nil/crypto3/math/polynomial/polynomial.hpp>
#include <nil/crypto3/math/polynomial/lagrange_interpolation.hpp>
#include <nil/crypto3/math/algorithms/unity_root.hpp>
#include <nil/crypto3/math/domains/evaluation_domain.hpp>
#include <nil/crypto3/math/algorithms/make_evaluation_domain.hpp>

#include <nil/crypto3/zk/snark/arithmetization/plonk/constraint.hpp>
#include <nil/crypto3/zk/snark/arithmetization/plonk/variable.hpp>
#include <nil/crypto3/zk/snark/arithmetization/plonk/params.hpp>
#include <nil/crypto3/zk/snark/arithmetization/plonk/assignment.hpp>

using namespace nil::crypto3;

BOOST_AUTO_TEST_SUITE(plonk_constraint_test_suite)

BOOST_AUTO_TEST_CASE(plonk_constraint_basic_test) {

    // setup
    using curve_type = algebra::curves::pallas;
    using FieldType = typename curve_type::base_field_type;

    using var = zk::snark::plonk_variable<FieldType>;

    using constraint_type = zk::snark::plonk_constraint<FieldType>;

    using arithmetization_params = zk::snark::plonk_arithmetization_params<5,5,5,5>;

    constraint_type constraint = var(0,0) + var(1,0) - var(2,0);
    constraint_type constraint1 = var(0,0) + var(1,0) - 2;
    constraint_type constraint2 = 2 - (var(0,0) + var(1,0));
    constraint_type constraint3 = 2 - var(0,0);
    constraint_type constraint4 = 2 - var(0,0)*var(0,0);
    constraint_type constraint5 = var(0,0) - var(0,0)*var(0,0);
    constraint_type constraint6 = var(0,0)*var(0,0) + var(0,0);
    constraint_type constraint7 = var(0,0)*var(0,0) - var(0,0);
    constraint_type constraint8 = var(0,0).pow(2) - var(0,0);
    constraint_type constraint9 = var(0,0).pow(1) - var(0,0);

    std::array<zk::snark::plonk_column<FieldType>,
        arithmetization_params::WitnessColumns> witness_columns;
    witness_columns[0] = {algebra::random_element<FieldType>()};
    witness_columns[1] = {algebra::random_element<FieldType>()};
    witness_columns[2] = {algebra::random_element<FieldType>()};

    std::cout << witness_columns[0][0].data << std::endl;
    std::cout << witness_columns[1][0].data << std::endl;
    std::cout << witness_columns[2][0].data << std::endl;

    zk::snark::plonk_private_assignment_table<FieldType,
        arithmetization_params> private_assignment(witness_columns);

    zk::snark::plonk_assignment_table<FieldType,
        arithmetization_params> assignment(private_assignment);

    BOOST_CHECK((witness_columns[0][0] + witness_columns[1][0] - witness_columns[2][0]) ==
        constraint.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0] + witness_columns[1][0] - typename FieldType::value_type(2)) ==
        constraint1.evaluate(0, assignment));

    BOOST_CHECK((typename FieldType::value_type(2) - (witness_columns[0][0] + witness_columns[1][0])) ==
        constraint2.evaluate(0, assignment));

    BOOST_CHECK((typename FieldType::value_type(2) - witness_columns[0][0]) ==
        constraint3.evaluate(0, assignment));

    BOOST_CHECK((typename FieldType::value_type(2) - witness_columns[0][0]*witness_columns[0][0]) ==
        constraint4.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0] - witness_columns[0][0]*witness_columns[0][0]) ==
        constraint5.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0]*witness_columns[0][0] + witness_columns[0][0]) ==
        constraint6.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0]*witness_columns[0][0] - witness_columns[0][0]) ==
        constraint7.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0].pow(2) - witness_columns[0][0]) ==
        constraint8.evaluate(0, assignment));

    BOOST_CHECK((witness_columns[0][0] - witness_columns[0][0]) ==
        constraint9.evaluate(0, assignment));
}

BOOST_AUTO_TEST_SUITE_END()