/**
 * @file 	ex8.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 10, 2017
 */

#include <iostream>
#include <boost/program_options.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <boost/timer/timer.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include "GraphParser.h"
#include "ShortestPathHeuristic.h"
#include "MyDijkstra.h"

using std::ifstream;
using std::string;
using std::cout;
using std::cerr;
using std::endl;
using namespace boost;
namespace po = boost::program_options;

using Graph = adjacency_list<vecS, vecS, undirectedS,
		no_property, property<edge_weight_t, double>>;

/**
 * The main function which reads in a graph from a .gph file, considers all
 * vertices with prime indices as terminals and calculates a steiner tree
 * using the shortest-path-heuristic and Dijkstra.
 * @param numargs number of inputs on command line
 * @param args array of inputs on command line
 * @return whether the program operated successfully
 */
int main(int numargs, char* args[]) {

	/*parsing command line options*/
	po::options_description desc("Allowed options");
	desc.add_options()
			("help,h", "produce help message")
			("input-file", po::value< string >(), "input file");
	po::positional_options_description p;
	p.add("input-file", -1);
	po::variables_map vm;
	po::store(po::command_line_parser(numargs, args).
			options(desc).positional(p).run(), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		exit(EXIT_SUCCESS);
	}

	string input;
	if (vm.count("input-file")) {
		input = vm["input-file"].as< string >();
	} else {
		cerr << "please specify an input file in the .gph format" << endl;
		exit(EXIT_FAILURE);
	}
	/*end of parsing command line options*/

	int numVertices;
	int numEdges;

	GraphParser parser(input);
	if (!parser.readSuccessfully) {
		cerr << "file could not be read" << endl;
		exit(EXIT_FAILURE);
	}

	//first line is read to get number of vertices and edges
	if (!parser.readFirstLine(numVertices, numEdges)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	Edge* edges = new Edge[numEdges];
	double* weights = new double[numEdges];

	//rest of the file is read and parsed to a graph
	if (!parser.read(edges, weights)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	//timer is started
	timer::cpu_timer t;

	//undirected graph is constructed with all edges and their weights
	Graph g(edges, edges + numEdges , weights, numVertices);

	//here the steiner tree is finally constructed
	double result = ShortestPathHeuristic::constructSteinerTree(g, numVertices);

	cout << "objective value of constructed steiner tree: " << result << endl;
	cout << "running time of the algorithm: " << t.format() << endl;

	delete[] edges;
	delete[] weights;

	exit(EXIT_SUCCESS);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN STDC Functional Test Suite

#include <boost/test/unit_test.hpp>

#include <stdc/functional.h>

BOOST_AUTO_TEST_SUITE( STDC_Functional_Test_Suite )

BOOST_AUTO_TEST_CASE( Less_Test_Cases )
{
    static_assert(stdc::less<int>()(0, 1), "");
    static_assert(not stdc::less<int>()(0, 0), "");

    auto less = stdc::less<int>();

    BOOST_CHECK(not less(0, 0));
    BOOST_CHECK(less(0, 1));
    BOOST_CHECK(less(-1, 1));
    BOOST_CHECK(not less(1, 0));
    BOOST_CHECK(not less(1, -1));
}

BOOST_AUTO_TEST_CASE( Less_Equal_Test_Cases )
{
    static_assert(stdc::less_equal<int>()(0, 0), "");
    static_assert(stdc::less_equal<int>()(0, 1), "");

    auto less_equal = stdc::less_equal<int>();

    BOOST_CHECK(less_equal(0, 0));
    BOOST_CHECK(less_equal(0, 1));
    BOOST_CHECK(less_equal(-1, 1));
    BOOST_CHECK(not less_equal(1, 0));
    BOOST_CHECK(not less_equal(1, -1));
}

BOOST_AUTO_TEST_CASE( Greater_Test_Cases )
{
    static_assert(stdc::greater<int>()(1, 0), "");
    static_assert(not stdc::greater<int>()(0, 0), "");

    auto greater = stdc::greater<int>();

    BOOST_CHECK(not greater(0, 0));
    BOOST_CHECK(greater(1, 0));
    BOOST_CHECK(greater(1, -1));
    BOOST_CHECK(not greater(0, 1));
    BOOST_CHECK(not greater(-1, 1));
}

BOOST_AUTO_TEST_CASE( Greater_Equal_Test_Cases )
{
    static_assert(stdc::greater_equal<int>()(1, 0), "");
    static_assert(stdc::greater_equal<int>()(0, 0), "");

    auto greater_equal = stdc::greater_equal<int>();

    BOOST_CHECK(greater_equal(0, 0));
    BOOST_CHECK(greater_equal(1, 0));
    BOOST_CHECK(greater_equal(1, -1));
    BOOST_CHECK(not greater_equal(0, 1));
    BOOST_CHECK(not greater_equal(-1, 1));
}

BOOST_AUTO_TEST_CASE( Less_Than_Test_Cases )
{
    auto compare = stdc::less_than<int>(0);

    BOOST_CHECK(compare(-10));
    BOOST_CHECK(compare(-1));
    BOOST_CHECK(not compare(0));
    BOOST_CHECK(not compare(1));
    BOOST_CHECK(not compare(10));
}

BOOST_AUTO_TEST_CASE( Greater_Than_Test_Cases )
{
    auto compare = stdc::greater_than<int>(0);

    BOOST_CHECK(not compare(-10));
    BOOST_CHECK(not compare(-1));
    BOOST_CHECK(not compare(0));
    BOOST_CHECK(compare(1));
    BOOST_CHECK(compare(10));
}

BOOST_AUTO_TEST_CASE( Equal_To_Test_Cases )
{
    auto compare = stdc::equal_to<int>(3);

    BOOST_CHECK(not compare(-10));
    BOOST_CHECK(not compare(-1));
    BOOST_CHECK(not compare(0));
    BOOST_CHECK(compare(3));
    BOOST_CHECK(compare(3.14l));
    BOOST_CHECK(compare(3.14L));
}

BOOST_AUTO_TEST_SUITE_END( /* STDC_Functional_Test_Suite */ )


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#include <vector>

#include "../base.hpp"

#include "../util/matrix_3_x_3.hpp"
#include "../util/vec.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/LU"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/LU>
    #include <Eigen/Geometry>
#endif

#endif

#ifndef PIC_EIGEN_UTIL
#define PIC_EIGEN_UTIL

#ifndef PIC_DISABLE_EIGEN
namespace Eigen {
    typedef Matrix<float,  3, 4> Matrix34f;
    typedef Matrix<double, 3, 4> Matrix34d;
}
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief readMatrix34dFromFile
 * @param nameFile
 * @return
 */
PIC_INLINE Eigen::Matrix34d readMatrix34dFromFile(std::string nameFile)
{
    Eigen::Matrix34d mat;

    FILE *file = fopen(nameFile.c_str(), "r");

    if(file == NULL) {
        return mat;
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            float val;
            fscanf(file, "%f", &val);
            mat(i, j) = double(val);
        }
    }

    fclose(file);

    return mat;
}

/**
 * @brief writeMatrix34dToFile
 * @param nameFile
 * @param mat
 * @return
 */
PIC_INLINE bool writeMatrix34dToFile(std::string nameFile, Eigen::Matrix34d &mat)
{
    FILE *file = fopen(nameFile.c_str(), "w");

    if(file == NULL) {
        return false;
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            fprintf(file, "%f ", mat(i, j));
        }

        if(i < 2) {
            fprintf(file, "\n");
        }
    }

    fclose(file);

    return true;
}

/**
 * @brief DiagonalMatrix creates a diagonal matrix.
 * @param D a vector of size 3.
 * @return It returns a diagonal matrix.
 */
PIC_INLINE Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
{
    Eigen::Matrix3d ret;

    ret.setZero();
    ret(0, 0) = D[0];
    ret(1, 1) = D[1];
    ret(2, 2) = D[2];

    return ret;
}

/**
 * @brief getDiagonalFromMatrix
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Vector3d getDiagonalFromMatrix(Eigen::Matrix3d &mat)
{
    Eigen::Vector3d D;

    D[0] = mat(0, 0);
    D[1] = mat(1, 1);
    D[2] = mat(2, 2);

    return D;
}

/**
 * @brief getSquareMatrix
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Matrix3d getSquareMatrix(Eigen::Matrix34d &mat)
{
    Eigen::Matrix3d ret;
    ret(0, 0) = mat(0, 0);
    ret(0, 1) = mat(0, 1);
    ret(0, 2) = mat(0, 2);

    ret(1, 0) = mat(1, 0);
    ret(1, 1) = mat(1, 1);
    ret(1, 2) = mat(1, 2);

    ret(2, 0) = mat(2, 0);
    ret(2, 1) = mat(2, 1);
    ret(2, 2) = mat(2, 2);

    return ret;
}

/**
 * @brief getLastColumn
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Vector3d getLastColumn(Eigen::Matrix34d &mat)
{
    Eigen::Vector3d ret;

    ret[0] = mat(0, 3);
    ret[1] = mat(1, 3);
    ret[2] = mat(2, 3);

    return ret;
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector3f addOne(Eigen::Vector2f &x)
{
    return Eigen::Vector3f(x[0], x[1], 1.0f);
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector3d addOne(Eigen::Vector2d &x)
{
    return Eigen::Vector3d(x[0], x[1], 1.0);
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector4d addOne(Eigen::Vector3d &x)
{
    return Eigen::Vector4d(x[0], x[1], x[2], 1.0);
}

/**
 * @brief printfVet3d
 * @param x
 */
PIC_INLINE void printfVet3d(Eigen::Vector3d &x)
{
    printf("%f %f %f\n", x[0], x[1], x[2]);
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::MatrixXd mat)
{
    for(int i = 0; i < mat.rows(); i++){
        for(int j = 0; j < mat.cols(); j++){
            printf("%3.3f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::Matrix3f &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            printf("%f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
  * @brief printf
  * @param mat
  */
PIC_INLINE void printfMat34d(Eigen::Matrix34d &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 4; j++){
            printf("%.9f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief fprintf
 * @param mat
 */
PIC_INLINE void fprintfMat(Eigen::MatrixXd &mat, std::string name)
{
    FILE *file = fopen(name.c_str(), "w");
    for(int i = 0; i < mat.rows(); i++){
        for(int j = 0; j < mat.cols(); j++){
            fprintf(file, "%.9f ", mat(i, j));
        }
        fprintf(file, "\n");
    }
    fclose(file);
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::Matrix3d &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            printf("%f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief getShiftScaleMatrix computes a shifting and scaling matrix
 * @param info is an array with the center (0 and 1) a scaling factor (3)
 * @return It returns a scaling and shifting matrix.
 */
PIC_INLINE Eigen::Matrix3d getShiftScaleMatrix(Eigen::Vector3f &info)
{
    Eigen::Matrix3d ret;

    double cX = info[0];
    double cY = info[1];
    double s  = 1.0 / info[2];

    ret(0,0) = s;   ret(0,1) = 0.0; ret(0,2) = -cX / info[2];
    ret(1,0) = 0.0; ret(1,1) = s;   ret(1,2) = -cY / info[2];
    ret(2,0) = 0.0; ret(2,1) = 0.0; ret(2,2) = 1.0;

    return ret;
}

/**
 * @brief CrossProduct computes a cross product matrix from a vector.
 * @param t a translation vector
 * @return It returns a cross product matrix.
 */
PIC_INLINE Eigen::Matrix3d CrossProduct(Eigen::Vector3d &t)
{
    Eigen::Matrix3d ret;
    ret(0, 0) =  0.0;  ret(0, 1) = -t[2]; ret(0, 2) =  t[1];
    ret(1, 0) =  t[2]; ret(1, 1) =  0.0;  ret(1, 2) = -t[0];
    ret(2, 0) = -t[1]; ret(2, 1) =  t[0]; ret(2, 2) =  0.0;
    return ret;
}

/**
 * @brief rigidTransform computes a rigidi transformation in 3D.
 * @param point is the point to be transformed.
 * @param R is a rotation matrix 3x3.
 * @param t is a translation vector.
 * @return
 */
PIC_INLINE Eigen::Vector3d rigidTransform(Eigen::Vector3d &point, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    return R * point + t;
}

/**
 * @brief RotationMatrixRefinement
 * @param R
 * @return
 */
PIC_INLINE Eigen::Matrix3d RotationMatrixRefinement(Eigen::Matrix3d &R)
{
    Eigen::Quaternion<double> reg(R);

    return reg.toRotationMatrix();
}

/**
 * @brief MatrixConvert converts a matrix from a Eigen::Matrix3f representation
 * into a Matrix3x3 representation.
 * @param mat is an Eigen 3x3 matrix.
 * @return It returns a Matrix3x3 with values from mat.
 */
PIC_INLINE Matrix3x3 MatrixConvert(Eigen::Matrix3f &mat)
{
    Matrix3x3 mtx;
    mtx.data[0] = mat(0, 0);
    mtx.data[1] = mat(0, 1);
    mtx.data[2] = mat(0, 2);

    mtx.data[3] = mat(1, 0);
    mtx.data[4] = mat(1, 1);
    mtx.data[5] = mat(1, 2);

    mtx.data[6] = mat(2, 0);
    mtx.data[7] = mat(2, 1);
    mtx.data[8] = mat(2, 2);

    return mtx;
}

/**
 * @brief MatrixConvert converts a matrix from a Eigen::Matrix3f representation
 * into a Matrix3x3 representation.
 * @param mat is an Eigen 3x3 matrix.
 * @return It returns a Matrix3x3 with values from mat.
 */
PIC_INLINE Matrix3x3 MatrixConvert(Eigen::Matrix3d &mat)
{
    Matrix3x3 mtx;
    mtx.data[0] = float(mat(0, 0));
    mtx.data[1] = float(mat(0, 1));
    mtx.data[2] = float(mat(0, 2));

    mtx.data[3] = float(mat(1, 0));
    mtx.data[4] = float(mat(1, 1));
    mtx.data[5] = float(mat(1, 2));

    mtx.data[6] = float(mat(2, 0));
    mtx.data[7] = float(mat(2, 1));
    mtx.data[8] = float(mat(2, 2));

    return mtx;
}

/**
 * @brief getLinearArray
 * @param mat
 * @return
 */
PIC_INLINE float *getLinearArrayFromMatrix(Eigen::Matrix3d &mat)
{
    int n = int(mat.cols() * mat.rows());

    float *ret = new float[n];
    int c = 0;
    for(int i = 0; i < mat.rows(); i++) {
        for(int j = 0; j < mat.cols(); j++) {
            ret[c] = float(mat(i, j));
            c++;
        }
    }

    return ret;
}

/**
 * @brief getLinearArray
 * @param mat
 * @return
 */
PIC_INLINE float *getLinearArrayFromMatrix(Eigen::Matrix3f &mat)
{
    int n = int(mat.cols() * mat.rows());

    float *ret = new float[n];
    int c = 0;
    for(int i = 0; i < mat.rows(); i++) {
        for(int j = 0; j < mat.cols(); j++) {
            ret[c] = mat(i, j);
            c++;
        }
    }

    return ret;
}

/**
 * @brief getMatrixFromLinearArray
 * @param array
 * @param rows
 * @param cols
 * @return
 */
PIC_INLINE Eigen::MatrixXf getMatrixfFromLinearArray(float *array, int rows, int cols)
{
    Eigen::MatrixXf ret = Eigen::MatrixXf(rows, cols);

    int c = 0;
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief getMatrixFromLinearArray
 * @param array
 * @param rows
 * @param cols
 * @return
 */
PIC_INLINE Eigen::MatrixXd getMatrixdFromLinearArray(float *array, int rows, int cols)
{
    Eigen::MatrixXd ret = Eigen::MatrixXd(rows, cols);

    int c = 0;
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief getMatrix3dFromLinearArray
 * @param array
 * @return
 */
PIC_INLINE Eigen::Matrix3d getMatrix3dFromLinearArray(float *array)
{
    Eigen::Matrix3d ret;

    int c = 0;
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief MatrixConvert
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Matrix3f MatrixConvert(Matrix3x3 &mat)
{
    Eigen::Matrix3f mtx;
    mtx(0, 0) = mat.data[0];
    mtx(0, 1) = mat.data[1];
    mtx(0, 2) = mat.data[2];

    mtx(1, 0) = mat.data[3];
    mtx(1, 1) = mat.data[4];
    mtx(1, 2) = mat.data[5];

    mtx(2, 0) = mat.data[6];
    mtx(2, 1) = mat.data[7];
    mtx(2, 2) = mat.data[8];

    return mtx;
}

/**
 * @brief ComputeNormalizationTransform
 * @param points
 * @return
 */
PIC_INLINE Eigen::Vector3f ComputeNormalizationTransform(std::vector< Eigen::Vector2f > &points)
{
    Eigen::Vector3f ret;

    if(points.size() < 2) {
        return ret;
    }

    ret[0] = 0.0f;
    ret[1] = 0.0f;

    for(unsigned int i = 0; i < points.size(); i++) {
        ret[0] += points[i][0];
        ret[1] += points[i][1];
    }

    float n = float(points.size());
    ret[0] /= n;
    ret[1] /= n;

    ret[2] = 0.0;
    for(unsigned int i = 0; i < points.size(); i++) {

        float dx = points[i][0] - ret[0];
        float dy = points[i][1] - ret[1];

        ret[2] += sqrtf(dx * dx + dy * dy);
    }

    ret[2] = ret[2] / n / sqrtf(2.0f);

    return ret;
}

/**
 * @brief convertFromEigenToVec
 * @param x
 */
PIC_INLINE Vec2i convertFromEigenToVec(Eigen::Vector2i &x)
{
    return Vec2i(x[0], x[1]);
}

#endif

}

#endif // PIC_EIGEN_UTIL


#define BOOST_TEST_MODULE pcraster geo square
#include <boost/test/unit_test.hpp>
#include "geo_square.h"

BOOST_AUTO_TEST_CASE(quad_square_at)
{
  using namespace geo;

  typedef Point<float,2> Punt;
  Punt c;
  c[0]=20; c[1]=20;
  typedef geo::Square<float,2> Kant;
  Kant s(c,10);
 /*  2D:
  *   NW 1(b) | NE 0(a)
  *           0(e)
  *  1(h)-----C---------0(g)
  *   SW 3(c) | SE 2(d)
  *           2(f)
  */
  Kant q;
  q = s.quadSquareAt(0);
  BOOST_CHECK(q.halfWidth()==5);
  c[0]=25; c[1]=25;
  BOOST_CHECK(q.centre()==c); // a

  q = s.quadSquareAt(1);
  BOOST_CHECK(q.halfWidth()==5);
  c[0]=15; c[1]=25;
  BOOST_CHECK(q.centre()==c); // b
  c[0]=15; c[1]=15;
  q = s.quadSquareAt(3);
  BOOST_CHECK(q.centre()==c); // c
  c[0]=25; c[1]=15;
  q = s.quadSquareAt(2);
  BOOST_CHECK(q.centre()==c); // d
}

BOOST_AUTO_TEST_CASE(contains)
{
  using namespace geo;

  typedef Point<float,2> Punt;
  Punt c; c[0]=20; c[1]=20;

  // default boundary, closed
  typedef geo::Square<float,2> Kant;
  Kant s(c,10);
  BOOST_CHECK(s.contains(c));

  Punt p;
  p[0]=12;p[1]=c[1];
  BOOST_CHECK(s.contains(p));

  p[0]=10; // on boundary
  BOOST_CHECK(s.contains(p));

  typedef geo::Square<float,2,OpenBoundaries> Open;
  Open open(c,10);
  // not the edge
  BOOST_CHECK(!open.contains(p));

  typedef geo::Square<float,2,ClosedBoundaries> Closed;
  Closed closed(c,10);
  // the edge is in
  BOOST_CHECK(closed.contains(p));

  typedef geo::Square<float,2,ClosedOpenBoundaries> ClosedOpen;
  ClosedOpen closedOpen(c,10);
  p[0]=10; // left/lower is closed
  BOOST_CHECK( closedOpen.contains(p));
  p[0]=30; // right/higher is open
  BOOST_CHECK(!closedOpen.contains(p));

  typedef geo::Square<float,2,OpenClosedBoundaries> OpenClosed;
  OpenClosed openClosed(c,10);
  p[0]=10; // left/lower is open
  BOOST_CHECK(!openClosed.contains(p));
  p[0]=30; // right/higher is closed
  BOOST_CHECK( openClosed.contains(p));

  {
   typedef geo::Square<float,2,ClosedOpenBoundaries> OpenClosed;
   OpenClosed oc(Punt(179020,330940),80);
   BOOST_CHECK(!oc.contains(Punt(179973,332255)));
  }
}

BOOST_AUTO_TEST_CASE(intersects)
{
  using namespace geo;

  typedef Point<float,2> Punt;
  Punt c(20,20);
  // default boundary, closed
  typedef geo::Square<float,2> Kant;
  Kant s(c,10);
  //! intersects with itself
  BOOST_CHECK(s.intersects(s));

  { // fully contained
   Kant is(c,5);
   BOOST_CHECK(is.intersects(s));
   BOOST_CHECK(s.intersects(is));
  }
  { // partial
    Kant is(Punt(15,15),8);
    BOOST_CHECK(is.intersects(s));
    BOOST_CHECK(s.intersects(is));
  }
  { // touch edge
    Kant is(Punt(5,5),5);
    BOOST_CHECK(is.intersects(s));
    BOOST_CHECK(s.intersects(is));
  }
  {
	// no edge touch with open boundaries
    typedef geo::Square<float,2,OpenBoundaries> OK;
    OK os(c,10);
    OK is(Punt(5,5),5);
    BOOST_CHECK(!os.intersects(is));
    BOOST_CHECK(!is.intersects(os));
  }
  { // no edge on in each other, midpoints
	// must be checked
	//
	Kant os(Punt(74,6),40);
	Kant is(Punt(0,0),60);
    BOOST_CHECK( os.intersects(is));
    BOOST_CHECK( is.intersects(os));
  }
  { // debug case
    Kant os(Punt(2,20),1);
    Kant is(Punt(0.78125f,21.0938f),0.78125f);
    BOOST_CHECK( os.intersects(is));
    BOOST_CHECK( is.intersects(os));
  }
}


/*
 * SigmoidTest.hpp
 *
 *  Created on: Dec 29, 2016
 *      Author: ken
 */

#ifndef SIGMOIDTEST_HPP_
#define SIGMOIDTEST_HPP_

#include "cute.h"
#include "ide_listener.h"
#include "xml_listener.h"
#include "cute_runner.h"
#include <armadillo>
#include <iostream>

#include "../src/layer/Sigmoid.hpp"

class SigmoidTest {
public:
	SigmoidTest();
	virtual ~SigmoidTest();

	static void feedForwardTest1();
	static void feedForwardTest2();
	static void backPropTest1();
	static void backPropTest2();
};

#endif /* SIGMOIDTEST_HPP_ */


/// My typedefs
#include <inttypes.h>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>

typedef int32_t    node_t;
typedef int32_t    edge_t;
typedef int64_t    cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::pair;
using std::make_pair;

/// Lemon Graph Library
#include <lemon/smart_graph.h>
using lemon::SmartDigraph;

#include <lemon/adaptors.h>
#include <lemon/concepts/maps.h>
#include <lemon/dijkstra.h>
#include <lemon/path.h>
typedef SmartDigraph::Arc  Arc;
typedef SmartDigraph::Node Node;
typedef SmartDigraph::ArcMap<cost_t>   LengthMap;

#include <lemon/fib_heap.h>
typedef SmartDigraph::NodeMap<int>  NodeMap;
typedef lemon::FibHeap<cost_t, NodeMap> FibonacciHeap;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra( char* argv[] ) {
   /// Read instance from the OR-lib
   std::ifstream infile(argv[1]);
   if (!infile)
      exit ( EXIT_FAILURE );

   int n;     /// Number of variables
   int m;     /// Number of constraints

   // reads file of the form
   // #nodes #edges
   // e_1 = v_i v_j cost[e_m]
   // ..
   // e_m = v_i v_j cost[e_m]

   /// Read the first line
   infile >> n >> m;
   fprintf(stdout,"n %d, m %d\t", n, m);
   /// Build the graph
   SmartDigraph G;
   G.reserveNode(n);
   G.reserveArc(m);
   vector<Node> vs;
   vs.reserve(n);
   for ( int i = 0; i < n; ++i )
      vs.push_back( G.addNode() );

   int v, w;
   cost_t c;
   cost_t T_dist;
   LengthMap    C(G);
   for ( int i = 0; i < m; i++ ) {
      infile >> v >> w >> c;
      Arc a;
      a = G.addArc(vs[v-1], vs[w-1]);
      C[a] = c;
   }

   timer TIMER;
   for ( int i = 0; i < 50; ++i ) {
      double t0 = TIMER.elapsed();
      Node S = vs[i];
      Node T = vs[n-1-i];
      //NodeMap heap_cross_ref(G);
      //FibonacciHeap heap(heap_cross_ref);
      //lemon::Dijkstra<SmartDigraph, LengthMap>::SetHeap<FibonacciHeap, NodeMap>::Create spp(G, C);
      //spp.heap( heap, heap_cross_ref );
      lemon::Dijkstra<SmartDigraph, LengthMap> spp(G, C);
      spp.run(S,T);
      T_dist = spp.dist(T);
      fprintf(stdout,"Time %.4f Cost %"PRId64"\n", TIMER.elapsed()-t0, T_dist);
   }
   fprintf(stdout,"Tot %.4f\n", TIMER.elapsed());

   return T_dist;
}

/// Main function
int
main (int argc, char **argv)
{
   if ( argc != 2 ) {
      fprintf(stdout, "usage: ./dijkstra <filename>\n");
      exit ( EXIT_FAILURE );
   }
   /// Measure overall time
   timer TIMER;
   /// Invoke the different Dijkstra algorithm implementations
   cost_t T_dist = runDijkstra(argv);
   /// Print basic figures
   fprintf(stdout,"Cost %"PRId64" - Time %.3f\n", T_dist, TIMER.elapsed());

   return 0;
}


/*
 * @file testRotateFactor.cpp
 * @brief Test RotateFactor class
 * @author Frank Dellaert
 * @date December 17, 2013
 */

#include <gtsam/slam/RotateFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <vector>

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

//*************************************************************************
// Create some test data
// Let's assume IMU is aligned with aero (X-forward,Z down)
// And camera is looking forward.
Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
Rot3 iRc(cameraX, cameraY, cameraZ);

// Now, let's create some rotations around IMU frame
Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
i2Ri3 = Rot3::rodriguez(p2, 1), //
i3Ri4 = Rot3::rodriguez(p3, 1);

// The corresponding rotations in the camera frame
Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
c3Zc4 = iRc.inverse() * i3Ri4 * iRc;

// The corresponding rotated directions in the camera frame
Unit3 z1 = iRc.inverse() * p1, //
z2 = iRc.inverse() * p2, //
z3 = iRc.inverse() * p3;

typedef noiseModel::Isotropic::shared_ptr Model;

//*************************************************************************
TEST (RotateFactor, checkMath) {
  EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1)));
  EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1)));
  EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1)));
}

//*************************************************************************
TEST (RotateFactor, test) {
  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
  RotateFactor f(1, i1Ri2, c1Zc2, model);
  EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));

  Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529);
