#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <angles/angles.h>
#include <robot_kf/robot_kf.h>

#include <iostream>

using namespace Eigen;

namespace robot_kf {

KalmanFilter::KalmanFilter(void)
    : x_(Vector3d::Zero())
    , cov_x_(9999 * Matrix3d::Identity())
    , A_(Matrix3d::Identity())
    , Hgps_((Matrix<double, 2, 3>() << 1, 0, 0, 0, 1, 0).finished())
    , Hcomp_((Matrix<double, 1, 3>() << 0, 0, 1).finished())
{}

KalmanFilter::~KalmanFilter(void)
{}

Vector3d KalmanFilter::getState(void) const
{
    return x_;
}

Matrix3d KalmanFilter::getCovariance(void) const
{
    return cov_x_;
}

void KalmanFilter::update_encoders(Vector2d enc, Matrix2d cov_enc, double separation)
{
    /* This is non-linear in theta because of the x and y elements:
     *      x = (r + l)/2 * cos(theta + (r - l) / (2s))
     *      y = (r + l)/2 * sin(theta + (r - l) / (2s))
     *  theta = (r - l)/(2s)
     * We can linearize this around the current state estimate using the
     * Jacobians A = Jac(f, x) and W = Jac(f, u).
     */
    double const dlinear = (enc[1] + enc[0]) / 2;
    double const dtheta  = (enc[1] - enc[0]) / separation;
    double const theta_halfway = x_[2] + dtheta / 2;

    Matrix3d const A = (Matrix3d() <<
        1, 0, -dlinear * sin(theta_halfway),
        0, 1, +dlinear * cos(theta_halfway),
        0, 0, 1).finished();
    Matrix<double, 3, 2> const W = (Matrix<double, 3, 2>() <<
        +dlinear * sin(theta_halfway) + 0.5 * cos(theta_halfway),
        -dlinear * sin(theta_halfway) + 0.5 * cos(theta_halfway),
        -dlinear * cos(theta_halfway) + 0.5 * sin(theta_halfway),
        +dlinear * cos(theta_halfway) + 0.5 * sin(theta_halfway),
        -1.0 / separation,
        +1.0 / separation).finished();

    x_ += (Vector3d() <<
            dlinear * cos(theta_halfway),
            dlinear * sin(theta_halfway),
    //        0).finished();
            dtheta).finished();
    cov_x_ = A * cov_x_ * A.transpose() + W * cov_enc * W.transpose();
    normalize_yaw();
}

void KalmanFilter::update_gps(Vector2d gps, Matrix2d cov_gps)
{
    measure(gps, cov_gps, Hgps_);
}

void KalmanFilter::update_compass(double compass, double cov_compass)
{
    // Renormalize the compass heading so the filter behaves correctly when
    // crossing +/-pi. This prevents the heading from slowly drifting the "long
    // way" around the circle (i.e. through 0 instead of +/-pi).
    if (x_[2] - compass > M_PI) {
        compass = compass + 2 * M_PI;
    } else if (x_[2] - compass < -M_PI) {
        compass = compass - 2 * M_PI;
    }

    Matrix<double, 1, 1> mat_compass, mat_cov_compass;
    mat_compass << compass;
    mat_cov_compass << cov_compass;

    measure(mat_compass, mat_cov_compass, Hcomp_);
    normalize_yaw();
}

template <int m>
void KalmanFilter::predict(Matrix<double, m, 1> u, Matrix<double, 3, 3> cov_process,
                           Matrix<double, 3, 3> A, Matrix<double, 3, m> B)
{
    x_ = A * x_ + B * u;
    cov_x_ = A * cov_x_ * A.transpose() + cov_process;
}

template <int m>
void KalmanFilter::measure(Matrix<double, m, 1> z, Matrix<double, m, m> cov_z,
                           Matrix<double, m, 3> H)
{
    Matrix<double, 3, m> const K = cov_x_ * H.transpose() * (H * cov_x_
                                  * H.transpose() + cov_z).inverse();
    x_ = x_ + K * (z - H * x_);
    cov_x_ = (Matrix3d::Identity() - K * H) * cov_x_;
}

void KalmanFilter::normalize_yaw(void) {
    x_[2] = angles::normalize_angle(x_[2]);
}

};


#include <Eigen/Core>
#include <iostream>
#include <sys/stat.h>
#include <igl/readPLY.h>
#include <igl/writePLY.h>
#include <igl/point_mesh_squared_distance.h>

#include <stdio.h>
#include <stdlib.h>

igl::AABB<Eigen::MatrixXd,3> tree;

float icp_step(Eigen::MatrixXd *Vmov, Eigen::MatrixXd Vref, Eigen::MatrixXi Fref)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist = 0;

  // compute closest points
  VectorXd sqrD;
  VectorXi I;
  MatrixXd Vout;

  igl::point_mesh_squared_distance(*Vmov, Vref, Fref, sqrD, I, Vout);

  // compute rotation and translation
  MatrixXd C = Matrix3d::Constant(0);
  VectorXd oout = Vout.colwise().sum()/Vout.rows();
  VectorXd omov = (*Vmov).colwise().sum()/(*Vmov).rows();

  for(i=0;i<(*Vmov).rows();i++)
    C += ((*Vmov).row(i) - omov.transpose()).transpose()
         * (Vout.row(i) - oout.transpose());

  JacobiSVD<MatrixXd> svd(C, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV().transpose();

  MatrixXd R = U*V;
  MatrixXd t = omov - R*oout;

  // apply rotation and translation
  (*Vmov)*=R;
  (*Vmov).transpose().colwise() -= t.col(0);

  // check convergence
  // angle_dist = arccos((trace(P*Q')-1)/2), P, Q rotation matrices
  // (here, Q=I)
  // http://www.boris-belousov.net/2016/12/01/quat-dist/
  dist = acos((R.trace()-1)/2.0);
  dist = dist + t.norm();

  return dist;
}

int icp(Eigen::MatrixXd Vref, Eigen::MatrixXi Fref,
        Eigen::MatrixXd *Vmov, Eigen::MatrixXi Fmov,
        float *diff)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist;
  int maxiter = 100;
  float tol = 1e-6;

  // init distance AABB tree
  tree.init(Vref, Fref);

  float dist0=-1;

  for(i=0;i<maxiter;i++)
  {
    dist = icp_step(Vmov, Vref, Fref);
    if(dist0<0)
    {
        *diff = dist;
        printf("initial distance: %g\n", *diff);
    }
    else
        *diff = fabs(dist-dist0);

    if(*diff < tol)
        break;

    dist0=dist;
  }
  printf("final distance: %g (after %i iterations)\n", *diff, i);

  return i;
}

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  /*
    Register mesh mov to mesh ref
  */
  char *path_ref = argv[1];
  char *path_mov = argv[2];
  char *path_result = argv[3];
  float diff;

  struct stat info;
  if(stat(path_ref,&info))
      printf("File %s not found\n", path_ref);
  if(stat(path_mov,&info))
      printf("File %s not found\n", path_mov);

  MatrixXd Vmov; // Vmov is changed
  MatrixXd Vref;
  MatrixXi Fmov;
  MatrixXi Fref;
  MatrixXd TMP1;
  MatrixXd TMP2;

  igl::readPLY(path_ref, Vref, Fref, TMP1, TMP2);
  std::cout<<"Ref nv, nt: "<<Vref.rows()<<", "<<Fref.rows()<<std::endl;

  igl::readPLY(path_mov, Vmov, Fmov, TMP1, TMP2);
  std::cout<<"Mov nv, nt: "<<Vmov.rows()<<", "<<Fmov.rows()<<std::endl;

  int iter = icp(Vref, Fref, &Vmov, Fmov, &diff);
  if(path_result)
  {
      igl::writePLY(path_result, Vmov, Fmov);
      printf("iterations: %i\n", iter);
  }
  else
      printf("No output path provided, nothing saved\n");

  return 0;
}


#include <iostream>
#include <stdexcept>

#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

void usage(char *progname) {
	cout << "This program returns the message m corresponding to "
		"two ciphertexts c1 = m^e1 (mod n), c2 = m^e2 (mod n)."
		<< endl;
	cout << "Usage: " << progname << " n e1 c1 e2 c2" << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 6) { usage(argv[0]); return 3; }

	ZZ n, e1, c1, e2, c2, m, u, v, g;
	n = conv<ZZ>(argv[1]);
	e1 = conv<ZZ>(argv[2]);
	c1 = conv<ZZ>(argv[3]);
	e2 = conv<ZZ>(argv[4]);
	c2 = conv<ZZ>(argv[5]);

	// g = u*e1 + v*e2
	XGCD(g, u, v, e1, e2);

	if(g != 1) { throw domain_error("e1 and e2 aren't coprime."); }

	if(u < 0) {
		InvMod(c1, c1, n);
		u *= -1;
	}
	if(v < 0) {
		InvMod(c2, c2, n);
		v *= -1;
	}

	MulMod(m, PowerMod(c1, u, n), PowerMod(c2, v, n), n);

	cout << m << endl;
}


#ifndef UCB_MULTIARM_BANDIT_HPP
#define UCB_MULTIARM_BANDIT_HPP

#include <stdlib.h>
#include <assert.h>
#include <limits>
#include <cmath>
#include <vector>

#include <Eigen/Dense>

namespace smmap
{
    template <typename Generator = std::mt19937_64>
    class UCB1Normal
    {
        public:
            UCB1Normal(size_t num_bandits = 1)
                : num_arms_(num_bandits)
                , total_reward_(num_arms_, 0.0)
                , sum_of_squared_reward_(num_arms_, 0.0)
                , num_pulls_(num_arms_, 0)
                , total_pulls_(0)
            {}

            ssize_t selectArmToPull(Generator& generator) const
            {
                (void)generator;
                if (total_pulls_ == 0UL)
                {
                    return 0L;
                }

                const double explore_threshold = 8.0 * std::log((double)total_pulls_ + 1.0);
                double highest_ucb = -std::numeric_limits<double>::infinity();
                ssize_t best_arm = -1;
                size_t lowest_num_exploration_pulls = (size_t)(-1);

                for (size_t arm_ind = 0; arm_ind < num_arms_; arm_ind++)
                {
                    if (num_pulls_[arm_ind] < explore_threshold)
                    {
                        if (num_pulls_[arm_ind] < lowest_num_exploration_pulls)
                        {
                            best_arm = arm_ind;
                            lowest_num_exploration_pulls = num_pulls_[arm_ind];
                        }
                    }
                }

                // If we found an arm that qualifies for exploration, pull that arm
                if (best_arm != -1)
                {
                    return best_arm;
                }

                for (size_t arm_ind = 0; arm_ind < num_arms_; arm_ind++)
                {
                    assert(num_pulls_[arm_ind] > 1);

                    const double average_reward = total_reward_[arm_ind] / (double)num_pulls_[arm_ind];
                    const double term1_numer = (sum_of_squared_reward_[arm_ind] - (double)num_pulls_[arm_ind] * average_reward * average_reward);
                    const double term1_denom = (double)(num_pulls_[arm_ind] - 1);
                    const double term1 = std::abs(term1_numer)/term1_denom;
                    const double term2 = std::log((double)total_pulls_) / (double)num_pulls_[arm_ind];
                    const double ucb = average_reward + std::sqrt(16.0 * term1 * term2);

                    assert(std::isfinite(ucb));

                    if (ucb > highest_ucb)
                    {
                        highest_ucb = ucb;
                        best_arm = (ssize_t)arm_ind;
                    }
                }

                assert(best_arm >= 0);
                return best_arm;
            }

