/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/mean_and_covar.hpp
 * @brief Functions to estimate mean value and covariance on manifolds.
 */

#ifndef MEAN_AND_COVAR_HPP_
#define MEAN_AND_COVAR_HPP_

#include <Eigen/Core>

namespace MTK {

/**
 * \defgroup MeanCov Mean and Covariance Calculation
 * @todo provide functions, which only calculate either mean or covariance
 */
//@{

/**
 * Estimate mean value and covariance of a set of manifold values.
 *
 * @tparam M    Manifold type. Must implement boxminus, boxplus and have
 *              @c typedef scalar and @c enum DOF.
 * @tparam Cont Container Type. Elements must be convertible to @c M.
 *
 * @param mean   reference to mean value (output)
 * @param cov    reference to covariance matrix (output)
 * @param values const reference to input container
 * @param max_it maximum number of iterations (optional).
 *
 * Mean value and covariance are estimated using algorithm described in
 * @cite{Hertzberg2011}
 */
template<class M, class Cont>
double mean_and_covariance(M& mean, Eigen::Matrix<typename M::scalar, M::DOF, M::DOF> &cov,
                           const Cont &values, int max_it = 16)
{
	enum {DOF = M::DOF};
	typedef typename M::scalar scalar;
	mean = values[0];
	double res;
	int i=0;
	do {
		Eigen::Matrix<scalar, DOF, 1> mean_delta, delta;
		mean_delta.setZero();
		for (typename Cont::const_iterator Xi = values.begin(); Xi != values.end(); ++Xi)
		{
			Xi->boxminus(delta.data(), mean);
			mean_delta += delta;
		}
		mean_delta /= values.size();
		res = mean_delta.norm();
		mean.boxplus(mean_delta.data());
	} while (res > 1e-6 && ++i < max_it);

	cov.setZero();
	for (typename Cont::const_iterator Xi = values.begin(); Xi != values.end(); ++Xi)
	{
		Eigen::Matrix<scalar, DOF, 1> delta;
		Xi->boxminus(delta.data(), mean);
		cov += delta * delta.transpose();
	}
	cov *= 0.5;

	return res;
}

//@}

}  // namespace MTK

#endif /* MEAN_AND_COVAR_HPP_ */


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4d m = Vector4d(1,2,3,4).asDiagonal();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is m.fixed<2, 2>(2, 2):" << endl << m.block<2, 2>(2, 2) << endl;
m.block<2, 2>(2, 0) = m.block<2, 2>(2, 2);
cout << "Now the matrix m is:" << endl << m << endl;

  return 0;
}


#pragma once

#include "EllipsoidalCalibration.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Cholesky>
#include <iostream>
namespace icarus
{
    template<typename T>
    struct EllipsoidalCalibrator
    {
        EllipsoidalCalibrator(size_t sampleCount) :
            mSamples(10, sampleCount),
            mIndex(0)
        {}

        void addSample(Eigen::Matrix<T, 3, 1> const & p);

        EllipsoidalCalibration<T> computeCalibration(T norm) const;
    private:
        static Eigen::Matrix<T, 6, 1> computePositiveEigenVector(Eigen::Matrix<T, 6, 6> const & SS);
        static Eigen::Matrix<T, 10, 1> computeEllipsoidEquation(Eigen::Matrix<T, 10, 10> const & S);
        static Eigen::Matrix<T, 3, 4> computeEllipsoidTransformation(Eigen::Matrix<T, 10, 1> const & v, T norm);

        Eigen::Matrix<T, 10, Eigen::Dynamic> mSamples;
        size_t mIndex;
    };

    template<typename T>
    void EllipsoidalCalibrator<T>::addSample(Eigen::Matrix<T, 3, 1> const & p)
    {
        mSamples.col(mIndex) <<
            p.x() * p.x(),
            p.y() * p.y(),
            p.z() * p.z(),
            2 * p.y() * p.z(),
            2 * p.x() * p.z(),
            2 * p.x() * p.y(),
            2 * p.x(),
            2 * p.y(),
            2 * p.z(),
            1;

        ++mIndex;
    }

    template<typename T>
    EllipsoidalCalibration<T> EllipsoidalCalibrator<T>::computeCalibration(T norm) const
    {
        Eigen::Matrix<T, 10, 10> S;
        // compute D * D.transpose() but in a way that is almost 3 times faster and uses 4 times less RAM
        S.setZero();
        S.template selfadjointView<Eigen::Lower>().rankUpdate(mSamples);
        S.template triangularView<Eigen::Upper>() = S.transpose();

        auto ellipsoidEquation = computeEllipsoidEquation(S);

        auto ellipsoid = computeEllipsoidTransformation(ellipsoidEquation, norm);

        return EllipsoidalCalibration<T>(ellipsoid);
    }

    template<typename T>
    Eigen::Matrix<T, 6, 1> EllipsoidalCalibrator<T>::computePositiveEigenVector(Eigen::Matrix<T, 6, 6> const & SS)
    {
        Eigen::Matrix<T, 6, 6> C;
        C.setZero();
        C.template block<3, 3>(0, 0).setOnes();
        C.diagonal() << -1, -1, -1, -4, -4, -4;

        Eigen::EigenSolver<Eigen::Matrix<T, 6, 6>> decomposition(C.lu().solve(SS));

        Eigen::Index maxCol;
        decomposition.eigenvalues().real().maxCoeff(&maxCol);
        Eigen::Matrix<T, 6, 1> v1 = decomposition.eigenvectors().col(maxCol).real();
        if (v1(1) < 0) {
            v1 = -v1;
        }
        return v1;
    }

    template<typename T>
    Eigen::Matrix<T, 10, 1> EllipsoidalCalibrator<T>::computeEllipsoidEquation(Eigen::Matrix<T, 10, 10> const & S)
    {
        auto S11 = S.template block<6, 6>(0, 0);
        auto S21 = S.template block<4, 6>(6, 0);
        auto S12 = S.template block<6, 4>(0, 6);//S21.transpose();
        auto S22 = S.template block<4, 4>(6, 6);
        Eigen::Matrix<T, 4, 6> const S22a = S22.inverse() * S21;
        Eigen::Matrix<T, 6, 6> const SS = S11 - S12 * S22a;

        Eigen::Matrix<T, 10, 1> v;
        auto v1 = v.template head<6>();
        auto v2 = v.template tail<4>();

        v1 = computePositiveEigenVector(SS);
        v2 = -S22a * v1;

        return v;
    }

    template<typename T>
    Eigen::Matrix<T, 3, 4> EllipsoidalCalibrator<T>::computeEllipsoidTransformation(Eigen::Matrix<T, 10, 1> const & v, T norm)
    {
        Eigen::Matrix<T, 3, 3> Q;
        Q << v[0], v[5], v[4],
             v[5], v[1], v[3],
             v[4], v[3], v[2];

        Eigen::Matrix<T, 3, 4> ret;
        auto Ainv = ret.template block<3, 3>(0, 0);
        auto B = ret.col(3);

        B = -Q.inverse() * v.template segment<3>(6);

        T scaling = norm / sqrt(B.transpose() * Q * B - v[9]);

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, 3, 3>> Qsolver(Q);
        Ainv.noalias() = scaling * Qsolver.operatorSqrt();
        B = -B;

        std::cout << ret << std::endl;

        return ret;
    }
}


#pragma once

#define _USE_MATH_DEFINES
#include <math.h>
//#include "trkapi.hpp"
#include <toffy/toffy_config.h>
#include <boost/log/trivial.hpp>

#if OCV_VERSION_MAJOR >= 3
#  include <opencv2/calib3d.hpp>
#else
#  include <opencv2/calib3d/calib3d.hpp>
#endif

namespace toffy {
namespace commons {

/** helper functions for camera handling; assumes we have standard optics (90deg hfov).
 */

const int hPix=160;
const int vPix=120;
const float hFov=90.;
const float vFov=65.;
const float pixSinAngle = 0.009817319; // = sin( deg2rad(90/160) );
const float pixCosAngle = 0.999951809; // = cos( deg2rad(90/160) );
const double fl_x_reciprocal = 1.0f / 8.8345892962843834e+01;
const double fl_y_reciprocal = 1.0f / 8.8395902341635306e+01;
const double center_x = 7.9460485484676596e+01;
const double center_y = 5.7816728185872989e+01;

static inline float deg2rad(float a) { return a/180.*M_PI; }

static inline double getPixelSize(double distance )
{
    //float maxFinger = 0.01 / (angl*pdis);  //==pixel width
    return pixSinAngle * distance;
}

static inline void depth2xyz(const cv::Point2f& p, float d, cv::Point3f& xyz)
{
    xyz.x = pixSinAngle*(p.x-hPix/2) *  pixSinAngle*(p.y-vPix/2) * d;
    xyz.y = pixSinAngle*(p.x-hPix/2) *  pixCosAngle*(p.y-vPix/2) * d;
    xyz.z = pixCosAngle*(p.x-hPix/2) * d;

    float rho = deg2rad( (p.x-hPix/2) );
    float tht = deg2rad( (p.y-vPix/2) );

    xyz.x = d * sin (rho) * sin(tht) ;
    xyz.y = d * sin (rho) * cos(tht) ;
    xyz.z = d * cos (rho);
}

static inline void depth2xyz(const cv::Point2f& p, float d, cv::Vec3f& xyz)
{
    xyz[0] = pixSinAngle*(p.x-hPix/2) *  pixSinAngle*(p.y-vPix/2) * d;
    xyz[1] = pixSinAngle*(p.x-hPix/2) *  pixCosAngle*(p.y-vPix/2) * d;
    xyz[2] = pixCosAngle*(p.x-hPix/2) * d;
}

/*static inline void depth2xyz(const cv::Point2f& p, float d, track::vec3f& xyz)
{
    float rho = deg2rad( (p.x-hPix/2) );
    float tht = deg2rad( (p.y-vPix/2) );

    xyz.x = d * sin (rho) * sin(tht) ;
    xyz.y = d * sin (rho) * cos(tht) ;
    xyz.z = d * cos (rho);
}*/

static inline cv::Point3d pointTo3D(cv::Point2d point, float depthValue, cv::Mat cameraMatrix, cv::Size imgSize)
{
	//BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << point;

	double fl_x_reciprocal, fl_y_reciprocal;
	cv::Point2d center;
	if(cameraMatrix.data) {
	    //Saving parameter from the camera matrix
	    fl_x_reciprocal = 1.0f / cameraMatrix.at<double>(0,0);
	    fl_y_reciprocal = 1.0f / cameraMatrix.at<double>(1,1);
	    //center_x = _cameraMatrix.at<double>(0,2);
	    //center_y = _cameraMatrix.at<double>(1,2);

	    double noV,
		    apertureWidth = (45/1000)*imgSize.width,
		    apertureHeight = (45/1000)*imgSize.height;
	    cv::calibrationMatrixValues(cameraMatrix, imgSize,
				    apertureWidth, apertureHeight,
				    noV, noV, noV, center, noV);
	} else {
	    BOOST_LOG_TRIVIAL(warning) <<"No cameraMatrix data.";
	    return cv::Point3d();
	}

	cv::Point3d out3Dp;

	//Saving parameter from the camera matrix
	out3Dp.x = (static_cast<float> (point.x) - center.x) * depthValue * fl_x_reciprocal; //X
	out3Dp.y = (static_cast<float> (point.y) - center.y) * depthValue * fl_y_reciprocal; //Y
	out3Dp.z = depthValue; //Z

	return out3Dp;
}

static inline cv::Point2i pointTo2D(cv::Point3d point, cv::Mat cameraMatrix, cv::Size imgSize)
{
	//BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;
	double fl_x_reciprocal, fl_y_reciprocal;
	cv::Point2d center;
	if(cameraMatrix.data) {
	    //Saving parameter from the camera matrix
	    fl_x_reciprocal = 1.0f / cameraMatrix.at<double>(0,0);
	    fl_y_reciprocal = 1.0f / cameraMatrix.at<double>(1,1);
	    //center_x = _cameraMatrix.at<double>(0,2);
	    //center_y = _cameraMatrix.at<double>(1,2);

	    double noV,
		    apertureWidth = (45/1000)*imgSize.width,
		    apertureHeight = (45/1000)*imgSize.height;
	    cv::calibrationMatrixValues(cameraMatrix, imgSize,
				    apertureWidth, apertureHeight,
				    noV, noV, noV, center, noV);
	} else {
	    BOOST_LOG_TRIVIAL(warning) <<"No cameraMatrix data.";
	    return cv::Point2i();
	}

	cv::Point2i out2Dp;

	out2Dp.x = (point.x/(point.z*fl_x_reciprocal)) + center.x;
	out2Dp.y = (point.y/(point.z*fl_y_reciprocal)) + center.y;
	// Saving found pixel
	return out2Dp;
}

static inline cv::Point3d pointTo3D(cv::Point point, float depthValue)
{
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

	cv::Point3d out3Dp;

	//Saving parameter from the camera matrix
	out3Dp.x = (static_cast<float> (point.x) - center_x) * depthValue * fl_x_reciprocal; //X
	out3Dp.y = (static_cast<float> (point.y) - center_y) * depthValue * fl_y_reciprocal; //Y
	out3Dp.z = depthValue; //Z

	return out3Dp;
}

static inline cv::Point2i pointTo2D(cv::Point3d point)
{
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

	cv::Point2i out2Dp;

	out2Dp.x = (point.x/(point.z*fl_x_reciprocal)) + center_x;
	out2Dp.y = (point.y/(point.z*fl_y_reciprocal)) + center_y;
	// Saving found pixel
	return out2Dp;
}

}}