#else
  Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
#endif
  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Rot3>(
        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
    f.evaluateError(iRc, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Rot3>(
        boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
    f.evaluateError(R, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
}

//*************************************************************************
TEST (RotateFactor, minimization) {
  // Let's try to recover the correct iRc by minimizing
  NonlinearFactorGraph graph;
  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
  graph.add(RotateFactor(1, i1Ri2, c1Zc2, model));
  graph.add(RotateFactor(1, i2Ri3, c2Zc3, model));
  graph.add(RotateFactor(1, i3Ri4, c3Zc4, model));

  // Check error at ground truth
  Values truth;
  truth.insert(1, iRc);
  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

  // Check error at initial estimate
  Values initial;
  double degree = M_PI / 180;
  Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
  initial.insert(1, initialE);

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
#else
  EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
#endif

  // Optimize
  LevenbergMarquardtParams parameters;
  //parameters.setVerbosity("ERROR");
  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
  Values result = optimizer.optimize();

  // Check result
  Rot3 actual = result.at<Rot3>(1);
  EXPECT(assert_equal(iRc, actual,1e-1));

  // Check error at result
  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

//*************************************************************************
TEST (RotateDirectionsFactor, test) {
  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
  RotateDirectionsFactor f(1, p1, z1, model);
  EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));

  Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  Vector expectedE = (Vector(2) << -0.0890529, -0.202981);
#else
  Vector expectedE = (Vector(2) << -0.08867, -0.20197);
#endif

  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Rot3>(
        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
            boost::none), iRc);
    f.evaluateError(iRc, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Rot3>(
        boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
            boost::none), R);
    f.evaluateError(R, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
}

//*************************************************************************
TEST (RotateDirectionsFactor, minimization) {
  // Let's try to recover the correct iRc by minimizing
  NonlinearFactorGraph graph;
  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
  graph.add(RotateDirectionsFactor(1, p1, z1, model));
  graph.add(RotateDirectionsFactor(1, p2, z2, model));
  graph.add(RotateDirectionsFactor(1, p3, z3, model));

  // Check error at ground truth
  Values truth;
  truth.insert(1, iRc);
  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

  // Check error at initial estimate
  Values initial;
  double degree = M_PI / 180;
  Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
  initial.insert(1, initialE);

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1);
#else
  EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);
#endif

  // Optimize
  LevenbergMarquardtParams parameters;
  //parameters.setVerbosity("ERROR");
  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
  Values result = optimizer.optimize();

  // Check result
  Rot3 actual = result.at<Rot3>(1);
  EXPECT(assert_equal(iRc, actual,1e-1));

  // Check error at result
  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

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


/**
 * @example eigenverb/test/eigenverb_test.cc
 */

#include <boost/test/unit_test_suite.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>
#include <boost/progress.hpp>
#include <usml/waveq3d/waveq3d.h>
#include <usml/eigenverb/eigenverb_collection.h>
#include <usml/eigenverb/envelope_collection.h>
#include <usml/eigenverb/eigenverb_interpolator.h>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <iostream>     // std::cout, std::fixed
#include <iomanip>      // std::setprecision

BOOST_AUTO_TEST_SUITE(eigenverb_test)

using namespace boost::unit_test;
using namespace usml::waveq3d;

/**
 * @ingroup eigenverb_test
 * @{
 */

static const double time_step = 0.100 ;
static const double src_lat = 45.0;        // location = mid-Atlantic
static const double src_lng = -45.0;
static const double c0 = 1500.0;           // constant sound speed

/**
 * Tests the basic features of the eigenverb generation process.
 *
 *   - Profile: constant 1500 m/s sound speed, Thorp absorption
 *   - Bottom: 1000 meters, sand
 *   - Source: 45N, 45W, on surface, 1000 Hz
 *   - Interfaces: bottom, surface, and volume
 *   - Time Step: 100 msec
 *   - Launch D/E: 5 degree linear spacing from -60 to 60 degrees
 *   - Launch AZ: 10 degree linear spacing from -40 to 40 degrees
 *
 * Automatically checks the accuracy of the eigenverbs for the bottom
 * to the analytic solution in the reverberation paper.
 *
 * To maximize accuracy we compute path length and angles on a round earth
 * with a flat bottom, using eqn. (25) - (27) from the verification test report.
 * For a path with a given DE (where negative is down), the path length for
 * the first interaction with the bottom is found by solving eqn. (25) for L:
 *

 *     Rb^2 = R^2 + L^2 - 2 R L sin(DE)
 *     L^2 - 2 R L sin(DE) + (R^2 - Rb^2) = 0
 *

 * where
 * 		- R = source distance from earth center,
 *		- Rb = bottom distance from earth center,
 *		- DE = launch D/E angle, and
 *		- P = path length.
 *
 * The quadratic equation solution for the path length is
 *

 * 		p = R sin(abs(DE))
 * 		q = R^2 - Rb^2
 * 		L = p - sqrt( p*p - q )
 *

 * The negative root has been chosen to make an acute angle between Rs and Rb.
 * The angle between Rs and Rb is given by:
 *

 * 		L^2 = R^2 + Rb^2 - 2 R Rb cos(alpha) ;
 * 		alpha = acos[ ( Rs^2 + Rb^2 - L^2 ) / (2 Rs Rb) ]
 *

 * The time of arrival and grazing angle are given by:
 *

 * 		time = L / c ;
 * 		grazing = DE - alpha
 *

 * The length and width of the eigenverb are computed using
 *

 * 		length = L * dDe / sin(grazing)
 * 		width = L * dAZ * cos(DE)
 *

 * where
 * 		- dDE = initial spacing between rays in DE direction (radians)
 * 		- dAZ = initial spacing between rays in AZ direction (radians)
 *
 * @xref Sean Reilly, David Thibaudeau, Ted Burns,
 * 		 "Fast computation of reverberation using Gaussian beam reflections",
 * 		 report prepared for NAWCTSD
 * @xref Sean Reilly, Gopu Potty, "Verification Tests for Hybrid Gaussian
 *		 Beams in Spherical/Time Coordinates", 10 May 2012.
 */
BOOST_AUTO_TEST_CASE( eigenverb_basic ) {
    cout << "=== eigenverb_test: eigenverb_basic ===" << endl;
    const char* ncname = USML_TEST_DIR "/eigenverb/test/eigenverb_basic_";
    const double time_max = 3.5;
    const double depth = 1000.0 ;
    const double de_spacing = 5.0 ;
    const double az_spacing = 10.0 ;

    // initialize propagation model

	profile_model* profile = new profile_linear(c0);
	boundary_model* surface = new boundary_flat();
	reflect_loss_model* bottom_loss = new reflect_loss_rayleigh(reflect_loss_rayleigh::SAND) ;
	boundary_model* bottom = new boundary_flat(depth,bottom_loss);
	ocean_model ocean(surface, bottom, profile);
	volume_model* layer = new volume_flat(300.0, 10.0, -40.0);
	ocean.add_volume(layer);

    seq_log freq( 1000.0, 10.0, 1 );
    wposition1 pos( src_lat, src_lng, 0.0 );
    seq_linear de( -80.0, de_spacing, 60.0 );
    seq_linear az( -40.0, az_spacing, 40.1 );

    // build a wavefront that just generates eigenverbs

    eigenverb_collection* eigenverbs = new eigenverb_collection( ocean.num_volume() ) ;
    wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
    wave.add_eigenverb_listener(eigenverbs) ;

    while ( wave.time() < time_max ) {
        wave.step();
    }

    // record eigenverbs for each interface to their own disk file

    for ( int n=0 ; n < eigenverbs->num_interfaces() ; ++n ) {
    	std::ostringstream filename ;
    	filename << ncname << n << ".nc" ;
    	eigenverbs->write_netcdf( filename.str().c_str(),n) ;
    }

    // test the accuracy of the eigenverb contributions
    // just tests downward facing rays to the bottom, along az=0

    const eigenverb_list& list = eigenverbs->eigenverbs(eigenverb::BOTTOM) ;
    BOOST_FOREACH( eigenverb verb, list ) {
		if ( verb.source_de < 0.0 && verb.source_az == 0.0 ) {

			// compute path length to first bottom bounce on a spherical earth

			int segments = verb.bottom + verb.surface + 1;
			double R = wposition::earth_radius ;
			double Rb = wposition::earth_radius - depth ;
			double p = R * sin(abs(verb.source_de)) ;
			double q = R * R - Rb * Rb ;
			double path_length = p - sqrt( p*p - q ) ; // quadratic equation

			// compute gazing angle, complete path length, and time of arrival

			double alpha = acos( ( R * R + Rb * Rb - path_length * path_length )
					/ ( 2 * R * Rb ) ) ;
			double grazing = abs(verb.source_de) - alpha ;
			path_length *= segments ;
			double time = path_length / c0 ;

			// compute height, width, and area area centered on ray

			const double de_angle = verb.source_de ;
			const double de_plus  = de_angle + 0.5 * to_radians(de_spacing) ;
			const double de_minus = de_angle - 0.5 * to_radians(de_spacing) ;

			const double az_angle = verb.source_az ;
			const double az_plus  = az_angle + 0.5 * to_radians(az_spacing) ;
			const double az_minus = az_angle - 0.5 * to_radians(az_spacing) ;

			const double area = (sin(de_plus) - sin(de_minus)) * (az_plus - az_minus);
			const double de_delta = de_plus - de_minus ;	// average height
			const double az_delta = area / de_delta ;		// average width

			double verb_length = path_length * de_delta / sin(grazing) ;
			double verb_width = path_length * az_delta ;

			// compare to results computed by model
			// 	  - accuracy of length/width just based on prior measurements,
			//    - length/width errors as high as 0.1 meters would still be good

			cout << std::fixed << std::setprecision(4)
				 << "de=" << to_degrees(verb.source_de)
				 << " s=" << verb.surface
				 << " b=" << verb.bottom
				 << "\tL=" << sqrt(verb.length2)
				 << " theory=" << verb_length
				 << "\tW=" << sqrt(verb.width2)
				 << " theory=" << verb_width
				 << endl ;
			BOOST_CHECK_SMALL( verb.time-time, 1e-3 ) ;
			BOOST_CHECK_SMALL( verb.grazing-grazing, 1e-6 ) ;
			BOOST_CHECK_SMALL( verb.direction-verb.source_az, 1e-6 ) ;
			BOOST_CHECK_SMALL( sqrt(verb.length2) - verb_length, 0.005 ) ;
			BOOST_CHECK_SMALL( sqrt(verb.width2) - verb_width, 0.005 ) ;
		}
    }

    // clean up and exit

    delete eigenverbs ;
}

/**
 * Tests the the eigenverb generation process using conditions like
 * those used in the eigenverb_demo.m scenario.
 *
 *   - Profile: constant 1500 m/s sound speed, no absorption
 *   - Bottom: 200 meters, sand
 *   - Source: 45N, 45W, on surface, 1000 Hz
 *   - Interfaces: bottom and surface, but limit to 2 bounces
 *   - Time Step: 100 msec
 *   - Launch D/E: 91 tangent spaced rays from -90 to +90 degrees
 *   - Launch AZ: Rays in the range [0,360) with 20 degree spacing.
 *
 * The primary motivation for this test is to generate an eigenverb netCDF
 * file that can be used to support off-line comparisons to the
 * eigenverb_demo.m scenario. The secondary motivation is to test un-even
 * ray spacing and test limiting the outputs to direct path. In addition to
 * these goals, it also automatically checks the accuracy of the eigenverbs
 * for the bottom to the analytic solution in the reverberation paper.
 */
BOOST_AUTO_TEST_CASE( eigenverb_analytic ) {
    cout << "=== eigenverb_test: eigenverb_analytic ===" << endl;
    const char* ncname = USML_TEST_DIR "/eigenverb/test/eigenverb_analytic_";
    const char* ncname_wave = USML_TEST_DIR "/eigenverb/test/eigenverb_analytic_wave.nc";
    const double time_max = 4.0;
    const double depth = 200.0 ;

    // initialize propagation model

    attenuation_model* attn = new attenuation_constant(0.0);
	profile_model* profile = new profile_linear(c0, attn);
	boundary_model* surface = new boundary_flat();
	reflect_loss_model* bottom_loss = new reflect_loss_rayleigh(reflect_loss_rayleigh::SAND) ;
	boundary_model* bottom = new boundary_flat(depth,bottom_loss);
	ocean_model ocean(surface, bottom, profile);

    seq_log freq( 1000.0, 10.0, 1 );
    wposition1 pos( src_lat, src_lng, 0.0 );
    seq_rayfan de( -90.0, 90.0, 181 );
    seq_linear az( 0.0, 20.0, 359.0 );

    // build a wavefront that just generates eigenverbs

    eigenverb_collection* eigenverbs = new eigenverb_collection( ocean.num_volume() ) ;
    wave_queue wave( ocean, freq, pos, de, az, time_step ) ;
    wave.add_eigenverb_listener(eigenverbs) ;
    wave.max_bottom(2) ;
    wave.max_surface(2) ;

    cout << "writing wavefronts to " << ncname_wave << endl;
    wave.init_netcdf( ncname_wave );
    wave.save_netcdf();
    while ( wave.time() < time_max ) {
        wave.step();
        wave.save_netcdf();
    }
    wave.close_netcdf();

    // record eigenverbs for each interface to their own disk file

    for ( int n=0 ; n < eigenverbs->num_interfaces() ; ++n ) {
    	std::ostringstream filename ;
    	filename << ncname << n << ".nc" ;
    	eigenverbs->write_netcdf( filename.str().c_str(),n) ;
    }

    // test the accuracy of the eigenverb contributions
    // just tests downward facing rays to the bottom, along az=0
    // because those are the rays which we have analytic solutions for

    const eigenverb_list& list = eigenverbs->eigenverbs(eigenverb::BOTTOM) ;
    BOOST_FOREACH( eigenverb verb, list ) {
		if ( verb.source_de < 0.0 && verb.source_az == 0.0
			&& verb.surface == 0 && verb.bottom == 0 )
		{

			// compute path length to first bottom bounce on a spherical earth

			int segments = verb.bottom + verb.surface + 1;
			double R = wposition::earth_radius ;
			double Rb = wposition::earth_radius - depth ;
			double p = R * sin(abs(verb.source_de)) ;
			double q = R * R - Rb * Rb ;
			double path_length = p - sqrt( p*p - q ) ; // quadratic equation

			// compute gazing angle, complete path length, and time of arrival

			double alpha = acos( ( R * R + Rb * Rb - path_length * path_length )
					/ ( 2 * R * Rb ) ) ;
			double grazing = abs(verb.source_de) - alpha ;
			path_length *= segments ;
			double time = path_length / c0 ;

			// compute height, width, and area area centered on ray

			const double de_angle = verb.source_de ;
			const double de_plus  = de_angle
				+ 0.5 * to_radians( de.increment(verb.de_index) ) ;
			const double de_minus = de_angle
				- 0.5 * to_radians( de.increment(verb.de_index-1) ) ;

			const double az_angle = verb.source_az ;
			const double az_plus  = az_angle
				+ 0.5 * to_radians( az.increment(verb.az_index) ) ;
			const double az_minus = az_angle
				- 0.5 * to_radians( az.increment(verb.az_index-1) ) ;

			const double area = (sin(de_plus) - sin(de_minus)) * (az_plus - az_minus);
			const double de_delta = de_plus - de_minus ;	// average height
			const double az_delta = area / de_delta ;		// average width

			double verb_length = path_length * de_delta / sin(grazing) ;
			double verb_width = path_length * az_delta ;

			// compare to results computed by model
			// 	  - accuracy of length/width just based on prior measurements,
			//    - length/width errors as high as 0.1 meters would still be good

			cout << std::fixed << std::setprecision(4)
				 << "de=" << to_degrees(verb.source_de)
				 << " s=" << verb.surface
				 << " b=" << verb.bottom
				 << "\tL=" << sqrt(verb.length2)
				 << " theory=" << verb_length
				 << "\tW=" << sqrt(verb.width2)
				 << " theory=" << verb_width
				 << endl ;
			BOOST_CHECK_SMALL( verb.time-time, 1e-3 ) ;
			BOOST_CHECK_SMALL( verb.grazing-grazing, 1e-6 ) ;
			BOOST_CHECK_SMALL( verb.direction-verb.source_az, 1e-6 ) ;
			BOOST_CHECK_SMALL( sqrt(verb.length2)-verb_length, 0.05 ) ;
			BOOST_CHECK_SMALL( sqrt(verb.width2)-verb_width, 0.05 ) ;
		}
    }

    // clean up and exit

    delete eigenverbs ;
}

/**
 * Test the ability to generate a individual envelope contributions and
 * write envelopes out to netCDF.  The eigenverbs are filled in "by hand"
 * instead of being calculated from physical principles. This gives us
 * better isolation between the testing of the eigenverb and envelope models.
 *
 *   - Profile: constant 1500 m/s sound speed
 *   - Frequencies: 1000, 2000, 3000 Hz
 *   - Scattering strength = 0.10, 0.11, 0.12 (linear units)
 *   - Pulse length = 1.0 sec
 *   - Grazing angle = 30 deg
 *   - Depth 1000 meters.
 *   - Eigenverb Power = 0.2. 0.2, 0.2 (linear units)
 *   - Eigenverb length = 20.0 meters
 *   - Eigenverb width = 10.0 meters
 *
 * One envelope contribution is created at a round trip travel time of
 * 10 seconds.  A second contribution, with have the power, is created
 * at 30 seconds. This tests the ability to accumulate an envelope from
 * multiple contributions.
 *
 * Automatically compares the peaks of the first contribution to the
 * monostatic solution.
 * $f[
 *		I_{monostatic} = \frac{ 0.5 T_0 E_s^2 \sigma }
 *							  { Tsr \sqrt{ 4 L_s^2 W_s^2 } }
 * $f]
 * Writes reverberation envelopes to the envelope_basic.nc files.
 */
BOOST_AUTO_TEST_CASE( envelope_basic ) {
    cout << "=== eigenverb_test: envelope_basic ===" << endl;
    const char* ncname = USML_TEST_DIR "/eigenverb/test/envelope_basic.nc";

    // setup scenario for 30 deg D/E in 1000 meters of water

    double angle = M_PI / 6.0 ;
    double depth = 1000.0 ;
    double range = sqrt(3.0) * depth / ( 1852.0 * 60.0 );
    double power = 0.2 ;
    double pulse_length = 1.0 ;

	// build a simple eigenverb

	eigenverb verb ;
	verb.time = 0.0 ;
	verb.position = wposition1(range,0.0,-depth) ;
	verb.direction = 0.0 ;
	verb.grazing = angle ;
	verb.sound_speed = c0 ;
	verb.de_index = 0 ;
	verb.az_index = 0 ;
	verb.source_de = -angle ;
	verb.source_az = 0.0 ;
	verb.surface = 0 ;
	verb.bottom = 0 ;
	verb.caustic = 0 ;
	verb.upper = 0 ;
	verb.lower = 0 ;

	seq_linear freq(1000.0,1000.0,3) ;
	verb.frequencies = &freq ;
	verb.power = vector<double>( freq.size(), power ) ;
	verb.length = 20.0 ;
	verb.width = 10.0 ;
	verb.length2 = verb.length * verb.length ;
	verb.width2 = verb.width * verb.width ;

	// construct an envelope_collection

	const seq_vector* travel_time = new seq_linear(0.0,0.1,400.0) ;
	envelope_collection envelopes(
		&freq,			// envelope_freq
		0,				// src_freq_first
		travel_time,	// travel_time, cloned by model
		40.0,			// reverb_duration
		pulse_length,	// pulse_length
		1e-30,			// threshold
		1, 				// num_azimuths
		1, 				// num_src_beams
		1,   			// num_rcv_beams
		0.0,            // initial_time - fill the api
		1,              // source_id - fill the api
		1,              // receiver_id - fill the api
		wposition1(0.0,0.0),   // src_pos - fill the api
	    wposition1(0.0,0.0) ); // rcv_pos - fill the api

    delete travel_time;

	vector<double> scatter( freq.size() ) ;
	matrix<double> src_beam( freq.size(), 1, 1.0 ) ;
	matrix<double> rcv_beam( freq.size(), 1, 1.0 ) ;
	for ( size_t f=0 ; f < freq.size() ; ++f ) {
		scatter[f] = 0.1 + 0.01 * f ;
	}

	// add contributions at t=10 and t=30 sec

	verb.time = 5.0 ;
	envelopes.add_contribution( verb, verb,
		src_beam, rcv_beam, scatter, 0.0, 0.0 ) ;

	verb.time = 15.0 ;
	verb.power *= 0.5 ;
	envelopes.add_contribution( verb, verb,
			src_beam, rcv_beam, scatter, 0.0, 0.0 ) ;

	envelopes.write_netcdf(ncname) ;

	// compare intensity to analytic solution for monostatic result (eqn. 31).
	// - divides total energy by duration to estimate peak
	// - note that (Ls2+Lr2)*(Ws2+Wr2) = 2 Ls2 Ws2 when s=r
	// - includes extra 4 pi in denominator, based on Matlab results

	double factor = cos(angle) / c0 ;
	double sigma2 = verb.length2 / 2.0 ;
	double duration = 0.5 * sqrt( pulse_length*pulse_length + factor*factor*sigma2 ) ;
	vector<double> theory = 10.0*log10(
			0.25 * 0.5 * pulse_length * power * power * scatter
			/ sqrt(4.0 * verb.length2 * verb.width2) / duration ) ;
	size_t index = 105 ;
	cout << "duration=" << duration << endl;
	for (size_t f = 0; f < freq.size(); ++f) {
		double model = 10.0*log10(envelopes.envelope(0,0,0)(f,index));
		cout << "theory=" << theory[f] << " model=" << model << endl;
		BOOST_CHECK_SMALL( abs(model-theory[f]), 1e-4);
	}
}

/**
 * Test the ability to compute source and receiver eigenverbs at different
 * frequencies.  Similar to envelope_basic test except that:
 *
 * 		- source and receiver are at different frequencies
 * 		- receiver is interpolated onto envelope frequency axis
 * 		- result is limited to first two source frequencies
 */
BOOST_AUTO_TEST_CASE( envelope_interpolate ) {
    cout << "=== eigenverb_test: envelope_interpolate ===" << endl;
    const char* ncname = USML_TEST_DIR "/eigenverb/test/envelope_interpolate.nc";

    // setup scenario for 30 deg D/E in 1000 meters of water

    double angle = M_PI / 6.0 ;
    double depth = 1000.0 ;
    double range = sqrt(3.0) * depth / ( 1852.0 * 60.0 );
    double power = 0.2 ;
    double pulse_length = 1.0 ;

	// build a simple source eigenverb

	eigenverb src_verb ;
	src_verb.time = 0.0 ;
	src_verb.position = wposition1(range,0.0,-depth) ;
	src_verb.direction = 0.0 ;
	src_verb.grazing = angle ;
	src_verb.sound_speed = c0 ;
	src_verb.de_index = 0 ;
	src_verb.az_index = 0 ;
	src_verb.source_de = -angle ;
	src_verb.source_az = 0.0 ;
	src_verb.surface = 0 ;
	src_verb.bottom = 0 ;
	src_verb.caustic = 0 ;
	src_verb.upper = 0 ;
	src_verb.lower = 0 ;

	seq_linear src_freq(1000.0,1000.0,3) ;
	src_verb.frequencies = &src_freq ;
	src_verb.power = vector<double>( src_freq.size(), power ) ;
	src_verb.length = 20.0 ;
	src_verb.width = 10.0 ;
	src_verb.length2 = src_verb.length * src_verb.length ;
	src_verb.width2 = src_verb.width * src_verb.width ;

	// build a simple receiver eigenverb
	// identical to src_verb except for frequency axis

	eigenverb rcv_verb_original ;
	rcv_verb_original.time = 0.0 ;
	rcv_verb_original.position = wposition1(range,0.0,-depth) ;
	rcv_verb_original.direction = 0.0 ;
	rcv_verb_original.grazing = angle ;
	rcv_verb_original.sound_speed = c0 ;
	rcv_verb_original.de_index = 0 ;
	rcv_verb_original.az_index = 0 ;
	rcv_verb_original.source_de = -angle ;
	rcv_verb_original.source_az = 0.0 ;
	rcv_verb_original.surface = 0 ;
	rcv_verb_original.bottom = 0 ;
	rcv_verb_original.caustic = 0 ;
	rcv_verb_original.upper = 0 ;
	rcv_verb_original.lower = 0 ;

	seq_linear rcv_freq(500.0,200.0,10) ;
	rcv_verb_original.frequencies = &rcv_freq ;
	rcv_verb_original.power = vector<double>( rcv_freq.size(), power ) ;
	rcv_verb_original.length = 20.0 ;
	rcv_verb_original.width = 10.0 ;
	rcv_verb_original.length2 = rcv_verb_original.length * rcv_verb_original.length ;
	rcv_verb_original.width2 = rcv_verb_original.width * rcv_verb_original.width ;

	// interpolate rcv_verb_original onto frequency axis of envelope

	seq_linear envelope_freq( 1000.0, 1000.0, 2 ) ;
	eigenverb rcv_verb ;
	rcv_verb.frequencies = &envelope_freq ;
	rcv_verb.power = vector<double>( envelope_freq.size() ) ;

	eigenverb_interpolator interpolator( &rcv_freq,  &envelope_freq ) ;
	interpolator.interpolate( rcv_verb_original, &rcv_verb ) ;

	// construct an envelope_collection

	const seq_vector* travel_time = new seq_linear(0.0,0.1,400.0) ;
	envelope_collection envelopes(
		&envelope_freq,			// envelope_freq
		0,				// src_freq_first
		travel_time,	// travel_time, cloned by model
		40.0,			// reverb_duration
		1.0, 			// pulse_length
		1e-30,			// threshold
		1, 				// num_azimuths
		1, 				// num_src_beams
		1,              // num_rcv_beams
		0.0,            // initial_time - fill the api
        1,              // source_id - fill the api
        1,              // receiver_id - fill the api
        wposition1(0.0,0.0),   // src_pos - fill the api
        wposition1(0.0,0.0) ); // rcv_pos - fill the api

    delete travel_time;

	vector<double> scatter( envelope_freq.size() ) ;
	matrix<double> src_beam( envelope_freq.size(), 1, 1.0 ) ;
	matrix<double> rcv_beam( envelope_freq.size(), 1, 1.0 ) ;
	for ( size_t f=0 ; f < envelope_freq.size() ; ++f ) {
		scatter[f] = 0.1 + 0.01 * f ;
	}

	// add contributions at t=10 and t=30 sec

	src_verb.time = 5.0 ;
	rcv_verb.time = 5.0 ;
	envelopes.add_contribution( src_verb, rcv_verb,
		src_beam, rcv_beam, scatter, 0.0, 0.0 ) ;

	src_verb.time = 15.0 ;
	rcv_verb.time = 15.0 ;
	src_verb.power *= 0.5 ;
	rcv_verb.power *= 0.5 ;
	envelopes.add_contribution( src_verb, rcv_verb,
			src_beam, rcv_beam, scatter, 0.0, 0.0 ) ;

	envelopes.write_netcdf(ncname) ;

	// compare intensity to analytic solution for monostatic result (eqn. 31).
	// - divides total energy by duration to estimate peak
	// - note that (Ls2+Lr2)*(Ws2+Wr2) = 2 Ls2 Ws2 when s=r
	// - includes extra 4 pi in denominator, based on Matlab results

	double factor = cos(angle) / c0 ;
	double sigma2 = src_verb.length2 / 2.0 ;
	double duration = 0.5 * sqrt( pulse_length*pulse_length + factor*factor*sigma2 ) ;
	vector<double> theory = 10.0*log10(
			0.25 * 0.5 * pulse_length * power * power * scatter
			/ sqrt(4.0 * src_verb.length2 * src_verb.width2) / duration ) ;
	size_t index = 105 ;
	cout << "duration=" << duration << endl;
	for (size_t f = 0; f < envelope_freq.size(); ++f) {
		double model = 10.0*log10(envelopes.envelope(0,0,0)(f,index));
		cout << "theory=" << theory[f] << " model=" << model << endl;
		BOOST_CHECK_SMALL( abs(model-theory[f]), 1e-4);
	}
}

/**
 * Test the ability to insert source eigenverbs generated from eigenverb_basic
 * test into a boost rtree and query them with an expected result.
 *      - All four volume interfaces are inserted into one rtree.
 *          Production code uses one rtree per interface.
 *      - This test uses points as the keys as they are faster to create than boxes
 *      - This test bulk inserts a collection_pairs std::list into the rtree
 *          constructor with iterator arguments.
 */

using namespace boost::geometry;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<double, 2, bg::cs::cartesian > point;

typedef bg::model::box<point> box;

typedef std::pair<point, eigenverb_list::iterator> value_pair;

typedef bgi::rtree<value_pair, bgi::rstar<16,4> > rtree_type;

BOOST_AUTO_TEST_CASE( rtree_basic ) {

    cout << "=== eigenverb_test: rtree_basic ===" << endl;
    const char* ncname = USML_TEST_DIR "/eigenverb/test/eigenverb_basic_";

    int interfaces = 4;
    eigenverb_collection collection(interfaces);
    eigenverb_list eigenverbs;
    eigenverb verb;

    // Use local pairs to package in rtree
    std::list<value_pair> collection_pairs;
    eigenverb_list::iterator iter;

    int i = 0;
    // Read eigenverbs for each interface from their own disk file
    for ( int n=0 ; n < interfaces ; ++n ) {
        std::stringstream filename ;
        filename << ncname << n << ".nc" ;
        eigenverbs = collection.read_netcdf( filename.str().c_str()) ;

        // get eigenverb values
        for (iter = eigenverbs.begin(); iter != eigenverbs.end(); ++iter ) {

            verb = *iter;
            collection_pairs.push_back(
                            std::make_pair(point(verb.position.latitude(),
                            verb.position.longitude()), iter));
            ++i;
        }
    }

    // Use Packed constructor of rtree for fastest insertion
    rtree_type rtree = rtree_type(collection_pairs.begin(), collection_pairs.end());

    // Test query box creation
    double q = 0.0;
    double delta_lat = 0.0;
    double delta_long = 0.0;
    double latitude = 0.0;
    double longitude = 0.0;

    // meters/degree  60 nmiles/degree * 1852 meters/nmiles
    double lat_scaler = (60.0*1852.0);

    // Use receiver eigenverbs lat, long, length and width
    // to create a bounding box.
    double rcv_verb_length = 200.0; // meters
    double rcv_verb_width = 200.0; // meters
    double rcv_verb_latitude = 45.0;  // North
    double rcv_verb_longitude = -45.0;// East

    q = max(rcv_verb_length, rcv_verb_width);
    latitude = rcv_verb_latitude;
    longitude = rcv_verb_longitude;
    delta_lat = q/lat_scaler;
    delta_long = q/(lat_scaler * cos(to_radians(latitude)));

    // create a box, first point bottom left, second point upper right
    box query_box(point( latitude - delta_lat, longitude - delta_long),
                point(latitude + delta_lat, longitude + delta_long));

    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;

    std::vector<value_pair> result_s;
    rtree.query(bgi::within(query_box), std::back_inserter(result_s));

    // display results
    std::cout << "spatial query result:" << std::endl;
    BOOST_CHECK_EQUAL(result_s.size(), 121);
    if (result_s.size() != 0) {
        BOOST_FOREACH(value_pair const& v, result_s)
                std::cout << bg::wkt<point>(v.first) << std::endl;
        cout << " Found " << result_s.size() << " results from " << i << " eigenverbs" << endl;
    } else {
        cout << " No results found " << endl;
    }

    std::cout << "=== rtree_basic: test completed! ===" << std::endl;

}

/// @}

BOOST_AUTO_TEST_SUITE_END()


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

#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY 10000000

#include "mp_t.hpp"
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

struct hypergeometric_0f2_gen
{
   mp_t operator()(mp_t b1, mp_t b2, mp_t z)
   {
      mp_t result = 0;
      mp_t abs_result = 0;
      mp_t term = 1;
      mp_t k = 0;

      do
      {
         result += term;
         abs_result += fabs(term);
         if (fabs(result) * boost::math::tools::epsilon<mp_t>() > fabs(term))
            break;
         ++k;
         term /= b1++;
         term /= b2++;
         term /= k;
         term *= z;
      } while (true);
      //
      // check precision:
      //
      if (abs_result * boost::math::tools::epsilon<mp_t>() / fabs(result) > 1e-40)
         throw std::domain_error("Unable to calculate result");

      std::cout << b1 << " " << b2 << " " << z << " " << result << std::endl;
      return result;
   }
};

int main(int, char* [])
{
   parameter_info<mp_t> arg1, arg2, arg3, arg4;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for 2F0:\n";

   std::string line;
   bool cont = true;

   while (cont)
   {
      float range;
      std::cout << "Enter the range to calculate over for b1 and b2 (single value, range will be -x to x): ";
      std::cin >> range;

      float z_range;
      std::cout << "Enter the range to calculate over for z (single value, range will be -x to x): ";
      std::cin >> z_range;

      int num_spots;
      std::cout << "Enter how many test points to calculate: ";
      std::cin >> num_spots;

      std::vector<mp_t> v;
      random_ns::mt19937 rnd;
      random_ns::uniform_real_distribution<float> ur_a(-range, range);
      random_ns::uniform_real_distribution<float> ur_z(-z_range, z_range);

      do
      {
         mp_t b1 = ur_a(rnd);
         mp_t b2 = ur_a(rnd);
         mp_t z = ur_z(rnd);

         arg1 = make_single_param(b1);
         arg2 = make_single_param(b2);
         arg3 = make_single_param(z);
         data.insert(hypergeometric_0f2_gen(), arg1, arg2, arg3);
      } while (num_spots--);

      std::cout << "Any more data?";
      std::cin >> cont;

   }

   std::cout << "Enter name of test data file [default=hypergeometric_0f2.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "hypergeometric_0f2.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}


#include <boost/math/special_functions/hermite.hpp>


#include "pose_2d.hpp"
#include "ackermann_kinematics.hpp"
#include "scan_simulator_2d.hpp"
#include "car_state.hpp"
#include "car_params.hpp"
#include "ks_kinematics.hpp"
#include "st_kinematics.hpp"
#include "racecar.hpp"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <vector>
#include <chrono>
#include <math.h>
using namespace racecar_simulator;
class StandaloneSimulator {
public:
    bool map_exists;
 StandaloneSimulator(int num_cars, double timestep, double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
 virtual ~StandaloneSimulator();
 void set_map(std::vector<double> map, int map_height, int map_width, double map_resolution, double origin_x, double origin_y, double free_threshold);
 std::vector<CarObs> step(std::vector<double> velocities, std::vector<double> steering_angles);
    void update_params(double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
 bool get_map_status();
    bool check_collision();
 void reset();
    void reset_bypose(std::vector<Pose2D> &poses);
private:
    double delt_t;
    int ego_agent_idx = 0;
    double safety_radius = 1.0;
    int num_agents;
    std::vector<RaceCar> agents;
    std::vector<Pose2D> agent_poses;
    std::vector<CarObs> current_obs;
    double car_width = 0.31;
    double car_length = 0.58;
    double scan_distance_to_base_link = 0.275;
    double wheel_base = 0.3302;
    std::vector<double> map;
    int map_height, map_width;
    double map_resolution, origin_x, origin_y, free_threshold;
    Eigen::Matrix4d get_transformation_matrix(const Pose2D &pose);
};


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

/// \file
/// Tests that exercise the functionality of our rigid transform typedefs, which
/// come straight from Eigen.

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/MathConvert.h"
#include "gtest/gtest.h"

template <class T>
class RigidTransformTestBase : public testing::Test
{
public:
	typedef T RigidTransform;
	typedef typename T::Scalar Scalar;
};

template <class T>
class RigidTransform3Tests : public RigidTransformTestBase<T>
{
};

typedef ::testing::Types<SurgSim::Math::RigidTransform3d,
		SurgSim::Math::RigidTransform3f> RigidTransform3Variants;
TYPED_TEST_CASE(RigidTransform3Tests, RigidTransform3Variants);

template <class T>
class AllRigidTransformTests : public RigidTransformTestBase<T>
{
};

typedef ::testing::Types<SurgSim::Math::RigidTransform2d,
		SurgSim::Math::RigidTransform2f,
		SurgSim::Math::RigidTransform3d,
		SurgSim::Math::RigidTransform3f> AllRigidTransformVariants;
TYPED_TEST_CASE(AllRigidTransformTests, AllRigidTransformVariants);

/// Test that rigid transforms can be constructed
TYPED_TEST(AllRigidTransformTests, CanConstruct)
{
	typename TestFixture::RigidTransform transform;
}

/// Test rigid transforms interpolation
TYPED_TEST(AllRigidTransformTests, Interpolation)
{
	using SurgSim::Math::makeRigidTransform;

	typedef typename TestFixture::Scalar T;
	typedef Eigen::Quaternion<T> Quaternion;
	typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;
	typedef Eigen::Matrix<T, 3, 1> Vector3;

	for (size_t numLoop = 0; numLoop < 100; numLoop++)
	{
		Quaternion q0(Eigen::Matrix<T, 4, 1>::Random());
		Quaternion q1(Eigen::Matrix<T, 4, 1>::Random());
		q0.normalize();
		q1.normalize();

		Vector3 t0(Vector3::Random());
		Vector3 t1(Vector3::Random());

		Transform transform0 = makeRigidTransform(q0, t0);
		Transform transform1 = makeRigidTransform(q1, t1);
		{
			Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.0));
			EXPECT_TRUE(transform.isApprox(transform0));
		}
		{
			Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(1.0));
			EXPECT_TRUE(transform.isApprox(transform1));
		}

		{
			Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.234));
			EXPECT_FALSE(transform.isApprox(transform0));
			EXPECT_FALSE(transform.isApprox(transform1));
		}

		{
			Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.5));
			EXPECT_FALSE(transform.isApprox(transform0));
			EXPECT_FALSE(transform.isApprox(transform1));

			// At t=0.5, the rotation interpolation should return (q0 + q1)/2 normalized
			// c.f. http://en.wikipedia.org/wiki/Slerp
			// If the quaternions are over PI angle, the slerp will interpolate between q0 and -q1
			// in this case, the interpolation is (q0 - q1)/2 normalized
			// From our specification, both quaternions could be considered negative, so we extend
			// the tests to these possibilities as well:
			// (-q0 + q1) / 2 normalized
			// (-q0 - q1) / 2 normalized
			Quaternion qHalf0((q0.coeffs() + q1.coeffs()) * 0.5);
			Quaternion qHalf1((q0.coeffs() - q1.coeffs()) * 0.5);
			Quaternion qHalf2((-q0.coeffs() + q1.coeffs()) * 0.5);
			Quaternion qHalf3((-q0.coeffs() - q1.coeffs()) * 0.5);
			qHalf0.normalize();
			qHalf1.normalize();
			qHalf2.normalize();
			qHalf3.normalize();

			Vector3 tHalf = (t0 + t1) * 0.5;
			Transform transformHalf0 = makeRigidTransform(qHalf0, tHalf);
			Transform transformHalf1 = makeRigidTransform(qHalf1, tHalf);
			Transform transformHalf2 = makeRigidTransform(qHalf2, tHalf);
			Transform transformHalf3 = makeRigidTransform(qHalf3, tHalf);
			EXPECT_TRUE(transform.isApprox(transformHalf0) || transform.isApprox(transformHalf1) ||
						transform.isApprox(transformHalf2) || transform.isApprox(transformHalf3));
		}

		{
			Transform transform = SurgSim::Math::interpolate(transform0, transform1, static_cast<T>(0.839));
			EXPECT_FALSE(transform.isApprox(transform0));
			EXPECT_FALSE(transform.isApprox(transform1));
		}
	}
}

