#pragma once

#include <cmath>
#include <cstdint>
#include <random>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace gaussian_distributions
{
/// See https://people.sc.fsu.edu/~jburkardt/presentations/truncated_normal.pdf
/// for details on the implementations here.

/// Evaluate the value of the CDF of the gaussian distribution specified by
/// @param mean and @param std_dev at @param val.
inline double EvaluateGaussianCDF(
    const double mean, const double std_dev, const double val)
{
  return 0.5 * (1.0 + std::erf(((val - mean) / std_dev) / std::sqrt(2.0)));
}

/// Evaluate the value of the PDF of the gaussian distribution specified by
/// @param mean and @param std_dev at @param val.
inline double EvaluateGaussianPDF(
    const double mean, const double std_dev, const double val)
{
  const double exponent = ((val - mean) * (val - mean))
                          / (2.0 * std_dev * std_dev);
  const double fraction = 1.0 / (std_dev * std::sqrt(2.0 * M_PI));
  const double pdf = fraction * std::exp(-exponent);
  return pdf;
}

/// Evaluate the value of the CDF of the truncated gaussian distribution
/// specified by @param mean and @param std_dev and bounds @param lower_bound
/// and @param upper_bound at @param val.
inline double EvaluateTruncatedGaussianCDF(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double val)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (val <= lower_bound)
  {
    return 0.0;
  }
  else if (val >= upper_bound)
  {
    return 1.0;
  }
  else
  {
    const double cdf_lower_bound
        = EvaluateGaussianCDF(mean, std_dev, lower_bound);
    const double numerator
        = EvaluateGaussianCDF(mean, std_dev, val) - cdf_lower_bound;
    const double denominator
        = EvaluateGaussianCDF(mean, std_dev, upper_bound) - cdf_lower_bound;
    return numerator / denominator;
  }
}

/// Evaluate the value of the PDF of the truncated gaussian distribution
/// specified by @param mean and @param std_dev and bounds @param lower_bound
/// and @param upper_bound at @param val.
inline double EvaluateTruncatedGaussianPDF(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double val)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (val <= lower_bound)
  {
    return 0.0;
  }
  else if (val >= upper_bound)
  {
    return 0.0;
  }
  else
  {
    const double cdf_upper = EvaluateGaussianCDF(mean, std_dev, upper_bound);
    const double cdf_lower = EvaluateGaussianCDF(mean, std_dev, lower_bound);
    const double probability_enclosed = cdf_upper - cdf_lower;
    const double gaussian_pdf = EvaluateGaussianPDF(mean, std_dev, val);
    const double pdf = gaussian_pdf / probability_enclosed;
    return pdf;
  }
}

/// Integrate the probability contained between @param lower_limit and @param
/// upper_limit for the gaussian distribution specified by @param mean and
/// @param std_dev.
inline double IntegrateGaussian(
    const double mean, const double std_dev, const double lower_limit,
    const double upper_limit)
{
  if (lower_limit > upper_limit)
  {
    throw std::invalid_argument("lower_limit > upper_limit");
  }
  const double upper_limit_cdf
      = EvaluateGaussianCDF(mean, std_dev, upper_limit);
  const double lower_limit_cdf
      = EvaluateGaussianCDF(mean, std_dev, lower_limit);
  const double probability = upper_limit_cdf - lower_limit_cdf;
  return probability;
}

/// Integrate the probability contained between @param lower_limit and @param
/// upper_limit for the truncated gaussian distribution specified by @param mean
/// and @param std_dev and bounds @param lower_bound and @param upper_bound.
inline double IntegrateTruncatedGaussian(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double lower_limit, const double upper_limit)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (lower_limit > upper_limit)
  {
    throw std::invalid_argument("lower_limit > upper_limit");
  }
  const double lower_limit_cdf
      = EvaluateTruncatedGaussianCDF(
          mean, lower_bound, upper_bound, std_dev, lower_limit);
  const double upper_limit_cdf
      = EvaluateTruncatedGaussianCDF(
          mean, lower_bound, upper_bound, std_dev, upper_limit);
  const double probability = upper_limit_cdf - lower_limit_cdf;
  return probability;
}

/// Note that comments come from (and refer to) parts of the original R plugin
/// that this implementation was derived from.
class TruncatedGaussianDistribution
{
private:

  double mean_ = 0.0;
  double stddev_ = 0.0;
  double std_lower_bound_ = 0.0;
  double std_upper_bound_ = 0.0;

  enum class DrawCases {TYPE_1, TYPE_2, TYPE_3, TYPE_4, NONE};
  DrawCases case_ = NONE;
  std::uniform_real_distribution<double> uniform_unit_dist_;
  std::uniform_real_distribution<double> uniform_range_dist_;
  std::exponential_distribution<double> exponential_dist_;
  std::normal_distribution<double> normal_dist_;

  bool CheckSimple(const double lower_bound, const double upper_bound) const
  {
    // Init Values Used in Inequality of Interest
    const double val1
        = (2 * std::sqrt(std::exp(1)))
          / (lower_bound + std::sqrt(std::pow(lower_bound, 2) + 4));
    const double val2
        = std::exp((std::pow(lower_bound, 2)
                    - lower_bound * std::sqrt(std::pow(lower_bound, 2) + 4))
                   / (4));
    if (upper_bound > lower_bound + val1 * val2)
    {
      return true;
    }
    else
    {
      return false;
    }
  }

  // Naive Accept-Reject algorithm
  template<typename Generator>
  double NaiveAcceptReject(
      const double lower_bound, const double upper_bound, Generator& prng)
  {
    while (true)
    {
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double draw = normal_dist_(prng);
      if ((draw <= upper_bound) && (draw >= lower_bound))
      {
          return draw;
      }
    }
  }

  // Accept-Reject Algorithm
  template<typename Generator>
  double SimpleAcceptReject(const double lower_bound, Generator& prng)
  {
    // Init Values
    const double alpha
        = (lower_bound + std::sqrt(std::pow(lower_bound, 2) + 4.0)) / (2.0);
    while (true)
    {
      // In the R plugin, this was a call to Rf_rexp(1.0), in pure C++ it is a
      // call to std::exponential_distribution<double>.
      const double e = exponential_dist_(prng);
      const double z = lower_bound + e / alpha;
      const double rho = std::exp(-std::pow(alpha - z, 2) / 2);
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double u = uniform_unit_dist_(prng);
      if (u <= rho)
      {
        return z;
      }
    }
  }

  // Accept-Reject Algorithm
  template<typename Generator>
  double ComplexAcceptReject(
      const double lower_bound, const double upper_bound, Generator& prng)
  {
    while (true)
    {
      // In the R plugin, this was a call to Rf_rnorm(lower_bound, upper_bound),
      // in pure C++ it is a call to std::normal_distribution<double>.
      const double z = uniform_range_dist_(prng);
      double rho = 0.0;
      if (0 < lower_bound)
      {
        rho = std::exp((std::pow(lower_bound, 2) - std::pow(z, 2)) / 2);
      }
      else if (upper_bound < 0)
      {
        rho = std::exp((std::pow(upper_bound, 2) - std::pow(z, 2)) / 2);
      }
      else if (0 < upper_bound && lower_bound < 0)
      {
        rho = std::exp(-std::pow(z, 2) / 2);
      }
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double u = uniform_unit_dist_(prng);
      if (u <= rho)
      {
        return z;
      }
    }
  }