            bool generateAllModelActions() const
            {
                return false;
            }

            void updateArms(const ssize_t arm_pulled, const double reward)
            {
                total_reward_[(size_t)arm_pulled] += reward;
                sum_of_squared_reward_[(size_t)arm_pulled] += reward * reward;
                num_pulls_[(size_t)arm_pulled]++;
                total_pulls_++;
            }

            Eigen::VectorXd getMean() const
            {
                Eigen::VectorXd mean((ssize_t)num_arms_);
                for (size_t arm_ind = 0; arm_ind < num_arms_; arm_ind++)
                {
                    mean((ssize_t)arm_ind) = total_reward_[arm_ind] / (double)num_pulls_[arm_ind];
                }
                return mean;
            }

            Eigen::VectorXd getUCB() const
            {
                Eigen::VectorXd ucb((ssize_t)num_arms_);
                for (size_t arm_ind = 0; arm_ind < num_arms_; arm_ind++)
                {
                    const double average_reward = total_reward_[arm_ind] / (double)num_pulls_[arm_ind];
                    const double term1_numer = (sum_of_squared_reward_[arm_ind] - (double)num_pulls_[arm_ind] * average_reward * average_reward);
                    const double term1_denom = (double)(num_pulls_[arm_ind] - 1);
                    const double term1 = std::abs(term1_numer)/term1_denom;
                    const double term2 = std::log((double)total_pulls_) / (double)num_pulls_[arm_ind];
                    ucb((ssize_t)arm_ind) = average_reward + std::sqrt(16.0 * term1 * term2);
                }
                return ucb;
            }

        private:
            size_t num_arms_;

            std::vector<double> total_reward_;
            std::vector<double> sum_of_squared_reward_;
            std::vector<size_t> num_pulls_;
            size_t total_pulls_;
    };
}

#endif // UCB_MULTIARM_BANDIT_HPP


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2018 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#include "problems.h"

// C++ standard library
#include <random>

// Eigen library
#include <Eigen/Dense>

// VCL
#include <vcl/math/polardecomposition.h>

void createRandomProblems(
	size_t nr_problems,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>* R)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	for (int i = 0; i < (int)nr_problems; i++)
	{
		// Rest-state
		Eigen::Matrix3f M;
		M << d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng);
		F.at<float>(i) = M;

		if (R)
		{
			Eigen::Matrix3f Rot;
			Vcl::Mathematics::PolarDecomposition(M, Rot, nullptr);
			R->template at<float>(i) = Rot;
		}
	}
}

void createSymmetricProblems(
	size_t nr_problems,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>* R)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	for (int i = 0; i < (int)nr_problems; i++)
	{
		// Rest-state
		Eigen::Matrix3f M;
		M << d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng);
		Eigen::Matrix3f MtM = M.transpose() * M;
		F.at<float>(i) = MtM;

		if (R)
		{
			Eigen::Matrix3f Rot;
			Vcl::Mathematics::PolarDecomposition(MtM, Rot, nullptr);
			R->template at<float>(i) = Rot;
		}
	}
}

void createRotationProblems(
	size_t nr_problems,
	float max_angle,
	float max_compression,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>* R)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;
	std::uniform_real_distribution<float> a{ -max_angle, max_angle };

	for (int i = 0; i < (int)nr_problems; i++)
	{
		// Rest-state
		Eigen::Matrix3f X0;
		X0 << d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng),
			d(rng), d(rng), d(rng);

		// Rotation angle
		float angle = a(rng);

		// Rotation axis
		Eigen::Matrix<float, 3, 1> rot_vec;
		rot_vec << d(rng), d(rng), d(rng);
		rot_vec.normalize();

		// Rotation matrix
		Eigen::Matrix3f Rot = Eigen::AngleAxis<float>{ angle, rot_vec }.toRotationMatrix();
		if (R)
			R->template at<float>(i) = Rot;

		if (max_compression > 0)
		{
			Eigen::Matrix<float, 3, 1> scaling;
			scaling << (1.0f - max_compression * d(rng)), (1.0f - max_compression * d(rng)), (1.0f - max_compression * d(rng));

			Eigen::JacobiSVD<Eigen::Matrix3f> svd{ Rot, Eigen::ComputeFullU | Eigen::ComputeFullV };
			Rot *= svd.matrixV() * scaling.asDiagonal() * svd.matrixV().transpose();
		}

		Eigen::Matrix3f X = Rot * X0;
		F.at<float>(i) = X * X0.inverse();
	}
}

void computeEigenReferenceSolution(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& ATA,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& U,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& S)
{
	// Compute reference using Eigen
	for (int i = 0; i < static_cast<int>(nr_problems); i++)
	{
		Vcl::Matrix3f A = ATA.at<float>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		U.at<float>(i) = solver.eigenvectors();
		S.at<float>(i) = solver.eigenvalues();
	}
}


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;

  for(int codim = 0; codim < 3; codim++) {
    int n = 0;
    for(auto& entity : dofhandler.Mesh()->Entities(codim)) {
       n += dofhandler.NumInteriorDofs(*entity);
    }
    entityDofs[codim] = n;
  }

  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;

      for(auto& entity : dofhandler.Mesh()->Entities(1)) {
        if(bd_flags(*entity)) {
          no_dofs_on_bd += dofhandler.NumInteriorDofs(*entity);
        }
      }

      for(auto& entity : dofhandler.Mesh()->Entities(2)) {
        if(bd_flags(*entity)) {
          no_dofs_on_bd += dofhandler.NumInteriorDofs(*entity);
        }
      }

  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
    double I = 0;

    for(auto entity : dofhandler.Mesh()->Entities(0)) {
      auto indices = dofhandler.GlobalDofIndices(*entity);

      assert(dofhandler.NumLocalDofs(*entity) == 3);

      double area = lf::geometry::Volume(*entity->Geometry());

      for(auto dof_idx : indices) {
        I += area * (mu[dof_idx]) / 3.;
      }

    }

  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;

  for(auto entity : dofhandler.Mesh()->Entities(0)) {
    auto indices = dofhandler.GlobalDofIndices(*entity);

    assert(dofhandler.NumLocalDofs(*entity) == 6);

    double area = lf::geometry::Volume(*entity->Geometry());

    int i = 0;
    for(auto dof_idx : indices) {

      if(i >= 3) {
        I += area * (mu[dof_idx]) / 3.;
      }

      i++;
    }

  }

  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    //====================
    assert(dofh_Linear_FE.NumLocalDofs(*cell) == 3);
    assert(dofh_Quadratic_FE.NumLocalDofs(*cell) == 6);
    //====================
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    //====================
    auto lin_indices = dofh_Linear_FE.GlobalDofIndices(*cell);
    auto quad_indices = dofh_Quadratic_FE.GlobalDofIndices(*cell);

    for(size_t i = 0; i < 3; i++) {
      zeta[quad_indices[i]] = mu[lin_indices[i]];
      zeta[quad_indices[i+3]] = 0.5 * (mu[lin_indices[i]] + mu[lin_indices[(i + 1) % 3]]);

      i++;
    }

    // assign the coefficients of mu to the correct entries of zeta, use
    // the previous subproblem 2-9.a
    //====================
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


#include <algorithm>
#include <dissimilaritymatrix.h>

#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>

using namespace Eigen;
using namespace std;

void DissimilarityMatrix::setNumItems(int count)
{
    this->_numItems = count;
}

void DissimilarityMatrix::setDiffVal(int id_0, int id_1, double val)
{
    int a = std::min(id_0, id_1);
    int b = std::max(id_0, id_1);

    this->_diffVals[std::tuple<int, int>(a, b)] = val;
}

double DissimilarityMatrix::getDiffVal(int id_0, int id_1)
{
    int a = std::min(id_0, id_1);
    int b = std::max(id_0, id_1);

    return this->_diffVals[std::tuple<int, int>(a, b)];
}

std::vector< std::shared_ptr<DataNode> > DissimilarityMatrix::getEigenDecomp(int order)
{
    std::vector< std::shared_ptr<DataNode> > ret;

    MatrixXd A(this->_numItems, this->_numItems);

    for (int i = 0; i < this->_numItems; ++i)
    {
        for (int j = 0; j < this->_numItems; ++j)
        {
            auto v = this->getDiffVal(i, j);
            if (v < 0)
            {
                v = 10;
            }

            A(i, j) = v;
        }
    }

     //cout << "The dissimilarity matrix is:" << endl << A << endl << endl;

    // http://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
    SelfAdjointEigenSolver<MatrixXd> es(A);
    MatrixXd V = es.eigenvectors();
    // cout << "The matrix of eigenvectors, V, is:" << endl << V << endl << endl;

    for (int i = 0; i < this->_numItems; ++i)
    {
        auto t = std::make_shared<DataNode>();
        t->id = i;
        for (int o = 0; o < order; ++o)
        {
            double v = (o < this->_numItems) ? V(i, o) : 0.0;
            t->dataSet.push_back(v);
        }
        ret.push_back(t);
    }

    return ret;
}


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <random>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "../include/core/config.hpp"

void eval_vshape_RT0_refEl(const double x, const double y,
                           DenseMatrix& vshape)
{
    int ndofs = 3;
    vshape.SetSize(ndofs, 2);
    vshape(0,0) = x  ; vshape(0,1) = y-1;
    vshape(1,0) = x  ; vshape(1,1) = y  ;
    vshape(2,0) = x-1; vshape(2,1) = y;
}

void eval_gradvshape_RT0_refEl(const double, const double,
                               DenseTensor& gradvshape)
{
    int ndofs = 3;
    gradvshape.SetSize(2,2,ndofs);
    gradvshape = 0;

    gradvshape(0,0,0) = gradvshape(1,1,0) = 1;
    gradvshape(0,0,1) = gradvshape(1,1,1) = 1;
    gradvshape(0,0,2) = gradvshape(1,1,2) = 1;
}

