/*
 * GridMapMathTest.cpp
 *
 *  Created on: Feb 10, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_core/GridMapMath.hpp"

// Eigen
#include <Eigen/Core>

// gtest
#include <gtest/gtest.h>

// Limits
#include <cfloat>

// Vector
#include <vector>

using namespace std;
using namespace grid_map;

TEST(PositionFromIndex, Simple)
{
  Length mapLength(3.0, 2.0);
  Position mapPosition(-1.0, 2.0);
  double resolution = 1.0;
  Size bufferSize(3, 2);
  Position position;

  EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(1, 0), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(1, 1), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(2, 1), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());

  EXPECT_FALSE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize));
}

TEST(PositionFromIndex, CircularBuffer)
{
  Length mapLength(0.5, 0.4);
  Position mapPosition(-0.1, 13.4);
  double resolution = 0.1;
  Size bufferSize(5, 4);
  Index bufferStartIndex(3, 1);
  Position position;

  EXPECT_TRUE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Index(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y());

  EXPECT_FALSE(getPositionFromIndex(position, Index(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
}

TEST(IndexFromPosition, Simple)
{
  Length mapLength(3.0, 2.0);
  Position mapPosition(-12.4, -7.1);
  double resolution = 1.0;
  Index bufferSize(3, 2);
  Index index;

  EXPECT_TRUE(getIndexFromPosition(index, Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_FALSE(getIndexFromPosition(index, Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
}

TEST(IndexFromPosition, EdgeCases)
{
  Length mapLength(3.0, 2.0);
  Position mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Size bufferSize(3, 2);
  Index index;

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_FALSE(getIndexFromPosition(index, Position(-1.5, 1.0), mapLength, mapPosition, resolution, bufferSize));
}

TEST(IndexFromPosition, CircularBuffer)
{
  Length mapLength(0.5, 0.4);
  Position mapPosition(0.4, -0.9);
  double resolution = 0.1;
  Size bufferSize(5, 4);
  Index bufferStartIndex(3, 1);
  Index index;

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Position(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));
}

TEST(checkIfPositionWithinMap, Inside)
{
  Length mapLength(50.0, 25.0);
  Position mapPosition(11.4, 0.0);

  EXPECT_TRUE(checkIfPositionWithinMap(Position(0.0, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(5.0, 5.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, 10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, -10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, 10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, -10.0) + mapPosition, mapLength, mapPosition));
}

TEST(checkIfPositionWithinMap, Outside)
{
  Length mapLength(10.0, 5.0);
  Position mapPosition(-3.0, 145.2);

  EXPECT_FALSE(checkIfPositionWithinMap(Position(5.5, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 3.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, -3.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(3.0, 3.0) + mapPosition, mapLength, mapPosition));
}

TEST(checkIfPositionWithinMap, EdgeCases)
{
  Length mapLength(2.0, 3.0);
  Position mapPosition(0.0, 0.0);

  /*
  *
  *  A (is inside)             B (is not inside)
  *   +-----------------------+
  *   |                       |
  *   |                       |
  *   |              X        |
  *   |             ^         |
  *   |             |         |
  *   |             |         |
  *   |       <-----+         |
  *   |      Y                |
  *   |                       |
  *   |                       |
  *   |                       |
  *   +-----------------------+
  *  C (is not inside)         D (is not inside)
  *
  * Resulting coordinates are:
  *  A: (1.0, 1.5)
  *  B: (1.0, -1.5)
  *  C: (-1.0, 1.5)
  *  D: (-1.0, -1.5)
  *
  */

  // Noise around A.
  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));

  // Noise around B.
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, - 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, - 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 + DBL_EPSILON), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 - DBL_EPSILON), mapLength, mapPosition));

  // Noise around C.
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));

  // Noise around D.
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, -1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, -1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 + DBL_EPSILON), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition));

  // Extra tests.
  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition));
}

TEST(getIndexShiftFromPositionShift, All)
{
  double resolution = 1.0;
  Index indexShift;

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution));
  EXPECT_EQ(0, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution));
  EXPECT_EQ(0, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution));
  EXPECT_EQ(-1, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution));
  EXPECT_EQ(1, indexShift(0));
  EXPECT_EQ(3, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2));
  EXPECT_EQ(2, indexShift(0));
  EXPECT_EQ(0, indexShift(1));
}

TEST(getPositionShiftFromIndexShift, All)
{
  double resolution = 0.3;
  Vector positionShift;

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution));
  EXPECT_DOUBLE_EQ(0.0, positionShift.x());
  EXPECT_DOUBLE_EQ(0.0, positionShift.y());

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution));
  EXPECT_DOUBLE_EQ(-0.3, positionShift.x());
  EXPECT_DOUBLE_EQ(0.3, positionShift.y());

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution));
  EXPECT_DOUBLE_EQ(-0.6, positionShift.x());
  EXPECT_DOUBLE_EQ(-0.3, positionShift.y());
}

TEST(checkIfIndexInRange, All)
{
  Size bufferSize(10, 15);
  EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize));
  EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize));
  EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize));
  EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize));
  EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize));
  EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize));
}

TEST(boundIndexToRange, All)
{
  int index;
  int bufferSize = 10;

  index = 0;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 1;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = -1;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 9;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 10;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 35;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = -19;
  boundIndexToRange(index, bufferSize);
  EXPECT_EQ(0, index);
}

TEST(wrapIndexToRange, All)
{
  int index;
  int bufferSize = 10;

  index = 0;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 1;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = -1;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 9;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 10;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 11;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = 35;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(5, index);

  index = -9;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = -19;
  wrapIndexToRange(index, bufferSize);
  EXPECT_EQ(1, index);
}

TEST(boundPositionToRange, Simple)
{
  double epsilon = 11.0 * numeric_limits<double>::epsilon();

  Length mapLength(30.0, 10.0);
  Position mapPosition(0.0, 0.0);
  Position position;

  position << 0.0, 0.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_DOUBLE_EQ(0.0, position.x());
  EXPECT_DOUBLE_EQ(0.0, position.y());

  position << 15.0, 5.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -15.0, -5.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon);
  EXPECT_LE(-5.0, position.y());

  position << 16.0, 6.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -16.0, -6.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon);
  EXPECT_LE(-5.0, position.y());

  position << 1e6, 1e6;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -1e6, -1e6;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon);
  EXPECT_LE(-5.0, position.y());
}

TEST(boundPositionToRange, Position)
{
  double epsilon = 11.0 * numeric_limits<double>::epsilon();

  Length mapLength(30.0, 10.0);
  Position mapPosition(1.0, 2.0);
  Position position;

  position << 0.0, 0.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_DOUBLE_EQ(0.0, position.x());
  EXPECT_DOUBLE_EQ(0.0, position.y());

  position << 16.0, 7.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -14.0, -3.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon);
  EXPECT_LE(-3.0, position.y());

  position << 17.0, 8.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -15.0, -4.0;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon);
  EXPECT_LE(-3.0, position.y());

  position << 1e6, 1e6;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -1e6, -1e6;
  boundPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon);
  EXPECT_LE(-3.0, position.y());
}

TEST(getSubmapInformation, Simple)
{
  // Map
  Length mapLength(5.0, 4.0);
  Position mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Size bufferSize(5, 4);

  // Requested submap
  Position requestedSubmapPosition;
  Position requestedSubmapLength;

  // The returned submap indeces
  Index submapTopLeftIndex;
  Index submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  requestedSubmapPosition << 0.0, 0.5;
  requestedSubmapLength << 0.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                           requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(1, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, Zero)
{
  // Map
  Length mapLength(5.0, 4.0);
  Position mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Size bufferSize(5, 4);

  // Requested submap
  Position requestedSubmapPosition;
  Length requestedSubmapLength;

  // The returned submap indeces
  Index submapTopLeftIndex;
  Index submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  requestedSubmapPosition << -1.0, -0.5;
  requestedSubmapLength << 0.0, 0.0;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(3, submapTopLeftIndex(0));
  EXPECT_EQ(2, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(1, submapSize(1));
  EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x());
  EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y());
  EXPECT_DOUBLE_EQ(resolution, submapLength(0));
  EXPECT_DOUBLE_EQ(resolution, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, ExceedingBoundaries)
{
  // Map
  Length mapLength(5.0, 4.0);
  Position mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Size bufferSize(5, 4);

  // Requested submap
  Position requestedSubmapPosition;
  Length requestedSubmapLength;

  // The returned submap indeces
  Index submapTopLeftIndex;
  Size submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  requestedSubmapPosition << 2.0, 1.5;
  requestedSubmapLength << 2.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(2, submapSize(1));
  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));

  requestedSubmapPosition << 0.0, 0.0;
  requestedSubmapLength << 1e6, 1e6;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(bufferSize(0), submapSize(0));
  EXPECT_EQ(bufferSize(1), submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
  EXPECT_EQ(2, requestedIndexInSubmap(0));
  EXPECT_LE(1, requestedIndexInSubmap(1));
  EXPECT_GE(2, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, CircularBuffer)
{
  // Map
  Length mapLength(5.0, 4.0);
  Position mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Size bufferSize(5, 4);
  Index bufferStartIndex(2, 1);

  // Requested submap
  Position requestedSubmapPosition;
  Length requestedSubmapLength;

  // The returned submap indeces
  Index submapTopLeftIndex;
  Size submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  requestedSubmapPosition << 0.0, 0.5;
  requestedSubmapLength << 0.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(4, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(1, requestedIndexInSubmap(1));

  requestedSubmapPosition << 2.0, 1.5;
  requestedSubmapLength << 2.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(2, submapSize(1));
  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));

  requestedSubmapPosition << 0.0, 0.0;
  requestedSubmapLength << 1e6, 1e6;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(bufferSize(0), submapSize(0));
  EXPECT_EQ(bufferSize(1), submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
  EXPECT_EQ(2, requestedIndexInSubmap(0));
  EXPECT_LE(1, requestedIndexInSubmap(1));
  EXPECT_GE(2, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, Debug1)
{
  // Map
  Length mapLength(4.98, 4.98);
  Position mapPosition(-4.98, -5.76);
  double resolution = 0.06;
  Size bufferSize(83, 83);
  Index bufferStartIndex(0, 13);

  // Requested submap
  Position requestedSubmapPosition(-7.44, -3.42);
  Length requestedSubmapLength(0.12, 0.12);

  // The returned submap indeces
  Index submapTopLeftIndex;
  Size submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.12, submapLength(0));
  EXPECT_DOUBLE_EQ(0.18, submapLength(1));
}

TEST(getSubmapInformation, Debug2)
{
  // Map
  Length mapLength(4.98, 4.98);
  Position mapPosition(2.46, -25.26);
  double resolution = 0.06;
  Size bufferSize(83, 83);
  Index bufferStartIndex(42, 6);

  // Requested submap
  Position requestedSubmapPosition(0.24, -26.82);
  Length requestedSubmapLength(0.624614, 0.462276);

  // The returned submap indeces
  Index submapTopLeftIndex;
  Size submapSize;
  Position submapPosition;
  Length submapLength;
  Index requestedIndexInSubmap;

  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_LT(0, submapSize(0));
  EXPECT_LT(0, submapSize(1));
  EXPECT_LT(0.0, submapLength(0));
  EXPECT_LT(0.0, submapLength(1));
}

TEST(getBufferRegionsForSubmap, Trivial)
{
  Size bufferSize(5, 4);
  Index submapIndex(0, 0);
  Size submapSize(0, 0);
  std::vector<BufferRegion> regions;

  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
  EXPECT_EQ(1, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
  EXPECT_EQ(0, regions[0].getSize()[0]);
  EXPECT_EQ(0, regions[0].getSize()[1]);

  submapSize << 0, 7;
  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));

  submapSize << 6, 7;
  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
}