  template<typename Generator>
  double Sample(Generator& prng)
  {
    if (case_ == DrawCases::TYPE_1)
    {
      const double draw
          = NaiveAcceptReject(std_lower_bound_, std_upper_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else if (case_ == DrawCases::TYPE_2)
    {
      const double draw = SimpleAcceptReject(std_lower_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else if (case_ == DrawCases::TYPE_3)
    {
      while (true)
      {
        const double draw = SimpleAcceptReject(std_lower_bound_, prng);
        if (draw <= std_upper_bound_)
        {
          return mean_ + stddev_ * draw;
        }
      }
    }
    else if (case_ == DrawCases::TYPE_4)
    {
      const double draw
          = ComplexAcceptReject(std_lower_bound_, std_upper_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else
    {
      if (case_ == DrawCases::NONE)
      {
        return mean_;
      }
      else
      {
        throw std::runtime_error("Invalid case");
      }
    }
  }

public:

  TruncatedGaussianDistribution(
      const double mean, const double stddev, const double lower_bound,
      const double upper_bound)
    : uniform_unit_dist_(0.0, 1.0),
      uniform_range_dist_(lower_bound, upper_bound),
      exponential_dist_(1.0), normal_dist_(0.0, 1.0)
  {
    // Set operating parameters
    mean_ = mean;
    stddev_ = stddev;
    if (std::abs(stddev_) == 0.0)
    {
      case_ = DrawCases::NONE;
    }
    else
    {
      // Standardize the lower and upper bounds
      std_lower_bound_ = (lower_bound - mean_) / stddev_;
      std_upper_bound_ = (upper_bound - mean_) / stddev_;
      // Set the operating case - i.e. which sampling method we will use
      case_ = DrawCases::NONE;
      if (0.0 <= std_upper_bound_ && 0.0 >= std_lower_bound_)
      {
        case_ = DrawCases::TYPE_1;
      }
      if (0.0 < std_lower_bound_
          && std_upper_bound_ == std::numeric_limits<double>::infinity())
      {
        case_ = DrawCases::TYPE_2;
      }
      if (0.0 > std_upper_bound_
          && std_lower_bound_ == -std::numeric_limits<double>::infinity())
      {
        std_lower_bound_ = -1 * std_upper_bound_;
        std_upper_bound_ = std::numeric_limits<double>::infinity();
        stddev_ = -1 * stddev_;
        case_ = DrawCases::TYPE_2;
      }
      if ((0.0 > std_upper_bound_ || 0.0 < std_lower_bound_)
          && !(std_upper_bound_ == std::numeric_limits<double>::infinity()
               || std_lower_bound_ == -std::numeric_limits<double>::infinity()))
      {
        if (CheckSimple(std_lower_bound_, std_upper_bound_))
        {
          case_ = DrawCases::TYPE_3;
        }
        else
        {
          case_ = DrawCases::TYPE_4;
        }
      }
      if ((case_ != DrawCases::TYPE_1) && (case_ != DrawCases::TYPE_2)
          && (case_ != DrawCases::TYPE_3) && (case_ != DrawCases::TYPE_4))
      {
        throw std::invalid_argument("case cannot be NONE with stddev == 0");
      }
    }
  }

  template<typename Generator>
  double operator()(Generator& prng)
  {
    return Sample(prng);
  }
};

/// Simple multivariate gaussian distribution.
class MultivariteGaussianDistribution
{
private:
  const Eigen::VectorXd mean_;
  const Eigen::MatrixXd norm_transform_;

  std::normal_distribution<double> unit_gaussian_dist_;

  template<typename Generator>
  Eigen::VectorXd Sample(Generator& prng)
  {
    Eigen::VectorXd draw;
    draw.resize(mean_.rows());
    for (ssize_t idx = 0; idx < draw.rows(); idx++)
    {
      draw(idx) = unit_gaussian_dist_(prng);
    }
    return norm_transform_ * draw + mean_;
  }

  static Eigen::MatrixXd CalculateNormTransform(
      const Eigen::MatrixXd& covariance)
  {
    Eigen::MatrixXd norm_transform;
    Eigen::LLT<Eigen::MatrixXd> chol_solver(covariance);
    if (chol_solver.info() == Eigen::Success)
    {
      // Use cholesky solver
      norm_transform = chol_solver.matrixL();
    }
    else
    {
      // Use eigen solver
      Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(covariance);
      norm_transform
          = eigen_solver.eigenvectors()
            * eigen_solver.eigenvalues().cwiseMax(0.0).cwiseSqrt().asDiagonal();
    }
    return norm_transform;
  }

public:
  MultivariteGaussianDistribution(
      const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance)
      : mean_(mean), norm_transform_(CalculateNormTransform(covariance)),
        unit_gaussian_dist_(0.0, 1.0)
  {
    if (mean_.rows() != covariance.rows() || mean_.cols() != covariance.cols())
    {
      throw std::invalid_argument("mean and covariance are different sizes");
    }
    const auto nan_check = [] (const double& val) { return std::isnan(val); };
    if ((norm_transform_.unaryExpr(nan_check)).any())
    {
      throw std::runtime_error(
          "NaN Found in norm_transform in MultivariateGaussianDistribution");
    }
    const auto inf_check = [] (const double& val) { return std::isinf(val); };
    if ((norm_transform_.unaryExpr(inf_check)).any())
    {
      throw std::runtime_error(
          "Inf Found in norm_transform in MultivariateGaussianDistribution");
    }
  }

  template<typename Generator>
  Eigen::VectorXd operator()(Generator& prng)
  {
    return Sample(prng);
  }
};
}  // namespace gaussian_distributions
}  // namespace common_robotics_utilities


#include <iostream>

using namespace std;

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

using namespace Eigen;

int main(int argc, char **argv){

    Matrix<float, 10, 10> originMat = Matrix<float, 10, 10>::Random(10, 10);
    cout << "original matrix: \n" << originMat << endl;

    Matrix<float, 3, 3> eyeMat = Matrix<float, 3, 3>::Identity(3,3);
    cout << "3x3 identity matrix: \n" << eyeMat << endl;

    originMat.block<3, 3>(0, 0) = eyeMat;
    cout << "original matrix with 3x3 identity block: \n" << originMat << endl;

    return 0;
}


/*!
 * @file basis_q1.hpp
 * @brief Contains implementation of \f$Q_1\f$-basis functions for a given quadrilateral.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_BASIS_Q1_HPP_
#define INCLUDE_BASIS_Q1_HPP_

// Deal.ii
#include <deal.II/base/function.h>

#include <deal.II/lac/full_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class BasisQ1
 * @brief Class implements scalar \f$Q_1\f$-basis functions for a given quadrilateral.
 */
template <int dim>
class BasisQ1 : public Function<dim>
{
public:
	BasisQ1();

	void set_index (unsigned int index);

	void set_coeff(const typename Triangulation<dim>::active_cell_iterator &cell);

	virtual double value(const Point<dim> &p,
						const unsigned int component = 0) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int component = 0) const override;

private:
	/*!
	 * Index of current basis function to be evaluated.
	 */
	unsigned int 		index_basis;

	/*!
	 * Matrix columns hold coefficients of basis functions.
	 */
	FullMatrix<double>	coeff_matrix;
	bool is_set_coeff_matrix;
};

/*!
 * Constructor.
 * @param cell
 */
template<int dim>
BasisQ1<dim>::BasisQ1 ()
:
Function<dim>(),
index_basis(0),
coeff_matrix(FullMatrix<double>(std::pow(2,dim),std::pow(2,dim))),
is_set_coeff_matrix(false)
{
}

/*!
 * Set the coefficients. Template specialization \f$dim=2\f$.
 * @param cell
 *
 * Build up coefficient matrix \f$A=(a_{ij})\f$ for basis polynomial
 * \f$\varphi_i(x,y)=a_i^0 + a_i^1 x + a_i^2 y + a_i^3 xy \f$. The \f$i\f$-th column
 * of the matrix hence contains the coefficients for the \f$i\f$-th basis associated
 * to the \f$i\f$-th vertex.
 */
template<>
void
BasisQ1<2>::set_coeff (const typename Triangulation<2>::active_cell_iterator &cell)

{
	FullMatrix<double>	point_matrix(4,4);

	for (unsigned int i=0; i<4; ++i)
	{
		const Point<2>& p = cell->vertex(i);

		point_matrix(i,0) = 1;
		point_matrix(i,1) = p(0);
		point_matrix(i,2) = p(1);
		point_matrix(i,3) = p(0)*p(1);
	}

	// Columns of coeff_matrix are the coefficients of the polynomial
	coeff_matrix.invert (point_matrix);

	is_set_coeff_matrix = true;
}

/*!
 * Set the coefficients. Template specialization \f$dim=3\f$.
 * @param cell
 *
 * Build up coefficient matrix \f$A=(a_{ij})\f$ for basis polynomial
 * \f$\varphi_i(x,y)=a_i^0 + a_i^1 x + a_i^2 y + a_i^3 xy \f$. The \f$i\f$-th column
 * of the matrix hence contains the coefficients for the \f$i\f$-th basis associated
 * to the \f$i\f$-th vertex.
 */
template<>
void
BasisQ1<3>::set_coeff (const typename Triangulation<3>::active_cell_iterator &cell)
{
	FullMatrix<double>	point_matrix(8,8);

	for (unsigned int i=0; i<8; ++i)
	{
		const Point<3>& p = cell->vertex(i);

		point_matrix(i,0) = 1;
		point_matrix(i,1) = p(0);
		point_matrix(i,2) = p(1);
		point_matrix(i,3) = p(2);
		point_matrix(i,4) = p(0)*p(1);
		point_matrix(i,5) = p(1)*p(2);
		point_matrix(i,6) = p(0)*p(2);
		point_matrix(i,7) = p(0)*p(1)*p(2);
	}

	// Columns of coeff_matrix are the coefficients of the polynomial
	coeff_matrix.invert (point_matrix);

	is_set_coeff_matrix = true;
}

/*!
 * Set the index of the basis function to be evaluated.
 *
 * @param index
 */
template<int dim>
void
BasisQ1<dim>::set_index (unsigned int index)
{
	index_basis = index;
}

/*!
 * Evaluate a basis function with a preset index at one given point in 2D.
 *
 * @param p
 * @param component
 */
template<>
double
BasisQ1<2>::value (const Point<2> &p,
					const unsigned int /* component */) const
{
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	double value = coeff_matrix(0,index_basis)
					+ coeff_matrix(1,index_basis)*p(0)
					+ coeff_matrix(2,index_basis)*p(1)
					+ coeff_matrix(3,index_basis)*p(0)*p(1);

	return value;
}

/*!
 * Evaluate a basis function with a preset index at one given point in 3D.
 *
 * @param p
 * @param component
 */
template<>
double
BasisQ1<3>::value (const Point<3> &p,
					const unsigned int /* component */) const
{
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	double value = coeff_matrix(0,index_basis)
					+ coeff_matrix(1,index_basis)*p(0)
					+ coeff_matrix(2,index_basis)*p(1)
					+ coeff_matrix(3,index_basis)*p(2)
					+ coeff_matrix(4,index_basis)*p(0)*p(1)
					+ coeff_matrix(5,index_basis)*p(1)*p(2)
					+ coeff_matrix(6,index_basis)*p(0)*p(2)
					+ coeff_matrix(7,index_basis)*p(0)*p(1)*p(2);

	return value;
}

/*!
 * Evaluate a basis function with a preset index on a given set of points in 2D.
 *
 * @param points
 * @param values
 */
template<>
void
BasisQ1<2>::value_list(const std::vector<Point<2>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = coeff_matrix(0,index_basis)
							+ coeff_matrix(1,index_basis)*points[p](0)
							+ coeff_matrix(2,index_basis)*points[p](1)
							+ coeff_matrix(3,index_basis)*points[p](0)*points[p](1);

	} // end ++p
}

/*!
 * Evaluate a basis function with a preset index on a given set of points in 3D.
 *
 * @param points
 * @param values
 */
template<>
void
BasisQ1<3>::value_list(const std::vector<Point<3>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = coeff_matrix(0,index_basis)
							+ coeff_matrix(1,index_basis)*points[p](0)
							+ coeff_matrix(2,index_basis)*points[p](1)
							+ coeff_matrix(3,index_basis)*points[p](2)
							+ coeff_matrix(4,index_basis)*points[p](0)*points[p](1)
							+ coeff_matrix(5,index_basis)*points[p](1)*points[p](2)
							+ coeff_matrix(6,index_basis)*points[p](0)*points[p](2)
							+ coeff_matrix(7,index_basis)*points[p](0)*points[p](1)*points[p](2);

	} // end ++p
}

} // end namespace Coefficients

#endif /* INCLUDE_BASIS_Q1_HPP_ */


#pragma once
#include "dirichlet_boundary.hpp"
#include "dofs.hpp"
#include "load_vector_assembly.hpp"
#include "stiffness_matrix_assembly.hpp"
#include <Eigen/Core>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <string>
#include <tuple>

typedef Eigen::VectorXd Vector;