class RT1_refEl
{
public:
    RT1_refEl ()
    {
        double c = 1./3.;

        Vector xi(2);
        xi(0) = (1-1./sqrt(3))/2;
        xi(1) = (1+1./sqrt(3))/2;

        nodes.SetSize(8,2);
        T.SetSize(8, 8);

        // set nodes
        nodes(0,0) = xi(0); nodes(0,1) = 0;
        nodes(1,0) = xi(1); nodes(1,1) = 0;

        nodes(2,0) = xi(1); nodes(2,1) = xi(0);
        nodes(3,0) = xi(0); nodes(3,1) = xi(1);

        nodes(4,0) = 0; nodes(4,1) = xi(1);
        nodes(5,0) = 0; nodes(5,1) = xi(0);

        nodes(6,0) = c; nodes(6,1) = c;
        nodes(7,0) = c; nodes(7,1) = c;

        // set matrix T
        T = 0.0;

        // dofs 0 and 1
        for (int i=0; i<=1; i++)
        {
            T(1,i) = -(2*(1 - nodes(i,0) - nodes(i,1))-1);
            T(3,i) = -(2*nodes(i,0)-1);
            T(5,i) = -(2*nodes(i,1)-1);
            T(6,i) = -(2*nodes(i,1)-1)*(nodes(i,1) - c);
            T(7,i) = -(2*nodes(i,0)-1)*(nodes(i,1) - c);
        }

        // dofs 2 and 3
        for (int i=2; i<=3; i++)
        {
            T(0,i) = T(1,i) = (2*(1 - nodes(i,0) - nodes(i,1))-1);
            T(2,i) = T(3,i) = (2*nodes(i,0)-1);
            T(4,i) = T(5,i) = (2*nodes(i,1)-1);
            T(6,i) = (2*nodes(i,1)-1)*(nodes(i,0) + nodes(i,1) - 2*c);
            T(7,i) = (2*nodes(i,0)-1)*(nodes(i,0) + nodes(i,1) - 2*c);
        }

        // dofs 4 and 5
        for (int i=4; i<=5; i++)
        {
            T(0,i) = -(2*(1 - nodes(i,0) - nodes(i,1))-1);
            T(2,i) = -(2*nodes(i,0)-1);
            T(4,i) = -(2*nodes(i,1)-1);
            T(6,i) = -(2*nodes(i,1)-1)*(nodes(i,0) - c);
            T(7,i) = -(2*nodes(i,0)-1)*(nodes(i,0) - c);
        }

        // dofs 6 and 7
        int i=6;
        {
            T(1,i) = -(2*(1 - nodes(i,0) - nodes(i,1))-1);
            T(3,i) = -(2*nodes(i,0)-1);
            T(5,i) = -(2*nodes(i,1)-1);
            T(6,i) = -(2*nodes(i,1)-1)*(nodes(i,1) - c);
            T(7,i) = -(2*nodes(i,0)-1)*(nodes(i,1) - c);

            i++;
            T(0,i) = -(2*(1 - nodes(i,0) - nodes(i,1))-1);
            T(2,i) = -(2*nodes(i,0)-1);
            T(4,i) = -(2*nodes(i,1)-1);
            T(6,i) = -(2*nodes(i,1)-1)*(nodes(i,0) - c);
            T(7,i) = -(2*nodes(i,0)-1)*(nodes(i,0) - c);
        }

        /*for (int k=0; k<T.NumRows(); k++) {
            for (int l=0; l<T.NumCols(); l++)
                std::cout << T(k,l) << " ";
            std::cout << "\n";
        }*/

        Ti.Factor(T);

        /*DenseMatrix invT;
        Ti.GetInverseMatrix(invT);
        for (int k=0; k<T.NumRows(); k++) {
            for (int l=0; l<T.NumCols(); l++)
                std::cout << invT(k,l) << " ";
            std::cout << "\n";
        }*/
    }

    void eval_vshape(const double x, const double y,
                     DenseMatrix &vshape)
    {
        double c = 1./3.;

        int ndofs = 8;
        DenseMatrix vshape_buf(ndofs, 2);
        vshape_buf = 0.0;

        vshape_buf(0,0) = (2*(1-x-y)-1);
        vshape_buf(1,1) = (2*(1-x-y)-1);

        vshape_buf(2,0) = (2*x-1);
        vshape_buf(3,1) = (2*x-1);

        vshape_buf(4,0) = (2*y-1);
        vshape_buf(5,1) = (2*y-1);

        vshape_buf(6,0) = (2*y-1)*(x - c);
        vshape_buf(6,1) = (2*y-1)*(y - c);
        vshape_buf(7,0) = (2*x-1)*(x - c);
        vshape_buf(7,1) = (2*x-1)*(y - c);

        Ti.Mult(vshape_buf, vshape);
    }

private:
    DenseMatrix nodes;
    DenseMatrix T;
    DenseMatrixInverse Ti;
};

TEST(RTspace, vshape_deg0_test1)
{
    std::string input_dir = "../input/";
    const std::string mesh_file = input_dir+"tri_elem1";
    Mesh mesh(mesh_file.c_str());
    assert(mesh.GetNE() == 1);

    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(0, mesh.Dimension());
    FiniteElementSpace *R_space
            = new FiniteElementSpace(&mesh, hdiv_coll);

    int j=0; // only 1 element in the mesh
    {
        const FiniteElement *fe = R_space->GetFE(j);
        ElementTransformation *trans
                = R_space->GetElementTransformation(j);

        const IntegrationRule *ir = &IntRules.Get(fe->GetGeomType(), 2);

        DenseMatrix vshape(3,2);
        DenseMatrix true_vshape(3,2);

        auto vshape_fn = [](Vector &x)
        {
            DenseMatrix vshape(3,2);
            vshape(0,0) = x(0)-2;
            vshape(0,1) = x(1);

            vshape(1,0) = x(0)-2;
            vshape(1,1) = x(1)-1;

            vshape(2,0) = x(0);
            vshape(2,1) = x(1);

            vshape *= 0.5;

            return vshape;
        };

        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            trans->SetIntPoint(&ip);
            fe->CalcVShape(*trans, true_vshape);

            Vector eip;
            trans->Transform(ip, eip);
            vshape = vshape_fn(eip);

            /*std::cout << "\n\nvshape:" << std::endl;
            for (int k=0; k<vshape.NumRows(); k++) {
                for (int l=0; l<vshape.NumCols(); l++)
                    std::cout << vshape(k,l) << " ";
                std::cout << "\n";
            }

            std::cout << "\n\nTrue vshape:" << std::endl;
            for (int k=0; k<vshape.NumRows(); k++) {
                for (int l=0; l<vshape.NumCols(); l++)
                    std::cout << true_vshape(k,l) << " ";
                std::cout << "\n";
            }*/

            double TOL = 1E-10;
            true_vshape -= vshape;
            ASSERT_LE(true_vshape.FNorm(), TOL);
        }
    }

    delete hdiv_coll;
    delete R_space;
}

TEST(RTspace, gradvshape_deg0)
{
    std::string input_dir = "../input/";
    const std::string mesh_file = input_dir+"tri_elem1";
    Mesh mesh(mesh_file.c_str());
    assert(mesh.GetNE() == 1);

    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(0, mesh.Dimension());
    FiniteElementSpace *R_space
            = new FiniteElementSpace(&mesh, hdiv_coll);

    int ndofs = 3;
    auto gradvshape_fn = [](Vector &)
    {
        DenseTensor gradvshape(2,2,3);
        gradvshape = 0.0;
        gradvshape(0,0,0) = 0.5; gradvshape(1,1,0) = 0.5;
        gradvshape(0,0,1) = 0.5; gradvshape(1,1,1) = 0.5;
        gradvshape(0,0,2) = 0.5; gradvshape(1,1,2) = 0.5;
        return gradvshape;
    };

    int j=0;
    {
        const FiniteElement *fe = R_space->GetFE(j);
        ElementTransformation *trans
                = R_space->GetElementTransformation(j);

        const IntegrationRule *ir = &IntRules.Get(fe->GetGeomType(), 2);

        DenseTensor true_gradvshape(2,2,3);
        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            trans->SetIntPoint(&ip);
            fe->CalcGradVShape(*trans, true_gradvshape);

            Vector eip;
            trans->Transform(ip, eip);
            auto gradvshape = gradvshape_fn(eip);
            for (int m=0; m < ndofs; m++)
            {
                /*std::cout << "\n\ngradvshape:" << std::endl;
                for (int k=0; k<gradvshape(m).NumRows(); k++) {
                    for (int l=0; l<gradvshape(m).NumCols(); l++)
                        std::cout << gradvshape(k,l,m) << " ";
                    std::cout << "\n";
                }

                std::cout << "\n\ntrue_gradvshape:" << std::endl;
                for (int k=0; k<gradvshape(m).NumRows(); k++) {
                    for (int l=0; l<gradvshape(m).NumCols(); l++)
                        std::cout << true_gradvshape(k,l,m) << " ";
                    std::cout << "\n";
                }*/

                true_gradvshape(m) -= gradvshape(m);
                double TOL = 1E-10;
                ASSERT_LE(true_gradvshape(m).FNorm(), TOL);
            }
        }
    }

    delete hdiv_coll;
    delete R_space;
}

TEST(RTspace, vshape_deg1)
{
    std::string input_dir = "../input/";
    const std::string mesh_file = input_dir+"tri_elem2";
    Mesh mesh(mesh_file.c_str());

    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(1, mesh.Dimension());
    FiniteElementSpace *R_space
            = new FiniteElementSpace(&mesh, hdiv_coll);

    for (int j=0; j<1; j++)
    {
        const FiniteElement *fe = R_space->GetFE(j);
        ElementTransformation *trans
                = R_space->GetElementTransformation(j);

        const IntegrationRule *ir = &IntRules.Get(fe->GetGeomType(), 2);

        DenseMatrix vshape(8,2);
        DenseMatrix true_vshape(8,2);

        RT1_refEl rt1_refEl;

        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            trans->SetIntPoint(&ip);
            //fe->CalcVShape(trans->GetIntPoint(), true_vshape);
            fe->CalcVShape(*trans, true_vshape);

            DenseMatrix vshape_buf;
            rt1_refEl.eval_vshape(ip.x, ip.y, vshape_buf);
            MultABt(vshape_buf, trans->Jacobian(), vshape);
            vshape *= (1./trans->Weight());

            /*std::cout << "\n\nvshape:" << std::endl;
            for (int k=0; k<vshape.NumRows(); k++) {
                for (int l=0; l<vshape.NumCols(); l++)
                    std::cout << vshape(k,l) << " ";
                std::cout << "\n";
            }

            std::cout << "\n\nTrue vshape:" << std::endl;
            for (int k=0; k<vshape.NumRows(); k++) {
                for (int l=0; l<vshape.NumCols(); l++)
                    std::cout << true_vshape(k,l) << " ";
                std::cout << "\n";
            }*/

            double TOL = 1E-10;
            true_vshape -= vshape;
            ASSERT_LE(true_vshape.FNorm(), TOL);
            //std::cout << "\nError: " << true_vshape.FNorm() << std::endl;
        }
    }

    delete hdiv_coll;
    delete R_space;
}

/*TEST(RTspace, gradvshape_deg0)
{
    std::string input_dir = "../input/";
    const std::string mesh_file = input_dir+"tri_elem2";
    Mesh mesh(mesh_file.c_str());

    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(0, mesh.Dimension());
    FiniteElementSpace *R_space
            = new FiniteElementSpace(&mesh, hdiv_coll);

    int ndofs = 3;
    int dim = 2;

    auto gradvshape_fn = [](Vector &x)
    {
        DenseTensor gradvshape(3,2,2);
        gradvshape = 0.0;
        gradvshape(0,0,0) = 0.5; gradvshape(0,1,1) = 0.5;
        gradvshape(1,0,0) = 0.5; gradvshape(1,1,1) = 0.5;
        gradvshape(2,0,0) = 0.5; gradvshape(2,1,1) = 0.5;
        return gradvshape;
    };

    for (int j=0; j<1; j++)
    {
        const FiniteElement *fe = R_space->GetFE(j);
        ElementTransformation *trans
                = R_space->GetElementTransformation(j);

        const IntegrationRule *ir = &IntRules.Get(fe->GetGeomType(), 2);

        //DenseTensor gradvshape(2,2,3);
        DenseTensor true_gradvshape(2,2,3);

        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            trans->SetIntPoint(&ip);
            fe->CalcGradVShape(*trans, true_gradvshape);

            DenseMatrix J, Jinv;
            DenseTensor gradvshape_buf;
            J = trans->Jacobian();
            Jinv = trans->InverseJacobian();
            eval_gradvshape_RT0_refEl(ip.x, ip.y, gradvshape_buf);

            for (int m=0; m < ndofs; m++)
            {
                // apply Piola transformation
                // gradvshape = |J|^{-1}*J * gradvshape_buf * invJ
                DenseMatrix tempMat(dim, dim);
                Mult(J, gradvshape_buf(m), tempMat);
                Mult(tempMat, Jinv, gradvshape(m));
                gradvshape(m) *= (1.0 / trans->Weight());

                true_gradvshape(m) -= gradvshape(m);
                double TOL = 1E-10;
                ASSERT_LE(true_gradvshape(m).FNorm(), TOL);
            }
        }
    }

    delete hdiv_coll;
    delete R_space;
}*/