#include <boost/optional/optional_io.hpp>
#include <catch.hpp>

#include "tests/netcode/launch.hh"

#include "netcode/detail/invert_matrix.hh"
#include "netcode/detail/square_matrix.hh"

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

using namespace ntc;

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

namespace /* unnamed */{

// Use jerasure implementation as a reference. Slighty modified to use our galois_field.
int
jerasure_invert_matrix( detail::square_matrix& mat, detail::square_matrix& inv
                      , detail::galois_field& gf)
{
  // Types are changed from int to unsigned int to avoid warnings.
  unsigned int i, j, k, x, rs2;
  unsigned int row_start, tmp, inverse;
  unsigned int cols = static_cast<unsigned int>(mat.dimension());
  unsigned int rows = cols;

  k = 0;
  for (i = 0; i < rows; i++) {
    for (j = 0; j < cols; j++) {
      inv[k] = (i == j) ? 1 : 0;
      k++;
    }
  }

  /* First -- convert into upper triangular  */
  for (i = 0u; i < cols; i++) {
    row_start = cols * i;

    /* Swap rows if we ave a zero i,i element.  If we can't swap, then the
     matrix was not invertible  */

    if (mat[row_start+i] == 0) {
      for (j = i+1; j < rows && mat[cols*j+i] == 0; j++) ;
      if (j == rows) return -1;
      rs2 = j*cols;
      for (k = 0; k < cols; k++) {
        tmp = mat[row_start+k];
        mat[row_start+k] = mat[rs2+k];
        mat[rs2+k] = tmp;
        tmp = inv[row_start+k];
        inv[row_start+k] = inv[rs2+k];
        inv[rs2+k] = tmp;
      }
    }

    /* Multiply the row by 1/element i,i  */
    tmp = mat[row_start+i];
    if (tmp != 1) {
//      inverse = galois_single_divide(1, tmp, w);
      inverse = gf.invert(tmp);
      for (j = 0; j < cols; j++) {
//        mat[row_start+j] = galois_single_multiply(mat[row_start+j], inverse, w);
        mat[row_start+j] = gf.multiply(mat[row_start+j], inverse);
//        inv[row_start+j] = galois_single_multiply(inv[row_start+j], inverse, w);
        inv[row_start+j] = gf.multiply(inv[row_start+j], inverse);
      }
    }

    /* Now for each j>i, add A_ji*Ai to Aj  */
    k = row_start+i;
    for (j = i+1; j != cols; j++) {
      k += cols;
      if (mat[k] != 0) {
        if (mat[k] == 1) {
          rs2 = cols*j;
          for (x = 0; x < cols; x++) {
            mat[rs2+x] ^= mat[row_start+x];
            inv[rs2+x] ^= inv[row_start+x];
          }
        } else {
          tmp = mat[k];
          rs2 = cols*j;
          for (x = 0; x < cols; x++) {
//            mat[rs2+x] ^= galois_single_multiply(tmp, mat[row_start+x], w);
            mat[rs2+x] ^= gf.multiply(tmp, mat[row_start+x]);
//            inv[rs2+x] ^= galois_single_multiply(tmp, inv[row_start+x], w);
            inv[rs2+x] ^= gf.multiply(tmp, inv[row_start+x]);
          }
        }
      }
    }
  }

  /* Now the matrix is upper triangular.  Start at the top and multiply down  */

//  for (i = rows-1; i >= 0; i--) {
  for (i = rows-1; ; i--) {
    row_start = i*cols;
    for (j = 0; j < i; j++) {
      rs2 = j*cols;
      if (mat[rs2+i] != 0) {
        tmp = mat[rs2+i];
        mat[rs2+i] = 0;
        for (k = 0; k < cols; k++) {
//          inv[rs2+k] ^= galois_single_multiply(tmp, inv[row_start+k], w);
          inv[rs2+k] ^= gf.multiply(tmp, inv[row_start+k]);
        }
      }
    }
    if (i == 0) // Added test .
    {
      break;
    }
  }
  return 0;
}

} // namespace unnamed

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

TEST_CASE("Compare with jerasure matrix inversion")
{
  launch([](std::uint8_t gf_size)
  {
    detail::galois_field gf{gf_size};

    detail::square_matrix m0{3};
    std::uint32_t k = 0u;
    for (auto i = 0ul; i < 3; ++i)
    {
      for (auto j = 0ul; j < 3; ++j, ++k)
      {
        m0(i, j) = k;
      }
    }
    auto m1 = m0;

    detail::square_matrix inv0{3};
    detail::square_matrix inv1{3};

    // Matrix is invertible.
    REQUIRE(jerasure_invert_matrix(m0, inv0, gf) == 0);
    REQUIRE(not detail::invert(gf, m1, inv1));

    // The result is the same as jerasure's.
    for (auto i = 0ul; i < 3 * 3; ++i)
    {
      REQUIRE(inv0[i] == inv1[i]);
    }
  });
}

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

TEST_CASE("Non-invertible matrix")
{
  /// @todo Find non invertible matrix for other GF sizes.
  detail::galois_field gf{8};

  detail::square_matrix m0{3};
  std::uint32_t k = 0u;
  for (auto i = 0u; i < 3; ++i)
  {
    for (auto j = 0u; j < 3; ++j, ++k)
    {
      m0[k] = (k+i) * 2;
    }
  }
  auto m1 = m0;

  detail::square_matrix inv0{3};
  detail::square_matrix inv1{3};

  REQUIRE(jerasure_invert_matrix(m0, inv0, gf) == -1);
  REQUIRE(detail::invert(gf, m1, inv1));
}

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

TEST_CASE("Matrix is correctly inverted")
{
  launch([&](std::uint8_t gf_size)
  {
    detail::galois_field gf{gf_size};

    detail::square_matrix m{3};
    std::uint32_t k = 0u;
    for (auto i = 0ul; i < 3; ++i)
    {
      for (auto j = 0ul; j < 3; ++j, ++k)
      {
        m(i, j) = k;
      }
    }

    // Inverted matrix is destroyed, we need a copy.
    auto copy = m;

    detail::square_matrix inv{3};

    // Matrix is invertible.
    REQUIRE(not detail::invert(gf, copy, inv));

    // Multiply m and inv
    detail::square_matrix identity{3};
    for (auto i = 0ul; i < identity.dimension(); ++i)
    {
      for (auto j = 0ul; j < identity.dimension(); ++j, ++k)
      {
        identity(i, j) = [&]
        {
          std::uint32_t tmp = 0;
          for (k = 0ul; k < identity.dimension(); ++k)
          {
            tmp ^= gf.multiply(m(i, k), inv(k, j));
          }
          return tmp;
        }();
      }
    }

    // Check that M * M^-1 = Id
    for (auto i = 0ul; i < identity.dimension(); ++i)
    {
      for (auto j = 0ul; j < identity.dimension(); ++j)
      {
        REQUIRE(identity(i,j) == (i == j ? 1 : 0));
      }
    }
  });
}

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


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Utilities/TypeTraits/RemoveReferenceWrapper.hpp"

/// \cond
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace domain {
namespace CoordinateMaps {

/*!
 * \ingroup CoordinateMapsGroup
 *
 * \brief Redistributes gridpoints within the unit sphere.
 * \image html SpecialMobius.png "A sphere with a `mu` of 0.25."
 *
 * \details A special case of the conformal Mobius transformation that
 * maps the unit ball to itself. This map depends on a single
 * parameter, `mu` \f$ = \mu\f$, which is the x-coordinate of the preimage
 * of the origin under this map. This map has the fixed points \f$x=1\f$ and
 * \f$x=-1\f$. The map is singular for \f$\mu=1\f$ but we have found that this
 * map is accurate up to 12 decimal places for values of \f$\mu\f$ up to 0.96.
 *
 * We define the auxiliary variables
 * \f[ r := \sqrt{x^2 + y^2 +z^2}\f]
 * and
 * \f[ \lambda := \frac{1}{1 - 2 x \mu + \mu^2 r^2}\f]
 *
 * The map corresponding to this transformation in cartesian coordinates
 * is then given by:
 *
 * \f[\vec{x}'(x,y,z) =
 * \lambda\begin{bmatrix}
 * x(1+\mu^2) - \mu(1+r^2)\\
 * y(1-\mu^2)\\
 * z(1-\mu^2)\\
 * \end{bmatrix}\f]
 *
 * The inverse map is the same as the forward map with \f$\mu\f$
 * replaced by \f$-\mu\f$.
 *
 * This map is intended to be used only inside the unit sphere.  A
 * point inside the unit sphere maps to another point inside the unit
 * sphere. The map can have undesirable behavior at certain points
 * outside the unit sphere: The map is singular at
 * \f$(x,y,z) = (1/\mu, 0, 0)\f$ (which is outside the unit sphere
 * since \f$|\mu| < 1\f$). Moreover, a point on the \f$x\f$-axis
 * arbitrarily close to the singularity maps to an arbitrarily large
 * value on the \f$\pm x\f$-axis, where the sign depends on which side
 * of the singularity the point is on.
 *
 * A general Mobius transformation is a function on the complex plane, and
 * takes the form \f$ f(z) = \frac{az+b}{cz+d}\f$, where
 * \f$z, a, b, c, d \in \mathbb{C}\f$, and \f$ad-bc\neq 0\f$.
 *
 * The special case used in this map is the function
 * \f$ f(z) = \frac{z - \mu}{1 - z\mu}\f$. This has the desired properties:
 * - The unit disk in the complex plane is mapped to itself.
 *
 * - The x-axis is mapped to itself.
 *
 * - \f$f(\mu) = 0\f$.
 *
 * The three-dimensional version of this map is obtained by rotating the disk
 * in the plane about the x-axis.
 *
 * This map is useful for performing transformations along the x-axis
 * that preserve the unit disk. A concrete example of this is in the BBH
 * domain, where two BBHs with a center-of-mass at x=\f$\mu\f$ can be shifted
 * such that the new center of mass is now located at x=0. Additionally,
 * the spherical shape of the outer wave-zone is preserved and, as a mobius
 * map, the spherical coordinate shapes of the black holes is also preserved.
 */
class SpecialMobius {
 public:
  static constexpr size_t dim = 3;
  explicit SpecialMobius(double mu) noexcept;
  SpecialMobius() = default;
  ~SpecialMobius() = default;
  SpecialMobius(SpecialMobius&&) = default;
  SpecialMobius(const SpecialMobius&) = default;
  SpecialMobius& operator=(const SpecialMobius&) = default;
  SpecialMobius& operator=(SpecialMobius&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  /// Returns boost::none for target_coords outside the unit sphere.
  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p) noexcept;  // NOLINT

  bool is_identity() const noexcept { return is_identity_; }

 private:
  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> mobius_distortion(
      const std::array<T, 3>& coords, double mu) const noexcept;
  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
  mobius_distortion_jacobian(const std::array<T, 3>& coords, double mu) const
      noexcept;
  friend bool operator==(const SpecialMobius& lhs,
                         const SpecialMobius& rhs) noexcept;

  double mu_{std::numeric_limits<double>::signaling_NaN()};
  bool is_identity_{false};
};
bool operator!=(const SpecialMobius& lhs, const SpecialMobius& rhs) noexcept;
}  // namespace CoordinateMaps
}  // namespace domain