//----------------solveBegin----------------
//! Solve the FEM system.
//!
//! @param[out] u will at the end contain the FEM solution.
//! @param[in] quadraticDofs containing mesh data
//! @param[in] f the RHS f (as in the exercise)
//! return number of degrees of freedom (without the boundary dofs)
int solveFiniteElement(Vector &     u,
                       const QDofs &quadraticDofs,
                       const std::function<double(double, double)> &f) {
	// get quadratic dofs
	Eigen::MatrixXi qdof;
	int             N = quadraticDofs.get_dofs(qdof);

	// get mesh vertices
	const Eigen::MatrixXd &vertices = quadraticDofs.get_vertices();

	SparseMatrix A;
	// (write your solution here)
	assembleStiffnessMatrix(A, vertices, qdof, N);

	Vector F;
	// (write your solution here)
	assembleLoadVector(F, vertices, qdof, N, f);

	u.resize(N);
	u.setZero();
	Eigen::VectorXi interiorDofs;

	auto zerobc = [](double x, double y) {
		std::ignore = x;
		std::ignore = y;
		return 0;
	};
	// set homogeneous Dirichlet Boundary conditions
	setDirichletBoundary(u, interiorDofs, quadraticDofs, zerobc);
	F -= A * u;

	SparseMatrix AInterior;
	igl::slice(A, interiorDofs, interiorDofs, AInterior);
	Eigen::SimplicialLDLT<SparseMatrix> solver;

	Vector FInterior;
	igl::slice(F, interiorDofs, FInterior);

	//initialize solver for AInterior
	// (write your solution here)
	solver.compute(AInterior);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix");
	}

	//solve interior system
	// (write your solution here)
	Vector uInterior = solver.solve(FInterior);
	igl::slice_into(uInterior, interiorDofs, u);

	return interiorDofs.size();
}
//----------------solveEnd----------------


#include "reverse.hpp"
#include "forward.hpp"
#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace autodiff {

/**
 * Set the value and derivative of the specified function at the
 * specified input.  The functor must define the method
 * `dual<double> operator()(const dual<double>& x) const;`
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] y function applied to input
 * @param[out] dy_dx derivative of y w.r.t. x
 */
template <typename F>
void derivative(const F& f, double x, double& y, double& dy_dx) {
  dual<double> xd{ x, 1 };
  auto yd = f(xd);
  y = yd.val();
  dy_dx = yd.tan();
}

/**
 * Set the value and gradient of the specified function at the
 * specified input.  The functor must define the method
 * `adj operator()(const Eigen::Matrix<adj, -1, 1>& x) const;`.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] fx function applied to input
 * @param[out] grad gradient of function applied to input
 */
template <typename F>
void gradient(const F& f, const Eigen::VectorXd& x,
              double& fx, Eigen::RowVectorXd& grad) {
  Eigen::Matrix<adj, -1, 1> xa(x.size());
  for (int i = 0; i < xa.size(); ++i)
    xa(i) = x(i);
  adj y = f(xa);
  std::vector<double> adjoints = chain(y);
  fx = y.val();
  grad.resize(x.size());
  for (int i = 0; i < x.size(); ++i)
    grad(i) = adjoints[xa(i).idx()];
}

/**
 * Set the value of the specified function at the specified value and
 * the product of the gradient of the function at the specified value
 * and the specified vector.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[in] v vector to multiply
 * @param[out] fx value of f(x)
 * @param[out] gradv product of gradient of f(x) and v
 */
template <typename F>
void vector_gradient_product(const F& f, const Eigen::VectorXd& x,
                             const Eigen::VectorXd v, double& fx,
                             double& gradv) {
  Eigen::Matrix<dual<double>, -1, 1> xd(x.size());
  for (int i = 0; i < xd.size(); ++i)
    xd(i) = { x(i), v(i) };
  dual<double> fxd = f(xd);
  fx = fxd.val();
  gradv = fxd.tan();
}

/**
 * Set the specified Hessian matrix to the Hessian of the specified
 * function at the specified value.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] H set to Hessian of function at input
 */
template <typename F>
void hessian(const F& f, const Eigen::VectorXd& x, Eigen::MatrixXd& H) {
  int N = x.size();
  H.resize(N, N);
  for (int m = 0; m < N; ++m) {  // m-th row of H
    Eigen::Matrix<dual<adj>, -1, 1> xd(N);
    for (int n = 0; n < N; ++n)
      xd(n) = { x(n), m == n };  // d.x(n)/d.x(m) = 1 iff m == n
    auto y = f(xd);
    std::vector<double> adjoints = chain(y.tan());
    for (int n = 0; n < N; ++n)
      H(m, n) = adjoints[xd(n).val().idx()];  // = bar(dot(x[m, n]))
  }
}

}


#include <Eigen/Core>

#include "quaternion_product.h"

/*
  Assume quaternion data format is (x, y, z, w) and translational format is (x,
  y, z)

  Verifies against Eigen cross product implementation
*/

__attribute__((always_inline)) Eigen::Vector3f crossProduct(Eigen::Vector3f lhs,
                                     Eigen::Vector3f rhs) {

  Eigen::Vector3f result(
      lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
      lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
      lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0));
  return result;
}

/*
  Computes the point product
*/
__attribute__((always_inline)) Eigen::Vector3f pointProduct(Eigen::Vector4f &q,
                                     Eigen::Vector3f &p) {
  Eigen::Vector3f qvec(q(0), q(1), q(2));

  Eigen::Vector3f uv = crossProduct(qvec, p); // qvec.cross(p);
  for (int i = 0; i < 3; i++) {
    uv(i) = uv(i) * 2;
  }
  Eigen::Vector3f qxuv = crossProduct(qvec, uv); // qvec.cross(uv);

  Eigen::Vector3f result;
  result = p + (q(3) * uv) + qxuv;

  return result;
}

/*
  Takes two quaternions and multiplies them together

  Verified against the SE3 product in Sophus
*/
SE3T quaternionProduct(SE3T &a, SE3T &b) {
  Eigen::Vector3f resultTranslation;
  Eigen::Vector4f resultQuaternion;

  Eigen::Vector4f aq = a.quaternion;
  Eigen::Vector4f bq = b.quaternion;

  // Hamilton product
  resultQuaternion(3) =
      aq(3) * bq(3) - aq(0) * bq(0) - aq(1) * bq(1) - aq(2) * bq(2);
  resultQuaternion(0) =
      aq(3) * bq(0) + aq(0) * bq(3) + aq(1) * bq(2) - aq(2) * bq(1);
  resultQuaternion(1) =
      aq(3) * bq(1) + aq(1) * bq(3) + aq(2) * bq(0) - aq(0) * bq(2);
  resultQuaternion(2) =
      aq(3) * bq(2) + aq(2) * bq(3) + aq(0) * bq(1) - aq(1) * bq(0);

  Eigen::Vector3f r = pointProduct(a.quaternion, b.translation);

  resultTranslation = a.translation + r;

  SE3T result = {resultQuaternion, resultTranslation};
  return result;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/diff.hpp>
#include <fcppt/math/pi.hpp>
#include <fcppt/math/vector/angle_between.hpp>
#include <fcppt/math/vector/angle_between_cast.hpp>
#include <fcppt/math/vector/componentwise_equal.hpp>
#include <fcppt/math/vector/hypersphere_to_cartesian.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/math/vector/signed_angle_between.hpp>
#include <fcppt/math/vector/signed_angle_between_cast.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <iostream>
#include <limits>
#include <fcppt/config/external_end.hpp>

namespace
{
typedef
float
real;

real const
epsilon(
	std::numeric_limits<
		real
	>::epsilon()
);

typedef
fcppt::math::vector::static_<
	unsigned,
	2
>
uivector2;

typedef
fcppt::math::vector::static_<real,1>
fvector1;

typedef
fcppt::math::vector::static_<real,2>
fvector2;

typedef
fcppt::math::vector::static_<real,3>
fvector3;

template<typename T>
bool
compare(
	T const &t1,
	T const &t2)
{
	return fcppt::math::diff(t1, t2) < epsilon;
}
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(vector_angle_between)
{
FCPPT_PP_POP_WARNING

	fvector2 const
		vec1(1.f, 0.f),
		vec2(0.f, 1.f);

	BOOST_CHECK(
		::compare(
			fcppt::math::vector::angle_between(
				vec1,
				vec2
			),
			fcppt::math::pi<real>() / 2.f
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(vector_angle_between_cast)
{
FCPPT_PP_POP_WARNING

	uivector2 const
		vec1(1u, 0u),
		vec2(0u, 1u);

	BOOST_CHECK(
		::compare(
			fcppt::math::vector::angle_between_cast<
				real
			>(
				vec1,
				vec2
			),
			fcppt::math::pi<real>() / 2.f
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(vector_signed_angle_between)
{
FCPPT_PP_POP_WARNING

	fvector2 const
		vec1(2.f, 1.f),
		vec2(2.f, 2.f);

	BOOST_CHECK(
		::compare(
			fcppt::math::vector::signed_angle_between(
				vec1,
				vec2
			),
			fcppt::math::pi<real>() / 2.f
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(vector_signed_angle_between_cast)
{
FCPPT_PP_POP_WARNING

	uivector2 const
		vec1(2u, 1u),
		vec2(2u, 2u);

	BOOST_CHECK(
		::compare(
			fcppt::math::vector::signed_angle_between_cast<
				real
			>(
				vec1,
				vec2
			),
			fcppt::math::pi<real>() / 2.f
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(vector_hypersphere_to_cartesian)
{
FCPPT_PP_POP_WARNING

	real const
		phi1 =
			fcppt::literal<real>(1.5),
		phi2 =
			fcppt::literal<real>(0.5);

	fvector2 const result2 =
		fcppt::math::vector::hypersphere_to_cartesian(
			fvector1(
				phi1));

	fvector3 const result3 =
		fcppt::math::vector::hypersphere_to_cartesian(
			fvector2(
				phi1,
				phi2));

	std::cout
		<< "Testing the result of 2D hypersphere_to_cartesian.\n"
		<< "The result should be:\n"
		<<
			fvector2(
				std::cos(
					phi1),
				std::sin(
					phi1))
		<< "\n and it is:\n"
		<< result2
		<< "\n";

	BOOST_CHECK(
		fcppt::math::vector::componentwise_equal(
			result2,
			fvector2(
				std::cos(
					phi1),
				std::sin(
					phi1)),
			epsilon));

	std::cout
		<< "Testing the result of 3D hypersphere_to_cartesian.\n"
		<< "The result should be:\n"
		<<
			fvector3(
				std::cos(
					phi1),
				std::sin(
					phi1) *
				std::cos(
					phi2),
				std::sin(
					phi1) *
				std::sin(
					phi2))
		<< "\n and it is:\n"
		<< result3
		<< "\n";

	BOOST_CHECK(
		fcppt::math::vector::componentwise_equal(
			result3,
			fvector3(
				std::cos(
					phi1),
				std::sin(
					phi1) *
				std::cos(
					phi2),
				std::sin(
					phi1) *
				std::sin(
					phi2)),
			epsilon));
}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef DISTANCE_HPP
#define DISTANCE_HPP

#include <cmath>
#include <limits>
#include <iterator>

#include <boost/function.hpp>

#include "../util/types.hpp"

// ----------------------------------------------------------------
// Note: only distance metrics allowed here, i.e. functions that
// describe the distance between two descriptors and thus smaller
// distances denote more similar objects. The dotproduct itself
// e.g. is NOT a distance measure, it is a similarity measure
// ----------------------------------------------------------------

namespace imdb
{

/**
 * \addtogroup distfn
 * @{
 */

/// L1 norm
template <class T, class R = float>
struct l1norm
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// L1 distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R d = static_cast<R>(*ai) - static_cast<R>(*bi);
            s += std::abs(d);
        }
        return s;
    }
};

/**
 * @brief Squared L2 (Euclidean) distance function.
 *
 * Using the squared Euclidean distance is a bit faster than the L2 norm as we avoid the call to sqrt.
 */
template <class T, class R = float>
struct l2norm_squared
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// Squared L2 distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R d = static_cast<R>(*ai) - static_cast<R>(*bi);
            s += d*d;
        }
        return s;
    }
};

/**
 * @brief L2 (Euclidean) distance function
 */
template <class T, class R = float>
struct l2norm
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    l2norm_squared<T,R> n;

    /// L2 (Euclidean) distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        return std::sqrt(n(a,b));
    }
};

/**
 * @brief One minus dot product distance function.
 *
 * 1 - <a,b> distance function. Assumes a and b are two vectors in R^d
 * having \b unit length
 */
template <class T, class R = float>
struct one_minus_dot
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// Computes 1 - <a,b>. As we assume that both a and b have unit length, the
    /// result is guaranteed to lie in [0,2]
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            s += static_cast<R>(*ai) * static_cast<R>(*bi);
        }
        return (1.0 - s);
    }
};

/// Jensen-Shannon divergence
template <class T, class R = float>
struct jsd
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R v0 = *ai;
            R v1 = *bi;
            R n = 2.0 / (v0 + v1);

            s += ((v0 > 0.0) ? v0 * std::log(v0*n):0.0) + ((v1 > 0.0) ? v1 * std::log(v1*n):0.0);
        }
        return s;
    }
};

/**
 * @brief Chi squared distance function.
 *
 * Chi squared distance function between two vectors k and h; d = sum_i[(ai - bi)^2 / (ai+bi)].
 * We avoid division through zero by adding a tiny value to the denominator:
 * -# if ki == hi == 0 then the nominator is zero and the overall result is zero as desired
 * -# if ki or hi != 0 then ki + hi == ki + hi + float_min
 */
template <class T, class R = float>
struct chi2
{

    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R v0 = *ai;
            R v1 = *bi;
            R nom = v0-v1;
            R denom = v0+v1+std::numeric_limits<R>::epsilon();
            s += nom*nom / denom;
        }
        return s;
    }
};