#ifndef GEOUTILS_HPP
#define GEOUTILS_HPP

#include <Eigen/Eigen>
#include <cfloat>
#include <chrono>
#include <cstdint>
#include <set>

#include "quickhull.hpp"
#include "sdlp.hpp"

namespace geoutils
{

  // Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
  // The outter_normal is assumed to be NORMALIZED
  inline bool findInterior(const Eigen::MatrixXd &hPoly,
                           Eigen::Vector3d &interior)
  {
    int m = hPoly.cols();

    Eigen::MatrixXd A(m, 4);
    Eigen::VectorXd b(m), c(4), x(4);
    A.leftCols<3>() = hPoly.topRows<3>().transpose();
    A.rightCols<1>().setConstant(1.0);
    b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum().transpose();
    c.setZero();
    c(3) = -1.0;

    double minmaxsd = sdlp::linprog(c, A, b, x);
    interior = x.head<3>();

    return minmaxsd < 0.0 && !std::isinf(minmaxsd);
  }

  inline double findInteriorDist(const Eigen::MatrixXd &hPoly,
                                 Eigen::Vector3d &interior)
  {
    int m = hPoly.cols();

    Eigen::MatrixXd A(m, 4);
    Eigen::VectorXd b(m), c(4), x(4);
    A.leftCols<3>() = hPoly.topRows<3>().transpose();
    A.rightCols<1>().setConstant(1.0);
    b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum().transpose();
    c.setZero();
    c(3) = -1.0;

    double minmaxsd = sdlp::linprog(c, A, b, x);
    interior = x.head<3>();

    return -minmaxsd;
  }

  struct filterLess
  {
    inline bool operator()(const Eigen::Vector3d &l,
                           const Eigen::Vector3d &r)
    {
      return l(0) < r(0) ||
             (l(0) == r(0) &&
              (l(1) < r(1) ||
               (l(1) == r(1) &&
                l(2) < r(2))));
    }
  };

  inline void filterVs(const Eigen::MatrixXd &rV,
                       const double &epsilon,
                       Eigen::MatrixXd &fV)
  {
    double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff()));
    double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON);
    std::set<Eigen::Vector3d, filterLess> filter;
    fV = rV;
    int offset = 0;
    Eigen::Vector3d quanti;
    for (int i = 0; i < rV.cols(); i++)
    {
      quanti = (rV.col(i) / res).array().round();
      if (filter.find(quanti) == filter.end())
      {
        filter.insert(quanti);
        fV.col(offset) = rV.col(i);
        offset++;
      }
    }
    fV = fV.leftCols(offset).eval();
    return;
  }

  // Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
  // The outter_normal is assumed to be NORMALIZED
  // proposed epsilon is 1.0e-6
  inline void enumerateVs(const Eigen::MatrixXd &hPoly,
                          const Eigen::Vector3d &inner,
                          Eigen::MatrixXd &vPoly,
                          const double epsilon = 1.0e-6)
  {
    Eigen::RowVectorXd b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum() -
                           inner.transpose() * hPoly.topRows<3>();
    Eigen::MatrixXd A = hPoly.topRows<3>().array().rowwise() / b.array();

    quickhull::QuickHull<double> qh;
    double qhullEps = std::min(epsilon, quickhull::defaultEps<double>());
    // CCW is false because the normal in quickhull towards interior
    const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps);
    const auto &idBuffer = cvxHull.getIndexBuffer();
    int hNum = idBuffer.size() / 3;
    Eigen::MatrixXd rV(3, hNum);
    Eigen::Vector3d normal, point, edge0, edge1;
    for (int i = 0; i < hNum; i++)
    {
      point = A.col(idBuffer[3 * i + 1]);
      edge0 = point - A.col(idBuffer[3 * i]);
      edge1 = A.col(idBuffer[3 * i + 2]) - point;
      normal = edge0.cross(edge1); //cross in CW gives an outter normal
      rV.col(i) = normal / normal.dot(point);
    }
    filterVs(rV, epsilon, vPoly);
    vPoly = (vPoly.array().colwise() + inner.array()).eval();
    return;
  }

  // Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
  // The outter_normal is assumed to be NORMALIZED
  // proposed epsilon is 1.0e-6
  inline bool enumerateVs(const Eigen::MatrixXd &hPoly,
                          Eigen::MatrixXd &vPoly,
                          const double epsilon = 1.0e-6)
  {
    Eigen::Vector3d inner;
    if (findInterior(hPoly, inner))
    {
      enumerateVs(hPoly, inner, vPoly, epsilon);
      return true;
    }
    else
    {
      return false;
    }
  }

} // namespace geoutils

#endif


#include <fstream>
#include <iostream>
#include <experimental/filesystem>
#include <stdio.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "../tasks.hh"
#include "../classes/BinnedTypedMatrix.hh"

namespace fs = std::experimental::filesystem;

using namespace EMC;

using namespace boost::numeric::ublas;

int EMC::invert(bool forceOverwrite, std::string input, std::string output) {

	if (!forceOverwrite && fs::exists(output)) {
		std::cout << "Output file already exists, use -f to force overwrite." << std::endl;
		return 1;
	}

	std::ifstream infile(input);
	BinnedTypedMatrix inputMatrix = BinnedTypedMatrix::readFromFile(infile);
	infile.close();

	BinnedTypedMatrix newMat(inputMatrix.columnIndex, inputMatrix.rowIndex, inputMatrix.matType);

	typedef permutation_matrix<std::size_t> pmatrix;
	// create a working copy of the input
	matrix<ValueError> A(inputMatrix.m);
	// create a permutation matrix for the LU-factorization
	pmatrix pm(A.size1());
	// perform LU-factorization
	int res = lu_factorize(A,pm);
	if( res != 0 )
		return false;
	// create identity matrix of "inverse"
	newMat.m.assign(identity_matrix<ValueError>(A.size1()));
	// backsubstitute to get the inverse
	lu_substitute(A, pm, newMat.m);

    std::ofstream outputF(output);
    newMat.writeToFile(outputF);
    outputF.close();

    return 0;
}


#include <Eigen/Geometry>
#include <iostream>

int main()
{
  Eigen::Isometry3d original = Eigen::Isometry3d::Identity();
  original.translate(1.414214*Eigen::Vector3d::UnitY());
  original.rotate(Eigen::AngleAxisd(45.0*M_PI/180.0, Eigen::Vector3d::UnitZ()));

  std::cout << "original:\n" << original.matrix() << "\n" << std::endl;

  Eigen::Isometry3d cycle1 = Eigen::Isometry3d::Identity();
  cycle1.translate(1.414214*Eigen::Vector3d::UnitY());
  cycle1.rotate(Eigen::AngleAxisd(90*M_PI/180.0, Eigen::Vector3d::UnitX()));
  cycle1.rotate(Eigen::AngleAxisd(45.0*M_PI/180.0, Eigen::Vector3d::UnitY()));

  std::cout << "cycle 1:\n" << cycle1.matrix() << "\n" << std::endl;

  Eigen::Isometry3d diff_front = cycle1 * original.inverse();
  std::cout << "diff_front:\n" << diff_front.matrix() << "\n" << std::endl;

  Eigen::Isometry3d diff_back = original.inverse() * cycle1;
  std::cout << "diff_back:\n" << diff_back.matrix() << "\n" << std::endl;

  Eigen::Isometry3d x_90 = Eigen::Isometry3d::Identity();
  x_90.rotate(Eigen::AngleAxisd(90.0*M_PI/180.0, Eigen::Vector3d::UnitX()));
  std::cout << "x_90:\n" << x_90.matrix() << "\n" << std::endl;
}


/*
Demonstrate the Eigen automatic differentiation module by training a multilayer perceptron (fully-connected feedforward neural-network).
Python3.8 and matplotlib are used for plotting the results.
Compile with:
    g++ eigen3_autodiff.cpp -o eigen3_autodiff -std=c++11 -O3 -ffast-math -I /usr/include/eigen3/ -I /usr/local/include/matplotlib-cpp.h -I /usr/include/python3.8 -l python3.8
    ./eigen3_autodiff 0.01 5  # step hidden_layer1_size hidden_layer2_size ...
*/
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>
#include "matplotlibcpp.h"
namespace pyplot = matplotlibcpp;

////////////////////////////////////////////////// ALIASES

using Jet = Eigen::AutoDiffScalar<Eigen::VectorXd>;                // real number with many dual parts
using Jetvec = Eigen::Matrix<Jet, Eigen::Dynamic, 1>;              // vector of jets
using Jetmat = Eigen::Matrix<Jet, Eigen::Dynamic, Eigen::Dynamic>; // matrix of jets

////////////////////////////////////////////////// HELPERS

// Returns a random real uniform on [-1, 1]
inline double randf();

// Returns a vector of just the real parts of the given jet vector
inline Eigen::VectorXd reals(Jetvec const& jv);

// Pulls scalar data embedded in Eigen objects out to plain doubles
inline std::vector<double> extract_values(std::vector<Eigen::Matrix<double, 1, 1>> const& vec_eig);

////////////////////////////////////////////////// MAIN CLASS

class MLP {
    std::vector<int> const dims; // network dimensionalities (input_size, hidden_layer1_size, ..., output_size)
    std::vector<Jetmat> weights; // container of weight matrices
    std::vector<Jetvec> biases; // container of bias vectors
    std::vector<Jet*> params; // pointers to each individual parameter

public:
    MLP(std::vector<int> const& dims) :
        dims(dims),
        weights(dims.size()-1),
        biases(dims.size()-1) {
        // Parse the dims architecture to count up the parameters
        int n_params = 0;
        for(int layer=0; layer<weights.size(); ++layer) {
            weights[layer] = Jetmat(dims[layer+1], dims[layer]);
            biases[layer] = Jetvec(dims[layer+1]);
            n_params += weights[layer].size() + biases[layer].size();
        }
        // Initialize parameters with random reals and orthogonal basis on the duals
        int param_id = 0;
        for(Jetmat & W : weights) {
            for(int col=0; col<W.cols(); ++col) {
                for(int row=0; row<W.rows(); ++row) {
                    W(row, col) = Jet(randf(), n_params, param_id); // (value, number_of_dual_parts, which_dual_part)
                    params.push_back(&W(row, col));
                    ++param_id;
                }
            }
        }
        for(Jetvec & b : biases) {
            for(int row=0; row<b.rows(); ++row) {
                b(row) = Jet(randf(), n_params, param_id);
                params.push_back(&b(row));
                ++param_id;
            }
        }
        assert(params.size() == n_params);
    }

    ////////////////////////////////////////////////// GETTERS

    inline std::vector<int> get_dims() const {
        return dims;
    }