/* test_binomial_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/binomial_distribution.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::binomial_distribution<>
#define BOOST_RANDOM_ARG1 t
#define BOOST_RANDOM_ARG2 p
#define BOOST_RANDOM_ARG1_DEFAULT 1
#define BOOST_RANDOM_ARG2_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 10
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 1
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX 10
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX 10

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 1

#define BOOST_RANDOM_TEST2_PARAMS (10, 0.25)
#define BOOST_RANDOM_TEST2_MIN 0
#define BOOST_RANDOM_TEST2_MAX 10

#include "test_distribution.ipp"


/**
 * @file feedforward_network_test.cpp
 * @author Marcus Edel
 * @author Palash Ahuja
 *
 * Tests the feed forward network.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/ann/ffn.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"
#include "custom_layer.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(FeedForwardNetworkTest);

/**
 * Train and evaluate a vanilla network with the specified structure.
 */
template<typename MatType = arma::mat>
void BuildVanillaNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t outputSize,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |     +>|     |     +>|     |
   * +-----+     | +--+--+     | +-----+
   *             |             |
   *  Bias       |  Bias       |
   *  Layer      |  Layer      |
   * +-----+     | +-----+     |
   * |     |     | |     |     |
   * |     +-----+ |     +-----+
   * |     |       |     |
   * +-----+       +-----+
   */

  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(trainData.n_rows, hiddenLayerSize);
  model.Add<SigmoidLayer<> >();
  model.Add<Linear<> >(hiddenLayerSize, outputSize);
  model.Add<LogSoftMax<> >();

  // RMSProp opt(0.01, 32, 0.88, 1e-8, maxEpochs * trainData.n_cols, -1);
  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, maxEpochs * trainData.n_cols, -1);
  model.Train(trainData, trainLabels, opt);

  MatType predictionTemp;
  model.Predict(testData, predictionTemp);
  MatType prediction = arma::zeros<MatType>(1, predictionTemp.n_cols);

  for (size_t i = 0; i < predictionTemp.n_cols; ++i)
  {
    prediction(i) = arma::as_scalar(arma::find(
        arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
  }

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
    if (int(arma::as_scalar(prediction.col(i))) ==
        int(arma::as_scalar(testLabels.col(i))))
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkTest)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildVanillaNetwork<>
      (trainData, trainLabels, testData, testLabels, 3, 8, 10, 0.1);

  arma::mat dataset;
  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);
  labels += 1;

  // Vanilla neural net with logistic activation function.
  BuildVanillaNetwork<>
      (dataset, labels, dataset, labels, 2, 10, 10, 0.2);
}

BOOST_AUTO_TEST_CASE(ForwardBackwardTest)
{
  arma::mat dataset;
  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);
  labels += 1;

  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(dataset.n_rows, 50);
  model.Add<SigmoidLayer<> >();
  model.Add<Linear<> >(50, 10);
  model.Add<LogSoftMax<> >();

  ens::VanillaUpdate opt;
  model.ResetParameters();
  opt.Initialize(model.Parameters().n_rows, model.Parameters().n_cols);
  double stepSize = 0.01;
  size_t batchSize = 10;

  size_t iteration = 0;
  bool converged = false;
  while (iteration < 100)
  {
    arma::running_stat<double> error;
    size_t batchStart = 0;
    while (batchStart < dataset.n_cols)
    {
      size_t batchEnd = std::min(batchStart + batchSize,
          (size_t) dataset.n_cols);
      arma::mat currentData = dataset.cols(batchStart, batchEnd - 1);
      arma::mat currentLabels = labels.cols(batchStart, batchEnd - 1);
      arma::mat currentResuls;
      model.Forward(currentData, currentResuls);
      arma::mat gradients;
      model.Backward(currentLabels, gradients);
      opt.Update(model.Parameters(), stepSize, gradients);
      batchStart = batchEnd;

      arma::mat prediction = arma::zeros<arma::mat>(1, currentResuls.n_cols);

      for (size_t i = 0; i < currentResuls.n_cols; ++i)
      {
        prediction(i) = arma::as_scalar(arma::find(
            arma::max(currentResuls.col(i)) == currentResuls.col(i), 1)) + 1;
      }

      size_t correct = 0;
      for (size_t i = 0; i < currentLabels.n_cols; i++)
      {
        if (int(arma::as_scalar(prediction.col(i))) ==
            int(arma::as_scalar(currentLabels.col(i))))
        {
          correct++;
        }
      }

      error(1 - (double) correct / batchSize);
    }
    Log::Debug << "Current training error: " << error.mean() << std::endl;
    iteration++;
    if (error.mean() < 0.05)
    {
      converged = true;
      break;
    }
  }

  BOOST_REQUIRE(converged);
}

/**
 * Train and evaluate a Dropout network with the specified structure.
 */
template<typename MatType = arma::mat>
void BuildDropoutNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t outputSize,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Dropout      Output
   *  Layer         Layer         Layer        Layer
   * +-----+       +-----+       +-----+       +-----+
   * |     |       |     |       |     |       |     |
   * |     +------>|     +------>|     +------>|     |
   * |     |     +>|     |       |     |       |     |
   * +-----+     | +--+--+       +-----+       +-----+
   *             |
   *  Bias       |
   *  Layer      |
   * +-----+     |
   * |     |     |
   * |     +-----+
   * |     |
   * +-----+
   */

  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(trainData.n_rows, hiddenLayerSize);
  model.Add<SigmoidLayer<> >();
  model.Add<Dropout<> >();
  model.Add<Linear<> >(hiddenLayerSize, outputSize);
  model.Add<LogSoftMax<> >();

  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, maxEpochs * trainData.n_cols, -1);

  model.Train(trainData, trainLabels, opt);

  MatType predictionTemp;
  model.Predict(testData, predictionTemp);
  MatType prediction = arma::zeros<MatType>(1, predictionTemp.n_cols);

  for (size_t i = 0; i < predictionTemp.n_cols; ++i)
  {
    prediction(i) = arma::as_scalar(arma::find(
        arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
  }

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
    if (int(arma::as_scalar(prediction.col(i))) ==
        int(arma::as_scalar(testLabels.col(i))))
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train the dropout network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DropoutNetworkTest)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildDropoutNetwork<>
      (trainData, trainLabels, testData, testLabels, 3, 8, 10, 0.1);

  arma::mat dataset;
  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);
  labels += 1;

  // Vanilla neural net with logistic activation function.
  BuildDropoutNetwork<>
      (dataset, labels, dataset, labels, 2, 10, 10, 0.2);
}

/**
 * Train and evaluate a DropConnect network(with a baselayer) with the
 * specified structure.
 */
template<typename MatType = arma::mat>
void BuildDropConnectNetwork(MatType& trainData,
                             MatType& trainLabels,
                             MatType& testData,
                             MatType& testLabels,
                             const size_t outputSize,
                             const size_t hiddenLayerSize,
                             const size_t maxEpochs,
                             const double classificationErrorThreshold)
{
 /*
  *  Construct a feed forward network with trainData.n_rows input nodes,
  *  hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
  *  network struct that looks like:
  *
  *  Input         Hidden     DropConnect     Output
  *  Layer         Layer         Layer        Layer
  * +-----+       +-----+       +-----+       +-----+
  * |     |       |     |       |     |       |     |
  * |     +------>|     +------>|     +------>|     |
  * |     |     +>|     |       |     |       |     |
  * +-----+     | +--+--+       +-----+       +-----+
  *             |
  *  Bias       |
  *  Layer      |
  * +-----+     |
  * |     |     |
  * |     +-----+
  * |     |
  * +-----+
  *
  *
  */

  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(trainData.n_rows, hiddenLayerSize);
  model.Add<SigmoidLayer<> >();
  model.Add<DropConnect<> >(hiddenLayerSize, outputSize);
  model.Add<LogSoftMax<> >();

  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, maxEpochs * trainData.n_cols, -1);

  model.Train(trainData, trainLabels, opt);

  MatType predictionTemp;
  model.Predict(testData, predictionTemp);
  MatType prediction = arma::zeros<MatType>(1, predictionTemp.n_cols);

  for (size_t i = 0; i < predictionTemp.n_cols; ++i)
  {
    prediction(i) = arma::as_scalar(arma::find(
        arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
  }

  size_t error = 0;
  for (size_t i = 0; i < testData.n_cols; i++)
  {
    if (int(arma::as_scalar(prediction.col(i))) ==
        int(arma::as_scalar(testLabels.col(i))))
    {
      error++;
    }
  }

  double classificationError = 1 - double(error) / testData.n_cols;
  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
}

/**
 * Train the dropconnect network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DropConnectNetworkTest)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildDropConnectNetwork<>
      (trainData, trainLabels, testData, testLabels, 3, 8, 10, 0.1);

  arma::mat dataset;
  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);
  labels += 1;

  // Vanilla neural net with logistic activation function.
  BuildDropConnectNetwork<>
      (dataset, labels, dataset, labels, 2, 10, 10, 0.2);
}

/**
 * Test miscellaneous things of FFN,
 * e.g. copy/move constructor, assignment operator.
 */
BOOST_AUTO_TEST_CASE(FFNMiscTest)
{
  FFN<MeanSquaredError<>> model;
  model.Add<Linear<>>(2, 3);
  model.Add<ReLULayer<>>();

  auto copiedModel(model);
  copiedModel = model;
  auto movedModel(std::move(model));
  movedModel = std::move(copiedModel);
}

/**
 * Test that serialization works ok.
 */
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(trainData.n_rows, 8);
  model.Add<SigmoidLayer<> >();
  model.Add<Dropout<> >();
  model.Add<Linear<> >(8, 3);
  model.Add<LogSoftMax<> >();

  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, trainData.n_cols /* 1 epoch */, -1);

  model.Train(trainData, trainLabels, opt);

  FFN<NegativeLogLikelihood<>> xmlModel, textModel, binaryModel;
  xmlModel.Add<Linear<>>(10, 10); // Layer that will get removed.

  // Serialize into other models.
  SerializeObjectAll(model, xmlModel, textModel, binaryModel);

  arma::mat predictions, xmlPredictions, textPredictions, binaryPredictions;
  model.Predict(testData, predictions);
  xmlModel.Predict(testData, xmlPredictions);
  textModel.Predict(testData, textPredictions);
  textModel.Predict(testData, binaryPredictions);

  CheckMatrices(predictions, xmlPredictions, textPredictions,
      binaryPredictions);
}