TYPED_TEST(AllRigidTransformTests, MakeLookAt)
{
	typedef typename TestFixture::Scalar T;
	typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;

	typedef Eigen::Matrix<T, 3, 1> Vector3;
	typedef Eigen::Matrix<T, 4, 1> Vector4;

	Vector3 origin(0.0, 0.0, 0.0);
	Vector3 eye(10.0, 10.0, 10.0);
	Vector3 up(0.0, 1.0, 0.0);

	Vector4 center4(0.0, 0.0, 0.0, 1.0);

	// This follows the OpenGl convention for the camera view matrix transform, any axis would do see
	// the documentation for makeRigidTransform and gluLookAt()
	Vector4 direction4(0.0, 0.0, -1.0, 1.0);
	Vector4 eye4(10.0, 10.0, 10.0, 1.0);

	Transform transform = SurgSim::Math::makeRigidTransform(eye, origin, up);

	EXPECT_TRUE(eye4.isApprox(transform * center4));

	Vector4 transformed = transform * direction4;

	Vector3 direction3(transformed[0], transformed[1], transformed[2]);
	EXPECT_TRUE(eye.normalized().isApprox(direction3.normalized()));
}

// Test conversion to and from yaml node
TYPED_TEST(AllRigidTransformTests, YamlConvert)
{
	using SurgSim::Math::makeRigidTransform;

	typedef typename TestFixture::Scalar T;
	typedef Eigen::Quaternion<T> Quaternion;
	typedef Eigen::Transform<T, 3, Eigen::Isometry> Transform;
	typedef Eigen::Matrix<T, 3, 1> Vector3;

	const T inputValues[4] = {1.1f, 2.2f, 3.3f, 4.4f};

	Quaternion quaternion(inputValues);
	quaternion.normalize();

	Vector3 translation(inputValues);

	Transform transform = makeRigidTransform(quaternion, translation);

	YAML::Node node;

	ASSERT_NO_THROW(node = transform);

	EXPECT_TRUE(node.IsMap());
	EXPECT_EQ(2u, node.size());

	Transform expected;

	ASSERT_NO_THROW(expected = node.as<Transform>());
	EXPECT_TRUE(transform.isApprox(expected));
}