TEST(getBufferRegionsForSubmap, Simple)
{
  Size bufferSize(5, 4);
  Index submapIndex(1, 2);
  Size submapSize(3, 2);
  std::vector<BufferRegion> regions;

  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
  EXPECT_EQ(1, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
  EXPECT_EQ(2, regions[0].getStartIndex()[1]);
  EXPECT_EQ(3, regions[0].getSize()[0]);
  EXPECT_EQ(2, regions[0].getSize()[1]);
}

TEST(getBufferRegionsForSubmap, CircularBuffer)
{
  Size bufferSize(5, 4);
  Index submapIndex;
  Size submapSize;
  Index bufferStartIndex(3, 1);
  std::vector<BufferRegion> regions;

  submapIndex << 3, 1;
  submapSize << 2, 3;
  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
  EXPECT_EQ(2, regions[0].getSize()[0]);
  EXPECT_EQ(3, regions[0].getSize()[1]);

  submapIndex << 4, 1;
  submapSize << 2, 3;
  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
  EXPECT_EQ(4, regions[0].getStartIndex()[0]);
  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
  EXPECT_EQ(1, regions[0].getSize()[0]);
  EXPECT_EQ(3, regions[0].getSize()[1]);
  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant());
  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
  EXPECT_EQ(1, regions[1].getStartIndex()[1]);
  EXPECT_EQ(1, regions[1].getSize()[0]);
  EXPECT_EQ(3, regions[1].getSize()[1]);

  submapIndex << 1, 0;
  submapSize << 2, 1;
  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant());
  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
  EXPECT_EQ(2, regions[0].getSize()[0]);
  EXPECT_EQ(1, regions[0].getSize()[1]);

  submapIndex << 3, 1;
  submapSize << 5, 4;
  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\
  EXPECT_EQ(4, regions.size());
  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
  EXPECT_EQ(2, regions[0].getSize()[0]);
  EXPECT_EQ(3, regions[0].getSize()[1]);
  EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant());
  EXPECT_EQ(3, regions[1].getStartIndex()[0]);
  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
  EXPECT_EQ(2, regions[1].getSize()[0]);
  EXPECT_EQ(1, regions[1].getSize()[1]);
  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant());
  EXPECT_EQ(0, regions[2].getStartIndex()[0]);
  EXPECT_EQ(1, regions[2].getStartIndex()[1]);
  EXPECT_EQ(3, regions[2].getSize()[0]);
  EXPECT_EQ(3, regions[2].getSize()[1]);
  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant());
  EXPECT_EQ(0, regions[3].getStartIndex()[0]);
  EXPECT_EQ(0, regions[3].getStartIndex()[1]);
  EXPECT_EQ(3, regions[3].getSize()[0]);
  EXPECT_EQ(1, regions[3].getSize()[1]);
}

TEST(checkIncrementIndex, Simple)
{
  Index index(0, 0);
  Size bufferSize(4, 3);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(1, index[1]);

  for (int i = 0; i < 6; i++) {
    EXPECT_TRUE(incrementIndex(index, bufferSize));
  }
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_FALSE(incrementIndex(index, bufferSize));
  EXPECT_EQ(index[0], index[0]);
  EXPECT_EQ(index[1], index[1]);
}

TEST(checkIncrementIndex, CircularBuffer)
{
  Size bufferSize(4, 3);
  Index bufferStartIndex(2, 1);
  Index index(bufferStartIndex);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(index[0], index[0]);
  EXPECT_EQ(index[1], index[1]);
}

TEST(checkIncrementIndexForSubmap, Simple)
{
  Index submapIndex(0, 0);
  Index index;
  Index submapTopLeftIndex(3, 1);
  Size submapBufferSize(2, 4);
  Size bufferSize(8, 5);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(1, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(2, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(3, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(0, submapIndex[1]);
  EXPECT_EQ(4, index[0]);
  EXPECT_EQ(1, index[1]);

  submapIndex << 1, 2;
  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(4, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));

  submapIndex << 2, 0;
  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
}

TEST(checkIncrementIndexForSubmap, CircularBuffer)
{
  Index submapIndex(0, 0);
  Index index;
  Index submapTopLeftIndex(6, 3);
  Size submapBufferSize(2, 4);
  Size bufferSize(8, 5);
  Index bufferStartIndex(3, 2);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(1, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(2, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(0, submapIndex[1]);
  EXPECT_EQ(7, index[0]);
  EXPECT_EQ(3, index[1]);

  submapIndex << 1, 2;
  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(7, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));

  submapIndex << 2, 0;
  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
}

TEST(getIndexFromLinearIndex, Simple)
{
  EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all());
  EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all());
  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all());
  EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all());
  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all());
  EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all());
}


// Implementation of Discrete Conformal Seamless Similarity Mapping
// along the lines of
// [Campen and Zorin 2017]: "Similarity Maps and Field-Guided T-Splines: a Perfect Couple"
// [Campen and Zorin 2017]: "On Discrete Conformal Seamless Similarity Maps"
//
// Author: Marcel Campen
//
// Version 1.0
// 21 July 2017

#include <Eigen/Sparse>
#include <set>
#include <queue>
#include <vector>
#include <igl/Timer.h>

class Mesh {
public:
  std::vector<int> n; // next halfedge of halfedge
  std::vector<int> to; // to vertex of halfedge
  std::vector<int> f; // face of halfedge
  std::vector<int> h; // one halfedge of face
  std::vector<int> out; // one outgoing halfedge of vertex

  std::vector<double> l; // discrete metric (length per edge)

  int n_halfedges() { return n.size(); }
  int n_edges() { return n_halfedges()/2; }
  int n_faces() { return h.size(); }
  int n_vertices() { return out.size(); }

  int e(int h) { return h/2; }
  int opp(int h) { return (h%2 == 0) ? (h+1) : (h-1); }
  int v0(int h) { return to[opp(h)]; }
  int v1(int h) { return to[h]; }
  int h0(int e) { return e*2; }
  int h1(int e) { return e*2+1; }
  double sign(int h) { return (h%2 == 0) ? 1.0 : -1.0; }

  virtual void init() {};

  virtual bool flip_ccw(int _h)
  {
    int ha = _h;
    int hb = opp(_h);
    int f0 = f[ha];
    int f1 = f[hb];
    if(f0 == f1) return false;
    int h2 = n[ha];
    int h3 = n[h2];
    int h4 = n[hb];
    int h5 = n[h4];
    out[to[hb]] = h4;
    out[to[ha]] = h2;
    f[h4] = f0;
    f[h2] = f1;
    h[f0] = h4;
    h[f1] = h2;
    to[ha] = to[h2];
    to[hb] = to[h4];
    n[h5] = h2;
    n[h3] = h4;
    n[h2] = hb;
    n[h4] = ha;
    n[ha] = h3;
    n[hb] = h5;
    return true;
  }

  virtual void get_mesh(std::vector<int>& _n, // next halfedge of halfedge
                        std::vector<int>& _to, // to vertex of halfedge
                        std::vector<int>& _f, // face of halfedge
                        std::vector<int>& _h, // one halfedge of face
                        std::vector<int>& _out) // one outgoing halfedge of vertex
  {
    _n = n;
    _to = to;
    _f = f;
    _h = h;
    _out = out;
  }

  template<typename T>
  std::vector<T> interpolate(const std::vector<T>& u)
  {
    return u;
  }

  bool is_complex()
  {
    int nh = n_halfedges();
    for(int i = 0; i < nh; i++)
    {
      if(to[i] == to[opp(i)]) return true; //contains loop edge
    }
    int nv = n_vertices();
    for(int i = 0; i < nv; i++)
    {
      std::set<int> onering;
      int h = out[i];
      if(h < 0) continue;
      int k = h;
      do {
        int v = to[k];
        if(onering.find(v) != onering.end()) return true; //contains multi-edges
        onering.insert(v);
        k = n[opp(k)];
      } while(k != h);
    }
    return false;
  }
};

class ConformalSeamlessSimilarityMapping {
public:

  Mesh& m;

  std::vector<double> Theta_hat; //target cone angles per vertex
  std::vector<double> kappa_hat; //target holonomy angles per gamma loop
  std::vector< std::vector<int> > gamma; //directed dual loops, represented by halfedges (the ones adjacent to the earlier triangles in the dual loop)