/**
 * Test if the custom layers work. The target is to see if the code compiles
 * when the Train and Prediction are called.
 */
BOOST_AUTO_TEST_CASE(CustomLayerTest)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  FFN<NegativeLogLikelihood<>, RandomInitialization, CustomLayer<> > model;
  model.Add<Linear<> >(trainData.n_rows, 8);
  model.Add<CustomLayer<> >();
  model.Add<Linear<> >(8, 3);
  model.Add<LogSoftMax<> >();

  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, 15, -1);
  model.Train(trainData, trainLabels, opt);

  arma::mat predictionTemp;
  model.Predict(testData, predictionTemp);
  arma::mat prediction = arma::zeros<arma::mat>(1, predictionTemp.n_cols);
}

/**
 * Test the overload of Forward function which allows partial forward pass.
 */
BOOST_AUTO_TEST_CASE(PartialForwardTest)
{
  FFN<NegativeLogLikelihood<>, RandomInitialization> model;
  model.Add<Linear<> >(5, 10);

  // Add a new Add<> module which adds a constant term to the input.
  Add<>* addModule = new Add<>(10);
  model.Add(addModule);

  LinearNoBias<>* linearNoBiasModule = new LinearNoBias<>(10, 10);
  model.Add(linearNoBiasModule);

  model.Add<Linear<> >(10, 10);

  model.ResetParameters();
  // Set the parameters of the Add<> module to a matrix of ones.
  addModule->Parameters() = arma::ones(10, 1);
  // Set the parameters of the LinearNoBias<> module to a matrix of ones.
  linearNoBiasModule->Parameters() = arma::ones(10, 10);

  arma::mat input = arma::ones(10, 1);
  arma::mat output;

  // Forward pass only through the Add module.
  model.Forward(input,
                output,
                1 /* Index of the Add module */,
                1 /* Index of the Add module */);

  // As we only forward pass through Add module, input and output should
  // differ by a matrix of ones.
  CheckMatrices(input, output - 1);

  // Forward pass only through the Add module and the LinearNoBias module.
  model.Forward(input,
                output,
                1 /* Index of the Add module */,
                2 /* Index of the LinearNoBias module */);

  // As we only forward pass through Add module followed by the LinearNoBias
  // module, output should be a matrix of 20s.(output = weight * input)
  CheckMatrices(output, arma::ones(10, 1) * 20);
}

/**
 * Test that FFN::Train() returns finite objective value.
 */
BOOST_AUTO_TEST_CASE(FFNTrainReturnObjective)
{
  // Load the dataset.
  arma::mat trainData;
  data::Load("thyroid_train.csv", trainData, true);

  arma::mat trainLabels = trainData.row(trainData.n_rows - 1);
  trainData.shed_row(trainData.n_rows - 1);

  arma::mat testData;
  data::Load("thyroid_test.csv", testData, true);

  arma::mat testLabels = testData.row(testData.n_rows - 1);
  testData.shed_row(testData.n_rows - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significantly better than 92%.
  FFN<NegativeLogLikelihood<> > model;
  model.Add<Linear<> >(trainData.n_rows, 8);
  model.Add<SigmoidLayer<> >();
  model.Add<Dropout<> >();
  model.Add<Linear<> >(8, 3);
  model.Add<LogSoftMax<> >();

  ens::RMSProp opt(0.01, 32, 0.88, 1e-8, trainData.n_cols /* 1 epoch */, -1);

  double objVal = model.Train(trainData, trainLabels, opt);

  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);
}
BOOST_AUTO_TEST_SUITE_END();


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <sstream>

#include "camera.hpp"

using std::cout;
using std::endl;
using std::max;
using std::min;
using std::ifstream;
using std::ofstream;

using namespace Eigen;
using namespace SnowSimulator;

void Camera::configure(double nearClip, double farClip, double hFov,
                       double vFov, size_t screenW, size_t screenH) {
  this->screenW = screenW;
  this->screenH = screenH;

  this->nearClip = nearClip;
  this->farClip = farClip;

  this->hFov = hFov;
  this->vFov = vFov;

  double ar1 = tan(radians(hFov) / 2) / tan(radians(vFov) / 2);
  ar = static_cast<double>(screenW) / screenH;

  if (ar1 < ar) {
    // hFov is too small
    hFov = 2 * degrees(atan(tan(radians(vFov) / 2) * ar));
  } else if (ar1 > ar) {
    // vFov is too small
    vFov = 2 * degrees(atan(tan(radians(hFov) / 2) / ar));
  }

  screenDist = ((double)screenH) / (2.0 * tan(radians(vFov) / 2));
}

void Camera::place(const Vector3d &targetPos, const double phi,
                   const double theta, const double r, const double minR,
                   const double maxR) {
  double r_ = min(max(r, minR), maxR);
  double phi_ = (sin(phi) == 0) ? (phi + EPS_F) : phi;

  this->targetPos = targetPos;
  this->phi = phi_;
  this->theta = theta;
  this->r = r_;
  this->minR = minR;
  this->maxR = maxR;

  compute_position();
}

void Camera::copy_placement(const Camera &other) {
  pos = other.pos;
  targetPos = other.targetPos;
  phi = other.phi;
  theta = other.theta;
  minR = other.minR;
  maxR = other.maxR;
  c2w = other.c2w;
}

void Camera::set_screen_size(const size_t screenW, const size_t screenH) {
  this->screenW = screenW;
  this->screenH = screenH;

  ar = 1.0 * screenW / screenH;
  hFov = 2 * degrees(atan(((double)screenW) / (2 * screenDist)));
  vFov = 2 * degrees(atan(((double)screenH) / (2 * screenDist)));
}

void Camera::move_by(const double dx, const double dy, const double d) {
  const double scaleFactor = d / screenDist;
  const Vector3d &displacement =
      c2w.col(0) * (dx * scaleFactor) + c2w.col(1) * (dy * scaleFactor);

  pos += displacement;
  targetPos += displacement;
}

void Camera::move_forward(const double dist) {
  double newR = min(max(r - dist, minR), maxR);
  pos = targetPos + ((pos - targetPos) * (newR / r));
  r = newR;
}

void Camera::rotate_by(const double dPhi, const double dTheta) {
  phi = clamp(phi + dPhi, 0.0, (double)PI);
  theta += dTheta;
  compute_position();
}

void Camera::compute_position() {
  double sinPhi = sin(phi);
  if (sinPhi == 0) {
    phi += EPS_F;
    sinPhi = sin(phi);
  }
  const Vector3d dirToCamera(r * sinPhi * sin(theta), r * cos(phi),
                             r * sinPhi * cos(theta));
  pos = targetPos + dirToCamera;

  Vector3d upVec(0, sinPhi > 0 ? 1 : -1, 0);
  Vector3d screenXDir = upVec.cross(dirToCamera);

  screenXDir.normalize();
  Vector3d screenYDir = dirToCamera.cross(screenXDir);
  screenYDir.normalize();

  c2w.col(0) = screenXDir;
  c2w.col(1) = screenYDir;

  // camera's view direction is the opposite of of dirToCamera, so directly
  // using dirToCamera as column 2 of the matrix takes [0 0 -1] to the world
  // space view direction
  c2w.col(2) = dirToCamera.normalized();
}

// void Camera::dump_settings(string filename) {
//   ofstream file(filename);
//   file << hFov << " " << vFov << " " << ar << " " << nearClip << " " <<
//   farClip
//        << endl;
//   for (int i = 0; i < 3; ++i)
//     file << pos[i] << " ";
//   for (int i = 0; i < 3; ++i)
//     file << targetPos[i] << " ";
//   file << endl;
//   file << phi << " " << theta << " " << r << " " << minR << " " << maxR <<
//   endl; for (int i = 0; i < 9; ++i)
//     file << c2w(i / 3, i % 3) << " ";
//   file << endl;
//   file << screenW << " " << screenH << " " << screenDist << endl;
//   cout << "[Camera] Dumped settings to " << filename << endl;
// }
//
// void Camera::load_settings(string filename) {
//   ifstream file(filename);
//
//   file >> hFov >> vFov >> ar >> nearClip >> farClip;
//   for (int i = 0; i < 3; ++i)
//     file >> pos[i];
//   for (int i = 0; i < 3; ++i)
//     file >> targetPos[i];
//   file >> phi >> theta >> r >> minR >> maxR;
//   for (int i = 0; i < 9; ++i)
//     file >> c2w(i / 3, i % 3);
//   file >> screenW >> screenH >> screenDist;
//   cout << "[Camera] Loaded settings from " << filename << endl;
// }


#include "ros_package_template/Algorithm.hpp"

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

// STD
#include <vector>

#include <Eigen/Core>

using namespace ros_package_template;

TEST(Algorithm, getWithoutSet
) {
Algorithm algorithm;
const double average = algorithm.getAverage();
EXPECT_EQ(0.0, average);
}

TEST(Algorithm, singleDataPoint
) {
const double inputData = 100.0 * (double) rand() / RAND_MAX;
Algorithm algorithm;
algorithm.
addData(inputData);
const double average = algorithm.getAverage();
EXPECT_NEAR(inputData, average,
1e-10);
}

TEST(Algorithm, singleDataVector
) {
const double inputValue = 100.0 * (double) rand() / RAND_MAX;
Algorithm algorithm;
Eigen::VectorXd inputData;
inputData.resize(2);
inputData << inputValue, 3 *
inputValue;
algorithm.
addData(inputData);
const double average = algorithm.getAverage();
EXPECT_NEAR(2 * inputValue, average, 1e-10);
}

TEST(Algorithm, multipleDataPoints
) {
size_t nMeasurements = 100;
std::vector<double> inputData(nMeasurements);
double sum = 0.0;
for (
auto &data
: inputData) {
data = 100.0 * (double) rand() / RAND_MAX;
sum +=
data;
}

Algorithm algorithm;
for (
const auto data
: inputData) {
algorithm.
addData(data);
}
const double average = algorithm.getAverage();
EXPECT_NEAR(sum
/ nMeasurements, average, 1e-10);
}


/**
 * @file convolution_test.cpp
 * @author Marcus Edel
 *
 * Tests for various convolution strategies.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/pooling_rules/max_pooling.hpp>
#include <mlpack/methods/ann/pooling_rules/mean_pooling.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(PoolingTest);

/**
 * Test the max pooling rule.
 */
BOOST_AUTO_TEST_CASE(MaxPoolingTest)
{
  // The data was generated by magic(6) in MATLAB.
  arma::mat input, output;
  input << 35 << 1 << 6 << 26 << 19 << 24 << arma::endr
        << 3 << 32 << 7 << 21 << 23 << 25 << arma::endr
        << 31 << 9 << 2 << 22 << 27 << 20 << arma::endr
        << 8 << 28 << 33 << 17 << 10 << 15 << arma::endr
        << 30 << 5 << 34 << 12 << 14 << 16 << arma::endr
        << 4 << 36 << 29 << 13 << 18 << 11;

  // Expected output of the generated 6 x 6 matrix.
  const double poolingOutput = 36;

  MaxPooling poolingRule;

  // Test the pooling function.
  BOOST_REQUIRE_EQUAL(poolingRule.Pooling(input), poolingOutput);

  // Test the unpooling function.
  poolingRule.Unpooling(input, input.max(), output);
  BOOST_REQUIRE_EQUAL(arma::accu(output), input.max());
}

/**
 * Test the mean pooling rule.
 */