    inline std::vector<Jetmat> get_weights() const {
        return weights;
    }

    inline std::vector<Jetvec> get_biases() const {
        return biases;
    }

    ////////////////////////////////////////////////// WORKERS

    // Returns the sigmoid function of a given scalar jet
    inline Jet neuron(Jet const& x) const {
        return pow(Jet(1)+exp(-x), -1);
    }

    // Returns the jet vector MLP output corresponding to the given real input vector
    template <class D>
    inline Jetvec feedforward(Eigen::MatrixBase<D> const& input) const {
        Jetvec act = input.template cast<Jet>();
        for(int layer=0; layer<weights.size()-1; ++layer) {
            act = weights[layer]*act;
            act += biases[layer]; // bit of Eigen weirdness, this has to be a separate statement
            for(int i=0; i<act.size(); ++i) act(i) = neuron(act(i));
        }
        return weights[weights.size()-1]*act + biases[biases.size()-1]; // don't saturate last layer
    }

    // Calls feedforward on each input in an input set to produce the MLP's corresponding output set
    template <class V>
    inline std::vector<V> feedforward(std::vector<V> const& input_set) const {
        std::vector<V> output_set;
        for(V const& input : input_set) {
            output_set.push_back(reals(feedforward(input)));
        }
        return output_set;
    }

    // Common quadratic cost function for a single input-output pair
    template <class D>
    Jet inline cost_function(Eigen::MatrixBase<D> const& input, Eigen::MatrixBase<D> const& output) const {
        Jetvec err = output.template cast<Jet>() - feedforward(input);
        return err.dot(err);
    }

    // Cost function summed over an entire input-output pair batch
    template <class V>
    Jet inline cost_function(std::vector<V> const& input_set, std::vector<V> const& output_set) const {
        Jet cost(0);
        for(int i=0; i<input_set.size(); ++i) {
            cost += cost_function(input_set[i], output_set[i]);
        }
        return cost;
    }

    // Stochastic gradient descent
    template <class V>
    void train(std::vector<V> const& input_set, std::vector<V> const& output_set,
               double step, double tol=1e-5, int max_epoch=10000) {
        assert(input_set.size() == output_set.size());
        std::cout << "Training with step-size " << step << "..." << std::endl;
        std::vector<int> ordering;
        for(int i=0; i<input_set.size(); ++i) ordering.push_back(i);
        double last_epoch_cost = std::numeric_limits<double>::infinity();
        int epoch = 0;
        while(epoch < max_epoch) {
            double epoch_cost = 0;
            for(int i=0; i<input_set.size(); ++i) {
                Jet cost = cost_function(input_set[ordering[i]], output_set[ordering[i]]);
                epoch_cost += cost.value();
                auto dp = (-step*cost.derivatives()).eval();
                for(int i=0; i<params.size(); ++i) *params[i] += dp(i);
            }
            if(!(epoch % 100)) std::cout << "    epoch " << epoch << ": " << epoch_cost << std::endl;
            if(fabs(epoch_cost-last_epoch_cost) < tol) break;
            std::random_shuffle(ordering.begin(), ordering.end());
            last_epoch_cost = epoch_cost;
            ++epoch;
        }
        std::cout << "Finished training on epoch " << epoch << "/" << max_epoch << std::endl;
    }
};

////////////////////////////////////////////////// MAIN EXECUTABLE

int main(int argc, char** argv) {
    if(argc < 3) {
        std::cout << "Please pass in a step-size and the size of each hidden layer!" << std::endl;
        return 1;
    }

    // We will make a fixed set of data within this code for now for simplicity
    using Edouble = Eigen::Matrix<double, 1, 1>;
    std::vector<Edouble> input_set;
    std::vector<Edouble> output_set;
    for(double val=-10; val<=10; val+=0.3) input_set.push_back(Edouble(val));
    for(Edouble const& input : input_set) output_set.push_back(Edouble(5*exp(-input(0)*input(0))) +
                                                               Edouble(2.5*exp(-pow(input(0)-3, 2))) +
                                                               Edouble(0.05*(randf())));

    // Construct and train our MLP from user choices
    std::vector<int> dims;
    dims.push_back(input_set[0].size());
    for(int i=2; i<argc; ++i) {
        int dim = atoi(argv[i]);
        assert(dim > 0);
        dims.push_back(dim);
    }
    dims.push_back(output_set[0].size());
    std::cout << "MLP Layout: ";
    for(int dim : dims) std::cout << dim << ' ';
    std::cout << std::endl;
    MLP mlp(dims);
    mlp.train(input_set, output_set, atof(argv[1]));

    // Evaluate fit
    std::vector<Edouble> fine_input_set;
    for(double val=-10; val<=10; val+=0.005) fine_input_set.push_back(Edouble(val));
    auto mlp_output_set = mlp.feedforward(fine_input_set);
    std::cout << "Plotting results..." << std::endl;
    pyplot::named_plot("data", extract_values(input_set), extract_values(output_set), "g--");
    pyplot::named_plot("fit", extract_values(fine_input_set), extract_values(mlp_output_set), "k");
    pyplot::legend();
    std::cout << "Close plots to finish..." << std::endl;
    pyplot::show();

    return 0;
}

////////////////////////////////////////////////// OTHER IMPLEMENTATION DETAILS

inline double randf() {
    return 2*Eigen::MatrixXd::Random(1, 1)(0) - 1;
}

inline Eigen::VectorXd reals(Jetvec const& jv) {
    Eigen::VectorXd v(jv.size());
    for(int i=0; i<v.size(); ++i) v(i) = jv(i).value();
    return v;
}

inline std::vector<double> extract_values(std::vector<Eigen::Matrix<double, 1, 1>> const& vec_eig) {
    std::vector<double> vec(vec_eig.size());
    for(int i=0; i<vec.size(); ++i) vec[i] = vec_eig[i](0);
    return vec;
}


//
// Created by keszocze on 27.09.18.
//

#pragma once

#include <map>
#include <string>
#include <vector>

#include <cudd/cplusplus/cuddObj.hh>
#include <boost/multiprecision/cpp_int.hpp>

#include "number_representation.hpp"