  const double cot_infty = 1e10;

  int n_s;
  int n_e;
  int n_h;
  int n_f;
  int n_v;

  std::vector<double> xi;
  std::vector<double> delta_xi;
  std::vector<double> cot_alpha;
  std::vector<double> alpha;

  Eigen::SparseMatrix<double> A;
  Eigen::VectorXd b;

  std::string log_dir;
  std::string name;
  double eps = 1e-12;

  ConformalSeamlessSimilarityMapping(Mesh& _m, const std::vector<double>& _Theta_hat, const std::vector<double>& _kappa_hat, std::vector< std::vector<int> >& _gamma, std::string _log_dir="", std::string _name="", double _eps=1e-12) : m(_m), Theta_hat(_Theta_hat), kappa_hat(_kappa_hat), gamma(_gamma), log_dir(_log_dir), name(_name), eps(_eps)
  {
    n_s = gamma.size();
    n_e = m.n_edges();
    n_h = m.n_halfedges();
    n_f = m.n_faces();
    n_v = m.n_vertices();

    xi.resize(n_h, 0.0);
    delta_xi.resize(n_h, 0.0);
    cot_alpha.resize(n_h);
    alpha.resize(n_h);
  }

  void log(const char* c)
  {
    std::cout << c << std::endl;
  }

  void compute_angles() // compute alpha and cot_alpha from scaled edge lengths
  {
    #pragma omp parallel for
    for(int f = 0; f < n_f; f++)
    {
      int hi = m.h[f];
      int hj = m.n[hi];
      int hk = m.n[hj];
      // (following "On Discrete Conformal Seamless Similarity Maps")
      double li = m.l[m.e(hi)] * std::exp(1.0/6.0*(xi[hk]-xi[hj]));
      double lj = m.l[m.e(hj)] * std::exp(1.0/6.0*(xi[hi]-xi[hk]));
      double lk = m.l[m.e(hk)] * std::exp(1.0/6.0*(xi[hj]-xi[hi]));
      // (following "A Cotangent Laplacian for Images as Surfaces")
      double s = (li+lj+lk)/2.0;
      double Aijk4 = 4.0*std::sqrt(std::max(0.0, s*(s-li)*(s-lj)*(s-lk)));
      double Ijk = (-li*li+lj*lj+lk*lk);
      double iJk = (li*li-lj*lj+lk*lk);
      double ijK = (li*li+lj*lj-lk*lk);
      cot_alpha[hi] = Aijk4 == 0.0 ? copysign(cot_infty,Ijk) : (Ijk/Aijk4);
      cot_alpha[hj] = Aijk4 == 0.0 ? copysign(cot_infty,iJk) : (iJk/Aijk4);
      cot_alpha[hk] = Aijk4 == 0.0 ? copysign(cot_infty,ijK) : (ijK/Aijk4);

      alpha[hi] = std::acos(std::min(1.0, std::max(-1.0, Ijk/(2.0*lj*lk))));
      alpha[hj] = std::acos(std::min(1.0, std::max(-1.0, iJk/(2.0*lk*li))));
      alpha[hk] = std::acos(std::min(1.0, std::max(-1.0, ijK/(2.0*li*lj))));
    }
  }

  void setup_b() // system right-hand sid
  {
    b.resize(n_v-1 + n_s + n_f-1);
    b.fill(0.0);

    std::vector<double> Theta(n_v, 0.0);
    std::vector<double> kappa(n_s, 0.0);

    for(int h = 0; h < n_h; h++)
    {
      Theta[m.to[m.n[h]]] += alpha[h];
    }
    #pragma omp parallel for
    for(int r = 0; r < n_v-1; r++)
    {
      b[r] = Theta_hat[r] - Theta[r];
    }
    #pragma omp parallel for
    for(int s = 0; s < n_s; s++)
    {
      kappa[s] = 0.0;
      int loop_size = gamma[s].size();
      for(int si = 0; si < loop_size; si++)
      {
        int h = gamma[s][si];
        int hn = m.n[h];
        int hnn = m.n[hn];
        if(m.opp(hn) == gamma[s][(si+1)%loop_size])
          kappa[s] -= alpha[hnn];
        else if(m.opp(hnn) == gamma[s][(si+1)%loop_size])
          kappa[s] += alpha[hn];
        else std::cerr << "ERROR: loop is broken" << std::endl;
      }
      b[n_v-1+s] = kappa_hat[s] - kappa[s];
    }
  }

  void setup_A() // system matrix
  {
    A.resize(n_v-1 + n_s + n_f-1, n_e);
    int loop_trips = 0;
    for(int i = 0; i < n_s; i++)
      loop_trips += gamma[i].size();

    typedef Eigen::Triplet<double> Trip;
    std::vector<Trip> trips;
    trips.clear();
    trips.resize(n_h*2 + loop_trips + (n_f-1)*3);
    #pragma omp parallel for
    for(int h = 0; h < n_h; h++)
    {
      int v0 = m.v0(h);
      int v1 = m.v1(h);
      if(v0 < n_v-1) trips[h*2] = Trip(v0, m.e(h), m.sign(h)*0.5*cot_alpha[h]);
      if(v1 < n_v-1) trips[h*2+1] = Trip(v1, m.e(h), -m.sign(h)*0.5*cot_alpha[h]);
    }

    int base = n_h*2;
    for(int s = 0; s < n_s; s++)
    {
      int loop_size = gamma[s].size();
      #pragma omp parallel for
      for(int si = 0; si < loop_size; si++)
      {
        int h = gamma[s][si];
        trips[base+si] = Trip(n_v-1 + s, m.e(h), m.sign(h)*0.5*(cot_alpha[h]+cot_alpha[m.opp(h)]));
      }
      base += loop_size;
    }

    #pragma omp parallel for
    for(int f = 0; f < n_f-1; f++)
    {
      int hi = m.h[f];
      int hj = m.n[hi];
      int hk = m.n[hj];
      trips[base+f*3] = Trip(n_v-1 + n_s + f, m.e(hi), m.sign(hi));
      trips[base+f*3+1] = Trip(n_v-1 + n_s + f, m.e(hj), m.sign(hj));
      trips[base+f*3+2] = Trip(n_v-1 + n_s + f, m.e(hk), m.sign(hk));
    }

    A.setFromTriplets(trips.begin(), trips.end());
  }

  double I(int i, int j, int k, double lambda = 0.0)
  {
    return m.l[m.e(i)]*std::exp((-xi[j]-delta_xi[j]*lambda)/2) + m.l[m.e(j)]*std::exp((xi[i]+delta_xi[i]*lambda)/2) - m.l[m.e(k)];
  }

  double firstDegeneracy(int& degen, double lambda)
  {
    bool repeat = true;
    while(repeat)
    {
      repeat = false;
      #pragma omp parallel for
      for(int i = 0; i < n_h; i++)
      {
        int j = m.n[i];
        int k = m.n[j];
        double local_lambda = lambda;
        if(I(i,j,k,local_lambda) < 0.0)
        {
          // root finding (from below) by bracketing bisection
          double lo = 0.0;
          double hi = local_lambda;
          for(int r = 0; r < 100; r++)
          {
            double mid = (lo+hi)*0.5;
            if(I(i,j,k,mid) <= 0.0)
              hi = mid;
            else
              lo = mid;
          }

          #pragma omp critical
          {
            if(lo < lambda)
            {
              lambda = lo;
              degen = k;
              repeat = true;
            }
          }
        }
      }
    }
    return lambda;
  }

  double avg_abs(const Eigen::VectorXd& v)
  {
    double res = 0.0;
    int v_size = v.size();
    for(int i = 0; i < v_size; i++)
      res += std::abs(b[i]);
    return res/v_size;
  }

  double max_abs(const Eigen::VectorXd& v)
  {
    double res = 0.0;
    int v_size = v.size();
    for(int i = 0; i < v_size; i++)
      res = std::max(res, std::abs(b[i]));
    return res;
  }