/**
 * @brief Distance function from 'Eitz et al. - An evaluation of descriptors for large-scale image retrieval from sketched feature lines'
 *
 * Used to compare two Tensor descriptors generated by a tensor_generator. Note that this distance function
 * is different from all others in that before you can actually use operator() you need to set the member
 * variable mask, a vector<bool> of size a.size()/3 which contains a boolean for each grid cell of the
 * Tensor descriptor. mask[i] = true indicates that cell i is masked out, i.e. this cell is ignored when
 * computing the distance.
 */
template <class T, class R = float>
struct dist_frobenius
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        assert(a.size() % 3 == 0);
        assert(a.size() == b.size());

        size_t numCells = a.size() / 3;
        assert(mask->size() == numCells);
        size_t index = 0;
        R dist = 0;

        for (size_t i = 0; i < numCells; i++)
        {
            // masking vector provided by query sketch, if
            // a cell in the query is empty (i.e. contains
            // no lines, we ignore it in the distance computation
            if ((*mask)[i]) continue;

            // difference tensor
            R dE = a[index] - b[index]; index++;
            R dF = a[index] - b[index]; index++;
            R dG = a[index] - b[index]; index++;

            // frobenius norm of difference tensor
            dist += std::sqrt(dE*dE + 2*dF*dF + dG*dG);
        }

        // normalization not required since the number of cells that are masked out
        // is defined by the query sketch and thus is constant for all comparison
        // required for one query
        return dist;
    }

    // if you use this distance function, you will need to manually set the correct
    // mask to be used for the query image
    const vector<bool>* mask;
};

/**
 * @brief Distance function from 'Lee & Funkhouser - Sketch-Based Search and Composition of 3D Models'.
 */
template <class T, class R = float>
struct dist_df
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /**
     * @brief a and b are two descriptors we want to compare. First half of the vectors
     * is expected to contain the distance transform, second half the image/sketch itself.
     *
     * @param a
     * @param b
     * @return Computes <a,dt(b)> + <b,dt(a)>, result range is [0,inf] where 0 means that a and b are equal
     */
    R operator() (const T& a, const T& b) const
    {
        assert(a.size() % 2 == 0);
        assert(a.size() == b.size());

        result_type dist = 0;
        size_t offset = a.size() / 2;
        for (size_t i = 0; i < offset; i++) {
            dist += a[i]*b[i+offset];
            dist += b[i]*a[i+offset];
        }
        return dist;
    }
};

/**
 * @brief 'Base' template factory class for generating a distance function
 * by name, we typically use the partially specialized version for vector<T>, though.
 */
template <class T, class R = float>
struct distance_functions
{
    typedef boost::function<R (const T&, const T&)> distfn_t;

    distfn_t make(const std::string& /*name*/)
    {
        return distfn_t();
    }
};

/**
 * @brief Factory class that generates a distance function from its name.
 *
 * Note that this is a partial specialization of distance_functions
 * for data of type vector<T>. This makes constructing our typical
 * distance function that actually work on a vector<float> just
 * a little bit more convenient than if we would use the more general version.
 */
template <class X, class R>
struct distance_functions<std::vector<X>, R>
{
    typedef std::vector<X> T;
    typedef boost::function<R (const T&, const T&)> distfn_t;

    distfn_t make(const std::string& name)
    {
        if (name == "l1norm") return l1norm<T>();
        if (name == "l2norm") return l2norm<T>();
        if (name == "l2norm_squared") return l2norm_squared<T>();
        if (name == "jsd") return jsd<T>();
        if (name == "chi2") return chi2<T>();
        if (name == "one_minus_dot") return one_minus_dot<T>();
        if (name == "df") return dist_df<T>();
        if (name == "frobenius") return dist_frobenius<T>();

        return distfn_t();
    }
};

/** @} */

} // namespace imdb

#endif // DISTANCE_HPP


// Eigen lib Tutorial
// Source: http://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html
// Compile: g++ -I /usr/include/eigen3/ eigen_ex4.cpp -o eigen_ex4

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

using namespace Eigen;

int main(void)
{
    // addition and subtraction
    Matrix2d a;
    a << 1, 2,
         3, 4;
    MatrixXd b(2,2);
    b << 2, 3,
         1, 4;
    std::cout << "a + b =\n" << a + b << std::endl;
    std::cout << "a - b =\n" << a - b << std::endl;
    std::cout << "Doing a += b;" << std::endl;
    a += b;
    std::cout << "Now a = \n" << a << std::endl;
    Vector3d v(1,2,3);
    Vector3d w(1,0,0);
    std::cout << "-v + w - v =\n" << -v + w - v << std::endl;

    // Scalar multiplication and division
    a -= b;
    std::cout << "a * 2,5 =\n" << a * 2.5  << std::endl;
    std::cout << "0.1 * v =\n" << 0.1 * v  << std::endl;
    std::cout << "Doing v *= 2;" << std::endl;
    v *= 2;
    std::cout << "Now v =\n" << v << std::endl;

    // Transposition and conjugation
    MatrixXcf c = MatrixXcf::Random(2,2);
    std::cout << "Here is the matrix c\n" << c  << std::endl;
    std::cout << "Here is the matrix c^T\n" << c.transpose() << std::endl;
    std::cout << "Here is the conjugate of c\n" <<
        c.conjugate() << std::endl;
    std::cout << "Here is the matrix c^*\n" << c.adjoint() << std::endl;

    // Matrix-matrix and matrix-vector multiplication
    Matrix2d mat;
    mat << 1, 2,
           3, 4;
    Vector2d u(-1,1), x(2,0);
    std::cout << "Here is mat*mat:\n" << mat*mat  << std::endl;
    std::cout << "Here is mat*u:\n" << mat*u << std::endl;
    std::cout << "Here is u^T*mat:\n" << u.transpose()*mat << std::endl;
    std::cout << "Here is u^T*x\n" << u.transpose()*x << std::endl;
    std::cout << "Here is u*x^T\n" << u*x.transpose() << std::endl;
    std::cout << "let's multiply mat by itself" << std::endl;
    mat = mat*mat;
    std::cout << "Now mat is mat:\n" << mat << std::endl;

    // Dot product and cross product
    Vector3d e(1,2,3);
    Vector3d f(0,1,2);

    std::cout << "Dot product: " << e.dot(f) << std::endl;
    double dp = e.adjoint()*f;  // automatic converiosn of the inner product to a scalar
    std::cout << "Dot product via a matrix product:" << dp << std::endl;
    std::cout << "Cross product:\n" << e.cross(f) << std::endl;

    // Basic arithmetic reduction operations
    Matrix2d mat2;
    mat2 << 1, 2,
            3, 4;
    std::cout << "Here is mat2.sum():       " << mat2.sum() << std::endl;
    std::cout << "Here is mat2.prod():      " << mat2.prod() << std::endl;
    std::cout << "Here is mat2.mean():      " << mat2.mean() << std::endl;
    std::cout << "Here is mat2.minCoeff():  " << mat2.minCoeff() << std::endl;
    std::cout << "Here is mat2.maxCoeff():  " << mat2.maxCoeff() << std::endl;
    std::cout << "Here is mat2.trace():     " << mat2.trace() << std::endl;

    return 0;
}


/*
 For more information, please see: http://software.sci.utah.edu

 The MIT License

 Copyright (c) 2015 Scientific Computing and Imaging Institute,
 University of Utah.

 License for the specific language governing rights and limitations under
 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.
 */

//ComputePCA algorithm test.
#include <gtest/gtest.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Testing/Utils/MatrixTestUtilities.h>
#include <Core/Algorithms/Math/ComputePCA.h>
#include <Eigen/SVD>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::TestUtils;