BOOST_AUTO_TEST_CASE(MeanPoolingTest)
{
  // The data was generated by magic(6) in MATLAB.
  arma::mat input, output;
  input << 35 << 1 << 6 << 26 << 19 << 24 << arma::endr
        << 3 << 32 << 7 << 21 << 23 << 25 << arma::endr
        << 31 << 9 << 2 << 22 << 27 << 20 << arma::endr
        << 8 << 28 << 33 << 17 << 10 << 15 << arma::endr
        << 30 << 5 << 34 << 12 << 14 << 16 << arma::endr
        << 4 << 36 << 29 << 13 << 18 << 11;

  // Expected output of the generated 6 x 6 matrix.
  const double poolingOutput = 18.5;

  MeanPooling poolingRule;

  // Test the pooling function.
  BOOST_REQUIRE_EQUAL(poolingRule.Pooling(input), poolingOutput);

  // Test the unpooling function.
  poolingRule.Unpooling(input, input.max(), output);
  bool b = arma::all(arma::vectorise(output) == (input.max() / input.n_elem));
  BOOST_REQUIRE_EQUAL(b, true);
}

BOOST_AUTO_TEST_SUITE_END();


#include "derivative_damping.hpp"
#include "laplacian.hpp"

#include <string>
#include <armadillo>
#include <yaml-cpp/yaml.h>

DerivativeDamping::DerivativeDamping(const std::string &filenm):
  Regularization(filenm)
{
  YAML::Node config = YAML::LoadFile(filenm);
  output_file_ = config["file_deriv"].as<std::string>();

  this->load();
  laplacian_ = laplacian2d(umodel_-rmodel_, ni1_, ni2_, nk1_, nk2_);
}

void DerivativeDamping::save()
{
  Regularization::save(output_file_);
}

void DerivativeDamping::cal_fitness()
{
  fitness_ = arma::accu(arma::square(laplacian_));
}

void DerivativeDamping::cal_gradient()
{
  gradient_ = 2 * laplacian2d(laplacian_, ni1_, ni2_, nk1_, nk2_);

}


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

#include "timer.h"
#include "matrix.h"

#if defined(AMATRIX_COMPARE_WITH_EIGEN)
#include "Eigen/Dense"
#endif

#if defined(AMATRIX_COMPARE_WITH_UBLAS)
#include <boost/numeric/ublas/matrix.hpp>
#endif

#define AMATRIX_MEASURE_ABC_OPERATION(name, operation)                   \
    void name() {                                                        \
        TMatrixType A(TSize1, TSize2);                                   \
        TMatrixType B(TSize1, TSize2);                                   \
        initialize(A);                                                   \
        initialize(B);                                                   \
        Timer timer;                                                     \
        for (std::size_t i_repeat = 0; i_repeat < mRepeat; i_repeat++) { \
            operation                                                    \
        }                                                                \
        auto elapsed = timer.elapsed().count();                          \
        std::cout << "\t\t" << elapsed;                                  \
    }

#define AMATRIX_MEASURE_ABCD_OPERATION(name, operation)                   \
    void name() {                                                         \
        TMatrixType A(TSize1, TSize2);                                    \
        TMatrixType B(TSize1, TSize2);                                    \
        initialize(A);                                                    \
        initializeInverse(B);                                             \
        initializeInverse(D);                                             \
        Timer timer;                                                      \
        for (std::size_t i_repeat = 0; i_repeat < mRepeat / (2 * TSize2); \
             i_repeat++) {                                                \
            operation                                                     \
        }                                                                 \
        auto elapsed = timer.elapsed().count();                           \
        std::cout << "\t\t" << elapsed;                                   \
    }

template <typename TMatrixType, std::size_t TSize1, std::size_t TSize2>
class ComparisonColumn {
   protected:
    static constexpr std::size_t mRepeat =
        static_cast<std::size_t>(1e8 / (TSize1 * TSize2));

    TMatrixType C;
    TMatrixType D;
    std::string mColumnName;

    void initialize(TMatrixType& TheMatrix) {
        for (std::size_t i = 0; i < TSize1; i++)
            for (std::size_t j = 0; j < TSize2; j++)
                TheMatrix(i, j) = j + 1.00;
    }

    void initializeInverse(TMatrixType& TheMatrix) {
        for (std::size_t i = 0; i < TSize1; i++)
            for (std::size_t j = 0; j < TSize2; j++)
                TheMatrix(i, j) = 1.00 / (i + 1);
    }

   public:
    ComparisonColumn() = delete;

    ComparisonColumn(std::string ColumnName)
        : C(TSize1, TSize2),
          D(TSize1, TSize2), mColumnName(ColumnName) {
        initialize(C);
    }
    std::string const& GetColumnName() { return mColumnName; }
    TMatrixType& GetResult() { return C; }

    template <typename TMatrixType2>
    bool CheckResult(TMatrixType2 const& Reference) {
        constexpr double tolerance = 1e-12;
        for (std::size_t i = 0; i < TSize1; i++)
            for (std::size_t j = 0; j < TSize2; j++)
                if (std::abs(C(i, j) - Reference(i, j)) > tolerance) {
                    std::cout << " " << C(i, j) << " != " << Reference(i, j);
                    return false;
                }

        return true;
    }

    AMATRIX_MEASURE_ABC_OPERATION(MeasureSumTime,C.noalias() = A + B; B.noalias() = C;)

    AMATRIX_MEASURE_ABCD_OPERATION(MeasureMultTime,C.noalias() = D * A; D.noalias() = B;)

    AMATRIX_MEASURE_ABCD_OPERATION(MeasureABAMultTime, C.noalias() = A * TMatrixType(D * A); D.noalias() = B;)

    AMATRIX_MEASURE_ABCD_OPERATION(MeasureATransposeBAMultTime,C.noalias() = A.transpose() * TMatrixType(D * A); D.noalias() = B;)

};

#if defined(AMATRIX_COMPARE_WITH_UBLAS)
using namespace boost::numeric::ublas;
template <typename TMatrixType, std::size_t TSize1,
    std::size_t TSize2>

class UblasComparisonColumn
    : public ComparisonColumn<TMatrixType, TSize1, TSize2> {
    void initialize(TMatrixType& TheMatrix) {
        for (std::size_t i = 0; i < TSize1; i++)
            for (std::size_t j = 0; j < TSize2; j++)
                TheMatrix(i, j) = j + 1.00;
    }

    void initializeInverse(TMatrixType& TheMatrix) {
        for (std::size_t i = 0; i < TSize1; i++)
            for (std::size_t j = 0; j < TSize2; j++)
                TheMatrix(i, j) = 1.00 / (i + 1);
    }
   public:
    using BaseType =
        ComparisonColumn<TMatrixType, TSize1, TSize2>;

    using BaseType::mRepeat;

    using BaseType::C;
    using BaseType::D;

    UblasComparisonColumn(std::string ColumnName)
        : ComparisonColumn<TMatrixType, TSize1, TSize2>(
              ColumnName) {}

    AMATRIX_MEASURE_ABC_OPERATION(MeasureSumTime, noalias(BaseType::C) =  A + B; noalias(B) = BaseType::C;)
    AMATRIX_MEASURE_ABCD_OPERATION(MeasureMultTime,noalias(BaseType::C) = prod(BaseType::D, A); noalias(BaseType::D) = B;)
    AMATRIX_MEASURE_ABCD_OPERATION(MeasureABAMultTime, noalias(BaseType::C) = prod(A, TMatrixType(prod(BaseType::D, A))); noalias(BaseType::D) = B;)
    AMATRIX_MEASURE_ABCD_OPERATION(MeasureATransposeBAMultTime, noalias(BaseType::C) = prod(trans(A), TMatrixType(prod(BaseType::D, A)));noalias(BaseType::D) = B;)

};
#endif

template <typename TMatrixType, std::size_t TSize1,
    std::size_t TSize2>
class EmptyComparisonColumn
    : public ComparisonColumn<TMatrixType, TSize1, TSize2> {
   public:
    EmptyComparisonColumn(std::string ColumnName)
        : ComparisonColumn<TMatrixType, TSize1, TSize2>("") {}
    void MeasureSumTime() { std::cout << "\t\t"; }
    void MeasureMultTime() { std::cout << "\t\t"; }
    void MeasureABAMultTime() { std::cout << "\t\t"; }
    void MeasureATransposeBAMultTime() { std::cout << "\t\t"; }

    template <typename TMatrixType2>
    bool CheckResult(TMatrixType2 const& Reference) {
        return true;
    }
};

#define RUN_BENCHMARK(name, function)                          \
    std::cout << name;                                         \
    mAMatrixColumn.function();                                 \
    mEigenColumn.function();                                   \
    if (!mEigenColumn.CheckResult(mAMatrixColumn.GetResult())) \
        std::cout << "(Failed!)";                              \
    mUblasColumn.function();                                   \
    if (!mUblasColumn.CheckResult(mAMatrixColumn.GetResult())) \
        std::cout << "(Failed!)";                              \
    std::cout << std::endl;

template <std::size_t TSize1, std::size_t TSize2>
class BenchmarkMatrix {
    ComparisonColumn<AMatrix::Matrix<double, TSize1, TSize2>,
        TSize1, TSize2>
        mAMatrixColumn;
#if defined(AMATRIX_COMPARE_WITH_EIGEN)
    ComparisonColumn<Eigen::Matrix<double, TSize1, TSize2>,
        TSize1, TSize2>
        mEigenColumn;
#else
    EmptyComparisonColumn<
        AMatrix::Matrix<double, TSize1, TSize2>, TSize1,
        TSize2>
        mEigenColumn;
#endif
#if defined(AMATRIX_COMPARE_WITH_UBLAS)
    UblasComparisonColumn<boost::numeric::ublas::bounded_matrix<double,
                              TSize1, TSize2>,
        TSize1, TSize2>
        mUblasColumn;
#else
    EmptyComparisonColumn<
        AMatrix::Matrix<double, TSize1, TSize2>, TSize1,
        TSize2>
        mUblasColumn;
#endif
   public:
    BenchmarkMatrix()
        : mAMatrixColumn("AMatrix"),
          mEigenColumn("Eigen"),
          mUblasColumn("Ublas") {
        std::cout << "Benchmark[" << TSize1 << "," << TSize2
                  << "]";
        std::cout << "\t\t" << mAMatrixColumn.GetColumnName();
        std::cout << "\t\t" << mEigenColumn.GetColumnName();
        std::cout << "\t\t" << mUblasColumn.GetColumnName();
        std::cout << std::endl;
    }

    ~BenchmarkMatrix() = default;
    void Run() {
        RUN_BENCHMARK("C = A + B", MeasureSumTime)
        RUN_BENCHMARK("C = A * B", MeasureMultTime)
        RUN_BENCHMARK("C = A * B * A", MeasureABAMultTime)
        RUN_BENCHMARK("C = A^T * B * A", MeasureATransposeBAMultTime)

        std::cout << std::endl;
    }
};