  void compute_metric()
  {

    igl::Timer timer;
    timer.start();

    // double eps = 1e-12; //if max curvature error below eps: consider converged
    int max_iter = 25; //max full Newton steps
    bool converged = false;

    log("computing angles");
    compute_angles();
    log("setup b");
    setup_b();

    std::vector< std::pair<double,double> > errors;
    int n_flips = 0;

    log("starting Newton");
    int degen = -1;
    while(!converged && max_iter > 0)
    {
      double error = max_abs(b);
      if(degen < 0) errors.push_back( std::pair<double,double>(avg_abs(b), error) );
      if(error <= eps) { converged = true; break; }

      double diff = avg_abs(b);

      log("setup A");
      setup_A();

      log("factorize A");
      Eigen::SparseLU< Eigen::SparseMatrix<double> > chol(A);
      log("solve Ax=b");
      Eigen::VectorXd result = chol.solve(b);
      if(chol.info() != Eigen::Success) { log("factorization failed"); return; }
      log("solved");

      #pragma omp parallel for
      for(int i = 0; i < n_e; i++)
      {
        delta_xi[i*2] = result[i];
        delta_xi[i*2+1] = -result[i];
      }

      log("line search");
      double lambda = 1.0;

      int max_linesearch = 25;
      degen = -1;
      while(true) // line search
      {
        log("  checking for degeneration events");
        double first_degen = firstDegeneracy(degen, lambda);
        if(first_degen < lambda)
        {
          lambda = first_degen;
          std::cout << "    degeneracy at lambda = " << lambda << std::endl;
        }

        log("  checking for improvement");
        std::vector<double> xi_old = xi;

        #pragma omp parallel for
        for(int i = 0; i < n_h; i++)
          xi[i] = xi_old[i] + lambda * delta_xi[i];

        compute_angles();
        setup_b();

        if(lambda == 0.0)
        {
          converged = true;
          break;
        }

        double new_diff = avg_abs(b);
        if(new_diff < diff)
        {
          std::cout << "    OK. (" << diff << " -> " << new_diff << ")" << std::endl;
          break;
        }

        lambda *= 0.5;
        if(max_linesearch-- == 0) lambda = 0.0;
        std::cout << "    reduced to    lambda = " << lambda << std::endl;
      }

      if(degen < 0) max_iter--; //no degeneration event

      if(!converged) //flip edge(s) of degeneracy/ies
      {
        std::set<int> degens;
        if(degen >= 0) degens.insert(m.e(degen));
        #pragma omp parallel for
        for(int i = 0; i < n_h; i++) //check for additional (simultaneous) degeneracies
        {
          int j = m.n[i];
          int k = m.n[j];
          if(I(i,j,k) <= 0.0)
          {
            #pragma omp critical
            {
              degens.insert(m.e(k));
            }
          }
        }
        int n_d = degens.size();
        if(n_d == 1) std::cout << "handling a degeneracy by edge flip" << std::endl;
        else if(n_d > 1) std::cout << "handling " << degens.size() << " degeneracies by edge flips" << std::endl;
        for(std::set<int>::iterator it = degens.begin(); it != degens.end(); it++)
        {
          int e = *it;
          int h = m.h0(e);
          int hl = m.n[h];
          int hr = m.n[m.n[m.opp(h)]];

          int hlu = m.n[hl];
          int hru = m.n[m.n[hr]];
          int ho = m.opp(h);
          int hu = h;

          double angle = alpha[m.n[hl]]+alpha[m.n[m.opp(h)]];
          double a = m.l[m.e(hl)] * std::exp(xi[hl]/2);
          double b = m.l[m.e(hr)] * std::exp(-xi[hr]/2);
          m.l[e] = std::sqrt(a*a + b*b - 2.0*a*b*std::cos(angle)) / std::exp((xi[hl]-xi[hr])/2); //intrinsic flip (law of cosines)

          xi[h] = xi[hl]+xi[hr];
          xi[m.opp(h)] = -xi[h];

          if(!m.flip_ccw(h)) { std::cerr << "ERROR: edge could not be flipped." << std::endl; converged = true; break; };
          n_flips++;

          if(m.l[e] <= 0.0)
          {
            m.l[e] = 1e-20;
            std::cerr << "WARNING: numerical issue: flipped edge had zero length.";
          }

          // adjust gamma loops that contain the flipped edge e
          #pragma omp parallel for
          for(int i = 0; i < n_s; ++i)
          {
            std::vector<int>& li = gamma[i];
            int n = li.size();
            for(int j = 0; j < n; ++j)
            {
              int hij = li[j];
              int hij1 = li[(j+1)%n];
              int hij2 = li[(j+2)%n];

              bool change = true;

              if(hij == hru && hij1 == m.opp(hr)) li.insert(li.begin()+j+1,ho);
              else if(hij == hru && hij1 == hu && hij2 == m.opp(hl)) li[(j+1)%n] = ho;
              else if(hij == hru && hij1 == hu && hij2 == m.opp(hlu)) li.erase(li.begin()+((j+1)%n));

              else if(hij == hr && hij1 == m.opp(hru)) li.insert(li.begin()+j+1,hu);
              else if(hij == hr && hij1 == hu && hij2 == m.opp(hlu)) li[(j+1)%n] = hu;
              else if(hij == hr && hij1 == hu && hij2 == m.opp(hl)) li.erase(li.begin()+((j+1)%n));

              else if(hij == hl && hij1 == m.opp(hlu)) li.insert(li.begin()+j+1,hu);
              else if(hij == hl && hij1 == ho && hij2 == m.opp(hru)) li[(j+1)%n] = hu;
              else if(hij == hl && hij1 == ho && hij2 == m.opp(hr)) li.erase(li.begin()+((j+1)%n));

              else if(hij == hlu && hij1 == m.opp(hl)) li.insert(li.begin()+j+1,ho);
              else if(hij == hlu && hij1 == ho && hij2 == m.opp(hr)) li[(j+1)%n] = ho;
              else if(hij == hlu && hij1 == ho && hij2 == m.opp(hru)) li.erase(li.begin()+((j+1)%n));

              else change = false;

              if(change) // cleanup "cusps" in loop
              {
                n = li.size();
                int j0 = j;
                int j1 = (j+1)%n;
                int j2 = (j+2)%n;
                if(li[j0] == m.opp(li[j1]))
                {
                  if(j1 < j0) std::swap(j0,j1);
                  li.erase(li.begin()+j1);
                  li.erase(li.begin()+j0);
                }
                else if(li[j1] == m.opp(li[j2]))
                {
                  if(j2 < j1) std::swap(j1,j2);
                  li.erase(li.begin()+j2);
                  li.erase(li.begin()+j1);
                }
              }
            }
          }
        }
        if(n_d > 0) //recompute angles after flipping
        {
          compute_angles();
          setup_b();

          //sanity check
          for(int i = 0; i < n_h; i++)
          {
            int j = m.n[i];
            int k = m.n[j];
            double indicator = I(i,j,k);
            if(indicator <= 0.0)
            {
              #pragma omp critical
              {
                if(indicator == 0.0) std::cerr << "WARNING: numerical issue: triangle("<<i<<", "<<j<<", "<<k<<") is degenerate after Newton step." << std::endl;
                if(indicator < 0.0) std::cerr << "ERROR: numerical issue: triangle("<<i<<", "<<j<<", "<<k<<") is violating after Newton step." << std::endl;
                degens.insert(m.e(k));
              }
            }
          }
        }
      }
    }

    double error = max_abs(b);

    if(error > eps) std::cerr << "WARNING: the final max error is larger than desired ("<<error<<")." << std::endl;

    std::cout << "\nSTATISTICS:\n";
    std::cout << "Flips: " << n_flips << std::endl;
    std::cout << "Error Decay: (iter, avg, max)" << std::endl;
    for(size_t i = 0; i < errors.size(); i++)
    {
      std::cout << i << ": " << errors[i].first << "  " << errors[i].second << std::endl;
    }
    std::cout << std::endl;
    if(n_flips > 0)
      std::cout << "HINT: The given mesh m has been modified by edge flips. Get the modified mesh by m.get_mesh(...)" << std::endl;
    if(n_flips > 0 && m.is_complex())
      std::cout << "HINT: The modified mesh is non-simple (e.g. contains a loop edge or multiple edges between a pair of vertices). Beware that many mesh data structures and libraries do not support this appropriately." << std::endl;
    std::cout << std::endl;

    // write n_flip and time spent to log
    auto total_time = timer.getElapsedTime();

    std::ofstream mf;
    mf.open(log_dir+"/summary_similarity.csv", std::ios_base::app);
    std::ifstream nf(log_dir+"/summary_similarity.csv");
    if (nf && nf.peek() == std::ifstream::traits_type::eof() ){
      // if stats file is empty then add column names
      nf.close();
      mf << "name, n_flips, max_error_similarity, Th_hat_preset, time\n" ;
    }

    mf << name << ", " << n_flips << ", " << error <<"," << Theta_hat[0] << ","<< total_time << std::endl;
    mf.close();

  }

  void compute_layout(std::vector<double>& u, std::vector<double>& v) //metric -> parametrization
  {
    std::vector<double> phi(n_h);

    u.resize(n_h);
    v.resize(n_h);

    //set starting point
    int h = 0;
    phi[h] = 0.0;
    u[h] = 0.0;
    v[h] = 0.0;
    h = m.n[h];
    phi[h] = xi[h];
    u[h] = m.l[m.e(h)]*std::exp(phi[h]/2);
    v[h] = 0.0;

    // layout the rest of the mesh by BFS
    std::vector<bool> visited(n_f, false);
    std::queue<int> q;
    q.push(h);
    visited[m.f[h]] = true;
    while(!q.empty())
    {
      h = q.front();
      q.pop();

      int hn = m.n[h];
      int hp = m.n[hn];

      phi[hn] = phi[h] + xi[hn];

      double len = m.l[m.e(hn)] * std::exp((phi[h]+phi[hn])/2);

      double ud = u[hp]-u[h];
      double vd = v[hp]-v[h];
      double d = std::sqrt(ud*ud + vd*vd);
      double co = std::cos(alpha[hp]);
      double si = std::sin(alpha[hp]);

      u[hn] = u[h] + (co*ud + si*vd)*len/d;
      v[hn] = v[h] + (co*vd - si*ud)*len/d;

      int hno = m.opp(hn);
      int hpo = m.opp(hp);
      if(!visited[m.f[hno]])
      {
        visited[m.f[hno]] = true;
        phi[hno] = phi[h];
        phi[m.n[m.n[hno]]] = phi[hn];
        u[hno] = u[h];
        v[hno] = v[h];
        u[m.n[m.n[hno]]] = u[hn];
        v[m.n[m.n[hno]]] = v[hn];
        q.push(hno);
      }
      if(!visited[m.f[hpo]])
      {
        visited[m.f[hpo]] = true;
        phi[hpo] = phi[hn];
        phi[m.n[m.n[hpo]]] = phi[hp];
        u[hpo] = u[hn];
        v[hpo] = v[hn];
        u[m.n[m.n[hpo]]] = u[hp];
        v[m.n[m.n[hpo]]] = v[hp];
        q.push(hpo);
      }
    }
  }

  void compute(std::vector<double>& u, std::vector<double>& v) //main method
  {
    compute_metric();
    compute_layout(u, v);
  }

};


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#pragma once

#include "libvideostitch/panoDef.hpp"
#include "libvideostitch/logging.hpp"

#include <Eigen/Dense>

#include <random>

namespace VideoStitch {
namespace Calibration {

class Camera;

/**
@brief  This class is used to filter outlier control points.
@details Given two inputs and the control points between them, it finds the best model to align two images in the
panorama
@details Returns the optimal relative position of the images and a list of control points containing only the inlier
control points
*/
class ControlPointFilter {
 public:
  ControlPointFilter(double cellFactor, double angleThreshold, double minRatioInliers, int minSamplesForFit,
                     double ratioOutliers, double probaDrawOutlierFreeSample);