namespace
{
    //Matrix for input.
    DenseMatrixHandle inputMatrix()
    {
        //Hard coded for testing.
        int column1 [12] = {5,4,7,8,10,10,4,6,8,7,9,6};
        int column2 [12] = {7,6,9,8,5,7,9,3,4,5,5,9};

        //Puts the columns into a matrix.
        DenseMatrixHandle inputM(boost::make_shared<DenseMatrix>(12,2));
        for (int i = 0; i < inputM->rows(); i++){
            (*inputM)(i,0) = column1[i];
            (*inputM)(i,1) = column2[i];
        }
        return inputM;
    }

    //The input matrix after centering.
    DenseMatrixHandle centeredInputMatrix()
    {
        //Hard coded for testing.
        double centeredColumn1 [12] = {-2.0,-3.0,-0.0,1.0,3.0,3.0,-3.0,-1.0,1.0,-0.0,2.0,-1.0};
        double centeredColumn2 [12] = {0.583333,-0.416667,2.583333,1.583333,-1.416667,0.583333,2.583333,-3.416667,-2.416667,-1.416667,-1.416667,2.583333};

        //Puts the columns into a matrix.
        DenseMatrixHandle centeredM(boost::make_shared<DenseMatrix>(12,2));
        for (int i = 0; i < centeredM->rows(); i++){
            (*centeredM)(i,0) = centeredColumn1[i];
            (*centeredM)(i,1) = centeredColumn2[i];
        }
        return centeredM;
    }
}

//Checks if the algorithm centers the data correctly.
TEST(ComputePCAtest, CenterData)
{
    ComputePCAAlgo algo;

    DenseMatrixHandle m1(inputMatrix());

    auto centered = ComputePCAAlgo::centerData(m1);

    auto expected = *centeredInputMatrix();

    //Checking every element with a tolerance of 1e-5.
    for (int i = 0; i < centered.rows(); ++i) {
        for (int j = 0; j < centered.cols(); ++j)
            ASSERT_NEAR(expected(i,j), centered(i,j), 1e-5);
    }
}

//Checks if the outputs are correct.
//U,S,V: U: Left principal matrix, S: principal values, V: Right principal matrix.
TEST(ComputePCAtest, checkOutputs)
{
    ComputePCAAlgo algo;

    DenseMatrixHandle m1(inputMatrix());
    DenseMatrixHandle LeftPrinMat_U;
    DenseMatrixHandle PrinVals_S;
    DenseMatrixHandle RightPrinMat_V;

    //Runs algorithm.
    algo.run(m1,LeftPrinMat_U,PrinVals_S,RightPrinMat_V);

    //Testing if the results are null.
    ASSERT_NE(nullptr,LeftPrinMat_U);
    ASSERT_NE(nullptr,PrinVals_S);
    ASSERT_NE(nullptr,RightPrinMat_V);

    //Check the dimensions of the matrices that were created for output.

    //Rows
    ASSERT_EQ(12,LeftPrinMat_U->rows());
    ASSERT_EQ(2,PrinVals_S->rows());
    ASSERT_EQ(2,RightPrinMat_V->rows());

    //Columns
    ASSERT_EQ(12,LeftPrinMat_U->cols());
    ASSERT_EQ(1,PrinVals_S->cols());
    ASSERT_EQ(2,RightPrinMat_V->cols());

    //Eigen does not create a diagonal matrix when it computes SVD, it just has a column with the principal values, so we must put it into a diagonal matrix to be able to do some matrix multiplication later.
    DenseMatrix sDiag = Eigen::MatrixXd::Constant(12,2,0);
    sDiag.diagonal() = PrinVals_S->col(0);

    //Multiplying back together and comparing to the centered matrix. They should be equal to each other with some tolerance.
    DenseMatrix product = (*LeftPrinMat_U) * sDiag * (*RightPrinMat_V).transpose();

    auto expected = *centeredInputMatrix();

    //Comparing each element in the matrices.
    for (int i = 0; i < product.rows(); ++i) {
        for (int j = 0; j < product.cols(); ++j)
            ASSERT_NEAR(expected(i,j), product(i,j), 1e-5);
    }

}

//Tests for input with a dimension of zero.
TEST(ComputePCAtest, ThrowsForZeroDimensionInput)
{
    ComputePCAAlgo algo;

    DenseMatrixHandle m1(new DenseMatrix(5, 0));
    DenseMatrixHandle m2(new DenseMatrix(0, 5));
    DenseMatrixHandle m3(new DenseMatrix(0, 0));

    DenseMatrixHandle LeftPrinMat_U;
    DenseMatrixHandle PrinVals_S;
    DenseMatrixHandle RightPrinMat_V;

    //Runs algorithm and expects an error.
    EXPECT_ANY_THROW(algo.run(m1,LeftPrinMat_U,PrinVals_S,RightPrinMat_V));
    EXPECT_ANY_THROW(algo.run(m2,LeftPrinMat_U,PrinVals_S,RightPrinMat_V));
    EXPECT_ANY_THROW(algo.run(m3,LeftPrinMat_U,PrinVals_S,RightPrinMat_V));

}


/**
 * Barrier functions that grow to infinity as x -> 0+. Includes gradient and
 * hessian functions, too. These barrier functions can be used to impose
 * inequlity constraints on a function.
 */

#pragma once

#include <array>

#include <Eigen/Core>

namespace ipc::rigid {

/// @brief Barrier functions to choose from.
enum BarrierType {
    IPC,
    POLY_LOG,
    SPLINE,
};

///////////////////////////////////////////////////////////////////////////
// Choose your barrier!

/**
 * @brief Function that grows to infinity as x approaches 0 from the right.
 *
 * @param x             The x value at which to evaluate.
 * @param s             Activation value of the barrier.
 * @param barrier_type  Barrier function type to use.
 * @return The value of the barrier function at x.
 */
template <typename T> T barrier(const T& x, double s, BarrierType barrier_type);

double barrier_gradient(double x, double s, BarrierType barrier_type);

double barrier_hessian(double x, double s, BarrierType barrier_type);

///////////////////////////////////////////////////////////////////////////
// Poly-Log Barrier

/// y := x/eps; b(x, ϵ) = -log(y)*(2*y^3-3*y^2+1)
template <typename T> T poly_log_barrier(const T& x, double s);

double poly_log_barrier_gradient(double x, double s);

double poly_log_barrier_hessian(double x, double s);

///////////////////////////////////////////////////////////////////////////
// Spline Barrier

/**
 * @brief Function that grows to infinity as x approaches 0 from the right.
 *
 * \begin{align}
 * g(x, s) = \frac{1}{s^3}x^3 - \frac{3}{s^2}x^2 + \frac{3}{s}x
 * \end{align}
 * \begin{align}
 * \phi_{\text{spline}}(x, s) = \frac{1}{g(x, s)} - 1
 * \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the barrier function at x.
 */
template <typename T> T spline_barrier(const T& x, double s);

/**
 * @brief Derivative of the spline_barrier function with respect to x.
 *
 * \begin{align}
 * g'(x, s) = \frac{3}{s^3}x^2 - \frac{6}{s^2}x + \frac{3}{s}
 * \end{align}
 * \begin{align}
 * \phi'_{\text{spline}}(x, s) = \frac{-1}{[g(x)]^2}g'(x)
 * \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the derivative of the barrier function at x.
 */
double spline_barrier_gradient(double x, double s);

/**
 * @brief Second derivative of the spline_barrier function with respect to
 * x.
 *
 * \begin{align}
 * g''(x, s) = \frac{6}{s^3}x - \frac{6}{s^2}
 * \end{align}
 * \begin{align}
 * \phi''_{\text{spline}}(x, s) = \frac{2}{[g(x)]^3}[g'(x)]^2 +
 * \frac{-1}{[g(x)]^2}g''(x) \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the second derivative of the barrier function at x.
 */
double spline_barrier_hessian(double x, double s);

} // namespace ipc::rigid

#include "barrier.tpp"


#include <iostream>
using namespace std;

#include <ctime>

// use eigin in c++
// Eigen Core part
#include <Eigen/Core>
// 稠密矩阵的代数运算
#include <Eigen/Dense>

#define MATRIX_SIZE    50

//---------------------介绍eigen库的基本用法-----------------------
//内置类型：Eigen::Matrix<>，适用于所有向量和矩阵
//这是一个模版类，三个参数为：数据类型，行，列