template <std::size_t TSize1, std::size_t TSize2>
class BenchmarkDynamicMatrix {
    ComparisonColumn<AMatrix::Matrix<double, 0, 0>, TSize1,
        TSize2>
        mAMatrixColumn;
#if defined(AMATRIX_COMPARE_WITH_EIGEN)
    ComparisonColumn<
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>, TSize1,
        TSize2>
        mEigenColumn;
#else
    EmptyComparisonColumn<
        AMatrix::Matrix<double, TSize1, TSize2>, TSize1,
        TSize2>
        mEigenColumn;
#endif
#if defined(AMATRIX_COMPARE_WITH_UBLAS)
    UblasComparisonColumn<boost::numeric::ublas::matrix<double>,
        TSize1, TSize2>
        mUblasColumn;
#else
    EmptyComparisonColumn<
        AMatrix::Matrix<double, TSize1, TSize2>, TSize1,
        TSize2>
        mUblasColumn;
#endif
   public:
    BenchmarkDynamicMatrix()
        : mAMatrixColumn("AMatrix"),
          mEigenColumn("Eigen"),
          mUblasColumn("Ublas") {
        std::cout << "Benchmark[" << TSize1 << "," << TSize2
                  << "]";
        std::cout << "\t\t" << mAMatrixColumn.GetColumnName();
        std::cout << "\t\t" << mEigenColumn.GetColumnName();
        std::cout << "\t\t" << mUblasColumn.GetColumnName();
        std::cout << std::endl;
    }

    ~BenchmarkDynamicMatrix() = default;
    void Run() {
        RUN_BENCHMARK("C = A + B", MeasureSumTime)
        RUN_BENCHMARK("C = A * B", MeasureMultTime)
        RUN_BENCHMARK("C = A * B * A", MeasureABAMultTime)
        RUN_BENCHMARK("C = A^T * B * A", MeasureATransposeBAMultTime)

        std::cout << std::endl;
    }
};

int main() {
    BenchmarkMatrix<3, 3> benchmark_3_3;
    benchmark_3_3.Run();

    BenchmarkMatrix<4, 4> benchmark_4_4;
    benchmark_4_4.Run();

    BenchmarkMatrix<6, 6> benchmark_6_6;
    benchmark_6_6.Run();

    BenchmarkMatrix<12, 12> benchmark_12_12;
    benchmark_12_12.Run();

    BenchmarkMatrix<16, 16> benchmark_16_16;
    benchmark_16_16.Run();

    BenchmarkDynamicMatrix<3, 3> dynamic_bechmark_3_3;
    dynamic_bechmark_3_3.Run();

    BenchmarkDynamicMatrix<4, 4> dynamic_benchmark_4_4;
    dynamic_benchmark_4_4.Run();

    BenchmarkDynamicMatrix<6, 6> dynamic_benchmark_6_6;
    dynamic_benchmark_6_6.Run();

    BenchmarkDynamicMatrix<12, 12> dynamic_benchmark_12_12;
    dynamic_benchmark_12_12.Run();

    BenchmarkDynamicMatrix<16, 16> dynamic_benchmark_16_16;
    dynamic_benchmark_16_16.Run();

    return 0;
}


#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arr_polyline_traits_2.h>
#include <CGAL/Arrangement_2.h>

#include <boost/iterator/function_output_iterator.hpp>

#include <array>
#include <cassert>

typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel>                Segment_traits_2;
typedef CGAL::Arr_polyline_traits_2<Segment_traits_2>     Geom_traits_2;

typedef Geom_traits_2::Point_2                            Point_2;
typedef Geom_traits_2::Segment_2                          Segment_2;
typedef Geom_traits_2::Curve_2                            Polyline_2;
typedef CGAL::Arrangement_2<Geom_traits_2>                Arrangement_2;
typedef Geom_traits_2::X_monotone_curve_2                 X_monotone_polyline;
typedef Geom_traits_2::X_monotone_subcurve_2              X_monotone_subcurve;

struct Test_functor
{
  const X_monotone_polyline* reference;

  Test_functor (const X_monotone_polyline& reference)
    : reference (&reference) { }

  void operator() (const CGAL::Object& obj) const
  {
     const X_monotone_polyline* poly
      = CGAL::object_cast<X_monotone_polyline>(&obj);
     assert(poly != nullptr); //  Intersection is not a polyline

    typename X_monotone_polyline::Point_const_iterator
      itref = reference->points_begin(),
      itpoly = poly->points_begin();

    for (; itref != reference->points_end()
           && itpoly != poly->points_end();
         ++ itref, ++ itpoly)
      assert(*itref == *itpoly);
  }
};

void test (const X_monotone_polyline& a, const X_monotone_polyline& b,
           const X_monotone_polyline& reference)
{
  Geom_traits_2 traits;
  Geom_traits_2::Intersect_2 intersect_2 =
    traits.intersect_2_object();

  std::cerr << " * Polyline A = " << a << std::endl
            << " * Polyline B = " << b << std::endl;

  intersect_2
    (a, b, boost::make_function_output_iterator (Test_functor(reference)));
}

int main()
{
  Geom_traits_2 traits;
  Arrangement_2 arr(&traits);

  Geom_traits_2::Construct_x_monotone_curve_2 x_mono_polyline_construct =
    traits.construct_x_monotone_curve_2_object();

  std::array<Segment_2, 2> r2l
    = { Segment_2(Point_2(1, 0), Point_2(0, 1)),
        Segment_2(Point_2(0, 1), Point_2(-1, 0)) };

  std::array<Segment_2, 2> l2r
    = { Segment_2(Point_2(-1, 0), Point_2(0, 1)),
        Segment_2(Point_2(0, 1), Point_2(1, 0)), };

  X_monotone_polyline p0l2r
    = x_mono_polyline_construct (l2r.begin(), l2r.end());
  X_monotone_polyline p1l2r
    = x_mono_polyline_construct (l2r.begin(), l2r.end());
  X_monotone_polyline p0r2l
    = x_mono_polyline_construct (r2l.begin(), r2l.end());
  X_monotone_polyline p1r2l
    = x_mono_polyline_construct (r2l.begin(), r2l.end());

  std::cerr << "Testing intersection left-to-right / left-to-right" << std::endl;
  test (p0l2r, p1l2r, p0l2r);
  std::cerr << "Testing intersection left-to-right / right-to-left" << std::endl;
  test (p0l2r, p1r2l, p0l2r);
  std::cerr << "Testing intersection right-to-left / left-to-right" << std::endl;
  test (p0r2l, p1l2r, p0l2r);
  std::cerr << "Testing intersection right-to-left / right-to-left" << std::endl;
  test (p0r2l, p1r2l, p0r2l);

  return EXIT_SUCCESS;
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/mldivide.hpp
 *
 * \brief Matrix left division.
 *
 * Inspired by the \c mldivide MATLAB function.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2012, Marco Guazzone
 *
 * 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)
 */

#ifndef BOOST_NUMERIC_UBLASX_MLDIVIDE_HPP
#define BOOST_NUMERIC_UBLASX_MLDIVIDE_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/lu.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

template<typename AMatrixT,
         typename BVectorT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixT>::size_type mldivide_inplace(matrix_expression<AMatrixT> const& A,
                                                             vector_container<BVectorT>& b)
{
    return lu_solve_inplace(A, b);
}

template<typename AMatrixT,
         typename BVectorT,
         typename XVectorT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixT>::size_type mldivide(matrix_expression<AMatrixT> const& A,
                                                     vector_expression<BVectorT> const& b,
                                                     vector_container<XVectorT>& x)
{
    return lu_solve(A, b, x());
}

template<typename AMatrixT,
         typename BMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixT>::size_type mldivide_inplace(matrix_expression<AMatrixT> const& A,
                                                             matrix_container<BMatrixT>& B)
{
    return lu_solve_inplace(A, B);
}

template<typename AMatrixT,
         typename BMatrixT,
         typename XMatrixT>