namespace abo::util {

/**
 * @brief Computes the label/number of the lowest terminal level of a given BDD (forest)
 * @param bdds BDDs in the forest
 * @return Lowest level in the forest
 */
unsigned int terminal_level(const std::vector<std::vector<BDD>>& bdds);

/**
 * @brief Evaluates the sum of a and b for an adder given as a BDD forest
 * @param adder BDD forest representing an adder
 * @param a The first summand
 * @param b The second summand
 * @param bits bit width of the adder
 * @return The value of a + b
 */
long eval_adder(const std::vector<BDD>& adder, long a, long b, int bits);

/**
 * @brief Evaluates the function with the given input
 * @param function Representing an unsigned integer
 * @param input The inputs to the function (see cudd BDD.eval)
 * @return The value of function with the given input
 */
long eval(const std::vector<BDD>& function, std::vector<int> input);

/**
 * @brief count_minterms Counts the number of minterms for each node in the given BDD in percent
 * (between 0 and 1). This counts how many percent of inputs that lead to a given node as an
 * intermediate result also lead to 1.
 * @param bdd The BDD for which the minters are counted
 * @return A map containing every node in the bdd and its minterm count
 */
std::map<DdNode*, double> count_minterms(const BDD& bdd);

/**
 * @brief count_solutions For each node reachable from the given root, count the number of solutions
 * @param bdd The root node to search from
 * @return A map containing the number of solutions for each reachable node
 */
std::map<DdNode*, double> count_solutions(const BDD& bdd);

/**
 * @brief random_satisfying_input Computes a random variable assignment that satisfies the function
 * given as a bdd
 * @param bdd The function that has to be satisfied by the assignment
 * @param minterm_count The minterm count for each node of the bdd, this must be computed by
 * count_minterms called on the bdd
 * @param max_level The maximum variable level that should be present in the variable assignment
 * @return A random variable assignment satisfying bdd. The vector consists of zeros and ones,
 * integers are only used for better compatibility with cudd functions
 */
std::vector<int> random_satisfying_input(const BDD& bdd,
                                         const std::map<DdNode*, double>& minterm_count,
                                         int max_level);

//! Returns the value of the given ADD node if it is a constant node and zero otherwise
unsigned int const_ADD_value(const ADD& add);

/**
 * @brief add_terminal_values Finds all terminal values present in the given ADD and how often each
 * one is reached
 * @param add The function to compute the terminal values of
 * @return A list of terminal values and their frequencies. The first value of the pair is the
 * terminal node value (unique in the list), the second value is how often it is reached in the ADD
 */
std::vector<std::pair<double, unsigned long>> add_terminal_values(const ADD& add);

/**
 * @brief bdd_forest_to_add Computes the ADD equivalent of a function represented by a BDD forest
 * @param mgr The cudd node manager to create the ADD in
 * @param bdds The function to convert to an ADD
 * @param num_rep The number format in which the bdds represent the function
 * @return The given function as an ADD
 */
ADD bdd_forest_to_add(const Cudd& mgr, const std::vector<BDD>& bdds,
                      const NumberRepresentation num_rep
                        = NumberRepresentation::BaseTwo);

/**
 * @brief xor_difference_add Computes the bit-wise XOR between the two given functions and converts
 * the result to an ADD The conversion creates unsigned integer values
 * @param mgr The cudd node manager to create the ADD in
 * @param f The first function to compute the difference of
 * @param f_hat The second function. Must have the same length as f
 * @return The XOR difference as an ADD
 */
ADD xor_difference_add(const Cudd& mgr, const std::vector<BDD>& f,
                       const std::vector<BDD>& f_hat);

/**
 * @brief absolute_difference_add Computes the absolute difference between to functions represented
 * as BDDs and returns the result as an ADD
 * @param mgr The cudd node manager to create the ADD in
 * @param f The first function to compute the difference of
 * @param f_hat The second function. Must have the same length as f
 * @param num_rep The number representation used for the interpretation of the functions f and f_hat
 * @return A function computing the absolute difference between f and f_hat for each input
 * represented by an ADD
 */
ADD absolute_difference_add(const Cudd& mgr, const std::vector<BDD>& f,
                            const std::vector<BDD>& f_hat,
                            const NumberRepresentation num_rep);

/**
 * @brief dump_dot A helper function for calling the dump dot method of cudd for a BDD forest
 * @param mgr The cudd node manager to use
 * @param bdd The function to dump as a dot file string
 * @param innames The names of the input variables to show in the dot file. May be left empty
 * @param outnames The names for each bit of the function to show in the dot file. May be left empty
 */
void dump_dot(const Cudd& mgr, const std::vector<BDD>& bdd,
              const std::vector<std::string>& innames = std::vector<std::string>(),
              const std::vector<std::string>& outnames = std::vector<std::string>());

/**
 * @brief dump_dot A helper function for calling the dump dot method of cudd for a single BDD
 * @param mgr The cudd node manager to use
 * @param bdd The function to dump as a dot file string
 * @param inames The names of the input variables to show in the dot file. May be left empty
 * @param funname The name of the function to show in the dot file output
 */
void dump_dot(const Cudd& mgr, const BDD& bdd,
              const std::vector<std::string>& inames = std::vector<std::string>(),
              const std::string& funname = std::string());

/**
 * @brief Retrives high-child of a BDD v
 * @param mgr The Cudd object manager
 * @param v
 * @return low(v)
 */
BDD high(const Cudd& mgr, const BDD& v);

/**
 * @brief Retrives low-child of a BDD v
 * @param mgr The Cudd object manager
 * @param v
 * @return low(v)
 */
BDD low(const Cudd& mgr, const BDD& v);

/**
 * @brief Creates a full adder of two BDDs and a carry BDD
 * @param f First summand BDD
 * @param g Second summand BDD
 * @param carry_in Carry BDD
 * @return Tuple of BDDs: Summ BDD and Carry BDD
 */
std::pair<BDD, BDD> full_adder(const BDD& f, const BDD& g, const BDD& carry_in);

/**
 * @brief Creates a (vector of) BDDs that represent the difference between to given (vectors of)
 * BDDs
 *
 * The supplied BDDs are supposed to compute values represented in Two's Copmlement. The BDD
 * returned by this function computes the difference between the outputs of these BDDs, again
 * represented in Two's Complement.
 *
 * @param mgr The Cudd object manager
 * @param minuend Minuend in Two's Complement
 * @param subtrahend  Subtrahend in Two's Complement
 * @return BDD computing the difference of the outputs of the two supplied BDDs
 */
std::vector<BDD> bdd_subtract(const Cudd& mgr, const std::vector<BDD>& minuend,
                              const std::vector<BDD>& subtrahend);

/**
 * @brief Creates a (vector of) BDDs that represent the absolute difference between to given
 * (vectors of) BDDs
 *
 * The supplied BDDs are supposed to compute values represented in Two's Copmlement. The BDD
 * returned by this function computes the difference between the outputs of these BDDs. It will
 * always return an unsigned number, not a number in Two's complement.
 *
 * @param mgr The Cudd object manager
 * @param f Function in Two's Complement
 * @param g Funtion in Two's Complement
 * @param num_rep The number representation for f and g
 * @return BDD computing the absolute difference of the outputs of the two supplied BDDs
 */
std::vector<BDD> bdd_absolute_difference(const Cudd& mgr,
                                         const std::vector<BDD>& f,
                                         const std::vector<BDD>& g,
                                         const NumberRepresentation num_rep);

/**
 * @brief Creates the BDD representing the sum of two functions
 * @param mgr The Cudd object manager
 * @param f First summand
 * @param g Second summand
 * @return BDD representing f+g
 */
std::vector<BDD> bdd_add(const Cudd& mgr,
                         const std::vector<BDD>& f,
                         const std::vector<BDD>& g);

/**
 * @brief Creates BDD representing abs(f). For unsigned numbers, the original function will be
 * returned
 * @param mgr The Cudd object manager
 * @param f Function in Two's Complement
 * @param num_rep The number representation for f
 * @return BDD representing abs(f)
 */
std::vector<BDD> bdd_abs(const Cudd& mgr, const std::vector<BDD>& f,
                        const NumberRepresentation num_rep);

/**
 * @brief Shifts a function f by a number of bits left or right
 *
 * The shifted result has the same number of bits as the input, bits that are shifted above or below
 * the original function bit range will be discarded
 *
 * @param mgr The Cudd object manager
 * @param f Function to shift
 * @param bits_to_shift Number of bits to shift. Can be both positive or negative
 * @return Shifted BDD
 */
std::vector<BDD> bdd_shift(const Cudd& mgr,
                           const std::vector<BDD>& f,
                           int bits_to_shift);

/**
 * @brief Multiplies a bdd function with a constant
 * @param mgr The Cudd object manager
 * @param f Function that is to be multiplied. The function is interpreted as returning an unsigned
 * integer
 * @param factor Constant multiplication factor
 * @param num_extra_bits Additional bits used to make sure that the result can be stored (must be
 * less than sizeof(unsigned long) * 8)
 * @return BDD representing the function "factor*f" (extended by up to num_extra_bits)
 */
std::vector<BDD> bdd_multiply_constant(const Cudd& mgr,
                                       const std::vector<BDD>& f,
                                       double factor,
                                       const unsigned int num_extra_bits = 16);

/**
 * @brief Multiplies a bdd function with a constant large value
 * @param mgr The Cudd object manager
 * @param f Function that is to be multiplied. The function is interpreted as returning an unsigned
 * integer
 * @param factor Constant multiplication factor
 * @return BDD representing the function "factor*f"
 */
std::vector<BDD> bdd_multiply_constant(const Cudd& mgr,
                                       const std::vector<BDD>& f,
                                       boost::multiprecision::uint256_t factor);
/**
 * @brief Checks if an input x exists such that int(f1(x)) >= int(f2(x))
 * @param mgr The Cudd object manager
 * @param f1 Function to compare in base two (interpreted as returning an unsigned integer)
 * @param f2 Function to compare in base two (interpreted as returning an unsigned integer)
 * @return {ge, e}
 * ge is true iff there exists an x such that int(f1(x)) >= int(f2(x))
 * e is true iff there exists an x such that int(f1(x)) = int(f2(x)), but no x exists with
 * int(f1(x)) > int(f2(x))
 */
std::pair<bool, bool> exists_greater_equals(const Cudd& mgr,
                                            const std::vector<BDD>& f1,
                                            const std::vector<BDD>& f2);

/**
 * @brief Creates a BDD representing f >= g
 * @param mgr The Cudd object manager
 * @param f Function to compare in base two (interpreted as returning an unsigned integer)
 * @param g Function to compare in base two (interpreted as returning an unsigned integer)
 * @return BDD representing f >= g
 */
BDD greater_equals(const Cudd& mgr,
                   const std::vector<BDD>& f,
                   const std::vector<BDD>& g);

/**
 * @brief Creates a BDD representing f > g
 * @param mgr The Cudd object manager
 * @param f Function to compare in base two (interpreted as returning an unsigned integer)
 * @param g Function to compare in base two (interpreted as returning an unsigned integer)
 * @return BDD representing f > g
 */
BDD greater_than(const Cudd& mgr,
                const std::vector<BDD>& f,
                const std::vector<BDD>& g);

/**
 * @brief Computes a function g such that int(g(x)) = max(1, int(f(x)) for an unsigned function f
 * @param mgr The Cudd object manager
 * @param f The function to use. Is interpreted as unsigned integers
 * @return Function g as vector of BDDS of same length as f
 */
std::vector<BDD> bdd_max_one(const Cudd& mgr, const std::vector<BDD>& f);

/**
 * @brief divide Computes a function h such that int(h(x)) = int(f(x)) / int(g(x))
 * @param mgr The Cudd object manager
 * @param f int(f(x)) = 0 must not be possible, f is interpreted as an unsigned integer
 * @param g is interpreted as an unsigned integer
 * @param extra_bits
 * @return
 */
std::vector<BDD> bdd_divide(const Cudd& mgr,
                            const std::vector<BDD>& f,
                            const std::vector<BDD>& g,
                            unsigned int extra_bits);

/**
 * @brief Converts an unsigned number to a BDD function representing that value
 * @param mgr The Cudd object manager
 * @param number The value the resulting function should return
 * @return A function that returns the given number for all inputs
 */
std::vector<BDD> number_to_bdds(const Cudd& mgr,
                                const boost::multiprecision::uint256_t &number);

} // namespace abo::util


#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename T = typename DerivedP::Scalar>
inline T point_edge_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    auto e = e1 - e0;
    return (p - e0).dot(e) / e.squaredNorm();
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute the barycentric coordinates of the closest points
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename T = typename DerivedEA0::Scalar>
inline Vector2<T> edge_edge_closest_point(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto eb_to_ea = ea0 - eb0;
    auto ea = ea1 - ea0;
    auto eb = eb1 - eb0;

    Eigen::Matrix<T, 2, 2> coefMtr;
    coefMtr(0, 0) = ea.squaredNorm();
    coefMtr(0, 1) = coefMtr(1, 0) = -eb.dot(ea);
    coefMtr(1, 1) = eb.squaredNorm();

    Vector2<T> rhs;
    rhs[0] = -eb_to_ea.dot(ea);
    rhs[1] = eb_to_ea.dot(eb);

    return coefMtr.ldlt().solve(rhs);
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute the barycentric coordinates of the closest point on the triangle.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename T = typename DerivedP::Scalar>
inline Vector2<T> point_triangle_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    Eigen::Matrix<T, 2, 3> basis;
    basis.row(0) = RowVector3<T>(t1 - t0); // edge 0
    basis.row(1) = RowVector3<T>(t2 - t0); // edge 1
    Matrix2<T> A = basis * basis.transpose();
    Vector2<T> b = basis * Vector3<T>(p - t0);
    Vector2<T> x = A.ldlt().solve(b);
    assert((A * x - b).norm() < 1e-10);
    return x;
}

} // namespace ipc