  /**
   * @brief Uses the control points to align inputs.
   * @param filteredControlPoints the returned list of filtered control points
   * @param camera1 the first camera object
   * @param camera2 the second camera object
   * @param currentControlPoints the list of control points extracted from the current input pictures
   * @param formerControlPoints the list of control points coming from the PanoDefinition (extracted during a former
   * calibration)
   * @param syntheticControlPoints the list of synthetic control points generated to cover the input areas where no
   * control point has been extracted
   * @param gen the random number generator
   * @note currentControlPoints take precedence over formerControlPoints, which take precedence over
   * syntheticControlPoints
   * @return true on success
   * @return false on failure
   */
  bool filterFromExtrinsics(Core::ControlPointList& filteredControlPoints, const std::shared_ptr<Camera>& camera1,
                            const std::shared_ptr<Camera>& camera2, const Core::ControlPointList& currentControlPoints,
                            const Core::ControlPointList& formerControlPoints,
                            const Core::ControlPointList& syntheticControlPoints, std::default_random_engine& gen);

  /**
   * @brief Uses the estimated rotation to project the control points.
   * @return true on success
   * @return false on failure
   */
  Status projectFromEstimatedRotation(Core::ControlPointList& filteredControlPoints,
                                      const std::shared_ptr<Camera>& camera1, const std::shared_ptr<Camera>& camera2);

  Eigen::Matrix3d getEstimatedRotation() { return estimatedR; }

  /**
   * @brief Get rotation score
   * @return sum of angular distances between rotated inliers
   */
  double getScore() { return score; }

  /**
   * @brief gets the number of inliers with found rotation
   * @return number of inliers with found rotation
   */
  size_t getConsensus() { return consensus; }

 private:
  Eigen::Matrix3d estimatedR;
  double score;
  size_t consensus;