//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to handle
//  types longer than 80-bit reals.
//  Updated 2015 to use Carlson's latest methods.
//
#ifndef BOOST_MATH_ELLINT_RF_HPP
#define BOOST_MATH_ELLINT_RF_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>

// Carlson's elliptic integral of the first kind
// R_F(x, y, z) = 0.5 * \int_{0}^{\infty} [(t+x)(t+y)(t+z)]^{-1/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost
{
namespace math
{
namespace detail
{

template <typename T, typename Policy>
T ellint_rf_imp(T x, T y, T z, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math;
    using std::swap;

    static const char* function = "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)";

    if (x < 0 || y < 0 || z < 0)
    {
        return policies::raise_domain_error<T>(
            function,
            "domain error, all arguments must be non-negative, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }
    if (x + y == 0 || y + z == 0 || z + x == 0)
    {
        return policies::raise_domain_error<T>(
            function,
            "domain error, at most one argument can be zero, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }
    //
    // Special cases from http://dlmf.nist.gov/19.20#i
    //
    if (x == y)
    {
        if (x == z)
        {
            // x, y, z equal:
            return 1 / sqrt(x);
        }
        else
        {
            // 2 equal, x and y:
            if (z == 0)
                return constants::pi<T>() / (2 * sqrt(x));
            else
                return ellint_rc_imp(z, x, pol);
        }
    }
    if (x == z)
    {
        if (y == 0)
            return constants::pi<T>() / (2 * sqrt(x));
        else
            return ellint_rc_imp(y, x, pol);
    }
    if (y == z)
    {
        if (x == 0)
            return constants::pi<T>() / (2 * sqrt(y));
        else
            return ellint_rc_imp(x, y, pol);
    }
    if (x == 0)
        swap(x, z);
    else if (y == 0)
        swap(y, z);
    if (z == 0)
    {
        //
        // Special case for one value zero:
        //
        T xn = sqrt(x);
        T yn = sqrt(y);

        while (fabs(xn - yn) >= 2.7 * tools::root_epsilon<T>() * fabs(xn))
        {
            T t = sqrt(xn * yn);
            xn = (xn + yn) / 2;
            yn = t;
        }
        return constants::pi<T>() / (xn + yn);
    }

    T xn = x;
    T yn = y;
    T zn = z;
    T An = (x + y + z) / 3;
    T A0 = An;
    T Q = pow(3 * boost::math::tools::epsilon<T>(), T(-1) / 8) *
          (std::max)((std::max)(fabs(An - xn), fabs(An - yn)), fabs(An - zn));
    T fn = 1;

    // duplication
    unsigned k = 1;
    for (; k < boost::math::policies::get_max_series_iterations<Policy>(); ++k)
    {
        T root_x = sqrt(xn);
        T root_y = sqrt(yn);
        T root_z = sqrt(zn);
        T lambda = root_x * root_y + root_x * root_z + root_y * root_z;
        An = (An + lambda) / 4;
        xn = (xn + lambda) / 4;
        yn = (yn + lambda) / 4;
        zn = (zn + lambda) / 4;
        Q /= 4;
        fn *= 4;
        if (Q < fabs(An))
            break;
    }
    // Check to see if we gave up too soon:
    policies::check_series_iterations<T>(function, k, pol);
    BOOST_MATH_INSTRUMENT_VARIABLE(k);

    T X = (A0 - x) / (An * fn);
    T Y = (A0 - y) / (An * fn);
    T Z = -X - Y;

    // Taylor series expansion to the 7th order
    T E2 = X * Y - Z * Z;
    T E3 = X * Y * Z;
    return (1 + E3 * (T(1) / 14 + 3 * E3 / 104) +
            E2 * (T(-1) / 10 + E2 / 24 - (3 * E3) / 44 - 5 * E2 * E2 / 208 +
                  E2 * E3 / 16)) /
           sqrt(An);
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
    ellint_rf(T1 x, T2 y, T3 z, const Policy& pol)
{
    typedef typename tools::promote_args<T1, T2, T3>::type result_type;
    typedef typename policies::evaluation<result_type, Policy>::type value_type;
    return policies::checked_narrowing_cast<result_type, Policy>(
        detail::ellint_rf_imp(static_cast<value_type>(x),
                              static_cast<value_type>(y),
                              static_cast<value_type>(z), pol),
        "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type ellint_rf(T1 x, T2 y,
                                                                T3 z)
{
    return ellint_rf(x, y, z, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_ELLINT_RF_HPP


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

#ifndef DESOLA_MATRIX_VECTOR_HPP
#define DESOLA_MATRIX_VECTOR_HPP

#include <cstddef>
#include <boost/array.hpp>
#include <desola/Desola_fwd.hpp>

namespace desola
{

namespace detail
{

template<typename T_element>
class MatrixMult : public BinOp<matrix, matrix, matrix, T_element>
{
private:
  inline static const boost::array<std::size_t, 2> createDims(const ExprNode<matrix, T_element>& l, const ExprNode<matrix, T_element>& r)
  {
    boost::array<std::size_t, 2> dimensions = { {l.getRowCount(), r.getColCount()} };
    return dimensions;
  }

public:
  MatrixMult(ExprNode<matrix, T_element>& left, ExprNode<matrix, T_element>& right) : BinOp<matrix, matrix, matrix, T_element>(createDims(left, right), left, right)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return Maybe<double>(this->getLeft().nnz()) * this->getRight().getColCount() * 2.0;
  }
};

template<typename T_element>
class MatrixVectorMult : public BinOp<vector, matrix, vector, T_element>
{
private:
  inline static const boost::array<std::size_t, 1> getDims(const ExprNode<matrix, T_element>& l, const ExprNode<vector, T_element>& r)
  {
    boost::array<std::size_t, 1> dimensions = { {l.getRowCount()} };
    return dimensions;
  }

public:
  MatrixVectorMult(ExprNode<matrix, T_element>& left, ExprNode<vector, T_element>& right) : BinOp<vector, matrix, vector, T_element>(getDims(left, right), left, right)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return Maybe<double>(this->getLeft().nnz()) * 2.0;
  }
};

template<typename T_element>
class TransposeMatrixVectorMult : public BinOp<vector, matrix, vector, T_element>
{
private:
  inline static const boost::array<std::size_t, 1> getDims(const ExprNode<matrix, T_element>& l, const ExprNode<vector, T_element>& r)
  {
    boost::array<std::size_t, 1> dimensions = { {l.getColCount()} };
    return dimensions;
  }

public:
  TransposeMatrixVectorMult(ExprNode<matrix, T_element>& left, ExprNode<vector, T_element>& right) : BinOp<vector, matrix, vector, T_element>(getDims(left, right), left, right)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return Maybe<double>(this->getLeft().nnz()) * 2.0;
  }
};

template<typename T_element>
class VectorDot : public BinOp<scalar, vector, vector, T_element>
{
private:
  inline static const boost::array<std::size_t, 0> getDims(const ExprNode<vector, T_element>& left, const ExprNode<vector, T_element>& right)
  {
    return boost::array<std::size_t, 0>();
  }

public:
  VectorDot(ExprNode<vector, T_element>& left, ExprNode<vector, T_element>& right) : BinOp<scalar, vector, vector, T_element>(boost::array<std::size_t, 0>(), left, right)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return 2.0 * this->getLeft().getRowCount() - 1.0;
  }
};

template<typename T_element>
class VectorCross : public BinOp<vector, vector, vector, T_element>
{
private:
  inline static const boost::array<std::size_t, 1>& getDims(const ExprNode<vector, T_element>& l, const ExprNode<vector, T_element>& r)
  {
    return l.getDims();
  }

public:
  VectorCross(ExprNode<vector, T_element>& left, ExprNode<vector, T_element>& right) : BinOp<vector, vector, vector, T_element>(getDims(left, right), left, right)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return 3.0 * this->getRowCount();
  }
};

template<typename T_element>
class VectorTwoNorm : public UnOp<scalar, vector, T_element>
{
public:
  VectorTwoNorm(ExprNode<vector, T_element>& left) : UnOp<scalar, vector, T_element>(boost::array<std::size_t, 0>(), left)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    // n multiplies + (n-1) additions + 1 sqrt
    return 2.0 * this->getOperand().getRowCount();
  }
};

template<typename T_element>
class MatrixTranspose : public UnOp<matrix, matrix, T_element>
{
private:
  inline static const boost::array<std::size_t, 2> createDims(const ExprNode<matrix, T_element>& m)
  {
    boost::array<std::size_t, 2> dimensions = { {m.getColCount(), m.getRowCount()} };
    return dimensions;
  }

public:
  MatrixTranspose(ExprNode<matrix, T_element>& m) : UnOp<matrix, matrix, T_element>(createDims(m), m)
  {
  }

  void accept(ExpressionNodeVisitor<T_element>& v)
  {
    v.visit(*this);
  }

  virtual Maybe<double> getFlops() const
  {
    return 0.0;
  }
};

}

}
#endif


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <map>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/base_units/imperial/foot.hpp>

//[runtime_unit_snippet_1

namespace {

using namespace boost::units;
using imperial::foot_base_unit;

std::map<std::string, quantity<si::length> > known_units;

}

quantity<si::length> calculate(const quantity<si::length>& t)
{
    return(boost::units::hypot(t, 2.0 * si::meters));
}

int main()
{
    known_units["meter"] = 1.0 * si::meters;
    known_units["centimeter"] = .01 * si::meters;
    known_units["foot"] =
        conversion_factor(foot_base_unit::unit_type(), si::meter) * si::meter;

    std::string output_type("meter");
    std::string input;

    while((std::cout << "> ") && (std::cin >> input))
    {
        if(!input.empty() && input[0] == '#')
        {
            std::getline(std::cin, input);
        }
        else if(input == "exit")
        {
            break;
        }
        else if(input == "help")
        {
            std::cout << "type \"exit\" to exit\n"
                "type \"return 'unit'\" to set the return units\n"
                "type \"'number' 'unit'\" to do a simple calculation"
                << std::endl;
        }
        else if(input == "return")
        {
            if(std::cin >> input)
            {
                if(known_units.find(input) != known_units.end())
                {
                    output_type = input;
                    std::cout << "Done." << std::endl;
                }
                else
                {
                    std::cout << "Unknown unit \"" << input << "\""
                         << std::endl;
                }
            }
            else
            {
                break;
            }
        }
        else
        {
            try
            {
                double value = boost::lexical_cast<double>(input);

                if(std::cin >> input)
                {
                    if(known_units.find(input) != known_units.end())
                    {
                        std::cout << static_cast<double>(
                            calculate(value * known_units[input]) /
                            known_units[output_type])
                            << ' ' << output_type << std::endl;
                    }
                    else
                    {
                        std::cout << "Unknown unit \"" << input << "\""
                            << std::endl;
                    }
                }
                else
                {
                    break;
                }
            }
            catch(...)
            {
                std::cout << "Input error" << std::endl;
            }
        }
    }
}

//]


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007, 2010.

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

#ifdef _MSC_VER
# pragma warning (disable : 4305) // 'initializing' : truncation from 'long double' to 'const eval_type'
# pragma warning (disable : 4244) //  conversion from 'long double' to 'const eval_type'
#endif

#include <iostream>
using std::cout; using std::endl;

//[policy_eg_3

#include <boost/math/distributions/binomial.hpp>
using boost::math::binomial_distribution;

// Begin by defining a policy type, that gives the behaviour we want:

//using namespace boost::math::policies; or explicitly
using boost::math::policies::policy;

using boost::math::policies::promote_float;
using boost::math::policies::discrete_quantile;
using boost::math::policies::integer_round_nearest;

typedef policy<
   promote_float<false>, // Do not promote to double.
   discrete_quantile<integer_round_nearest> // Round result to nearest integer.
> mypolicy;
//
// Then define a new distribution that uses it:
typedef boost::math::binomial_distribution<float, mypolicy> mybinom;

//  And now use it to get the quantile:

int main()
{
   cout << "quantile(mybinom(200, 0.25), 0.05) is: " <<
      quantile(mybinom(200, 0.25), 0.05) << endl;
}

//]

/*

Output:

  quantile(mybinom(200, 0.25), 0.05) is: 40

*/


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem/blackBoxOptimisationBenchmark.hpp"

namespace mant {
  namespace bbob {
    class SchaffersF7Function : public BlackBoxOptimisationBenchmark {
     public:
      explicit SchaffersF7Function(
          const arma::uword numberOfDimensions);

     protected:
      const arma::vec parameterConditioning_;
      // Keeps randomly set data non-`const`, to be changed within tests.
      arma::mat rotationQ_;
    };
  }
}


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include "SymmetricMatrix.h"

using Eigen::MatrixXd;

int main()
{
  std::clog << "Starting tests..." << std::endl
            << std::endl;

  const int ORDER = 10;

  // Constructor and operator() tests.

  MatrixXd m1 = MatrixXd::Random(ORDER, ORDER);

  SymMat<double> sym1(m1);

  for (int i = 0; i < ORDER; ++i)
  {
    for (int j = i; j < ORDER; ++j)
    {
      if (sym1(i, j) != m1(i, j))
      {
        std::clog << "Test failed: SymMat and upper triangular part of Eigen::Matrix are not equal." << std::endl;
        i = ORDER;
        j = ORDER;
      }
    }
  }

  for (int i = 0; i < ORDER; ++i)
  {
    for (int j = i; j < ORDER; ++j)
    {
      if (sym1(i, j) != sym1(j, i))
      {
        std::clog << "Test failed: SymMat(i, j) != SymMat(j, i)." << std::endl;
        i = ORDER;
        j = ORDER;
      }
    }
  }

  // Addition, subtraction and multiplication tests.

  MatrixXd ident = MatrixXd::Identity(ORDER, ORDER);
  SymMat<double> symIdent(ident);

  MatrixXd mzero(ORDER, ORDER);
  mzero.setZero(ORDER, ORDER);

  MatrixXd m2 = MatrixXd::Random(ORDER, 5);

  if (sym1 * mzero != mzero)
  {
    std::clog << "Test failed: SymMat * 0 != 0." << std::endl;
  }

  if ((symIdent + symIdent) * ident != ident + ident ||
      (symIdent + ident) != ident + ident)
  {
    std::clog << "Test failed: Addition or multiplication failed." << std::endl;
  }

  if ((sym1 - sym1) * ident != mzero ||
      (symIdent - ident) != mzero)
  {
    std::clog << "Test failed: Subtraction or multiplication failed." << std::endl;
  }

  auto m3 = sym1 * m2;
  if (m3.rows() != ORDER || m3.cols() != 5)
  {
    std::clog << "Test failed: Multiplcation gave a matrix with wrong dimensions." << std::endl;
  }

  // Exceptions tests.

  try
  {
    SymMat<double> sym2(m2);
    std::clog << "Test failed: SymMat constructor didn't throw an exception with non square matrix." << std::endl;
  }
  catch (std::exception &e)
  {
  }

  try
  {
    sym1(ORDER + 1, 0);
    std::clog << "Test failed: SymMat didn't throw an exception with out of bounds indices." << std::endl;
  }
  catch (std::exception &e)
  {
  }

  try
  {
    sym1 + m2;
    std::clog << "Test failed: SymMat + Eigen::Matrix didn't throw an exception with different order matrices." << std::endl;
  }
  catch (std::exception &e)
  {
  }

  try
  {
    sym1 - m2;
    std::clog << "Test failed: SymMat - Eigen::Matrix didn't throw an exception with different order matrices." << std::endl;
  }
  catch (std::exception &e)
  {
  }

  try
  {
    MatrixXd m4(5, ORDER);
    sym1 * m4;
    std::clog << "Test failed: SymMat * Eigen::Matrix didn't throw an exception with different order matrices." << std::endl;
  }
  catch (std::exception &e)
  {
  }

  std::cout << std::endl
            << "Tests finished." << std::endl;
}


/**
 * @file Didson.cpp
 * @brief Projection functions for DIDSON
 * @author: Michael Kaess
 * @date: Aug 2014
 */

#include <vector>
#include <utility>

#include <opencv2/opencv.hpp>

#include <Eigen/Dense>

#include "Didson.h"

using namespace boost;
using namespace std;
using namespace isam;
using namespace Eigen;

namespace sonar {

const float degtorad = M_PI / 180.;
const float radtodeg = 180. / M_PI;

class DidsonConstants {
public:
  double bearingFov;
  double elevationFov;
  int numBearings;
  int numRanges;
  double startBase;
  std::vector<double> lengths;
  DidsonConstants() {
    bearingFov = 28.8 * degtorad; // 28.8 or 29? (both are given in the specs)
    elevationFov = 28. * degtorad; // 28 degrees with spreader lens (default without lens is 14 degrees)
    numBearings = 96;
    numRanges = 512;
#if 1 // todo: unknown if DIDSON in CW or XW mode
    // classic window mode (CW)
    startBase = 0.375;
    lengths.resize (4);
    lengths[0] = 1.125; lengths[1] = 2.25; lengths[2] = 4.5; lengths[3] = 9.;
#else
    // extended window mode (XW)
    startBase = 0.42;
    lengths = {1.25, 2.5, 5., 10.};
#endif
  }
};

const DidsonConstants consts;

pair<int, int> DidsonCartesian::bearingRange2Cartesian(int bearing,
    int range) {
  int pos = _invMap[range * consts.numBearings + bearing];
  int col = pos % image.cols;
  int row = (pos - col) / image.cols;
  // HACK: don't just return col, it needs to be flipped
  return make_pair(row, (image.cols - 1) - col);
}

// conversion of Matlab DIDSON code from Soundmetrics ---
int Didson::lensDistortion(int nbeams, double theta) {
  double* a = NULL;
  double factor = 0.0;
  double a48[4] = { 0.0015, -0.0036, 1.3351, 24.0976 };
  double a189[4] = { 0.0015, -0.0036, 1.3351, 24.0978 };
  double a96[4] = { 0.0030, -0.0055, 2.6829, 48.04 };
  double a381[4] = { 0.0030, -0.0055, 2.6829, 48.04 };

  switch (nbeams) {
  case 48:
    factor = 1.0;
    a = a48;
    break;
  case 189:
    factor = 4.026;
    a = a189;
    break;
  case 96:
    factor = 1.012;
    a = a96;
    break;
  case 381:
    factor = 4.05;
    a = a381;
    break;
  }
  return (int) round(
      factor
          * (a[0] * theta * theta * theta + a[1] * theta * theta + a[2] * theta
              + a[3] + 1));
}

// conversion of Matlab DIDSON code from Soundmetrics ---
/**
 * Calculates a map from cartesian to polar coordinates
 *
 * ixsize  - number of pixels in horizontal direction in image space
 * rmax    - maximum range in meters
 * rmin    - minimum range in meters
 * halffov - one-half of sector field of view in radians
 * nbins   - number of range bins in sample space
 */
pair<vector<int>, int> Didson::createMapping(int ixsize, double rmax,
    double rmin, double halffov, int nbeams, int nbins) {
  //double d2 = rmax*cos(halffov); // distance from point scan touches image boundary to origin
  double d3 = rmin * cos(halffov); // bottom of image frame to r,theta origin in meters
  double c1 = (consts.numRanges - 1) / (rmax - rmin); // precalculation of constants used in do loop below
  //double c2 = (nbeams-1)/(2*halffov);
  double gamma = ixsize / (2 * rmax * sin(halffov)); // Ratio of pixel number to position in meters

  int iysize = (int) (gamma * (rmax - d3) + 0.5); // number of pixels in image in vertical direction

  vector<int> map(ixsize * iysize); // Stores the index map

  // ix,iy   - coordinates of a pixel in image space
  for (int iy = 1; iy <= iysize; iy++) {
    for (int ix = 1; ix <= ixsize; ix++) {
      double x = ((ix - 1) - ixsize / 2) / gamma; // Convert from pixels to meters

      double z = 0.0;
      double y = rmax - (iy - 1) / gamma; // Convert from pixels to meters

      double r = sqrt(y * y + x * x + z * z); // Convert to polar coordinates
      double theta = radtodeg * atan2(x, y); // Theta is in degrees
      int binnum = (int) ((r - rmin) * c1 + 1.5); // the rangebin number
      int beamnum = lensDistortion(nbeams, theta); // Remove the lens distortation using empirical formula
      int pos = 0; // invalid == -1, note pos-1 below
      if ((beamnum > 0) && (beamnum <= nbeams) && (binnum > 0)
          && (binnum <= nbins)) {
        pos = (binnum - 1) * nbeams + beamnum;
      }
      map[(iy - 1) * ixsize + ix - 1] = pos - 1;
    }
  }
  return make_pair(map, iysize);
}

// create a Cartesian image suitable for texture mapping from the raw
// bearing/range measurements; also return a mask of valid image regions
shared_ptr<DidsonCartesian> Didson::getCartesian(int width, int widthTmp) const {
  // generate map for Cartesian image
  vector<int> map;
  int height;
  pair<vector<int>, int> tmp1 = createMapping(width, maxRange(), minRange(),
      consts.bearingFov * 0.5, numBearings(), numRanges());
  map = tmp1.first;
  height = tmp1.second;

  // avoid having to write out the inverse mapping function by creating
  // a map with sufficiently high resolution as a lookup table for the inverse map
  // not ideal, but works...
  vector<int> invMap(consts.numRanges * consts.numBearings);
  vector<int> mapTmp;
  int heightTmp;
  pair<vector<int>, int> tmp2 = createMapping(widthTmp, maxRange(), minRange(),
      consts.bearingFov * 0.5, numBearings(), consts.numRanges);
  mapTmp = tmp2.first;
  heightTmp = tmp2.second;

  int c = 0;
  for (int y = 0; y < heightTmp; y++) {
    for (int x = 0; x < widthTmp; x++) {
      int idx = mapTmp[c];
      if (idx != -1) {
        int icol = x * ((double) width / (double) widthTmp);
        int irow = y * ((double) height / (double) heightTmp);
        int i = irow * width + icol;
        invMap[idx] = i;
      }
      c++;
    }
  }

  shared_ptr<DidsonCartesian> cartesian(new DidsonCartesian(map, invMap));
  cartesian->image = cv::Mat(height, width, CV_8UC1);
  cartesian->mask = cv::Mat(height, width, CV_8UC1);
  for (int i = 0; i < width * height; i++) {
    if (map[i] == -1) {
      cartesian->image.data[i] = 0;
      cartesian->mask.data[i] = 0;
    } else {
      cartesian->image.data[i] = _image.data[map[i]];
      cartesian->mask.data[i] = 255;
    }
  }

  return cartesian;
}

// constructor taking a DIDSON frame
Didson::Didson(int windowStart, int windowLength,
               const isam::Pose3d& vehiclePose, double tiltRad, double rollRad,
               const unsigned char* data, bool transformFrame) :
    Sonar(consts.startBase * windowStart,
        consts.startBase * windowStart + consts.lengths[windowLength],
        consts.bearingFov, consts.elevationFov, consts.numBearings,
        consts.numRanges), _tiltRad(tiltRad), _rollRad(
        rollRad), _vehiclePose(vehiclePose) {
  if (windowStart < 1 || windowStart > 31) {
    cout << "ERROR: windowStart out of range" << endl;
    exit(1);
  }
  if (windowLength < 0 || windowLength > 3) {
    cout << "ERROR: windowLength out of range" << endl;
    exit(1);
  }

  _image = cv::Mat(consts.numRanges, consts.numBearings, CV_8UC1);
  if (data)
      memcpy(_image.data, data, consts.numRanges * consts.numBearings);

  // we assume DIDSON facing to the right (DVL in hull lock mode)
  // i.e. 90 degrees yaw and -90 degrees roll configuration + tilt and roll actuators
  if (transformFrame) {
      Pose3d didsonOffset(0, 0.2, 0, 0, 0, 0); // DIDSON is to the right of the DVL along y  // todo: check DIDSON offset
      Pose3d didsonActuators(0, 0, 0, M_PI * 0.5, tiltRad, rollRad - M_PI * 0.5);
      _didsonPose = vehiclePose.oplus(didsonOffset).oplus(didsonActuators);
  } else {
      _didsonPose = vehiclePose;
  }
}

Pose3d Didson::didsonHuls3Pose() const {
  return _didsonPose;
}

vector<Point3d> Didson::getFrustum() const {
  return Sonar::getFrustum(didsonHuls3Pose());
}

bool Didson::project(const isam::Point3d& point,
    isam::Point2d& projection) const {
  return Sonar::project(didsonHuls3Pose(), point, projection);
}

} /* namespace sonar */


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2015 Oracle and/or its affiliates.

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_sym_difference_areal_areal
#endif

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#define BOOST_GEOMETRY_DEBUG_TURNS
#define BOOST_GEOMETRY_DEBUG_SEGMENT_IDENTIFIER
#endif

#include <boost/test/included/unit_test.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/algorithms/sym_difference.hpp>

#include "../difference/test_difference.hpp"
#include <from_wkt.hpp>

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::ring<point_type> ring_type; // ccw, closed
typedef bg::model::polygon<point_type> polygon_type; // ccw, closed
typedef bg::model::multi_polygon<polygon_type> multi_polygon_type;

double const default_tolerance = 0.0001;

template
<
    typename Areal1, typename Areal2, typename PolygonOut
>
struct test_sym_difference_of_areal_geometries
{
    static inline void apply(std::string const& case_id,
                             Areal1 const& areal1,
                             Areal2 const& areal2,
                             int expected_polygon_count,
                             int expected_point_count,
                             double expected_area,
                             double tolerance = default_tolerance)
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        bg::model::multi_polygon<PolygonOut> sdf;

        bg::sym_difference(areal1, areal2, sdf);

        std::cout << "Case ID: " << case_id << std::endl;
        std::cout << "Geometry #1: " << bg::wkt(areal1) << std::endl;
        std::cout << "Geometry #2: " << bg::wkt(areal2) << std::endl;
        std::cout << "Sym diff: " << bg::wkt(sdf) << std::endl;
        std::cout << "Polygon count: expected: "
                  << expected_polygon_count
                  << "; detected: " << sdf.size() << std::endl;
        std::cout << "Point count: expected: "
                  << expected_point_count
                  << "; detected: " << bg::num_points(sdf) << std::endl;
        std::cout << "Area: expected: "
                  << expected_area
                  << "; detected: " << bg::area(sdf) << std::endl;
#endif
        ut_settings settings;
        settings.percentage = tolerance;

        test_difference
            <
                PolygonOut
            >(case_id, areal1, areal2,
              expected_polygon_count, expected_point_count, expected_area,
              true, settings);
    }
};

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_sym_difference_ring_ring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** RING / RING SYMMETRIC DIFFERENCE ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef ring_type R;
    typedef polygon_type PG;

    typedef test_sym_difference_of_areal_geometries<R, R, PG> tester;

    tester::apply("r-r-sdf00",
                  from_wkt<R>("POLYGON((0 0,0 10,10 10,10 0,0 0))"),
                  from_wkt<R>("POLYGON((10 0,10 20,20 20,20 0,10 0))"),
                  1,
                  8,
                  300);

    tester::apply("r-r-sdf01",
                  from_wkt<R>("POLYGON((0 0,0 10,10 10,10 0,0 0))"),
                  from_wkt<R>("POLYGON((9 0,9 20,20 20,20 0,9 0))"),
                  2,
                  12,
                  300);
}

BOOST_AUTO_TEST_CASE( test_sym_difference_polygon_multipolygon )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POLYGON / MULTIPOLYGON SYMMETRIC DIFFERENCE ***"
              << std::endl;
    std::cout << std::endl;
#endif

    typedef polygon_type PG;
    typedef multi_polygon_type MPG;

    typedef test_sym_difference_of_areal_geometries<PG, MPG, PG> tester;

    tester::apply
        ("pg-mpg-sdf00",
         from_wkt<PG>("POLYGON((10 0,10 10,20 10,20 0,10 0))"),
         from_wkt<MPG>("MULTIPOLYGON(((0 0,0 10,10 10,10 0,0 0)),\
                       ((20 0,20 10,30 10,30 0,20 0)))"),
         1,
         6,
         300);
}