int main(int argc, char const *argv[])
{
   //---------------------创建不同类型的向量或者矩阵------------------

   //声明一个2*3的float矩阵
   Eigen::Matrix<float, 2, 3> matrix_23;

   //Eigen 通过 typedef 提供了许多内置类型，不过底层仍然是 Eigen::Matrix
   //例如， Vector3d三位向量 就是 Eigen::Matrix<double,3,1>
   Eigen::Vector3d            v_3d;   //这两个等价, 但是这个数据类型是double
   Eigen::Matrix<float, 3, 1> v_3d_1; //这两个等价，这个数据类型是float

   //Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
   Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零

   //动态大小的矩阵，矩阵大小不确定
   //第一行用最基本的类型，第二行是eigen用typedef提供的
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
   Eigen::MatrixXd matrix_dynamic_1;

   //---------------------对Eigen矩阵的操作------------------

   //数据初始化
   matrix_23 << 1, 2, 3, 4, 5, 6;

   //输出
   cout << "直接用cout输出\n" << matrix_23 << endl;
   //遍历矩阵元素
   //
   // for (size_t i = 0; i < 2; i++)
   // {
   //    for (size_t j = 0; j < 3; j++)
   //    {
   //       cout << matrix_23(i, j) << "\t";
   //    }
   //    cout << endl;
   // }

   //矩阵间乘法，或者矩阵和向量
   //要注意的：
   //1. 两个对象内元素的数据类型要一样
   //   Wrong example:
   //   Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
   //2. 遵循矩阵乘法规则, 例如矩阵维度
   //   Wrong example:
   //   Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
   v_3d << 3, 2, 1;   //v_3d数据类型是double
   v_3d_1 << 4, 5, 6; //v_3d_1数据类型是float

   //四则运算 用 + - * /
   //这里展示乘法
   //matrix_23数据类型是float，如果要和v_3d相乘，需要显示转换
   //Eigen为了性能，不会提供隐士转换，转换成员函数是 .cast<double>()
   Eigen::Matrix<double, 2, 1> result  = matrix_23.cast<double>() * v_3d;
   Eigen::Matrix<float, 2, 1>  result2 = matrix_23 * v_3d_1;

   //矩阵运算
   matrix_33 = Eigen::Matrix3d::Random();                             //随机3x3的矩阵，但是感觉怎么都不变...
   cout << "matrix \n" << matrix_33 << endl << endl;
   cout << "matrix transpose\n" << matrix_33.transpose() << endl;     // 转置
   cout << "matrix sum\n" << matrix_33.sum() << endl;                 // 各元素和
   cout << "matrix trace\n" << matrix_33.trace() << endl;             // 迹
   cout << "matrix inverse\n" << matrix_33.inverse() << endl;         // 逆
   cout << "matrix determinant\n" << matrix_33.determinant() << endl; // 行列式

   //----------------------eigenvalues & eigenvectors-----------------
   // Computes eigenvalues and eigenvectors of selfadjoint matrices.
   // define a selfadjoint Matrix
   Eigen::Matrix3d S = matrix_33.transpose() * matrix_33;
   // use solver on 3x3 matrix S
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(S);
   cout << "Eigen values = \n" << es.eigenvalues() << endl;
   cout << "Eigen vectors = \n" << es.eigenvectors() << endl;

   //-------------------------解方程-----------------------------
   //Solve matrix_NN * x = v_Nd
   //直接求逆求解 与 QR decomposition求解
   //define matrix_NN and v_Nd
   Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
   matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); //另一种使用random的用法
   Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
   v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);
   Eigen::Matrix<double, MATRIX_SIZE, 1> x;
   //set timer
   clock_t time_start = clock();
   //1. solve by inverse, time used is4.151ms
   x = matrix_NN.inverse() * v_Nd;
   cout << "time used is" << 1000 * (clock() - time_start) / (double)CLOCKS_PER_SEC << "ms" << endl;
   //2. solve by QR decomposition, time used is0.082ms
   time_start = clock();
   x          = matrix_NN.colPivHouseholderQr().solve(v_Nd);
   cout << "time used is" << 1000 * (clock() - time_start) / (double)CLOCKS_PER_SEC << "ms" << endl;

   return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
                  {

 /*!
    @ingroup group-arithmetic
    Function object implementing average

    Computes the arithmetic mean of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = average(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = (x+y)/T(2);
    @endcode

    for integer types, it returns a rounded value at a distance guaranteed
    to be less than or equal to 0.5 of the average floating value, but may differ
    by unity from the truncation given by (x1+x2)/T(2).

    @par Note:
      This function does not overflow.

    @see meanof

  **/
  Value average(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/simd/average.hpp>

#endif


/**
 * @file exponentialintegrator_main.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>
/**
 * @file exponentialintegrator_main.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "exponentialintegrator.h"

int main() {
  /* SAM_LISTING_BEGIN_0 */
  // Final time
  double T = 1.0;
  // Initial value
  Eigen::VectorXd y0(1);
  y0 << 0.1;
  // Function and Jacobian and exact solution
  auto f = [](const Eigen::VectorXd &y) { return y(0) * (1.0 - y(0)); };
  auto df = [](const Eigen::VectorXd &y) {
    Eigen::VectorXd dfy(1);
    dfy << 1.0 - 2.0 * y(0);
    return dfy;
  };
  double exactyT = y0(0) / (y0(0) + (1.0 - y0(0)) * std::exp(-T));

  // Container for errors
  std::vector<double> error(15);

  // Test many step sizes
  for (int j = 0; j < 15; ++j) {
    int N = std::pow(2, j + 1);
    Eigen::VectorXd y = y0;
    double h = T / N;
#if SOLUTION
    for (int k = 0; k < N; ++k) {
      y = ExponentialIntegrator::exponentialEulerStep(y, f, df, h);
    }
#else
    //====================
    // Your code goes here
    // TODO: Perform N timesteps with inital data y0 and store the result in y.
    //====================
#endif

    error[j] = std::abs(y(0) - exactyT);
    std::cout << std::left << std::setfill(' ') << std::setw(3)
              << "N = " << std::setw(7) << N << std::setw(8)
              << "Error = " << std::setw(13) << error[j];
    if (j > 0) {
      std::cout << std::left << std::setfill(' ') << std::setw(10)
                << "Approximated order = " << std::log2(error[j - 1] / error[j])
                << std::endl;
    } else
      std::cout << std::endl;
  }
  /* SAM_LISTING_END_0 */
  return 0;
}


/**
 * \file boost/numeric/ublasx/operation/linspace.hpp
 *
 * \brief Linearly spaced vector
 *
 * Inspired by MATLAB's linspace function.
 *
 *

 *
 * Copyright (c) 2013, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP

#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

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

/**
 * \brief Generates a linearly spaced vector.
 *
 * Generates \a n values linearly spaced between \a a and \a b.
 * Note, in case \f$a<b\f$, generates a decreasing sequence.
 *
 * Inspired by MATLAB's linspace function.
 *
 * \param a The starting value of the linearly spaced sequence.
 * \param b The final value of the linearly spaced sequence.
 * \param n The number of values to generate
 * \return A vector of linearly spaced values in \f$[a,b]\f$; if
 *  n=1, returns \a b.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
BOOST_UBLAS_INLINE
vector<ValueT> linspace(ValueT a, ValueT b, std::size_t n = 100)
{
	// pre: n > 0
	BOOST_UBLAS_CHECK( n > 0,
					   bad_argument() );

	if (n < 2)
	{
		return scalar_vector<ValueT>(1, b);
	}

	vector<ValueT> x(n);

	--n;

	// Check for possible overflows generated by opposite signs
	const ValueT c = (b-a)*(n-1);
	if (std::isinf(c) && std::isinf(b-a))
	{
		// Overflows is happened => split computations

		const ValueT an = a/n;
		const ValueT bn = b/n;

		for (std::size_t i = 0; i <= n; ++i)
		{
			x[i] = a + bn*i - an*i;
		}
	}
	else
	{
		// No overflow

		const ValueT step = (b-a)/n;

		for (std::size_t i = 0; i <= n; ++i)
		{
			x[i] = a + step*i;
		}
	}

	// just make sure that endpoints are honored
	x[0] = a;
	x[n] = b;

	return x;
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP


#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <solvers/sqp.hpp>
#include <unsupported/Eigen/AutoDiff>

using namespace sqp;

namespace sqp_test_autodiff {

template <typename SCALAR, typename DERIVED>
struct NonLinearProblemAutoDiff : public NonLinearProblem<SCALAR> {
    using Scalar = SCALAR;
    using Vector = NonLinearProblem<double>::Vector;
    using Matrix = NonLinearProblem<double>::Matrix;

    using ADScalar = Eigen::AutoDiffScalar<Vector>;
    using ADVector = Eigen::Matrix<ADScalar, Eigen::Dynamic, 1>;

    template <typename vec>
    void ADVectorSeed(vec& x) {
        for (int i = 0; i < x.rows(); i++) {
            x[i].derivatives() = Vector::Unit(this->num_var, i);
        }
    }

    void objective(const Vector& x, Scalar& obj) override {
        static_cast<DERIVED*>(this)->objective(x, obj);
    }

    void objective_linearized(const Vector& x, Vector& grad, Scalar& obj) override {
        ADVector ad_x = x;
        ADScalar ad_obj;
        ADVectorSeed(ad_x);
        /* Static polymorphism using CRTP */
        static_cast<DERIVED*>(this)->objective(ad_x, ad_obj);
        obj = ad_obj.value();
        grad = ad_obj.derivatives();
    }

    void constraint(const Vector& x, Vector& c, Vector& l, Vector& u) override {
        static_cast<DERIVED*>(this)->constraint(x, c, l, u);
    }

    void constraint_linearized(const Vector& x, Matrix& Jc, Vector& c, Vector& l,
                               Vector& u) override {
        ADVector ad_c(this->num_constr);
        ADVector ad_x = x;

        ADVectorSeed(ad_x);
        static_cast<DERIVED*>(this)->constraint(ad_x, ad_c, l, u);

        // fill constraint Jacobian
        for (int i = 0; i < ad_c.rows(); i++) {
            c[i] = ad_c[i].value();
            Eigen::Ref<Vector> deriv = ad_c[i].derivatives();
            Jc.row(i) = deriv.transpose();
        }
    }
};

template <typename Scalar>
inline Scalar rosenbrock(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> x) {
    const double a = 1;
    const double b = 100;

    Scalar z = 0;
    for (int i = 0; i < x.rows() - 1; i++) {
        z += pow(a - x[i], 2) + b * pow(x[i + 1] - pow(x[i], 2), 2);
    }
    return z;
}

struct ConstrainedRosenbrock2D : public NonLinearProblemAutoDiff<double, ConstrainedRosenbrock2D> {
    const Scalar a = 1;
    const Scalar b = 100;
    Eigen::Matrix<Scalar, 2, 1> SOLUTION = {0.707106781, 0.707106781};

    ConstrainedRosenbrock2D() {
        num_var = 2;
        num_constr = 2;
    }

    template <typename DerivedA, typename DerivedB>
    void objective(const DerivedA& x, DerivedB& obj) {
        // (a-x)^2 + b*(y-x^2)^2
        // obj = pow(a - x(0), 2) + b * pow(x(1) - pow(x(0), 2), 2);
        obj = rosenbrock(x);
    }

    template <typename A, typename B>
    void constraint(const A& x, B& c, Vector& l, Vector& u) {
        const Scalar infinity = std::numeric_limits<Scalar>::infinity();
        // y >= x
        // x^2 + y^2 == 1
        c << x(0) - x(1), x.squaredNorm();
        u << 0, 1;
        l << -infinity, 1;
    }
};

TEST(SQPAutoDiff, TestConstrainedRosenbrock2D) {
    ConstrainedRosenbrock2D problem;
    SQP<double> solver;
    Eigen::VectorXd x0 = Eigen::Vector2d(0, 0);
    Eigen::VectorXd y0 = Eigen::VectorXd::Zero(2);

    solver.settings().max_iter = 100;
    // solver.settings().line_search_max_iter = 10;
    // solver.settings().second_order_correction = true;

    // solver.settings().eta = 0.5;
    solver.solve(problem, x0, y0);

    solver.info().print();
    std::cout << "primal solution " << solver.primal_solution().transpose() << std::endl;
    std::cout << "dual solution " << solver.dual_solution().transpose() << std::endl;

    EXPECT_TRUE(solver.primal_solution().isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}

struct Rosenbrock : public NonLinearProblemAutoDiff<double, Rosenbrock> {
    const Scalar a = 1;
    const Scalar b = 100;
    Vector SOLUTION;

    Rosenbrock(int n) {
        num_var = n;
        num_constr = n;
        SOLUTION = Vector::Ones(n);
    }

    template <typename DerivedA, typename DerivedB>
    void objective(const DerivedA& x, DerivedB& obj) {
        obj = rosenbrock(x);
    }

    template <typename A, typename B>
    void constraint(const A& x, B& c, Vector& l, Vector& u) {
        c << x;
        u.setConstant(1);
        l.setConstant(0);
    }
};

TEST(SQPAutoDiff, TestRosenbrock) {
    for (int i = 2; i < 4; i++) {
        Rosenbrock problem(i);
        SQP<double> solver;

        solver.settings().max_iter = 100;
        // solver.settings().line_search_max_iter = 100;
        // solver.settings().second_order_correction = true;
        solver.solve(problem);

        solver.info().print();
        std::cout << "primal solution " << solver.primal_solution().transpose() << std::endl;
        std::cout << "dual solution " << solver.dual_solution().transpose() << std::endl;

        EXPECT_TRUE(solver.primal_solution().isApprox(problem.SOLUTION, 1e-2));
        EXPECT_LT(solver.info().iter, solver.settings().max_iter);
    }
}

template <typename T>
void callback(SQP<T>& solver) {
    Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ",", "[", "],");
    std::cout << solver.x_.transpose().format(fmt) << '\n';
}