/*
 * File: fast_hankel_transform.hpp
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:38:09 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#ifndef FASTHANKELTRANSFORM_FASTHANKELTRANSFORM_H_
#define FASTHANKELTRANSFORM_FASTHANKELTRANSFORM_H_

#include <Eigen/Dense>
#include <complex>

class FastHankelTransform {
public:
  FastHankelTransform(int num_sample, double ux_, double uy_);
  ~FastHankelTransform();
  Eigen::VectorXd sampling();
  void set_feval(const Eigen::Ref<const Eigen::VectorXd> &feval);
  Eigen::VectorXd calculate();

private:
  double evaluate_alpha();
  double evaluate_k0(double alpha);
  void evaluate_phi();
  void evaluate_j1();
  const int num_sample_;
  const double ux_;
  const double uy_;
  double alpha_;
  double k0_;

  Eigen::VectorXd x_;
  Eigen::VectorXd f_;
  std::complex<double> *phi_;
  std::complex<double> *j1_;

  bool x_updated_ = false;
  bool f_updated_ = false;
};

#endif


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

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/geometry/geometry_barycentric.h>

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

BOOST_AUTO_TEST_SUITE(opentissue_geometry_util_compute_distance_to_triangle);

BOOST_AUTO_TEST_CASE(inside_simplex_testing)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         V;
  typedef math_types::real_type                            T;

  V const a = V( 0.0, 0.0, 0.0);
  V const b = V(-1.0, 0.0, 0.0);
  V const c = V( 0.0, 1.0, 0.0);
  V const d = V( -2.0, -2.0, 1.0);

  // Test barycentric coordinates of a point lying on an edge
  {
    for(size_t i = 0;i<100;++i)
    {
      T WA = i*0.01;
      T WB = 1.0 - WA;
      V const p = WA*a + WB*b;
      T wa = 10.0;
      T wb = 10.0;
      OpenTissue::geometry::barycentric_geometric( a, b, p, wa, wb );
      BOOST_CHECK_CLOSE(wa, WA, 0.01 );
      BOOST_CHECK_CLOSE(wb, WB, 0.01 );
    }
  }
  // Test barycentric coordinates of a point lying on a triangle
  {
    for(size_t i = 1;i<10;++i)
    for(size_t j = 1;j<10;++j)
    for(size_t k = 1;k<10;++k)
    {
      T WA = i*0.1;
      T WB = j*0.1;
      T WC = k*0.1;
      T W = WA + WB + WC;
      WA /= W;
      WB /= W;
      WC /= W;
      V const p = WA*a + WB*b + WC*c;
      T wa = 10.0;
      T wb = 10.0;
      T wc = 10.0;
      // geometric method
      OpenTissue::geometry::barycentric_geometric( a, b, c, p, wa, wb, wc );
      BOOST_CHECK_CLOSE(wa, WA, 0.01 );
      BOOST_CHECK_CLOSE(wb, WB, 0.01 );
      BOOST_CHECK_CLOSE(wc, WC, 0.01 );

      wa = 10.0;
      wb = 10.0;
      wc = 10.0;
      // algebraic method
      OpenTissue::geometry::barycentric_algebraic( a, b, c, p, wa, wb, wc );
      BOOST_CHECK_CLOSE(wa, WA, 0.01 );
      BOOST_CHECK_CLOSE(wb, WB, 0.01 );
      BOOST_CHECK_CLOSE(wc, WC, 0.01 );

    }
  }
  // Test barycentric coordinates of a point lying in a tetrahedron
  {
    for(size_t i = 1;i<10;++i)
    for(size_t j = 1;j<10;++j)
    for(size_t k = 1;k<10;++k)
    for(size_t m = 1;m<10;++m)
    {
      T WA = i*0.1;
      T WB = j*0.1;
      T WC = k*0.1;
      T WD = m*0.1;
      T W = WA + WB + WC + WD;
      WA /= W;
      WB /= W;
      WC /= W;
      WD /= W;
      V const p = WA*a + WB*b + WC*c+ WD*d;
      T wa = 10.0;
      T wb = 10.0;
      T wc = 10.0;
      T wd = 10.0;
      // Geometric method
      OpenTissue::geometry::barycentric_geometric( a, b, c, d, p, wa, wb, wc, wd );
      BOOST_CHECK_CLOSE(wa, WA, 0.01 );
      BOOST_CHECK_CLOSE(wb, WB, 0.01 );
      BOOST_CHECK_CLOSE(wc, WC, 0.01 );
      BOOST_CHECK_CLOSE(wd, WD, 0.01 );

      wa = 10.0;
      wb = 10.0;
      wc = 10.0;
      wd = 10.0;
      // algebraic method
      OpenTissue::geometry::barycentric_algebraic( a, b, c, d, p, wa, wb, wc, wd );
      BOOST_CHECK_CLOSE(wa, WA, 0.01 );
      BOOST_CHECK_CLOSE(wb, WB, 0.01 );
      BOOST_CHECK_CLOSE(wc, WC, 0.01 );
      BOOST_CHECK_CLOSE(wd, WD, 0.01 );
    }
  }

}

BOOST_AUTO_TEST_SUITE_END();


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

using namespace std;

typedef mtl::compressed2D<double> m_type;

void assemble_poisson2D(m_type& A, int n)
{
    m_type dia_block(n, n), off_block(n, n);
    {
	mtl::mat::inserter<m_type> ins(dia_block);
	for (int i= 0; i < n; i++) {
	    ins[i][i] << 4.0;
	    if (i > 0) ins[i][i-1] << -1.0;
	    if (i < n-1) ins[i][i+1] << -1.0;
	}
    }
    off_block= -1.0;
    A.change_dim(n*n, n*n);
    {
	mtl::mat::inserter<m_type> ins(A);
	for (int i= 0; i < n; i++) {
	    ins[i*n][i*n] << dia_block;
	    if (i > 0) ins[i*n][i*n-n] << off_block;
	    if (i < n-1) ins[i*n][i*n+n] << off_block;
	}
    }
}

//
// Inverse iteration: is almost a literal copy from algorithm 9.1.2
//
// LinearSolver: BinaryFunction
// EigenVector: GLAS dense vector
template <class LinearSolver, class EigenVector>
double inverse_iteration( LinearSolver& solver, EigenVector& x, int m )
{
    EigenVector y( size(x), 0.0 ) ;
    double lambda = 0.0 ;
    x /= two_norm(x) ;
    for (int i=0; i<m; ++i) {
	solver( x, y ) ;
	lambda = mtl::sum(x) / mtl::sum(y) ;
	x = y / two_norm(y) ;
    }
    return lambda ;
}

//
// Inverse iteration: is almost a literal copy from algorithm 9.1.2
//
// LinearSolver: BinaryFunction
// EigenVector: GLAS dense vector
template <class LinearSolver, class Matrix, class EigenVector>
double condition( LinearSolver& solver, const Matrix& A, EigenVector& x, int m ) {
  EigenVector y( size(x) ) ;
  double lambda= 0.0 ;
  x/= two_norm(x);
  for (int i=0; i<m; ++i) {
      y= A * x;
      lambda = mtl::sum(y) / mtl::sum(x) ;
      x = y / two_norm(y) ;
  }
  return std::abs(lambda / inverse_iteration(solver, x, m));
}

template <typename Matrix>
class cg_solver
{
public:
    cg_solver(const Matrix& A) : A(A) {}

    template <typename VectorIn, typename VectorOut>
    void operator() (const VectorIn& v_in, VectorOut& v_out)
    {
	itl::pc::diagonal<Matrix>     P(A);
	itl::cyclic_iteration<double>  iter(v_in, 500, 1.e-6);

	cg(A, v_out, v_in, P, iter);
    }

private:
    const Matrix& A;
};

int main(int, char**)
{
    m_type A;
    assemble_poisson2D(A, 10);

    mtl::dense_vector<double>   x(num_rows(A));
    mtl::seed<double>           seed;
    random(x, seed);

    cg_solver<m_type> sol(A);
    double lambda= inverse_iteration(sol, x, 10);
    cout << "lambda is " << lambda << '\n';

    double cond= condition(sol, A, x, 10);
    cout << "cond is " << cond << '\n';

    return 0;
}


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * 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 copyright holder 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 HOLDER 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.
 *
 * ****************************************************************************
 * Statistical Analysis.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_STATISTICS_HPP
#define VISIONCORE_MATH_STATISTICS_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <VisionCore/Buffers/Buffer1D.hpp>
#include <VisionCore/Buffers/Buffer2D.hpp>
#include <VisionCore/Types/Polynomial.hpp>
#include <VisionCore/Types/Gaussian.hpp>
#include <VisionCore/Math/PolarSpherical.hpp>

/**
 * @note:
 * http://rebcabin.github.io/blog/2013/01/22/covariance-matrices/
 */

namespace vc
{

namespace math
{

/**
 * Running mean, variation, std. deviation, count and min,max.
 */
template<typename T>
class RunningStats
{
public:
    typedef T Scalar;
    static constexpr int Rows = 1;
    static constexpr int Cols = 1;
    static constexpr int Dimension = Rows * Cols;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline RunningStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = std::numeric_limits<T>::max();
        maxV = -std::numeric_limits<T>::max();
        sumX = sumXX = T(0.0);
    }

    EIGEN_DEVICE_FUNC inline void operator()(const T& x)
    {
        using Eigen::numext::mini;
        using Eigen::numext::maxi;

        // process
        cnt++;

        sumX += x;
        sumXX += (x * x);

        maxV = max(maxV, x); // NOTE what about weight?
        minV = min(minV, x);
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline T mean() const
    {
        if(cnt > 0)
        {
            return sumX / T(cnt);
        }
        else
        {
            return T(0.0);
        }
    }

    EIGEN_DEVICE_FUNC inline T variance() const
    {
        if(cnt > 0)
        {
            const T bessel_correction = T(cnt) / T(cnt - 1);
            const T cm = mean();
            const T term1 = sumXX / T(cnt);

            return bessel_correction * (term1 - (cm*cm));
        }
        else
        {
            return T(0.0);
        }
    }

    EIGEN_DEVICE_FUNC inline T stddev() const
    {
        using Eigen::numext::sqrt;
        return sqrt(variance());
    }

    EIGEN_DEVICE_FUNC inline T running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline T min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline T max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::Gaussian<T> gaussian() const
    {
        return types::Gaussian<T>(mean(), variance());
    }
private:
    std::size_t cnt;
    T sumX, sumXX;
    T minV, maxV;
};

/**
 * Specialization for Eigen.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class RunningStats<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options>>
{
public:
    typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options> VectorType;
    typedef _Scalar Scalar;
    static constexpr int Rows = _Rows;
    static constexpr int Cols = _Cols;
    static constexpr int Dimension = Rows * Cols;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline RunningStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = VectorType::Constant(std::numeric_limits<Scalar>::max());
        maxV = VectorType::Constant(-std::numeric_limits<Scalar>::max());
        sumX = VectorType::Zero();
        sumXX = VectorType::Zero();
    }

    EIGEN_DEVICE_FUNC inline void operator()(const VectorType& x)
    {
        // process
        cnt++;

        sumX += x;
        sumXX += VectorType(x.array() * x.array());

        maxV = maxV.array().cwiseMax(x.array());
        minV = minV.array().cwiseMin(x.array());
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline VectorType mean() const
    {
        if(cnt > 0)
        {
            return sumX / Scalar(cnt);
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType variance() const
    {
        if(cnt > 0)
        {
            const Scalar bessel_correction = Scalar(cnt) / Scalar(cnt - 1);
            const VectorType cm = mean();
            const VectorType term1 = sumXX / Scalar(cnt);

            return bessel_correction * (term1 - VectorType(cm.array() * cm.array()));
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType stddev() const
    {
        return sqrt(variance().array());
    }

    EIGEN_DEVICE_FUNC inline VectorType running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline VectorType min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline VectorType max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::Gaussian<VectorType> gaussian() const
    {
        return types::Gaussian<VectorType>(mean(), variance());
    }
private:
    std::size_t cnt;
    VectorType sumX, sumXX, minV, maxV;
};

// ******************** RUNNING MULTIVARIATE *******************************

/**
 * Multivariate analysis.
 */
template<typename T>
class MultivariateStats;

template<typename _Scalar, int Rows, int Options>
class MultivariateStats<Eigen::Matrix<_Scalar, Rows, 1, Options>>
{
public:
    typedef Eigen::Matrix<_Scalar, Rows, 1, Options> VectorType;
    typedef Eigen::Matrix<_Scalar, Rows, Rows, Options> CovarianceType;
    typedef _Scalar Scalar;
    static constexpr std::size_t Dimensions = Rows;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline MultivariateStats()
    {
        reset();
    }

    EIGEN_DEVICE_FUNC inline void reset()
    {
        cnt = 0;
        minV = VectorType::Constant(std::numeric_limits<Scalar>::max());
        maxV = VectorType::Constant(-std::numeric_limits<Scalar>::max());
        sumX = VectorType::Zero();
        sumXX = CovarianceType::Zero();
    }

    EIGEN_DEVICE_FUNC inline void operator()(const VectorType& x)
    {
        // process
        cnt++;

        sumX += x;

        sumXX += (x * x.transpose());

        maxV = maxV.array().cwiseMax(x.array());
        minV = minV.array().cwiseMin(x.array());
    }

    EIGEN_DEVICE_FUNC inline std::size_t count() const
    {
        return cnt;
    }