BOOST_UBLAS_INLINE
typename matrix_traits<AMatrixT>::size_type mldivide(matrix_expression<AMatrixT> const& A,
                                                     matrix_expression<BMatrixT> const& B,
                                                     matrix_container<XMatrixT>& X)
{
    return lu_solve(A, B, X());
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_MLDIVIDE_HPP


#ifndef ASLAM_SCALAR_EXPRESSION_HPP
#define ASLAM_SCALAR_EXPRESSION_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include <aslam/backend/JacobianContainer.hpp>
#include <aslam/backend/Differential.hpp>
#include <set>

namespace aslam {
  namespace backend {

    using std::sqrt;
    using std::log;
    using std::exp;

    class ExpressionNodeVisitor;
    class ScalarExpressionNode;

    class ScalarExpression
    {
    public:
      enum { Dimension = 1 };
      typedef double Value;
      typedef double value_t;
      typedef ScalarExpressionNode node_t;

      explicit ScalarExpression(double value);
      ScalarExpression(const char * name, double value);
      ScalarExpression(ScalarExpressionNode * designVariable);
      ScalarExpression(boost::shared_ptr<ScalarExpressionNode> designVariable);
      ~ScalarExpression();

      double toScalar() const;
      double toValue() const { return toScalar(); }
      double evaluate() const { return toScalar(); }

      void evaluateJacobians(JacobianContainer & outJacobians) const;
      void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      void getDesignVariables(DesignVariable::set_t & designVariables) const;

      boost::shared_ptr<ScalarExpressionNode> root() const { return _root; }
      bool isEmpty() const { return false; }  //TODO feature: support empty scalar expression

      ScalarExpression operator+(const ScalarExpression & s) const;
      ScalarExpression operator-(const ScalarExpression & s) const;
      ScalarExpression operator*(const ScalarExpression & s) const;
      ScalarExpression operator/(const ScalarExpression & s) const;
      ScalarExpression operator+(double s) const;
      ScalarExpression operator-(double s) const;
      ScalarExpression operator-() const;
      ScalarExpression operator*(double s) const;
      ScalarExpression operator/(double s) const;
      bool operator < (const ScalarExpression & s) const { return this->toValue() < s.toValue(); }
      bool operator > (const ScalarExpression & s) const { return this->toValue() > s.toValue(); }
      bool operator <= (const ScalarExpression & s) const { return this->toValue() <= s.toValue(); }
      bool operator >= (const ScalarExpression & s) const { return this->toValue() >= s.toValue(); }
      bool operator == (const ScalarExpression & s) const { return this->toValue() == s.toValue(); }
      bool operator != (const ScalarExpression & s) const { return this->toValue() != s.toValue(); }
      bool operator < (const double s) const { return this->toValue() < s; }
      bool operator > (const double s) const { return this->toValue() > s; }
      bool operator <= (const double s) const { return this->toValue() <= s; }
      bool operator >= (const double s) const { return this->toValue() >= s; }
      bool operator == (const double s) const { return this->toValue() == s; }
      bool operator != (const double s) const { return this->toValue() != s; }

      void accept(ExpressionNodeVisitor& visitor) const;
    private:
      /// \todo make the default constructor private.
      ScalarExpression();

      boost::shared_ptr<ScalarExpressionNode> _root;

      friend class EuclideanExpression;

    };

    std::ostream& operator<<(std::ostream& os, const ScalarExpression& e);

    ScalarExpression sqrt(const ScalarExpression& e);
    ScalarExpression log(const ScalarExpression& e);
    ScalarExpression exp(const ScalarExpression& e);
    ScalarExpression atan(const ScalarExpression& e);
    ScalarExpression tanh(const ScalarExpression& e);
    ScalarExpression atan2(const ScalarExpression& e0, const ScalarExpression& e1);
    ScalarExpression sin(const ScalarExpression& e);
    ScalarExpression cos(const ScalarExpression& e);
    ScalarExpression acos(const ScalarExpression& e);
    ScalarExpression acosSquared(const ScalarExpression& e);
    ScalarExpression inverseSigmoid(const ScalarExpression& e, const double height, const double scale, const double shift);
    ScalarExpression powerExpression(const ScalarExpression& e, const int k);
    ScalarExpression piecewiseExpression(const ScalarExpression& e1, const ScalarExpression& e2, std::function<bool()> useFirst);
    inline ScalarExpression operator / (const double num, const ScalarExpression& den) {
      return ScalarExpression(num) / den;
    }

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_SCALAR_EXPRESSION_HPP */


//  (C) Copyright John Maddock 2018.
//  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_TEST_MODULE test_recurrences

#include <boost/config.hpp>

#ifndef BOOST_NO_CXX11_HDR_TUPLE
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/tools/recurrence.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
//#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/concepts/real_concept.hpp>

#ifdef BOOST_MSVC
#pragma warning(disable:4127)
#endif

template <class T>
struct bessel_jy_recurrence
{
   bessel_jy_recurrence(T v, T z) : v(v), z(z) {}
   boost::math::tuple<T, T, T> operator()(int k)const
   {
      return boost::math::tuple<T, T, T>(T(1), -2 * (v + k) / z, T(1));
   }

   T v, z;
};

template <class T>
struct bessel_ik_recurrence
{
   bessel_ik_recurrence(T v, T z) : v(v), z(z) {}
   boost::math::tuple<T, T, T> operator()(int k)const
   {
      return boost::math::tuple<T, T, T>(T(1), -2 * (v + k) / z, T(-1));
   }

   T v, z;
};

template <class T>
void test_spots(T, const char* name)
{
   std::cout << "Running tests for type " << name << std::endl;
   T tol = boost::math::tools::epsilon<T>() * 5;
   if ((std::numeric_limits<T>::digits > 53) || (std::numeric_limits<T>::digits == 0))
      tol *= 5;
   //
   // Test forward recurrence on Y_v(x):
   //
   {
      T v = 22.25;
      T x = 4.125;
      bessel_jy_recurrence<T> coef(v, x);
      T prev;
      T first = boost::math::cyl_neumann(v - 1, x);
      T second = boost::math::cyl_neumann(v, x);
      T sixth = boost::math::tools::apply_recurrence_relation_forward(coef, 6, first, second, (long long*)0, &prev);
      T expected1 = boost::math::cyl_neumann(v + 6, x);
      T expected2 = boost::math::cyl_neumann(v + 5, x);
      BOOST_CHECK_CLOSE_FRACTION(sixth, expected1, tol);
      BOOST_CHECK_CLOSE_FRACTION(prev, expected2, tol);

      boost::math::tools::forward_recurrence_iterator< bessel_jy_recurrence<T> > it(coef, first, second);
      for (unsigned i = 0; i < 15; ++i)
      {
         expected1 = boost::math::cyl_neumann(v + i, x);
         T found = *it;
         BOOST_CHECK_CLOSE_FRACTION(found, expected1, tol);
         ++it;
      }

      if (std::numeric_limits<T>::max_exponent > 300)
      {
         //
         // This calculates the ratio Y_v(x)/Y_v+1(x) from the recurrence relations
         // which are only transiently stable since Y_v is not minimal as v->-INF
         // but only as v->0.  We have to be sure that v is sufficiently large that
         // convergence is complete before we reach the origin.
         //
         v = 102.75;
         boost::uintmax_t max_iter = 200;
         T ratio = boost::math::tools::function_ratio_from_forwards_recurrence(bessel_jy_recurrence<T>(v, x), boost::math::tools::epsilon<T>(), max_iter);
         first = boost::math::cyl_neumann(v, x);
         second = boost::math::cyl_neumann(v + 1, x);
         BOOST_CHECK_CLOSE_FRACTION(ratio, first / second, tol);

         boost::math::tools::forward_recurrence_iterator< bessel_jy_recurrence<T> > it2(bessel_jy_recurrence<T>(v, x), boost::math::cyl_neumann(v, x));
         for (unsigned i = 0; i < 15; ++i)
         {
            expected1 = boost::math::cyl_neumann(v + i, x);
            T found = *it2;
            BOOST_CHECK_CLOSE_FRACTION(found, expected1, tol);
            ++it2;
         }
      }

   }
   //
   // Test backward recurrence on J_v(x):
   //
   {
      if ((std::numeric_limits<T>::digits > 53) || !std::numeric_limits<T>::is_specialized)
         tol *= 5;

      T v = 22.25;
      T x = 4.125;
      bessel_jy_recurrence<T> coef(v, x);
      T prev;
      T first = boost::math::cyl_bessel_j(v + 1, x);
      T second = boost::math::cyl_bessel_j(v, x);
      T sixth = boost::math::tools::apply_recurrence_relation_backward(coef, 6, first, second, (long long*)0, &prev);
      T expected1 = boost::math::cyl_bessel_j(v - 6, x);
      T expected2 = boost::math::cyl_bessel_j(v - 5, x);
      BOOST_CHECK_CLOSE_FRACTION(sixth, expected1, tol);
      BOOST_CHECK_CLOSE_FRACTION(prev, expected2, tol);

      boost::math::tools::backward_recurrence_iterator< bessel_jy_recurrence<T> > it(coef, first, second);
      for (unsigned i = 0; i < 15; ++i)
      {
         expected1 = boost::math::cyl_bessel_j(v - i, x);
         T found = *it;
         BOOST_CHECK_CLOSE_FRACTION(found, expected1, tol);
         ++it;
      }

      boost::uintmax_t max_iter = 200;
      T ratio = boost::math::tools::function_ratio_from_backwards_recurrence(bessel_jy_recurrence<T>(v, x), boost::math::tools::epsilon<T>(), max_iter);
      first = boost::math::cyl_bessel_j(v, x);
      second = boost::math::cyl_bessel_j(v - 1, x);
      BOOST_CHECK_CLOSE_FRACTION(ratio, first / second, tol);

      boost::math::tools::backward_recurrence_iterator< bessel_jy_recurrence<T> > it2(bessel_jy_recurrence<T>(v, x), boost::math::cyl_bessel_j(v, x));
      //boost::math::tools::backward_recurrence_iterator< bessel_jy_recurrence<T> > it3(bessel_jy_recurrence<T>(v, x), boost::math::cyl_neumann(v+1, x), boost::math::cyl_neumann(v, x));
      for (unsigned i = 0; i < 15; ++i)
      {
         expected1 = boost::math::cyl_bessel_j(v - i, x);
         T found = *it2;
         BOOST_CHECK_CLOSE_FRACTION(found, expected1, tol);
         ++it2;
      }

   }
}

BOOST_AUTO_TEST_CASE( test_main )
{
   BOOST_MATH_CONTROL_FP;
#if !defined(TEST) || TEST == 1
   test_spots(0.0F, "float");
   test_spots(0.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_spots(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
#endif
#if !defined(TEST) || TEST == 2 || TEST == 3
   test_spots(boost::multiprecision::cpp_bin_float_quad(), "cpp_bin_float_quad");
#endif
}

#else

int main() { return 0; }

#endif


#define BZ_DISABLE_KCC_COPY_PROPAGATION_KLUDGE

#include <blitz/tinyvec-et.h>

using namespace blitz;

typedef TinyVector<double,3> vec;

inline void reflect(vec& reflection, const vec& incident,
    const vec& surfaceNormal)
{
    // The surface normal must be unit length to use this equation.

    reflection = incident - 2 * dot(incident,surfaceNormal) * surfaceNormal;
}

template<typename T>
void optimizationSink(T&);

void foo(TinyVector<double,3>& x)
{
    TinyVector<double,3> y, z;

    y =  1.00,  0.40, -1.00;
    z =  0.31,  0.20,  0.93;

    reflect(x, y, z);
}


#include "GEFE_utility.h"
#include <armadillo>

// Given the corresponding eigenvalues of H and H with the jth column removed, returns the ith
// row eigenvector. This is denoted as:
// prod(lambda_H_i - lambda_Hj_k) / prod(lambda_H_i - lambda_H_k), when i != k.
double getRowiEigenvector(int i, const arma::mat& H_eigenvalues, const arma::mat& Hj_eigenvalues) {
    const double lambda_H_i = H_eigenvalues[i];
    double v_ij = 1;
    const int jth_column = H_eigenvalues.size() - 1;

    for (int k = 0; k < H_eigenvalues.size(); ++k) {
        double numerator = 1;   // lambda_H_i - lambda_Hj_k
        double denominator = 1; // lambda_H_i - lambda_H_k
        if (k != jth_column) {
            numerator = lambda_H_i - Hj_eigenvalues[k];
        }
        const double lambda_H_k = H_eigenvalues[k];
        if (lambda_H_k != lambda_H_i) {
            denominator = lambda_H_i - lambda_H_k;
        }
        v_ij *= numerator/denominator;
    }
    return v_ij;
}

double getEigenvectorFromEigenvalues(const arma::mat& H, int i, int j) {
    if (!H.is_square()) {
        throw std::invalid_argument("\nH is not square.");
    }
    if (i < 0 || i > H.n_rows - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "row of the desired eigenvector values, "
                                    "such that 0 <= i <= N-1.");
    }
    if (j < 0 || j > H.n_cols - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "column of the desired eigenvector values, "
                                    "such that 0 <= j <= N-1.");
    }

    const arma::vec H_eigenvalues = eig_sym(H);

    arma::mat Hj_eigenvalues = H;
    Hj_eigenvalues.shed_col(j);
    Hj_eigenvalues.shed_row(j);

    return getRowiEigenvector(i, H_eigenvalues, Hj_eigenvalues);
}

arma::vec getEigenvectorFromEigenvalues(const arma::mat& H, const arma::vec& ii, int j) {
    if (!H.is_square()) {
        throw std::invalid_argument("\nH is not square.");
    }
    for (const double i : ii) {
        if (i < 0 || i > H.n_rows - 1) {
            throw std::invalid_argument("\nEach i in ii must be a row of the matrix H. "
                                        "For each i, 0 <= i <= N-1.");
        }
    }
    if (j < 0 || j > H.n_cols - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "column of the desired eigenvector values, "
                                    "such that 0 <= j <= N-1.");
    }

    const arma::vec H_eigenvalues = eig_sym(H);

    arma::mat Hj_eigenvalues = H;
    Hj_eigenvalues.shed_col(j);
    Hj_eigenvalues.shed_row(j);

    arma::vec v_ij(ii.size(), arma::fill::zeros);
    for (const double i : ii) {
        v_ij[i] = getRowiEigenvector(i, H_eigenvalues, Hj_eigenvalues);
    }
    return v_ij;
}


// Copyright (c) 2011 The University of Sydney

#include <cmath>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/foreach.hpp>
#include <Eigen/Core>
#include <opencv2/core/version.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "region_properties.h"
#include <comma/base/exception.h>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS( cs::cartesian )

namespace snark{ namespace imaging {

/// compute the area of a polygon and its convex hull
/// @param points polygon points
/// @param area area of the polygon
/// @param convexArea area of the convex hull
void compute_area( const std::vector< cv::Point >& points, double& area, double& convexArea )
{
    boost::geometry::model::polygon< boost::tuple< double, double > > polygon;
    for( unsigned int i = 0; i < points.size(); i++ )
    {
        boost::geometry::append( polygon, boost::make_tuple( points[i].x, points[i].y ) );
    }
    boost::geometry::append( polygon, boost::make_tuple( points[0].x, points[0].y ) ); // close polygon

    area = boost::geometry::area( polygon );

    boost::geometry::model::polygon< boost::tuple<double, double> > hull;
    boost::geometry::convex_hull( polygon, hull );

    convexArea = boost::geometry::area( hull );
}

/// constructor
/// @param image input image, is considered as a binary image ( all non-zero pixels are 1 )
region_properties::region_properties ( const cv::Mat& image, double minArea ):
    m_minArea( minArea )
{
    cv::Mat binary;
    if( image.channels() == 3 )
    {
        cv::cvtColor( image, binary, cv::COLOR_RGB2GRAY );
    }
    else if( image.channels() == 1 )
    {
        binary = image;
    }
    else
    {
        COMMA_THROW( comma::exception, "incorrect number of channels, should be 1 or 3, not " << image.channels() );
    }
//     cv::Mat closed;
//     cv::morphologyEx( binary, closed, cv::MORPH_CLOSE, cv::Mat::ones( 3, 3, CV_8U) );
    std::vector< std::vector<cv::Point> > contours;
    cv::findContours( binary, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE );

    for( unsigned int i = 0; i < contours.size(); i++ )
    {
        binary = cv::Scalar(0);
        #if defined( CV_VERSION_EPOCH ) && CV_VERSION_EPOCH == 2
            cv::drawContours( binary, contours, i, cv::Scalar(0xFF), 1 );
        #else
            cv::drawContours( binary, contours, i, cv::Scalar(0xFF), cv::FILLED );
        #endif
        cv::Rect rect = cv::boundingRect( cv::Mat( contours[i]) );
        cv::Moments moments = cv::moments( binary( rect ), true );
        double x = moments.m10/moments.m00;
        double y = moments.m01/moments.m00;
        double area = moments.m00; // cv::countNonZero( binary( rect ) )
        if( area > m_minArea )
        {
            // see wikipedia, image moments
            double diff = moments.nu20 - moments.nu02;
            double a = 0.5 * ( moments.nu20 + moments.nu02 );
            double b = 0.5 * std::sqrt( 4 * moments.nu11 * moments.nu11 + diff * diff );
            double minEigenValue = a - b;
            double maxEigenValue = a + b;
    //         std::cerr << " min " << minEigenValue << " max " << maxEigenValue << std::endl;
            double theta = 0.5 * std::atan2( 2 * moments.nu11, diff );
            double eccentricity = 1;
            if( std::fabs( maxEigenValue ) > 1e-15 )
            {
                eccentricity = std::sqrt( 1 - minEigenValue / maxEigenValue );
            }

            double polygonArea;
            double convexArea;
            compute_area( contours[i], polygonArea, convexArea );
    //         std::cerr << " area " << area << " polygon " << polygonArea << " convex " << convexArea << std::endl;

            blob blob;
            blob.majorAxis = 2 * std::sqrt( moments.m00 * maxEigenValue );
            blob.minorAxis = 2 * std::sqrt( moments.m00 * minEigenValue );
            blob.orientation = theta;
            blob.centroid = cv::Point( x + rect.x, y + rect.y );
            blob.area = area;
            blob.eccentricity = eccentricity;
            blob.solidity = 0;
            if( std::fabs( convexArea ) > 1e-15 )
            {
                blob.solidity = polygonArea / convexArea;
            }
            m_blobs.push_back( blob );
        }
    }
}

/// draw debug information on the image
void region_properties::show( cv::Mat& image, bool text )
{
    for( unsigned int i = 0; i < m_blobs.size(); i++ )
    {
        cv::Point centroid = m_blobs[i].centroid;
        cv::circle( image, centroid, 3, cv::Scalar( 0, 0, 255 ), 2 );
        std::stringstream s;
        s << i;
        if( text )
        {
            cv::putText( image, s.str(), centroid + cv::Point( 2, 2 ), cv::FONT_HERSHEY_PLAIN ,1, cv::Scalar( 0, 0, 255 ) );
        }
        cv::ellipse( image, centroid, cv::Size( m_blobs[i].majorAxis, m_blobs[i].minorAxis ), m_blobs[i].orientation * 180.0 / M_PI, 0, 360, cv::Scalar( 0, 255, 0 ) );
//         std::cerr << i << ": area " << m_blobs[i].area << " eccentricity " << m_blobs[i].eccentricity << " solidity " << m_blobs[i].solidity << std::endl;
    }
}

} }


/**
@file SBGATMassPropertiesUQ.hpp
@class  SBGATMassPropertiesUQ
@author Benjamin Bercovici
@date January 2019

@brief  Evaluation of the formal uncertainty in the volume, center of mass, inertia tensor parametrization
from a topologically-closed, constant density polyhedron.

@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATMassPropertiesUQUQ_hpp
#define SBGATMassPropertiesUQUQ_hpp

#include <armadillo>
#include "SBGATMassProperties.hpp"
#include <SBGATFilterUQ.hpp>

class SBGATMassPropertiesUQ : public SBGATFilterUQ{
public:

  /**
  Sets the model associated to this uncertainty quantification container
  and updates the partials of the mass properties relative to the shape
  @param[in] pgm pointer to valid SBGATFilter
  @param[in]
  */
  virtual void SetModel(vtkSmartPointer<SBGATFilter> model){this -> model = model;}

  /**
  Runs a finite-differencing based test of the implemented PGM partials
  @param input path to obj file used to test the partials
  @param tol relative tolerance
  @param shape_in_meters true if tested shape has its coordinates expressed in meters
  */
  static void TestPartials(std::string input , double tol ,bool shape_in_meters);

/**
Return the partial derivative of the shape's center of mass with respect to the shape's vertices coordinates
@return partial derivative of the shape's center of mass with respect to the shape's vertices
*/
  const arma::mat & GetPartialComPartialC() const {return this -> precomputed_partialGpartialC;}

  /**
  Return the partial derivative of the 6 unique components of the inertia tensor {I(0,0),I(1,1),I(2,2),I(0,1),I(0,2),I(1,2)}
  with respect to the shape coordinates
  @return partial derivative
  */
  const arma::mat & GetPartialIPartialC() const {return this -> precomputed_partialIpartialC;}

  /**
  Return the partial derivative of the MRP orienting the body-frame (B) to principal-frame (P) dcm (PB)
  with respect to the shape vertices coordinates
  @return partial derivative
  */
  const arma::mat & GetPartialSigmaPartialC() const { return this -> precomputed_partialSigmapartialC;}

  /**
  Return the partial derivative of the volume
  with respect to the shape coordinates
  @return partial derivative
  */
  const arma::rowvec & GetPartialVolumePartialC() const {return this -> precomputed_partialVpartialC;}

  /**
  Applies prescribed deviation to all the N_vertices control points and updates model
  @param delta_C deviation (3 * N_vertices x 1)
  */
  virtual void ApplyDeviation(const arma::vec & delta_C);

  /**
  Evaluates the partial of the volume, center of mass and mrp orienting the principal axes
  relative to the vertices coordinates and stores the computed partials in designated containers
  */
  void PrecomputeMassPropertiesPartials();

  /**
  Return the partial derivative of the unit density moments relative to a change in the inertia tensor parametrization
  @return  partial derivative of the unit density moments relative to a change in the inertia tensor parametrization
  */
  const arma::mat & GetPartialUnitDensityMomentsPartialI() const{return this -> precomputed_partialUnitDensityMomentsPartialI;}

  /**
  Return the partial derivative of the MRP orienting the body-frame (B) to principal-frame (P) dcm (PB)
  with respect to the inertia tensor parametrization
  @return partial derivative
  */
  const arma::mat::fixed<3,6> & GetPartialSigmaPartialI() const {return this -> precomputed_partialSigmapartialI;}

/**
  Runs a Monte Carlo on the shape and the volume, center-of-mass and inertia tensor parametrizaton
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] all_volumes holds N_samples of the volume
  @param[out] all_com holds N_samples of the center-of-mass
  @param[out] all_inertia holds N_samples of the inertia tensor parametrization
  */

  static void RunMCUQVolumeCOMInertia(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    std::string output_dir,
    int N_saved_shapes,
    arma::mat & deviations,
    arma::vec & all_volumes,
    arma::mat &  all_com,
    arma::mat & all_inertia);

protected:

  /**
  Returns the partial derivative of the MRP orienting the principal axes with respect to the parametrization
  of the unit-density inertia tensor
  @return partial derivative
  */
  arma::mat::fixed<3,6> PartialSigmaPartialI() const;

/**
Returns partial derivative of unit-density inertia moments relative to the parametrization
of the unit-density inertia tensor
*/
  arma::mat::fixed<3,6>  PartialUnitDensityMomentsPartialI() const;

/**
Return the partial derivative of the (q,r) component of the contribution of the f-facet
to the shape's inertia tensor, relative to the f-facet
vertices coordinates
@param f facet index
@param q first index
@param r second index
@param Tf 9x1 vector holding coordinates of vertices in facet EXPRESSED IN METERS
@return partial derivative of the (q,r) component of the contribution of the f-facet
to the shape's inertia tensor
*/
  arma::rowvec::fixed<9> PartialEqDeltaIfErPartialTf(const int & f, const int & q, const int & r,const arma::vec::fixed<9> & Tf) const;

  /**
  Return the partial derivative of the shape's center-of-mass
  with respect to the f-th facet coordinates
  @param f facet index
  @return partial derivative of shape's center-of-mass with respect to facet coordinates
  */
  arma::mat::fixed<3,9> PartialDeltaComPartialTf(const int & f) const;

  /**
  Return the partial derivative of e_q.T * DeltaIOverDeltaVfEr * e_r with respect to the f-facet
  vertices coordinates
  @param e_q first 3x1 vector canonical unit vector
  @param e_r first 3x1 vector canonical unit vector
  @param Tf 9x1 vector holding coordinates of vertices in facet EXPRESSED IN METERS
  @return the partial derivative of e_q.T * DeltaIOverDeltaVfEr * e_r with respect to the f-facet
  vertices coordinates
  */
  arma::rowvec::fixed<9> PartialEqDeltaIOverDeltaVfErPartialTf(const arma::vec::fixed<3> & e_q,const arma::vec::fixed<3> & e_r,
    const arma::vec::fixed<9> & Tf) const;

  /**
  Return the partial derivative of the volume of the tetrahedron subtended by facet f
  with respect to the facet coordinates
  @param f facet index
  @return partial derivative of tetrahedron volume with respect to facet coordinates
  */
  arma::rowvec::fixed<9> PartialDeltaVfPartialTf(const int & f) const;

  /**
  Return the partial derivative of the center of mass of the considered tetrahedron
  with respect to the facet coordinates
  @return partial derivative of center of mass with respect to facet coordinates
  */
  static arma::mat::fixed<3,9> PartialDeltaCMfPartialTf();

  /**
  Return the partial derivative of the tetrahedron's inertia tensor parametrization
  relative to the facet coordinates
  @param f facet index
  @return partial derivative of the tetrahedron's inertia tensor parametrization
  relative to the facet coordinates
  */
  arma::mat::fixed<6,9> PartialDeltaIfPartialTf(const int & f) const;

  /**
  Return the partial derivative of a tetrahedron's  inertia-times-volume tensor parametrization
  with respect to the subtending facet's vertices coordinates
  @param f facet index
  @return partial derivative of the tetrahedron's inertia-times-volume tensor parametrization
  with respect to the subtending facet's vertices coordinates
  */
  arma::mat::fixed<6,9> PartialDeltaIOverDeltaVPartialTf(const int & f) const;

  /**
  Applies deviation to the coordinates of the vertices in the prescribed facet
  and updates the pgm
  @param delta_Tf deviation
  @param f facet index
  */
  virtual void ApplyTfDeviation(arma::vec::fixed<9> delta_Tf,const int & f);

  static void TestPartialDeltaVfPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaIOverDeltaVPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaIfPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaVPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialVolumePartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialComPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialIPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialAllInertiaPartialC(std::string input,double tol,bool shape_in_meters) ;
  static void TestPartialEqDeltaIfErPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialSigmaPartialC(std::string input,double tol,bool shape_in_meters);

  arma::rowvec precomputed_partialVpartialC;
  arma::mat precomputed_partialGpartialC;
  arma::mat precomputed_partialSigmapartialC;
  arma::mat precomputed_partialIpartialC;
  arma::mat::fixed<3,6> precomputed_partialUnitDensityMomentsPartialI;
  arma::mat::fixed<3,6> precomputed_partialSigmapartialI;

};

#endif