struct SimpleNLP : public NonLinearProblemAutoDiff<double, SimpleNLP> {
    Eigen::Vector2d SOLUTION = {1, 1};
    const Scalar infinity = std::numeric_limits<Scalar>::infinity();

    SimpleNLP() {
        num_var = 2;
        num_constr = 3;
    }

    template <typename A, typename B>
    void objective(const A& x, B& obj) {
        obj = -x.sum();
    }

    template <typename A, typename B>
    void constraint(const A& x, B& c, Vector& l, Vector& u) {
        // 1 <= x0^2 + x1^2 <= 2
        // 0 <= x0
        // 0 <= x1
        c << x.squaredNorm(), x;
        l << 1, 0, 0;  // x0 > 0 and x1 > 0
        u << 2, infinity, infinity;
    }
};

TEST(SQPAutoDiff, TestSimpleNLP) {
    SimpleNLP problem;
    SQP<double> solver;

    // feasible initial point
    Eigen::VectorXd x0 = Eigen::Vector2d(1.2, 0.1);
    Eigen::VectorXd y0 = Eigen::VectorXd::Zero(3);

    // TODO(mi): doesn't work for higher values
    // solver.settings().line_search_max_iter = 10;
    solver.settings().max_iter = 100;
    solver.settings().second_order_correction = false;
    // solver.settings().iteration_callback = callback<double>;

    solver.solve(problem, x0, y0);

    solver.info().print();
    std::cout << "primal solution " << solver.primal_solution().transpose() << std::endl;
    std::cout << "dual solution " << solver.dual_solution().transpose() << std::endl;

    EXPECT_TRUE(solver.primal_solution().isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}

TEST(SQPAutoDiff, TestSimpleNLP_SOC) {
    SimpleNLP problem;
    SQP<double> solver;

    // feasible initial point
    Eigen::VectorXd x0 = Eigen::Vector2d(1.2, 0.1);
    Eigen::VectorXd y0 = Eigen::VectorXd::Zero(3);

    // TODO(mi): doesn't work for higher values
    // solver.settings().line_search_max_iter = 10;
    solver.settings().max_iter = 100;
    solver.settings().second_order_correction = true;
    // solver.settings().iteration_callback = callback<double>;

    solver.solve(problem, x0, y0);

    solver.info().print();
    std::cout << "primal solution " << solver.primal_solution().transpose() << std::endl;
    std::cout << "dual solution " << solver.dual_solution().transpose() << std::endl;

    EXPECT_TRUE(solver.primal_solution().isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}

/** Example 12.1 from Nonlinear Optimization by Nocedal, Wright */
struct SimpleNLP2 : public NonLinearProblemAutoDiff<double, SimpleNLP2> {
    Eigen::Vector2d SOLUTION = {-1, -1};

    SimpleNLP2() {
        num_var = 2;
        num_constr = 1;
    }

    template <typename A, typename B>
    void objective(const A& x, B& obj) {
        obj = x.sum();
    }

    template <typename A, typename B>
    void constraint(const A& x, B& c, Vector& l, Vector& u) {
        // x0^2 + x1^2 = 2
        c << x.squaredNorm();
        l << 2;
        u << 2;
    }
};

TEST(SQPAutoDiff, TestSimpleNLP2) {
    SimpleNLP2 problem;
    SQP<double> solver;

    Eigen::VectorXd x0 = Eigen::Vector2d(1.2, 0.1);
    Eigen::VectorXd y0 = Eigen::VectorXd::Zero(1);

    solver.solve(problem, x0, y0);

    solver.info().print();
    std::cout << "primal solution " << solver.primal_solution().transpose() << std::endl;
    std::cout << "dual solution " << solver.dual_solution().transpose() << std::endl;

    EXPECT_TRUE(solver.primal_solution().isApprox(problem.SOLUTION, 1e-2));
    EXPECT_LT(solver.info().iter, solver.settings().max_iter);
}

}  // namespace sqp_test_autodiff


/*
This program is meant to compute maximal Lyapunov expoennt of Lorenz system.
*/
#include <iostream>
#include <cmath>
#include <complex>
#include <fstream>
#include <string>
#include <ctime>
#include <cstdlib>
#include <blitz/array.h>
#include <sys/stat.h>

using namespace blitz;
// ============ Declaration of functions ===================
double Euclidean_measure(Array<double,1>);
Array<double,1> Lorenz_system(Array<double,1>);
Array<double,1> RK4_integrator(Array<double,1>, double, Array<double,1> (*F)(Array<double, 1>) );
Array<double,1> linearized_Lorenz_system(Array<double,1>, Array<double,1>);
Array<double,1> error_RK4_integrator(Array<double,1>, Array<double,1>, double, Array<double,1> (*F)(Array<double,1>, Array<double,1>));
double max_lypo_expo(Array<double,1> , double, Array<double,1> (*F)(Array<double,1>) );

// =========== I/O files ==================================
ofstream time_series_file;

int main(int argc, char const *argv[])
{
	Array<double,1> X(3);								// State vector for the reference trajectory
	X = -3.16,-5.31,13.31;							// Initial condition r = 28
	// X = 0.62225717,-0.08232857,30.60845379;				// Initial condition r = 45.92
	// X = 4.29033, -0.577049, 28.8962;
	double dt = 0.001;
	max_lypo_expo(X, dt, Lorenz_system);
	return 0;
}

double Euclidean_measure(Array<double,1> A)
{
	int N = A.numElements();
	double d = 0;
	for (int i = 0; i < N; ++i)
	{
		d += pow(A(i),2);
	}
	d = sqrt(d);
	return d;
}

Array<double,1> RK4_integrator(Array<double,1> X0, double dt, Array<double,1> (*F)(Array<double,1>) )
{
	int dim = X0.numElements();
	Array<double,1> X(dim);				// State vector
	// -------- Intermediate derivatives --------
	Array<double,1> K1(dim);
	Array<double,1> K2(dim);
	Array<double,1> K3(dim);
	Array<double,1> K4(dim);
	// ------------------------------------------
	K1 = dt*F(X0);
	X = X0 + 0.5*K1;
	K2 = dt*F(X);
	X = X0 + 0.5*K2;
	K3 = dt*F(X);
	X = X0 + K3;
	K4 = dt*F(X);
	X = X0 + (1.0/6)*(K1 + 2*K2 + 2*K3 + K4);
	return X;

}

Array<double,1> error_RK4_integrator( Array<double,1> E0, Array<double,1> X, double dt, Array<double,1> (*F)(Array<double,1>, Array<double,1>))
{
	// X is the state vector
	int dim = E0.numElements();
	Array<double,1> E(dim);				// Tangent vector
	// -------- Intermediate derivatives --------
	Array<double,1> K1(dim);
	Array<double,1> K2(dim);
	Array<double,1> K3(dim);
	Array<double,1> K4(dim);
	// ------------------------------------------
	K1 = dt*F(E0, X);
	E = E0 + 0.5*K1;
	K2 = dt*F(E0, X);
	E = E0 + 0.5*K2;
	K3 = dt*F(E0, X);
	E = E0 + K3;
	K4 = dt*F(E0, X);
	E = E0 + (1.0/6)*(K1 + 2*K2 + 2*K3 + K4);
	return E;

}

Array<double,1> Lorenz_system(Array<double,1> X)
{
	// ------ Parameters ------------------------
	double sigma = 10;//16; //10;
	double r = 28;//45.92; //28;
	double b = 8.0/3;//4; //8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-X(0) + X(1));
	F(1) = (r- X(2))*X(0) - X(1);
	F(2) = X(0)*X(1) - b*X(2);
	return F;
}

/*Array<double,1> Rotating_Lorenz_system(Array<double,1> X)
{
	// ------ Parameters ------------------------
	double sigma = 16; //10;
	double r = 45.92; //28;
	double b = 4; //8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-X(0) + X(1));
	F(1) = (r- X(2))*X(0) - X(1);
	F(2) = X(0)*X(1) - b*X(2);
	return F;
}*/

Array<double,1> linearized_Lorenz_system(Array<double,1> E, Array<double,1> X)
{
	// X is the reference trajectory while
	// E is the error along the refrence trajectory.
	// ------ Parameters ------------------------
	double sigma = 10;
	double r = 28;
	double b = 8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-E(0) + E(1));
	F(1) = (r- X(2))*E(0) - E(1)- X(0)*E(2);
	F(2) = X(1)*E(0) + X(0)*E(1)- b*E(2);
	return F;
}