    EIGEN_DEVICE_FUNC inline VectorType mean() const
    {
        if(cnt > 0)
        {
            return sumX / Scalar(cnt);
        }
        else
        {
            return VectorType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline CovarianceType covariance() const
    {
        if(cnt > 0)
        {
            const Scalar bessel_correction = Scalar(cnt) / Scalar(cnt - 1);
            const VectorType m = mean();
            return bessel_correction * ( (sumXX / Scalar(cnt)) - (m * m.transpose()) );
        }
        else
        {
            return CovarianceType::Zero();
        }
    }

    EIGEN_DEVICE_FUNC inline VectorType running_sum() const { return sumX; }

    EIGEN_DEVICE_FUNC inline VectorType min_value() const { return minV; }
    EIGEN_DEVICE_FUNC inline VectorType max_value() const { return maxV; }

    EIGEN_DEVICE_FUNC inline types::MultivariateGaussian<Scalar,Rows> gaussian() const
    {
        return types::MultivariateGaussian<Scalar,Rows>(mean(), covariance());
    }
private:
    std::size_t cnt;
    VectorType sumX, minV, maxV;
    CovarianceType sumXX;
};

// ******************** ALLAN VARIANCE *******************************
// From: http://www.leapsecond.com/tools/adev_lib.c

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type allanDeviation(std::size_t tau,
                                                                        InputIterator itbegin,
                                                                        InputIterator itend,
                                                                        bool isoverlapping = false,
                                                                        std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0);
    const std::size_t stride = isoverlapping ? 1 : tau;

    for ( ; (itbegin + 2 * tau) < itend ; itbegin += stride)
    {
        const T v = *(itbegin + 2 * tau) - T(2.0) * *(itbegin + tau) + *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(2.0);

    if(terms != nullptr) { *terms = n; }

    if (n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type hadamardDeviation(std::size_t tau,
                                                                           InputIterator itbegin,
                                                                           InputIterator itend,
                                                                           bool isoverlapping = false,
                                                                           std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0);
    const std::size_t stride = isoverlapping ? 1 : tau;

    for ( ; (itbegin + 3 * tau) < itend; itbegin += stride)
    {
        const T v = *(itbegin + 3 * tau) - T(3.0) * *(itbegin + 2 * tau) + T(3.0) * *(itbegin + tau) - *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(6.0);

    if(terms != nullptr) { *terms = n; }

    if (n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type modifiedAllanDeviation(std::size_t tau,
                                                                                InputIterator itbegin,
                                                                                InputIterator itend,
                                                                                std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;

    std::size_t n = 0;
    T sum = T(0.0), v = T(0.0);

    InputIterator orgbegin = itbegin;

    for ( ; (itbegin + 2 * tau) < itend && (std::size_t)std::distance(orgbegin, itbegin) < tau; ++itbegin)
    {
        v += *(itbegin + 2 * tau) - T(2.0) * *(itbegin + tau) + *itbegin;
    }

    sum += v * v;
    n += 1;

    itbegin = orgbegin;

    for ( ; (itbegin + 3 * tau) < itend; ++itbegin)
    {
        v += *(itbegin + 3 * tau) - T(3.0) * *(itbegin + 2 * tau) + T(3.0) * *(itbegin + tau) - *itbegin;
        sum += v * v;
        n += 1;
    }

    sum /= T(2.0) * T(tau) * T(tau);

    if(terms != nullptr) { *terms = n; }
    if(n < 3) { return T(0.0); }

    return sqrt(sum / n) / tau;
}

template<typename InputIterator>
typename std::iterator_traits<InputIterator>::value_type timeDeviation(std::size_t tau,
                                                                       InputIterator itbegin,
                                                                       InputIterator itend,
                                                                       std::size_t* terms = nullptr)
{
    typedef typename std::iterator_traits<InputIterator>::value_type T;
    using namespace std;
    return T(tau) * modifiedAllanDeviation(tau, itbegin, itend, terms) / sqrt(3.0);
}

// ******************** PEARSON PRODUCT MOMENT *******************************

/**
 * Calculate r_xy from data.
 */
template<typename T>
class PearsonProductMoment;

template<typename Derived, typename BaseType>
class PearsonProductMomentBase
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC void reset()
    {
        stats_x.reset();
        stats_x2.reset();
        stats_y.reset();
        stats_y2.reset();
        stats_xy.reset();
    }

    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsX() const { return stats_x; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsY() const { return stats_y; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsX2() const { return stats_x2; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsY2() const { return stats_y2; }
    EIGEN_DEVICE_FUNC const RunningStats<BaseType>& statsXY() const { return stats_xy; }
protected:
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsX() { return stats_x; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsY() { return stats_y; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsX2() { return stats_x2; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsY2() { return stats_y2; }
    EIGEN_DEVICE_FUNC RunningStats<BaseType>& statsXY() { return stats_xy; }

private:
    RunningStats<BaseType> stats_x;
    RunningStats<BaseType> stats_y;
    RunningStats<BaseType> stats_xy;
    RunningStats<BaseType> stats_x2;
    RunningStats<BaseType> stats_y2;
};

template<typename T>
class PearsonProductMoment : public PearsonProductMomentBase<PearsonProductMoment<T>, T>
{
    typedef PearsonProductMomentBase<PearsonProductMoment<T>, T> Base;
public:
    typedef T BaseType;
    typedef types::Polynomial<BaseType,1> PolynomialT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline PearsonProductMoment()
    {
        Base::reset();
    }

    EIGEN_DEVICE_FUNC void operator()(const T& x, const T& y)
    {
        Base::statsX()(x);
        Base::statsX2()(x*x);
        Base::statsY()(y);
        Base::statsY2()(y*y);
        Base::statsXY()(x*y);
    }

    EIGEN_DEVICE_FUNC BaseType result() const
    {
        using Eigen::numext::sqrt;
        return (Base::statsXY().mean() - (Base::statsX().mean() * Base::statsY().mean())) /
                sqrt( ( Base::statsX2().mean() - ( Base::statsX().mean() * Base::statsX().mean() ) ) *
                      ( Base::statsY2().mean() - ( Base::statsY().mean() * Base::statsY().mean() ) ) );
    }

    EIGEN_DEVICE_FUNC inline BaseType coefficientOfDetermination() const
    {
        BaseType r = result();
        return r*r;
    }

    EIGEN_DEVICE_FUNC inline PolynomialT lineEquation() const
    {
        PolynomialT ret;
        BaseType b = result() * ( Base::statsY().stddev() / Base::statsX().stddev() );
        BaseType a = Base::statsY().mean() - b * Base::statsX().mean();
        ret.coeff() << a , b;
        return ret;
    }
};

template<typename _Scalar, int _Rows, int _Cols>
class PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > :
  public PearsonProductMomentBase<PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > , Eigen::Matrix<_Scalar, _Rows, _Cols>>
{
    typedef PearsonProductMomentBase<PearsonProductMoment<Eigen::Matrix<_Scalar, _Rows, _Cols> > , Eigen::Matrix<_Scalar, _Rows, _Cols>> Base;
public:
    typedef Eigen::Matrix<_Scalar, _Rows, _Cols> BaseType;
    typedef types::Polynomial<BaseType,1> PolynomialT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_DEVICE_FUNC inline PearsonProductMoment()
    {
        Base::reset();
    }

    EIGEN_DEVICE_FUNC void operator()(const BaseType& x, const BaseType& y)
    {
        Base::statsX()(x);
        Base::statsX2()(x.array() * x.array());
        Base::statsY()(y);
        Base::statsY2()(y.array() * y.array());
        Base::statsXY()(x.array() * y.array());
    }

    EIGEN_DEVICE_FUNC BaseType result() const
    {
        using Eigen::numext::sqrt;

        return (Base::statsXY().mean().array() - (Base::statsX().mean().array() * Base::statsY().mean().array())) /
                sqrt( ( Base::statsX2().mean().array() - ( Base::statsX().mean().array() * Base::statsX().mean().array() ) ) *
                      ( Base::statsY2().mean().array() - ( Base::statsY().mean().array() * Base::statsY().mean().array() ) ) );
    }

    EIGEN_DEVICE_FUNC inline BaseType coefficientOfDetermination() const
    {
        BaseType r = result();
        return r.array() * r.array();
    }

    EIGEN_DEVICE_FUNC inline PolynomialT lineEquation()
    {
        PolynomialT ret;
        BaseType b = result().array() * ( Base::statsY().stddev().array() / Base::statsX().stddev().array() );
        BaseType a = Base::statsY().mean().array() - b.array() * Base::statsX().mean().array();
        ret.coeff() << a , b;
        return ret;
    }
};

}

}

#endif // VISIONCORE_MATH_STATISTICS_HPP


//  boost sinc.hpp header file

//  (C) Copyright Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_SINC_HPP
#define BOOST_SINC_HPP

#include <cmath>
#include <limits>
#include <string>
#include <stdexcept>

#include <boost/config.hpp>

// These are the the "Sinus Cardinal" functions.

namespace boost
{
    namespace math
    {
#if        defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::sin;

        using    ::std::numeric_limits;
#endif    /* defined(__GNUC__) && (__GNUC__ < 3) */

        // This is the "Sinus Cardinal" of index Pi.

        template<typename T>
        inline T    sinc_pi(const T x)
        {
#ifdef    BOOST_NO_STDC_NAMESPACE
            using    ::abs;
            using    ::sin;
            using    ::sqrt;
#else    /* BOOST_NO_STDC_NAMESPACE */
            using    ::std::abs;
            using    ::std::sin;
            using    ::std::sqrt;
#endif    /* BOOST_NO_STDC_NAMESPACE */

            using    ::std::numeric_limits;

            static T const    taylor_0_bound = numeric_limits<T>::epsilon();
            static T const    taylor_2_bound = sqrt(taylor_0_bound);
            static T const    taylor_n_bound = sqrt(taylor_2_bound);

            if    (abs(x) >= taylor_n_bound)
            {
                return(sin(x)/x);
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 0
                T    result = static_cast<T>(1);

                if    (abs(x) >= taylor_0_bound)
                {
                    T    x2 = x*x;

                    // approximation by taylor series in x at 0 up to order 2
                    result -= x2/static_cast<T>(6);

                    if    (abs(x) >= taylor_2_bound)
                    {
                        // approximation by taylor series in x at 0 up to order 4
                        result += (x2*x2)/static_cast<T>(120);
                    }
                }

                return(result);
            }
        }

#ifdef    BOOST_NO_TEMPLATE_TEMPLATES
#else    /* BOOST_NO_TEMPLATE_TEMPLATES */
        template<typename T, template<typename> class U>
        inline U<T>    sinc_pi(const U<T> x)
        {
#ifdef    BOOST_NO_STDC_NAMESPACE
            using    ::abs;
            using    ::sin;
            using    ::sqrt;
#else    /* BOOST_NO_STDC_NAMESPACE */
            using    ::std::abs;
            using    ::std::sin;
            using    ::std::sqrt;
#endif    /* BOOST_NO_STDC_NAMESPACE */

            using    ::std::numeric_limits;

            static T const    taylor_0_bound = numeric_limits<T>::epsilon();
            static T const    taylor_2_bound = sqrt(taylor_0_bound);
            static T const    taylor_n_bound = sqrt(taylor_2_bound);

            if    (abs(x) >= taylor_n_bound)
            {
                return(sin(x)/x);
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 0
                U<T>    result = static_cast< U<T> >(1);

                if    (abs(x) >= taylor_0_bound)
                {
                    U<T>    x2 = x*x;

                    // approximation by taylor series in x at 0 up to order 2
                    result -= x2/static_cast<T>(6);

                    if    (abs(x) >= taylor_2_bound)
                    {
                        // approximation by taylor series in x at 0 up to order 4
                        result += (x2*x2)/static_cast<T>(120);
                    }
                }

                return(result);
            }
        }
#endif    /* BOOST_NO_TEMPLATE_TEMPLATES */
    }
}

#endif /* BOOST_SINC_HPP */