 private:
  double cellFactor;       // Cell for homogeneous distribution
  double angleThreshold;   // max angular distance between two reprojected CPs to be considered as inliers, in degrees
  double minRatioInliers;  // minimum ratio of inliers that need to be found to validate a model
  int minSamplesForFit;    // minimum number of control points needed to estimate a model
  int numIters;            // number of RANSAC iterations
};

}  // namespace Calibration
}  // namespace VideoStitch


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/continuous/continuous_default_motion_policy.h>
#include <OpenTissue/collision/continuous/continuous_motion_interpolation.h>
#include <OpenTissue/collision/gjk/gjk_support_functors.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_continuous_conservative_advancement);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::value_traits                         value_traits;
  typedef math_types::vector3_type                         V;
  typedef math_types::real_type                            T;
  typedef math_types::quaternion_type                      Q;
  typedef math_types::coordsys_type                        X;

  // Setup a central impact, two sphere hitting in perfect symmetry, totally independent of their rotationnal motion!
  {
    OpenTissue::gjk::Sphere<math_types> const A;
    OpenTissue::gjk::Sphere<math_types> const B;

    X A_from, A_to;
    X B_from, B_to;

    A_from.T() = V(-2.0, 0.0, 0.0);
    A_from.Q().identity();
    A_to.T() = V(0.0, 0.0, 0.0);
    A_to.Q().Ry( 5.0 );
    T r_max_a = A.radius();

    B_from.T() = V( 2.0, 0.0, 0.0);
    B_from.Q().identity();
    B_to.T() = V( 0.0, 0.0, 0.0);
    B_to.Q().Rz( 5.0 );
    T r_max_b = B.radius();

    OpenTissue::collision::continuous::DefaultMotionPolicy const motion_policy = OpenTissue::collision::continuous::DefaultMotionPolicy();
    size_t const max_iterations = 100u;
    T      const epsilon = 0.0001;
    T toi;
    size_t iterations;
    V p_a;
    V p_b;

    bool impact = OpenTissue::collision::continuous::motion_interpolation(
        A_from
      , A_to
      , A
      , r_max_a
      , B_from
      , B_to
      , B
      , r_max_b
      , p_a
      , p_b
      , toi
      , iterations
      , epsilon
      , max_iterations
      , motion_policy
      );

    BOOST_CHECK( impact );
    BOOST_CHECK_CLOSE( toi, 0.5, 0.01 );

    BOOST_CHECK( fabs( p_a(0) ) < epsilon );
    BOOST_CHECK( fabs( p_a(1) ) < epsilon );
    BOOST_CHECK( fabs( p_a(2) ) < epsilon );
    BOOST_CHECK( fabs( p_b(0) ) < epsilon );
    BOOST_CHECK( fabs( p_b(1) ) < epsilon );
    BOOST_CHECK( fabs( p_b(2) ) < epsilon );
  }

  // Setup separating motion, two spheres moving away from each other
  {
    OpenTissue::gjk::Sphere<math_types> const A;
    OpenTissue::gjk::Sphere<math_types> const B;

    X A_from, A_to;
    X B_from, B_to;

    A_from.T() = V(-2.0, 0.0, 0.0);
    A_from.Q().identity();
    A_to.T() = V(-4.0, 0.0, 0.0);
    A_to.Q().Ry( 5.0 );
    T r_max_a = A.radius();

    B_from.T() = V( 2.0, 0.0, 0.0);
    B_from.Q().identity();
    B_to.T() = V( 4.0, 0.0, 0.0);
    B_to.Q().Rz( 5.0 );
    T r_max_b = B.radius();

    OpenTissue::collision::continuous::DefaultMotionPolicy const motion_policy = OpenTissue::collision::continuous::DefaultMotionPolicy();
    size_t const max_iterations = 100u;
    T      const epsilon = 0.0001;
    T toi;
    size_t iterations;
    V p_a;
    V p_b;

    bool impact = OpenTissue::collision::continuous::motion_interpolation(
        A_from
      , A_to
      , A
      , r_max_a
      , B_from
      , B_to
      , B
      , r_max_b
      , p_a
      , p_b
      , toi
      , iterations
      , epsilon
      , max_iterations
      , motion_policy
      );

    BOOST_CHECK( !impact );
  }

  // Setup two sphere moving close by each other but never impacting
  {
    OpenTissue::gjk::Sphere<math_types> const A;
    OpenTissue::gjk::Sphere<math_types> const B;

    X A_from, A_to;
    X B_from, B_to;

    A_from.T() = V(-2.0, 1.01, 0.0);
    A_from.Q().identity();
    A_to.T() = V(0.0, 1.01, 0.0);
    A_to.Q().Ry( 5.0 );
    T r_max_a = A.radius();

    B_from.T() = V( 2.0, -1.01, 0.0);
    B_from.Q().identity();
    B_to.T() = V( 0.0, -1.01, 0.0);
    B_to.Q().Rz( 5.0 );
    T r_max_b = B.radius();

    OpenTissue::collision::continuous::DefaultMotionPolicy const motion_policy = OpenTissue::collision::continuous::DefaultMotionPolicy();
    size_t const max_iterations = 100u;
    T      const epsilon = 0.0001;
    T toi;
    size_t iterations;
    V p_a;
    V p_b;

    bool impact = OpenTissue::collision::continuous::motion_interpolation(
        A_from
      , A_to
      , A
      , r_max_a
      , B_from
      , B_to
      , B
      , r_max_b
      , p_a
      , p_b
      , toi
      , iterations
      , epsilon
      , max_iterations
      , motion_policy
      );

    BOOST_CHECK( !impact );
  }

  // Setup test case where only rotational motion is of importance!
  {
    OpenTissue::gjk::Cylinder<math_types> A;
    OpenTissue::gjk::Sphere<math_types> B;

    A.half_height() = 10.0;
    A.radius()      =  1.0;
    B.radius()      =  1.0;

    X A_from, A_to;
    X B_from, B_to;

    A_from.T() = V(0.0, 0.0, 2.0);
    A_from.Q().identity();
    A_to.T() = V(0.0, 0.0, 2.0);
    A_to.Q().Rx( -value_traits::pi() );
    T r_max_a = sqrt( A.half_height()*A.half_height() + A.radius()*A.radius() );

    B_from.T() = V( 0.0, 10.0, 0.0);
    B_from.Q().identity();
    B_to.T() = V( 0.0, 10.0, 0.0);
    B_to.Q().identity();
    T r_max_b = B.radius();

    OpenTissue::collision::continuous::DefaultMotionPolicy const motion_policy = OpenTissue::collision::continuous::DefaultMotionPolicy();
    size_t const max_iterations = 100u;
    T      const epsilon = 0.0001;
    T toi;
    size_t iterations;
    V p_a;
    V p_b;

    bool impact = OpenTissue::collision::continuous::motion_interpolation(
        A_from
      , A_to
      , A
      , r_max_a
      , B_from
      , B_to
      , B
      , r_max_b
      , p_a
      , p_b
      , toi
      , iterations
      , epsilon
      , max_iterations
      , motion_policy
      );

    BOOST_CHECK( impact );
    BOOST_CHECK_CLOSE( toi, 0.5, 0.01 );
    BOOST_CHECK( norm(p_a-p_b) < epsilon );
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*
Copyright (c) 2012 David Koes and University of Pittsburgh

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.
*  */

/* 9/18/2012  dkoes
 * This is a simple little program that uses Shake-Rupley and OpenBabel
 * to calculate the solvent accessible surface area of a molecule.
 * I'm trying to make it fast.
 *
 * If no file is given, assume a pdb on stdin.  Always outputs to stdout.
 *
 */

#include <cmath>
#include <cstdio>
#include <sstream>
#include <string>
#include <openbabel/mol.h>
#include <boost/algorithm/string.hpp>
#include <ANN/ANN.h>

#define MAXVDW (3.0) /* assumed maximum vdw */
using namespace std;
using namespace OpenBabel;
using namespace boost::algorithm;

struct Coord
{
	double x;
	double y;
	double z;

	Coord(): x(0), y(0), z(0) {}

	Coord(double a, double b, double c): x(a), y(b), z(c) {}

	double distSq(const Coord& r) const
	{
		double a = x-r.x;
		double b = y-r.y;
		double c = z-r.z;
		return a*a+b*b+c*c;
	}
};

//represents an atom from a pdb file
class Atom
{
	string line; //full line of pdb file
	Coord position; //coordinates
	double r; //vdw radius

public:
	Atom(): r(0) {}

	Atom(const string& l): line(l), r(0) //initialize from pdb file
	{
		//assume this an atom
		stringstream x(line.substr(30,8));
		stringstream y(line.substr(38,8));
		stringstream z(line.substr(46,8));

		x >> position.x;
		y >> position.y;
		z >> position.z;

		string el = line.substr(76,2);
		trim(el);
		//remove any digits or special characters
		trim_if(el, is_any_of("0123456789+-"));
		unsigned anum = etab.GetAtomicNum(el.c_str());
		r = etab.GetVdwRad(anum);
	}

	//return true if pdb line is an atom
	static bool isAtom(const string& l)
	{
		if(l.substr(0,4) == "ATOM" || l.substr(0,6) == "HETATM")
			return true;
		else
			return false;
	}

	const Coord& coord() const { return position; }
	const double radius() const { return r; }

	void print(FILE *out, double bfactor) const
	{
		if(bfactor >= 100)
			bfactor = 99.999;
		fprintf(out, "%s%6.3f%6.3f%s\n",line.substr(0,54).c_str(),bfactor,r,line.substr(66).c_str());
	}
};

void readAtoms(istream& in, vector<Atom>& mol)
{
	mol.clear();
	string line;
	while(getline(in, line))
	{
		if(Atom::isAtom(line))
			mol.push_back(Atom(line));
	}
}

//create uniformish sphere of n points
void generateSpherePoints(unsigned n, vector<Coord>& points)
{
	points.clear(); points.reserve(n);
    double inc = M_PI * (3 - sqrt(5));
    double offset = 2 / double(n);
    for(unsigned k = 0; k < n; k++)
    {
    	double y = k * offset - 1 + (offset / 2);
        double r = sqrt(1 - y*y);
        double phi = k * inc;
        points.push_back(Coord(cos(phi)*r, y, sin(phi)*r));
    }
}

//find all atoms close the the specified atomi,
//it's faster to do this once per an atom and then brute force the
//collisions with the sphere points
void find_neighbor_indices(vector<Atom>& mol, ANNkd_tree& tree,  double probe, unsigned atomi, vector<unsigned>& neighbor_indices)
{
	neighbor_indices.clear();
	ANNidx idxArray[mol.size()];

	const Coord& avec = mol[atomi].coord();
	double radius = mol[atomi].radius() + probe + probe;

	double maxr = radius+MAXVDW;
	ANNcoord querypt[3] = {avec.x, avec.y, avec.z};
	unsigned numneigh = tree.annkFRSearch(querypt, maxr*maxr, mol.size(), idxArray);

	for (unsigned idx = 0; idx < numneigh; idx++)
	{
		unsigned ai = idxArray[idx];
		if(ai != atomi)
		{
			const Atom& a = mol[ai];
			double distSq = avec.distSq(a.coord());
			double r = radius+a.radius();
			if(distSq < r*r)
				neighbor_indices.push_back(ai);
		}
	}
}

//compute asa of each atom of mol and print as it is computed
void printASA(vector<Atom>& mol, const vector<Coord>& points, ANNkd_tree& tree, double probe, FILE * out)
{
    const double Const = 4.0 * M_PI / double(points.size());
    Coord test_point;
    vector<unsigned> neighbor_indices;
    double total = 0;
	for (unsigned ai = 0, na = mol.size(); ai < na; ai++)
	{
		const Atom& a = mol[ai];
		find_neighbor_indices(mol, tree, probe, ai, neighbor_indices);

		double radius = probe + a.radius();
		unsigned n_accessible_point = 0;

		for(unsigned i = 0, n = points.size(); i < n; i++)
		{
			bool is_accessible = true;
			test_point.x = points[i].x*radius + a.coord().x;
			test_point.y = points[i].y*radius + a.coord().y;
			test_point.z = points[i].z*radius + a.coord().z;

			for(unsigned j = 0, m = neighbor_indices.size(); j < m; j++)
			{
				const Atom& atom_j = mol[neighbor_indices[j]];
				double r = atom_j.radius() + probe;
				double diff_sq = test_point.distSq(atom_j.coord());
				if(diff_sq < r*r)
				{
					is_accessible = false;
					break;
				}
			}
			if(is_accessible)
				n_accessible_point++;
		}

		double area = Const*n_accessible_point*radius*radius;
		total += area;
		a.print(out, area);
	}
	fprintf(out,"END\n");
	fprintf(out,"REMARK Total %f\n",total);
}

int main(int argc, char *argv[])
{
	unsigned n_sphere_point = 960;
	double probe = 1.4;

	vector<Atom> mol;
	if(argc > 1)
	{
		ifstream in(argv[1]);
		if(!in)
		{
			cerr << "Error opening " << argv[1] << endl;
			exit(-1);
		}
		readAtoms(in, mol);
	}
	else
	{
		readAtoms(cin, mol);
	}

	//initialize spatial index
	ANNpointArray atompts = annAllocPts(mol.size(),3);
	for(unsigned i = 0, n = mol.size(); i < n; i++)
	{
		atompts[i][0] = mol[i].coord().x;
		atompts[i][1] = mol[i].coord().y;
		atompts[i][2] = mol[i].coord().z;
	}
	ANNkd_tree tree(atompts, mol.size(), 3);

	vector<Coord> sphpoints;
	generateSpherePoints(n_sphere_point, sphpoints);

	printASA(mol, sphpoints, tree, probe, stdout);

	//clean up memory
	annDeallocPts(atompts);
	annClose();
}


#ifndef KDTREE_TRIANGLES_HPP
#define KDTREE_TRIANGLES_HPP

#include <algorithm>
#include <vector>
#include <memory>

#include "point_triangle.hpp"

#include <Eigen/Core>

template<class Real, class Index, std::size_t N = 3>
struct kd_tree {
  using index_type = Index;
  using real_type = Real;

  struct vertex {
    const real_type* coords;
  };

  struct triangle {
    const index_type* indices;
    std::size_t counter = 0;

    // real_type distance2(vertex query, const vertex* data) const {
    //   using vec3 = Eigen::Matrix<real_type, 3, 1>;
    //   using mat3x2 = Eigen::Matrix<real_type, 3, 2>;

    //   const auto q = vec3::Map(query.coords);
    //   const auto p = {vec3::Map(data + indices[0]),
    //                   vec3::Map(data + indices[1]),
    //                   vec3::Map(data + indices[2])};
    //   mat3x2 A;
    //   A << (p[1] - p[0]), (p[2] - p[0]);

    //   const vec3 n = A.col(0).cross(A.col(1));
    //   const vec3 s = q - p[0];

    //   const vec3 b = s - (n * n.dot(s)) / n.squaredNorm();

    // }
    // TODO point-triangle distance2
  };

  class plane {
    vertex origin;
    std::size_t axis;
  public:
    plane(vertex origin, std::size_t axis):
      origin(origin), axis(axis) {}

    bool positive(vertex v) const {
      return v.coord[axis] >= origin.coord[axis];
    }

    real_type distance2(vertex v) const {
      const real_type delta = v.coord[axis] - origin.coord[axis];
      // TODO distance?
      return delta * delta;
    }
  };

  // partition(3) triangle range according to given plane
  static std::pair<triangle*, triangle*>
  split_triangles(triangle* first, triangle* last, const vertex* vertices,
                  plane p) {
    // compute positive vertices
    for(auto it = first; it != last; ++it) {
      it->counter = 0;
      for(std::size_t i = 0; i < N; ++i) {
        if(p.positive(vertices[(it->indices[i])])) {
          ++it->counter;
        }
      }
    }

    // partition
    const auto negative = std::partition(
        first, last, [](const triangle& self) { return self.counter > 0; });

    const auto positive = std::partition(
        negative, last, [](const triangle& self) { return self.counter < 3; });

    return std::make_pair(negative, positive);
  }

  template<class T>
  using ref = std::shared_ptr<T>;

  struct node {
    const plane p;

    const triangle* first;
    const triangle* last;

    const ref<node> negative, overlap, positive;
    node(plane p, const triangle* first, const triangle* last,
         ref<node> negative, ref<node> overlap, ref<node> positive):
        p(p),
        first(first),
        last(last),
        negative(negative),
        overlap(overlap),
        positive(positive) {
      assert(last > first);
    }

    struct closest_type {
      const triangle* tri = nullptr;
      real_type distance2 = std::numeric_limits<real_type>::infinity();
      // TODO store proj as well?

      closest_type min(closest_type other) const {
        if(distance2 < other.distance2) {
          return *this;
        } else {
          return other;
        }
      }
    };

    static closest_type closest_brute_force(vertex query, const triangle* first,
                                            const triangle* last, real_type) {
      // TODO exploit best?
      closest_type res;
      for(auto it = first; it != last; ++it) {
        const real_type distance2 = it->distance2(query);
        if(distance2 < res.distance2) {
          res.distance2 = distance2;
          res.tri = *it;
        }
      }

      return res;
    }

    closest_type closest(vertex query, const ref<node>& near, const ref<node>& far, real_type best) const {
      closest_type res;
      for(auto sub: {near, overlap}) {
        if(sub) {
          res = res.min(sub->closest(query, best));
          if(res.distance2 < best) {
            best = res.distance2;
          }
        }
      }

      if(p.distance2(query) < best) {
        res = res.min(far->closest(query, best));
      }

      return res;
    }

    static constexpr std::size_t brute_force_threshold = 1;

    closest_type closest(vertex query,
                         real_type best=std::numeric_limits<real_type>::infinity()) const {
      if(last - first < brute_force_threshold) {
        return closest_brute_force(query, first, last, best);
      }

      closest_type res;
      if(p.positive(query)) {
        return closest(query, positive, negative, best);
      } else {
        return closest(query, negative, positive, best);
      }
    }

  };

  static ref<node> create(triangle* first,
                          triangle* last,
                          const vertex* vertex_data, std::size_t axis) {
    // base case
    if(first == last) {
      return {};
    }

    // 1. fetch triangle vertices
    std::vector<vertex> vertices;
    for(auto it = first; it != last; ++it) {
      for(std::size_t i = 0; i < 3; ++i) {
        vertices.emplace_back(vertex_data[i]);
      }
    }

    // 2. sort vertices along axis
    const auto first_vertex = vertices.begin();
    auto last_vertex = vertices.end();
    std::sort(first_vertex, last_vertex, [axis](vertex lhs, vertex rhs) {
      return lhs.coords[axis] < rhs.coords[axis];
    });

    // 3. prune duplicates
    last_vertex = std::unique(first_vertex, last_vertex);

    // 4. find pivot
    const auto pivot = (first_vertex + last_vertex) / 2;
    const plane p = {pivot->coords, axis};

    // 5. partition triangles
    const auto subsets =
        split_triangles(first, last, vertex_data, axis);

    // 7. recurse on subclasses
    const std::size_t next = ++axis % N;
    const auto negative = create(first, subsets.first, vertex_data, next);
    const auto overlap = create(subsets.first, subsets.second, vertex_data, next);
    const auto positive = create(subsets.second, last, vertex_data, next);

    // 8. node
    return std::make_shared<node>(p, first, last, negative, overlap, positive);
  }

  struct result {
    ref<node> root;
    std::vector<vertex> vertices;
    std::vector<triangle> triangles;

  };

  static void create(result& out,
                     const real_type* point_data, std::size_t point_size,
                     const index_type* triangle_data, std::size_t triangle_size) {
    // build vertex array
    out.vertices.resize(point_size);
    for(std::size_t i = 0; i < point_size; ++i) {
      out.vertices[i].coords = point_data + (i * N);
    }

    // build triangle array
    out.triangles.resize(triangle_size);
    for(std::size_t i = 0; i < triangle_size; ++i) {
      out.triangles[i].indices = triangle_data + (i * 3);
    }

    // create
    const std::size_t axis = 0;
    return create(out.triangles.data(), out.triangles.data() + triangle_size,
                  out.vertices.data(), axis);
  }

};

#endif


#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/calib3d.hpp>

#include <vector>

#include "random.h"
#include "so3.h"

#include "affine_problem.h"

struct AffineRegion
{
    Eigen::Vector3d X;
    Eigen::Vector3d u1;
    Eigen::Vector3d u2;
    Eigen::Vector3d u3;
    Eigen::Vector3d u4;
};

Eigen::Matrix2d getRelativeAffineMatrix(AffineRegion Au, AffineRegion Av,
                                       Eigen::Vector3d cu, Eigen::Vector3d cv)
{
    // calculate affine matrix from u image to v image
    // solves A*(u-center_u) = (v-center_v)
    // where v is projected point in v image, center_v is projection of PX in v image
    // where u is projected point in u image, center_u is projection of X in u image
    Eigen::Vector3d center_u3 = Au.X-cu;
    Eigen::Vector2d center_u(center_u3[0]/center_u3[2],center_u3[1]/center_u3[2]);

    Eigen::Vector3d u13 = Au.u1 - cu;
    Eigen::Vector3d u23 = Au.u2 - cu;
    Eigen::Vector3d u33 = Au.u3 - cu;
    Eigen::Vector3d u43 = Au.u4 - cu;

    Eigen::Vector2d u1(u13[0]/u13[2],u13[1]/u13[2]);
    Eigen::Vector2d u2(u23[0]/u23[2],u23[1]/u23[2]);
    Eigen::Vector2d u3(u33[0]/u33[2],u33[1]/u33[2]);
    Eigen::Vector2d u4(u43[0]/u43[2],u43[1]/u43[2]);

    Eigen::Vector3d center_v3 = Av.X-cv;
    Eigen::Vector2d center_v(center_v3[0]/center_v3[2],center_v3[1]/center_v3[2]);

    Eigen::Vector3d v13 = Av.u1 - cv;
    Eigen::Vector3d v23 = Av.u2 - cv;
    Eigen::Vector3d v33 = Av.u3 - cv;
    Eigen::Vector3d v43 = Av.u4 - cv;

    Eigen::Vector2d v1(v13[0]/v13[2],v13[1]/v13[2]);
    Eigen::Vector2d v2(v23[0]/v23[2],v23[1]/v23[2]);
    Eigen::Vector2d v3(v33[0]/v33[2],v33[1]/v33[2]);
    Eigen::Vector2d v4(v43[0]/v43[2],v43[1]/v43[2]);

    std::vector<cv::Point2f> srcPoints(4);
    std::vector<cv::Point2f> dstPoints(4);
    srcPoints[0] = cv::Point2f(u1(0),u1(1));
    srcPoints[1] = cv::Point2f(u2(0),u2(1));
    srcPoints[2] = cv::Point2f(u3(0),u3(1));
    srcPoints[3] = cv::Point2f(u4(0),u4(1));
    dstPoints[0] = cv::Point2f(v1(0),v1(1));
    dstPoints[1] = cv::Point2f(v2(0),v2(1));
    dstPoints[2] = cv::Point2f(v3(0),v3(1));
    dstPoints[3] = cv::Point2f(v4(0),v4(1));
    cv::Mat Hmat = cv::findHomography(srcPoints, dstPoints);
    Eigen::MatrixXf H(3,3);
    for ( int i = 0; i < 3; i++ )
        for ( int j = 0; j < 3; j++ )
            H(i,j) = Hmat.at<double>(i,j);

    float s = H(2,0) * center_v(0) + H(2,1) * center_v(1) + H(2,2) ;

    Eigen::Matrix2d affine_matrix;

    affine_matrix << (H(0,0) - H(2,0) * v1(0))/s ,
                     (H(0,1) - H(2,1) * v1(0))/s,
                     (H(1,0) - H(2,0) * v1(1))/s,
                     (H(1,1) - H(2,1) * v1(1))/s;

    return affine_matrix;
}

Eigen::Vector3d add_noise(Eigen::Vector3d p, Eigen::Vector2d noise)
{
    double unorm = p.norm();
    Eigen::Vector3d uproj = p/p(2);
    uproj.head(2) += noise;
    uproj /= uproj.norm();
    uproj *= unorm;
    if ( p.dot(uproj) < 0 ) uproj = -uproj;

    return uproj;
}

Eigen::Vector2d generate_noise(double noise)
{
    return grand2f()*noise;
}

AffineRegion generateAffineRegion(Eigen::Vector3d X)
{
    const double R = -.01;
    AffineRegion a;
    a.X = X;
    a.u1 = offset(X,-R,-R);
    a.u2 = offset(X,R,-R);
    a.u3 = offset(X,-R,R);
    a.u4 = offset(X,R,R);
    return a;
}

void generateAffineProblem( double trans_mag, double angle, double noise, double affine_noise, AffineProblem &prob, int nrays )
{
    Eigen::Vector3d X;    // 3D point
    Eigen::Vector3d cu;   // camera center for first multi-camera rig
    Eigen::Vector3d cv;   // camera center for second multi-camera rig
    Eigen::Vector3d PX;   // 3D point after R,t transformation
    AffineRay u;          // observations in first multi-camera rig (Pluecker line + affine)
    AffineRay v;          // observations in second multi-camera rig

    AffineRegion A1;      // affine region in first camera
    AffineRegion A2;      // affine region in second camera

    Eigen::Vector3d w = rand3f();  // make random axis for rotation
    w = w/w.norm();                // normalize rotation axis
    double theta = angle*M_PI/180; // make random angle for rotation
    prob.R = so3exp(w*theta);                // random rotation
    prob.t = rand3f();                       // random translation
    while ( prob.t.norm() < 1e-10 ) prob.t = rand3f();  // ensure translation is not all zeros

    prob.ray_pairs.clear(); // clear out ray pairs to be safe

    // generate observations
    for ( size_t i = 0; i < nrays; i++ )
    {
        X = rand3f();                          // sample random point
        while ( X.norm() == 0 ) X = rand3f();  // ensure point is not all zeros
        X(2) = 4.;
        X = X/X.norm();                        // normalize point
        double scale_factor = ((double)rand()/RAND_MAX) * 4 + 4;
        X *= scale_factor;                     // stretch point out to distance in range [4,8]

        A1 = generateAffineRegion(X);     // generate affine region

        cu = rand3f();        // sample random camera locations
        cv = rand3f();        // inter-camera
        //cv = cu;            // intra-camera: camera center is same in both rigs

        PX = prob.R*X+prob.t;    // transform point to second rig's frame
        A2.X = PX;               // transform affine region
        A2.u1 = prob.R*A1.u1+prob.t;
        A2.u2 = prob.R*A1.u2+prob.t;
        A2.u3 = prob.R*A1.u3+prob.t;
        A2.u4 = prob.R*A1.u4+prob.t;

        // affine matrix for first rig is identity
        u.x = X-cu;
        u.c = cu;
        u.A << 1, 0,
               0, 1;

        // calculate relative affine matrix for second rig
        v.x = PX-cv;
        v.c = cv;
        v.A = getRelativeAffineMatrix(A1, A2, cu, cv);

        if ( noise > 0. )    // add noise with requested standard deviation
        {
            // add gaussian noise to center points
            u.x = add_noise(u.x, generate_noise(noise));
            v.x = add_noise(v.x, generate_noise(noise));
        }

        if ( affine_noise > 0. )
        {
            // add gaussian noise to affine parameters
            v.A(0,0) += grandf()*affine_noise;
            v.A(0,1) += grandf()*affine_noise;
            v.A(1,0) += grandf()*affine_noise;
            v.A(1,1) += grandf()*affine_noise;
        }

        // store observation
        prob.ray_pairs.push_back( AffineRayPair( u, v ) );
    }
}


#include "teca_vorticity.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {

template <typename num_t>
constexpr num_t deg_to_rad() { return num_t(M_PI)/num_t(180); }

template <typename num_t>
constexpr num_t earth_radius() { return num_t(6371.0e3); }

// compute vorticicty
// this is based on TECA 1's calculation, and hence
// assumes fixed mesh spacing. here we add periodic bc in lon
// and apply unit stride vector optimization strategy to loops
template <typename num_t, typename pt_t>
void vorticity(num_t *w, const pt_t *lon, const pt_t *lat,
    const num_t *u, const num_t *v, unsigned long n_lon,
    unsigned long n_lat, bool periodic_lon=true)
{
    size_t n_bytes = n_lat*sizeof(num_t);
    num_t *delta_u = static_cast<num_t*>(malloc(n_bytes));

    // delta lon as a function of latitude
    num_t d_lon = (lon[1] - lon[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    for (unsigned long j = 0; j < n_lat; ++j)
        delta_u[j] = d_lon * cos(lat[j] * deg_to_rad<num_t>());

    // delta lat
    num_t delta_v = (lat[1] - lat[0]) * deg_to_rad<num_t>() * earth_radius<num_t>();
    num_t dv = num_t(2)*delta_v;

    unsigned long max_i = n_lon - 1;
    unsigned long max_j = n_lat - 1;

    // vorticity
    for (unsigned long j = 1; j < max_j; ++j)
    {
        unsigned long jj = j*n_lon;
        const num_t *uu_2 = u + jj + n_lon;
        const num_t *uu_0 = u + jj - n_lon;
        const num_t *vv_2 = v + jj + 1;
        const num_t *vv_0 = v + jj - 1;
        num_t *ww = w + jj;
        num_t du = num_t(2)*delta_u[j];

        for (unsigned long i = 1; i < max_i; ++i)
        {
            ww[i] = (vv_2[i] - vv_0[i]) / du -
                    (uu_2[i] - uu_0[i]) / dv ;
        }
    }

    if (periodic_lon)
    {
        // periodic in longitude
        for (unsigned long j = 1; j < max_j; ++j)
        {
            unsigned long jj = j*n_lon;
            const num_t *uu_2 = u + jj + n_lon;
            const num_t *uu_0 = u + jj - n_lon;
            const num_t *vv_2 = v + jj + 1;
            const num_t *vv_0 = v + jj + max_i;
            num_t *ww = w + jj;
            num_t du = num_t(2)*delta_u[j];

            ww[0] = (vv_2[0] - vv_0[0]) / du -
                    (uu_2[0] - uu_0[0]) / dv ;
        }

        for (unsigned long j = 1; j < max_j; ++j)
        {
            unsigned long jj = j*n_lon;
            const num_t *uu_2 = u + jj + max_i + n_lon;
            const num_t *uu_0 = u + jj + max_i - n_lon;
            const num_t *vv_2 = v + jj;
            const num_t *vv_0 = v + jj + max_i - 1;
            num_t *ww = w + jj + max_i;
            num_t du = num_t(2)*delta_u[j];

            ww[0] = (vv_2[0] - vv_0[0]) / du -
                    (uu_2[0] - uu_0[0]) / dv ;
        }
    }
    else
    {
        // zero it out
        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon] = num_t();

        for (unsigned long j = 1; j < max_j; ++j)
            w[j*n_lon + max_i] = num_t();
    }

    // extend values into lat boundaries
    num_t *dest = w;
    num_t *src = w + n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i+n_lon];

    dest = w + max_j*n_lon;
    src = dest - n_lon;
    for (unsigned long i = 0; i < n_lon; ++i)
        dest[i] = src[i];

    free(delta_u);

    return;
}
};