double max_lypo_expo(Array<double,1> X, double dt, Array<double,1> (*F)(Array<double,1>) )
{

	// Array<double,1> X(3);				// State vector for the reference trajectory
	// X = -3.16,-5.31,13.31;				// Initial condition r = 28
	// X = 0.62225717,-0.08232857,30.60845379;				// Initial condition r = 45.92
	// X = 4.29033, -0.577049, 28.8962;

	int T_transient = 32000;
	// ======  Removing the transience to enter the attractor========
	for (int i = 0; i < T_transient; ++i)
	{
		X = RK4_integrator(X, dt, F);
	}
	// ==============================================================

	cout<<X<<endl;						// After transient, the initial condition to start with.
	// ======================== Calculation of Lyapunov exponent ===============================
	Array<double,1> Y(3);				// State vector for the second trajectory

	Array<double,1> E(3);				// Tangent vector
	E = 1e-8,0,0;						// Initial tangent vector

	double d0;
	d0 = Euclidean_measure(E); 			// Initial error
	double d = 0;						// length of tangent vector after one iteration
	double div_rate = 0;				// divergence rate of trajectories defined as log(d/d0).
	double le=0;						// Lyapunov exponent defined as the average of div_rate along the trajectories.
	int T = 100000;
	// time_series_file.open("test_data_lypo_r45.d");
	for (int i = 1; i <= T; ++i)
	{
		Y = X+E;						// Initial condition for the second trajectory

		//------Simultaneous evolution of the two trajectories by one time step ------
		X = RK4_integrator(X, dt, F);
		Y = RK4_integrator(Y, dt, F);
		//---------------------------------------------------------------------------

		E = Y-X;						// The tangent vector after one time step
		// cout<<E<<endl;
		d = Euclidean_measure(E);		// Length of the tangent vector
		// cout<<"d: "<<d<<endl;
		div_rate = log(d/d0);			// Local divergence rate of the trajectories
		le +=div_rate;					// For calculating running average of divergence rate
		E = (d0/d)*E;					// Renormalization of the tangent vector to length d0
		// cout<<E<<endl;
		// time_series_file<<div_rate<<endl;	// Writing local div rate in file
		if (i%1000==0)
		{
			cout<<i<<"\t"<<le/i<<endl;		//  Writing running average on std output
		}
	}
	// time_series_file.close();
	le = le/(T*dt);
	cout<<"Lyapunov exponent: \t"<<le<<endl;		//  Writing running average on std output

	// =========================================================================================
	return le;

}


/*
 * BezierCurve.cpp
 *
 *  Created on: Dec 5, 2008
 *      Author: eddyc
 */

#include <iostream>
#include <assert.h>
#include <vector>
#include <cmath>
#include "Point.h"
#include "Vector.h"
#include "matrix.h"
#include "BezierCurve.h"
#include <boost/foreach.hpp>

using namespace std;
using namespace math;

BezierCurve::BezierCurve(Point p1, Point p2, Point p3, Point p4) {
	this->controls.SetSize(4,4);
	controls(0,0) = p1;
	controls(1,0) = p2;
	controls(2,0) = p3;
	controls(3,0) = p4;
	curve.push_back(p1);
	curve.push_back(p2);
	curve.push_back(p3);
	curve.push_back(p4);
	initMatrices();
}

BezierCurve::BezierCurve(vector<Point> controls) : curve(controls) {
	assert(controls.size() == 4);
	this->controls(0,0) = controls[0];
	this->controls(1,0) = controls[1];
	this->controls(2,0) = controls[2];
	this->controls(3,0) = controls[3];
	initMatrices();
}

BezierCurve::BezierCurve(matrix<Point> c) : controls(c) {
	curve.push_back(c(0,0));
	curve.push_back(c(1,0));
	curve.push_back(c(2,0));
	curve.push_back(c(3,0));
	initMatrices();
}

BezierCurve::~BezierCurve() {

}

void BezierCurve::initMatrices() {
	Sk.SetSize(4,4);
	Sk(0,0) = 1.0;
	Sk(1,0) = Sk(1,1) = Sk(2,1) = 0.5;
	Sk(2,0) = Sk(2,2) = 0.25;
	Sk(3,0) = Sk(3,3) = 1.0 / 8.0;
	Sk(3,1) = Sk(3,2) = 3.0 / 8.0;

	Si.SetSize(4,4);
	Si(0,3) = Si(2,1) = 1.0;
	Si(1,2) = Si(3,0) = -1.0;
	Si(1,3) = 2.0;
	Si(2,2) = -4.0;
	Si(2,3) = 4.0;
	Si(3,1) = 6.0;
	Si(3,2) = -12.0;
	Si(3,3) = 8.0;
}

matrix<Point> BezierCurve::apply(matrix<double> S, matrix<Point> controls) {
	matrix<Point> newControls(4,1);
	newControls(0,0) = S(0,0) * controls(0,0) + S(0,1) * controls(1,0) +
						S(0,2) * controls(2,0) + S(0,3) * controls(3,0);
	newControls(1,0) = S(1,0) * controls(0,0) + S(1,1) * controls(1,0) +
						S(1,2) * controls(2,0) + S(1,3) * controls(3,0);
	newControls(2,0) = S(2,0) * controls(0,0) + S(2,1) * controls(1,0) +
						S(2,2) * controls(2,0) + S(2,3) * controls(3,0);
	newControls(3,0) = S(3,0) * controls(0,0) + S(3,1) * controls(1,0) +
						S(3,2) * controls(2,0) + S(3,3) * controls(3,0);
	return newControls;
}

void BezierCurve::refine(int level) {
	matrix<Point> subdivision = controls;
	curve.clear();

	// get the first subdivision by applying Sk level times
	for (int k = 0; k < level; k++)
		subdivision = apply(Sk, subdivision);
	curve.push_back(subdivision(0,0));
	curve.push_back(subdivision(1,0));
	curve.push_back(subdivision(2,0));
	curve.push_back(subdivision(3,0));

	// get subsequent subdivisions by applying Si 2^level - 1 times
	for (int i = 0; i < (int)pow(2.0,level) - 1; i++) {
		subdivision = apply(Si, subdivision);
		curve.push_back(subdivision(0,0));
		curve.push_back(subdivision(1,0));
		curve.push_back(subdivision(2,0));
		curve.push_back(subdivision(3,0));
	}
}

void BezierCurve::print() {
	BOOST_FOREACH(Point p, curve) {
		cout << p << endl;
	}
}


/**
 * \file RobertBristowJohnsonFilter.cpp
 */

#include <boost/math/constants/constants.hpp>

#include "RobertBristowJohnsonFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  RobertBristowJohnsonLowPassCoefficients<DataType>::RobertBristowJohnsonLowPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - cosw0)/2 / (1 + alpha);
    coefficients_in[1] = (1 - cosw0) / (1 + alpha);
    coefficients_in[0] = (1 - cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonLowPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonHighPassCoefficients<DataType>::RobertBristowJohnsonHighPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_in[1] = -(1 + cosw0) / (1 + alpha);
    coefficients_in[0] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonHighPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassCoefficients<DataType>::RobertBristowJohnsonBandPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = Q * alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = - Q * alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPass2Coefficients<DataType>::RobertBristowJohnsonBandPass2Coefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPass2Coefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = -alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPass2Coefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonBandPass2Coefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandStopCoefficients<DataType>::RobertBristowJohnsonBandStopCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandStopCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = 1 / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandStopCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonBandStopCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonAllPassCoefficients<DataType>::RobertBristowJohnsonAllPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - alpha) / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1;
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonAllPassCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassPeakCoefficients<DataType>::RobertBristowJohnsonBandPassPeakCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + alpha * gain) / (1 + alpha / gain);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha / gain);
    coefficients_in[0] = (1 - alpha * gain) / (1 + alpha / gain);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha / gain);
    coefficients_out[0] = (alpha / gain - 1) / (1 + alpha / gain);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonLowShelvingCoefficients<DataType>::RobertBristowJohnsonLowShelvingCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1), gain(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);
    DataType d = (gain + 1) + (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) - (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = 2 * gain * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = 2 * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonHighShelvingCoefficients<DataType>::RobertBristowJohnsonHighShelvingCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1), gain(1)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);
    DataType d = (gain + 1) - (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) + (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = -2 * gain * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = -2 * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  DataType_ RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template class RobertBristowJohnsonLowPassCoefficients<float>;
  template class RobertBristowJohnsonLowPassCoefficients<double>;
  template class RobertBristowJohnsonHighPassCoefficients<float>;
  template class RobertBristowJohnsonHighPassCoefficients<double>;
  template class RobertBristowJohnsonBandPassCoefficients<float>;
  template class RobertBristowJohnsonBandPassCoefficients<double>;
  template class RobertBristowJohnsonBandPass2Coefficients<float>;
  template class RobertBristowJohnsonBandPass2Coefficients<double>;
  template class RobertBristowJohnsonAllPassCoefficients<float>;
  template class RobertBristowJohnsonAllPassCoefficients<double>;
  template class RobertBristowJohnsonBandStopCoefficients<float>;
  template class RobertBristowJohnsonBandStopCoefficients<double>;
  template class RobertBristowJohnsonBandPassPeakCoefficients<float>;
  template class RobertBristowJohnsonBandPassPeakCoefficients<double>;
  template class RobertBristowJohnsonLowShelvingCoefficients<float>;
  template class RobertBristowJohnsonLowShelvingCoefficients<double>;
  template class RobertBristowJohnsonHighShelvingCoefficients<float>;
  template class RobertBristowJohnsonHighShelvingCoefficients<double>;

  template class IIRFilter<RobertBristowJohnsonLowPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonLowPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonHighPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonHighPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPass2Coefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPass2Coefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandStopCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandStopCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonAllPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonAllPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPassPeakCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPassPeakCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonLowShelvingCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonLowShelvingCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonHighShelvingCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonHighShelvingCoefficients<double> >;
}


/*
 * Code copied from
 * http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
 */

#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.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 <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool invert_matrix (const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {
   typedef ublas::permutation_matrix<std::size_t> pmatrix;

   // create a working copy of the input
   ublas::matrix<T> A(input);

   // create a permutation matrix for the LU-factorization
   pmatrix pm(A.size1());

   // perform LU-factorization
   int res = ublas::lu_factorize(A,pm);
   if( res != 0 ) return false;

   // create identity matrix of "inverse"
   inverse.assign(ublas::identity_matrix<T>(A.size1()));

   // backsubstitute to get the inverse
   ublas::lu_substitute(A, pm, inverse);

   return true;
}
#endif //INVERT_MATRIX_HPP


#include <ipc/utils/eigen_ext.hpp>

#include <stdexcept> // std::runtime_error

#include <Eigen/Eigenvalues>

#include <ipc/utils/logger.hpp>

namespace ipc {

// Matrix Projection onto Positive Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_pd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A,
    double eps)
{
    assert(eps > 0);

    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(error("unable to project matrix onto positive definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] > 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative or zero values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] <= 0.0) {
            D.diagonal()[i] = eps;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

// Matrix Projection onto Positive Semi-Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_psd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A)
{
    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(
            error("unable to project matrix onto positive semi-definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are zero or positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] >= 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] < 0.0) {
            D.diagonal()[i] = 0.0;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

template <typename DerivedA, typename DerivedB, typename Result>
Result cross(
    const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b)
{
    assert(a.size() == 3 && b.size() == 3);
    Result c(a.rows(), a.cols());
    c(0) = a(1) * b(2) - a(2) * b(1);
    c(1) = a(2) * b(0) - a(0) * b(2);
    c(2) = a(0) * b(1) - a(1) * b(0);
    return c;
}

} // namespace ipc