// --------------------------------------------------------------------------
teca_vorticity::teca_vorticity() :
    component_0_variable(), component_1_variable(),
    vorticity_variable("vorticity")
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_vorticity::~teca_vorticity()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_vorticity::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_vorticity":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_0_variable,
            "array containg lon component of the vector")
        TECA_POPTS_GET(std::string, prefix, component_1_variable,
            "array containg lat component of the vector")
        TECA_POPTS_GET(std::string, prefix, vorticity_variable,
            "array to store the computed vorticity in")
        ;

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_vorticity::set_properties(
    const string &prefix, variables_map &opts)
{
    TECA_POPTS_SET(opts, std::string, prefix, component_0_variable)
    TECA_POPTS_SET(opts, std::string, prefix, component_1_variable)
    TECA_POPTS_SET(opts, std::string, prefix, vorticity_variable)
}
#endif

// --------------------------------------------------------------------------
std::string teca_vorticity::get_component_0_variable(
    const teca_metadata &request)
{
    std::string comp_0_var = this->component_0_variable;

    if (comp_0_var.empty() &&
        request.has("teca_vorticity::component_0_variable"))
            request.get("teca_vorticity::component_0_variable", comp_0_var);

    return comp_0_var;
}

// --------------------------------------------------------------------------
std::string teca_vorticity::get_component_1_variable(
    const teca_metadata &request)
{
    std::string comp_1_var = this->component_1_variable;

    if (comp_1_var.empty() &&
        request.has("teca_vorticity::component_1_variable"))
            request.get("teca_vorticity::component_1_variable", comp_1_var);

    return comp_1_var;
}

// --------------------------------------------------------------------------
std::string teca_vorticity::get_vorticity_variable(
    const teca_metadata &request)
{
    std::string vort_var = this->vorticity_variable;

    if (vort_var.empty())
    {
        if (request.has("teca_vorticity::vorticity_variable"))
            request.get("teca_vorticity::vorticity_variable", vort_var);
        else
            vort_var = "vorticity";
    }

    return vort_var;
}

// --------------------------------------------------------------------------
teca_metadata teca_vorticity::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_vorticity::get_output_metadata" << endl;
#endif
    (void)port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);
    out_md.append("variables", this->vorticity_variable);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_vorticity::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    vector<teca_metadata> up_reqs;

    // get the name of the arrays we need to request
    std::string comp_0_var = this->get_component_0_variable(request);
    if (comp_0_var.empty())
    {
        TECA_ERROR("component 0 array was not specified")
        return up_reqs;
    }

    std::string comp_1_var = this->get_component_1_variable(request);
    if (comp_1_var.empty())
    {
        TECA_ERROR("component 0 array was not specified")
        return up_reqs;
    }

    // copy the incoming request to preserve the downstream
    // requirements and add the arrays we need
    teca_metadata req(request);

    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    arrays.insert(this->component_0_variable);
    arrays.insert(this->component_1_variable);

    // capture the array we produce
    arrays.erase(this->get_vorticity_variable(request));

    // update the request
    req.set("arrays", arrays);

    // send it up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_vorticity::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_vorticity::execute" << endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_ERROR("teca_cartesian_mesh is required")
        return nullptr;
    }

    // get component 0 array
    std::string comp_0_var = this->get_component_0_variable(request);

    if (comp_0_var.empty())
    {
        TECA_ERROR("component_0_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array comp_0
        = in_mesh->get_point_arrays()->get(comp_0_var);

    if (!comp_0)
    {
        TECA_ERROR("requested array \"" << comp_0_var << "\" not present.")
        return nullptr;
    }

    // get component 1 array
    std::string comp_1_var = this->get_component_1_variable(request);

    if (comp_1_var.empty())
    {
        TECA_ERROR("component_1_variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array comp_1
        = in_mesh->get_point_arrays()->get(comp_1_var);

    if (!comp_1)
    {
        TECA_ERROR("requested array \"" << comp_1_var << "\" not present.")
        return nullptr;
    }

    // get the input coordinate arrays
    const_p_teca_variant_array lon = in_mesh->get_x_coordinates();
    const_p_teca_variant_array lat = in_mesh->get_y_coordinates();

    if (!lon || !lat)
    {
        TECA_ERROR("lat lon mesh cooridinates not present.")
        return nullptr;
    }

    // allocate the output array
    p_teca_variant_array vort = comp_0->new_instance();
    vort->resize(comp_0->size());

    // compute vorticity
    NESTED_TEMPLATE_DISPATCH_FP(
        const teca_variant_array_impl,
        lon.get(), 1,

        const NT1 *p_lon = dynamic_cast<const TT1*>(lon.get())->get();
        const NT1 *p_lat = dynamic_cast<const TT1*>(lat.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(
            teca_variant_array_impl,
            vort.get(), 2,

            const NT2 *p_comp_0 = dynamic_cast<const TT2*>(comp_0.get())->get();
            const NT2 *p_comp_1 = dynamic_cast<const TT2*>(comp_1.get())->get();
            NT2 *p_vort = dynamic_cast<TT2*>(vort.get())->get();

            ::vorticity(p_vort, p_lon, p_lat,
                p_comp_0, p_comp_1, lon->size(), lat->size());
            )
        )

    // create the output mesh, pass everything through, and
    // add the vorticity array
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    out_mesh->get_point_arrays()->append(
        this->get_vorticity_variable(request), vort);

    return out_mesh;
}


// BoostMath.hpp
//
// (C) Datasim Education BV  2009

#pragma once

#include "UniformDistribution.hpp"
#include "BernoulliDistribution.hpp"
#include "ChiSquaredDistribution.hpp"
#include "NonCentralChiSquaredDistribution.hpp"

#include <boost/math/distributions.hpp>

using namespace System;

namespace Wrapper
{

	// Wrapper for the boost::math::uniform_ditribution class
	// We use the .NET naming conventions instead of the original C++ name
	public ref class BoostMath
	{
	private:

	public:
		// Because boost cdf, pdf etc. work with templates, we need to have overloads
		// for each type we pass to these functions.
		// In CLR we can not use templates, generics or base classes for this. (GetNative() can't return a base class since there is none)

		static double Cdf(UniformDistribution^ distribution, double x);
		static double Cdf(BernoulliDistribution^ distribution, double x);
		static double Cdf(ChiSquaredDistribution^ distribution, double x);
		static double Cdf(NonCentralChiSquaredDistribution^ distribution, double x);

		static double Pdf(UniformDistribution^ distribution, double x);
		static double Pdf(BernoulliDistribution^ distribution, double x);
		static double Pdf(ChiSquaredDistribution^ distribution, double x);
		static double Pdf(NonCentralChiSquaredDistribution^ distribution, double x);

		static double Quantile(UniformDistribution^ distribution, double x);
		static double Quantile(BernoulliDistribution^ distribution, double x);
		static double Quantile(ChiSquaredDistribution^ distribution, double x);
		static double Quantile(NonCentralChiSquaredDistribution^ distribution, double x);
	};
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/Wrapper/BoostMath.hpp