#ifndef EXPSUM_EXPONENTIAL_SUM_HPP
#define EXPSUM_EXPONENTIAL_SUM_HPP

#include <cassert>
#include <iosfwd>
#include <stdexcept>

#include <armadillo>

namespace expsum
{

template <typename ResultT, typename ParamT = ResultT>
class exponential_sum;

//
// Univariate function expressed as an exponential sum
//
template <typename ResultT, typename ParamT>
class exponential_sum
{
public:
    using result_type      = ResultT;
    using argument_type    = typename arma::get_pod_type<ResultT>::result;
    using parameter_type   = ParamT;
    using size_type        = arma::uword;
    using parameter_vector = arma::Col<parameter_type>;

private:
    template <bool>
    struct eval_dispacher
    {
    };

    using eval_default      = eval_dispacher<false>;
    using eval_as_real_func = eval_dispacher<true>;

    parameter_vector exponent_; // exponents
    parameter_vector weight_;   // weights

public:
    constexpr static const bool is_real_function =
        arma::is_real<result_type>::value;

    // Default constructor
    exponential_sum() = default;

    // Create `n` term exponential sum
    exponential_sum(size_type n) : exponent_(n), weight_(n)
    {
    }

    // Create from the vectors of exponents and weights
    template <typename V1, typename V2>
    exponential_sum(
        const V1& exponents, const V2& weights,
        typename std::enable_if<(arma::is_arma_type<V1>::value &&
                                 arma::is_arma_type<V2>::value)>::type* = 0)
        : exponent_(exponents), weight_(weights)
    {
        assert(exponents.is_vec() && weights.is_vec());
        assert(exponents.n_elem == weights.n_elem);
    }

    // Default copy constructor
    exponential_sum(const exponential_sum&) = default;

    // Default move constructor
    exponential_sum(exponential_sum&&) = default;

    // Default destructor
    ~exponential_sum() = default;

    // Default copy assignment operator
    exponential_sum& operator=(const exponential_sum&) = default;

    // Default move assignment operator
    exponential_sum& operator=(exponential_sum&&) = default;

    //
    // Get the number of terms
    //
    size_type size() const
    {
        return exponent_.size();
    }
    //
    // Return true if container is empty
    //
    bool empty() const
    {
        return exponent_.empty();
    }
    //
    // Evaluate value at the given point
    //
    result_type operator()(const argument_type& x) const
    {
        return eval_impl(
            x, eval_dispacher<(arma::is_real<result_type>::value &&
                               arma::is_complex<parameter_type>::value)>());
    }
    //
    // Get const reference to the vector of exponents
    //
    const parameter_vector& exponent() const
    {
        return exponent_;
    }
    //
    // Get const reference to the `i`-th exponent
    //
    const parameter_type& exponent(size_type i) const
    {
        return exponent_(i);
    }
    //
    // Get reference to the `i`-th exponent
    //
    parameter_type& exponent(size_type i)
    {
        return exponent_(i);
    }
    //
    // Get const reference to the vector of weights
    //
    const parameter_vector& weight() const
    {
        return weight_;
    }
    //
    // Get const reference to the `i`-th weight
    //
    const parameter_type& weight(size_type i) const
    {
        return weight_(i);
    }
    //
    // Get reference to the `i`-th weight
    //
    parameter_type& weight(size_type i)
    {
        return weight_(i);
    }
    //
    // Change the container size
    //
    void resize(size_type n)
    {
        exponent_.set_size(n);
        weight_.set_size(n);
    }
    //
    // Set new exponents and weights
    //
    template <typename V1, typename V2>
    typename std::enable_if<(arma::is_arma_type<V1>::value &&
                             arma::is_arma_type<V2>::value)>::type
    set(const V1& new_exponents, const V2& new_weights)
    {
        assert(new_exponents.is_vec() && new_weights.is_vec() &&
               new_exponents.n_elem == new_weights.n_elem);
        exponent_ = new_exponents;
        weight_   = new_weights;
    }
    //
    // Remove terms with small weights
    //
    void remove_small_terms(argument_type tolerance)
    {
        const auto max_weight = arma::max(arma::abs(weight_));
        const auto selected =
            arma::find(arma::abs(weight_) > max_weight * tolerance);
        parameter_vector tmp_e(exponent_(selected));
        parameter_vector tmp_w(weight_(selected));
        exponent_ = std::move(tmp_e);
        weight_   = std::move(tmp_w);
    }
    //
    // Output to ostream
    //
    template <typename Ch, typename Tr>
    void print(std::basic_ostream<Ch, Tr>& os) const
    {
        os << size() << '\n'; // number of terms
        for (size_type i = 0; i < size(); ++i)
        {
            os << exponent(i) << '\t' << weight(i) << '\n';
        }
    }

private:
    result_type eval_impl(const argument_type& x, eval_default) const
    {
        return arma::sum(arma::exp(-x * exponent_) % weight_);
    }

    result_type eval_impl(const argument_type& x, eval_as_real_func) const
    {
        return std::real(arma::sum(arma::exp(-x * exponent_) % weight_));
    }

    // Sort by absolute value of exponent
    void sort()
    {
        arma::uvec index = arma::linspace<arma::uvec>(0, size() - 1, size());
        std::sort(std::begin(index), std::end(index),
                  [this](size_type i, size_type j) {
                      return std::abs(exponent(i)) < std::abs(exponent(j));
                  });
        parameter_vector tmp(exponent_(index));
        exponent_ = tmp;
        tmp       = weight_(index);
        weight_   = std::move(tmp);
    }
};

// Ostream operator
template <typename Ch, typename Tr, typename ResultT, typename ParamT>
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os,
           const exponential_sum<ResultT, ParamT>& fn)
{
    fn.print(os);
    return os;
}

//=============================================================================
// Function multiplication
//=============================================================================
//
// Multiply two exponential sum functions.
//
template <typename T1, typename P1, typename T2, typename P2>
auto multiply(const exponential_sum<T1, P1>& x,
              const exponential_sum<T2, P2>& y)
    -> exponential_sum<decltype(T1() + T2()), decltype(P1() + P2())>
{
    using result_type =
        exponential_sum<decltype(T1() + T2()), decltype(P1() + P2())>;
    using size_type = typename result_type::size_type;

    result_type z(x.size() * y.size());

    size_type k = 0;
    for (size_type j = 0; j < y.size(); ++j)
    {
        for (size_type i = 0; i < x.size(); ++i, ++k)
        {
            z.exponent(k) = x.exponent(i) + y.exponent(j);
            z.weight(k)   = x.weight(i) * y.weight(j);
        }
    }

    return z;
}

} // namespace: expsum

#endif /* EXPSUM_EXPONENTIAL_SUM_HPP */


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

#ifndef HEADER_EXT_VECTOR4_HPP_INCLUDED
#define HEADER_EXT_VECTOR4_HPP_INCLUDED

#include <cmath>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Vector4 :
		boost::equality_comparable<Vector4<T>,
		boost::additive<Vector4<T>,
		boost::multiplicative<Vector4<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x;
		Type y;
		Type z;
		Type w;

		Vector4() : x{0}, y{0}, z{0}, w{0} {}
		Vector4(Type x_, Type y_, Type z_, Type w_) : x{x_}, y{y_}, z{z_}, w{w_} {}
		template <typename U>
		Vector4(Vector4<U> const& v) : x{v.x}, y{v.y}, z{v.z}, w{v.w} {}
	};

	using Vector4f = Vector4<float>;
	using Vector4d = Vector4<double>;
	using Vector4ld = Vector4<long double>;

	template <typename T>
	bool operator==(Vector4<T> const& v1, Vector4<T> const& v2)
	{
		return v1.x == v2.x && v1.y == v2.y && v1.z == v2.z && v1.w == v2.w;
	}

	template <typename T>
	Vector4<T>& operator+=(Vector4<T>& v1, Vector4<T> const& v2)
	{
		v1.x += v2.x;
		v1.y += v2.y;
		v1.z += v2.z;
		v1.w += v2.w;
		return v1;
	}

	template <typename T>
	Vector4<T>& operator-=(Vector4<T>& v1, Vector4<T> const& v2)
	{
		v1.x -= v2.x;
		v1.y -= v2.y;
		v1.z -= v2.z;
		v1.w -= v2.w;
		return v1;
	}

	template <typename T>
	Vector4<T>& operator*=(Vector4<T>& v, T t)
	{
		v.x *= t;
		v.y *= t;
		v.z *= t;
		v.w *= t;
		return v;
	}

	template <typename T>
	Vector4<T>& operator/=(Vector4<T>& v, T t)
	{
		v.x /= t;
		v.y /= t;
		v.z /= t;
		v.w /= t;
		return v;
	}

	template <typename T>
	Vector4<T> operator+(Vector4<T> const& v)
	{
		return v;
	}

	template <typename T>
	Vector4<T> operator-(Vector4<T> const& v)
	{
		return {-v.x, -v.y, -v.z, -v.w};
	}

	template <typename T>
	T dot(Vector4<T> const& v1, Vector4<T> const& v2)
	{
		return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
	}

	template <typename T>
	T norm(Vector4<T> const& v)
	{
		return std::sqrt(dot(v, v));
	}

	template <typename T>
	Vector4<T> normalize(Vector4<T> const& v)
	{
		return v / norm(v);
	}

} // namespace ext

#endif // !HEADER_EXT_VECTOR4_HPP_INCLUDED


#include <tansa/algorithm.h>

#include <vector>
#include <iostream>
#include <cfloat>

#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;

namespace tansa {

void RigidPointCorrespondenceSolver::arrange(const vector<Vector3d> &bs, const vector<int> &c, vector<Vector3d> *out) {
	out->resize(c.size());
	for(unsigned i = 0; i < c.size(); i++) {
		(*out)[i] = bs[c[i]];
	}
}

bool RigidPointCorrespondenceSolver::solve(const vector<Vector3d> &as, const vector<Vector3d> &bs, vector<int> *c, bool ideal) {

	if(as.size() != bs.size() && ideal) {
		printf("Both sets must be complete!\n");
		return false;
	}

	/* Compute set centers */
	Vector3d cA = centroid(as), cB = centroid(bs);

	MatrixXd Ac(as.size(), 3), Bc(bs.size(), 3);
	for(unsigned i = 0; i < as.size(); i++){
		Ac.block<1,3>(i, 0) = as[i] - cA; // TODO: Don't these need to be tranposed
		Bc.block<1,3>(i, 0) = bs[i] - cB;
	}

	// Get the eigenvalues and eigenvectors of the scatter matrices
	Vector3d dA, dB;
	MatrixXd Qa(as.size(), 3), Qb(bs.size(), 3);
	decompose_eigenstructure(Ac * Ac.transpose(), &dA, &Qa);
	decompose_eigenstructure(Bc * Bc.transpose(), &dB, &Qb);

	// There will potentially be sign inconsistency in the eigenvectors
	// Therefore we must consider all possible permutations on the signs of one of the eigenvectors
	// We put the permutation that results in the lowest error
	// TODO: If we wanted a quick and 'ok' solution, we could take the absolute value of both eigenvector sets and simplify this to only one sign iteration
	// TODO: Ratio test best and second best matches
	double minCost = DBL_MAX;
	vector<int> minCorresp;
	vector<int> corresp(as.size(), -1);
	for(unsigned it = 0; it < 8; it++) {
		RowVector3d signA(it & 0b001? 1 : -1, it & 0b010? 1 : -1, it & 0b100? 1 : -1);
		cout << signA << endl;

		// Make a copy of Qa with the currently proposed signs
		MatrixXd Qai(Qa.rows(), Qa.cols());
		for(unsigned i = 0; i < Qa.rows(); i++) {
			Qai.row(i) = Qa.row(i).cwiseProduct(signA);
		}

		double cost = ideal? solve_ideal(Qai, Qb, &corresp) : solve_general(dA, Qai, dB, Qb, &corresp);
		if(cost < minCost) {
			minCorresp = corresp;
			minCost = cost;
		}
	}

	*c = minCorresp;
	return true;
}

void RigidPointCorrespondenceSolver::decompose_eigenstructure(const MatrixXd &M, Vector3d *d, MatrixXd *Q) {
	// Gets the eigenvalues/vectors in increasing order
	SelfAdjointEigenSolver<MatrixXd> eig(M);

	MatrixXd vecs = eig.eigenvectors();
	VectorXd vals = eig.eigenvalues();

	// We assume that we are given one of the scatter matrices which will definitely be positive semi-definite (so no negative eigenvalues)
	// Take last three values/vectors
	for(unsigned i = 0; i < 3; i++) {
		unsigned idx = vecs.cols() - i - 1;
		Q->col(i) = vecs.col(idx);
		(*d)(i) = vals(idx);
	}
}

double RigidPointCorrespondenceSolver::solve_ideal(const MatrixXd &Qa, const MatrixXd &Qb, std::vector<int> *c) {

// TODO:
	// MatrixXd P = Qa * Qb.transpose(); // eigA.eigenvectors() * eigB.eigenvectors().transpose();

	// 'Feature vector' are the columns of these
	//cout << Qa.transpose() << endl << endl;
	//cout << Qb.transpose() << endl << endl;

	// Determine column permutation matrix based on best matches
	//MatrixXd P = MatrixXd::Zero(as.size(), bs.size());
	// TODO: What if two i indices have the same best
	double cost = 0;
	for(unsigned i = 0; i < Qa.rows(); i++) {

		double bestE = DBL_MAX;
		int bestJ = 0;

		for(unsigned j = 0; j < Qb.rows(); j++) {
			double e = (Qa.block<1,3>(i, 0) - Qb.block<1,3>(j, 0)).squaredNorm();

			if(e < bestE) {
				bestE = e;
				bestJ = j;
			}
		}

		(*c)[i] = bestJ;
		cost += bestE;

		//P(bestJ, i) = 1;
	}

	//if(P.rank() != as.size()) {
	//	// the matches were probably not sufficiently distinct
	//	// TODO: Ratio test?
	//
	//		cout << "failed" << endl;
	//	}

	return cost;
}

double RigidPointCorrespondenceSolver::solve_general(const Vector3d &dA, const MatrixXd &Qa, const Vector3d &dB, const MatrixXd &Qb, std::vector<int> *c) {

	MatrixXd h(Qa.rows(), Qb.rows()); // Affinity matrix

	for(int i = 0; i < h.rows(); i++) {
		for(int j = 0; j < h.cols(); j++) {

			double s = 0;
			for(int k = 0; k < 3; k++) {
				// Eigenvalues corresponding to the vectors
				double dAk = dA(k),
					   dBk = dB(k);

				double pik = Qa(i, k),
					   qjk = Qb(j, k);

				s += dAk * dBk * pow(abs(pik - qjk), 2);
			}

			// We do not need the negative sign as in the paper if the assignment algorithm is minimizing
			h(i, j) = s; // -s;
		}
	}

	AssignmentSolver as;
	return as.solve(h, c);

	// TODO: Also implement the outlier rejection method
}

}


#pragma once
#include <armadillo>

using namespace arma;

namespace TimeSeries {

constexpr double pmSd = 3.0;
constexpr double minPr = 1.0E-4;

struct DiscreteRV {
  vec support;
  vec probabilities;
};

struct MvNormal {
  vec mean;
  mat varCovar;
};

/*
 * Normal helpers
 */
DiscreteRV
discretizeNormal(const double mean,
    const double sigma,
    const uword n_elem = 21,
    const double pmSigma = pmSd,
    bool cutTails = true);
vec discretizeNormal(vec grid,
    const double mean,
    const double sigma,
    bool cutTails = true);

/*
 *
 * AR(1) process
 *
 */
class AR {
  public:
  inline double
  intercept() const
  {
    return m_intercept;
  }
  inline double
  rho() const
  {
    return m_rho;
  }
  inline double
  sigma() const
  {
    return m_sigma;
  }
  inline double
  stationaryMean() const
  {
    return m_intercept / (1.0 - m_rho);
  }
  inline double
  stationarySigma() const
  {
    return m_sigma / std::sqrt(1.0 - m_rho * m_rho);
  }
  inline bool
  stationaryQ() const
  {
    return std::abs(m_rho) < 1.0;
  }

  AR(double intercept, double rho, double sigma)
      : m_intercept(intercept)
      , m_rho(rho)
      , m_sigma(sigma)
  {
  }

  private:
  double m_intercept;
  double m_rho;
  double m_sigma;
};

/*
 *
 * VAR(1) process
 *
 */
class VAR {
  public:
  inline uword
  size() const
  {
    return m_size;
  }
  inline const vec&
  intercept() const
  {
    return m_intercept;
  }
  inline const mat&
  rho() const
  {
    return m_rho;
  }
  inline const mat&
  sigma() const
  {
    return m_sigma;
  }

  const MvNormal conditional(vec& current) const;
  vec& stationaryMean();
  mat& stationarySigma();
  bool stationaryQ();
  void print();

  VAR(vec intercept, mat rho, mat sigma)
      : m_intercept(intercept)
      , m_rho(rho)
      , m_sigma(sigma)
  {
    m_size = intercept.n_elem;
  }

  private:
  uword m_size;
  vec m_intercept;
  mat m_rho;
  mat m_sigma;

  vec m_statMean;
  mat m_statSigma;
  bool is_stationary;

  void findStationary();
};

/*
 *
 * Orthogonalized VAR(1) and "rotation" matirx
 *
 */
enum OrthoMethod { //
  SVD,
  Cholesky
};

class OrthogonalizedVAR {
  public:
  inline const VAR&
  getVAR() const
  {
    return m_ortho;
  }
  inline const VAR&
  getOriginalVAR() const
  {
    return m_original;
  }
  inline const mat&
  getSupportRotationMatrix() const
  {
    return LL;
  }

  OrthogonalizedVAR(VAR original, OrthoMethod method = OrthoMethod::Cholesky)
      : m_original(original)
      , m_ortho(original)
  {
    const mat mEye = eye(original.size(), original.size());
    mat Atilde = original.intercept();
    mat Btilde = original.rho();
    mat GG = original.sigma();
    vec DD = ones(original.size());
    mat diagDD = mEye;
    LL = mEye;

    switch (method) {
    case OrthoMethod::SVD: {
      mat VV;
      svd(LL, DD, VV, GG);

      Atilde = LL.t() * original.intercept();
      Btilde = LL.t() * original.rho() * LL;
      diagDD = diagmat(DD);
      break;
    }
    case OrthoMethod::Cholesky: {
      LL = chol(GG, "lower");
      mat invLL = inv(LL);

      Atilde = invLL * original.intercept();
      Btilde = invLL * original.rho() * LL;
      break;
    }
    default: {
      cout << "Unknown method." << endl;
      exit(1);
    }
    }

    m_ortho = VAR(Atilde, Btilde, diagDD);
  }

  private:
  VAR m_original;
  VAR m_ortho;
  mat LL;
};

/*
 *
 * Markov Chain
 *
 */
rowvec
stationaryDistribution(const mat& transitionMatrix);

uvec simulateChain(const mat& transitionMatrix,
    const uword initState,
    const uword simSz);

class MarkovChain {
  public:
  inline uword
  size() const
  {
    return m_size;
  }

  inline rowvec&
  support()
  {
    return m_support;
  }
  inline const rowvec&
  support() const
  {
    return m_support;
  }

  inline mat&
  transition()
  {
    return m_tran;
  }
  inline const mat&
  transition() const
  {
    return m_tran;
  }

  void save(std::string fname) const;
  void print() const;

  const rowvec& stationary();

  MarkovChain() { }
  MarkovChain(rowvec support, mat transition)
      : m_support(support)
      , m_tran(transition)
  {
    m_size = support.n_elem;
  }
  MarkovChain(const AR process,
      const uword sz = 21,
      const double pmMCsd = pmSd,
      bool expSupport = false);

  private:
  uword m_size;
  rowvec m_support;
  rowvec m_stationary;
  mat m_tran;
};

/*
 *
 * Discrete approximation of VAR(1)
 *
 */
class DiscreteVAR {
  public:
  inline uword
  size() const
  {
    return m_size;
  }
  inline uword
  flatSize() const
  {
    return m_flatSize;
  }
  inline uword
  midIx() const
  {
    return m_midIx;
  }
  inline const vec
  grid(uword varIx) const
  {
    return m_grids.col(varIx);
  }
  inline const rowvec
  gridValues(uword ix) const
  {
    return m_grids.row(ix);
  }
  inline const mat
  transition() const
  {
    return m_tran;
  }
  void save(std::string fname) const;
  void print() const;

  DiscreteVAR(const VAR var,
      const uword supportSize,
      bool trimGrids = true,
      OrthoMethod mm = OrthoMethod::Cholesky)
      : m_var(var)
  {
    m_supportSizes.set_size(m_var.size());
    m_supportSizes.fill(supportSize);
    impl(trimGrids, mm);
  }

  DiscreteVAR(const VAR var,
      const uvec gridSizes,
      bool trimGrids = true,
      OrthoMethod mm = OrthoMethod::Cholesky)
      : m_var(var)
      , m_supportSizes(gridSizes)
  {
    impl(trimGrids, mm);
  }

  private:
  VAR m_var;

  uword m_size;
  uword m_flatSize;
  uword m_midIx;

  mat m_grids;
  uvec m_supportSizes;
  mat m_tran;

  void impl(bool trimGrids, OrthoMethod mm);
};

/*
 *
 * Discrete approximation to VAR(1) with common AR(1) stochastic volatility,
 * Basal-Yaron-like.
 *
 */
class DiscreteStochVolVAR {
  public:
  inline uword
  size() const
  {
    return m_size;
  }
  inline uword
  flatSize() const
  {
    return m_flatSize;
  }
  inline uword
  midIx() const
  {
    return m_midIx;
  }
  inline const mat
  transition() const
  {
    return m_tran;
  }
  inline const vec
  grid(uword iIx) const
  {
    return m_grids.col(iIx);
  }
  inline const rowvec
  gridValues(uword ix) const
  {
    return m_grids.row(ix);
  }
  void save(std::string fname) const;
  void print() const;

  DiscreteStochVolVAR(const VAR var,
      const AR vol,
      const uvec gridSizes,
      const uword volGridSize,
      bool trimGrids = true)
      : m_var(var)
      , m_vol(vol)
      , m_supportSizes(gridSizes)
      , m_volGridSize(volGridSize)
  {
    impl(trimGrids);
  };

  private:
  VAR m_var;
  AR m_vol;

  uword m_size;
  uword m_flatSize;
  uword m_midIx;

  mat m_grids;
  mat m_tran;
  uvec m_supportSizes;
  uword m_volGridSize;

  void impl(bool trimGrids);
};

void trimMarkovChain(mat& grids, mat& transition);

} // namespace TimeSeries


/*Copyright (c) 2020 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <vector>
#include <math.h>
#include "overlap.h"
#include <cstdlib>
#include "utils.h"
#include "BasisSet.h"
#include "gto_ordering.h"
#include <Eigen/Dense>

//overlap between pair of gaussians
double gauss_integral(double exp_a,std::array<size_t, 3> cart_a,std::array<double,3> coord_a,
		double exp_b,std::array<size_t, 3> cart_b, std::array<double,3> coord_b)
{
	std::vector<double> integrals;
	for(int i=0;i<3;i++)
	{
		double dist = coord_a[i]-coord_b[i];
		double res = mcmurchie_davidson(exp_a,exp_b,0,cart_a[i],cart_b[i],dist);
		integrals.push_back(res);
	}
	double int_tot = 1.0;
	for (unsigned int i=0;i<integrals.size();i++)
	{
		int_tot*=integrals[i];
	}
	return int_tot*pow(M_PIl/(exp_a+exp_b),1.5);
}

//recursive formula for cartesian overlap distributions
double mcmurchie_davidson(double exp_a, double exp_b, int t, int angmom_a, int angmom_b, double dist)
{
	double p = exp_a + exp_b;
	double q = exp_a*exp_b/p;
	if (t<0 || t>(angmom_a+angmom_b))
	{
		return 0.0;
	}
	else if(angmom_a==angmom_b && angmom_b==0)
	{
		return exp(-q*dist*dist);
	}
	else if(angmom_b==0)
	{
		//decrement angmom_a
		return(
				(1/(2*p))*mcmurchie_davidson(exp_a,exp_b,t-1,angmom_a-1,angmom_b,dist)   -
				(q*dist/exp_a)*mcmurchie_davidson(exp_a,exp_b,t,angmom_a-1,angmom_b,dist) +
				(t+1)*mcmurchie_davidson(exp_a,exp_b,t+1,angmom_a-1,angmom_b,dist));
	}
	else
	{
		//decrement angmom_b
		return(
				(1/(2*p))*mcmurchie_davidson(exp_a,exp_b,t-1,angmom_a,angmom_b-1,dist)+
				(q*dist/exp_b)*mcmurchie_davidson(exp_a,exp_b,t,angmom_a,angmom_b-1,dist)+
				(t+1)*mcmurchie_davidson(exp_a,exp_b,t+1,angmom_a,angmom_b-1,dist));
	}
}

//overlap between two normalized, contracted gaussians
double overlap_integral(Shell a, std::array<size_t,3> cart_a, Shell b,
		std::array<size_t,3> cart_b)
{
	double res = 0.0;
	for(size_t i=0;i<a.num_prims;i++)
	{
		for(size_t j=0;j<b.num_prims;j++)
		{
			res+=a.coeffs[i]*b.coeffs[j]*
			gauss_integral(a.exps[i],cart_a,a.origin,b.exps[j],cart_b,b.origin);
		}
	}
	return res;
}

Eigen::MatrixXd shell_overlap(Shell shell_a, Shell shell_b)
{
	Eigen::MatrixXd block(shell_a.num_carts(),shell_b.num_carts());
	std::vector<std::array<size_t,3>> order_a = opencap_carts_ordering(shell_a.l);
	std::vector<std::array<size_t,3>> order_b = opencap_carts_ordering(shell_b.l);
	for(size_t i=0;i<shell_a.num_carts();i++)
	{
		std::array<size_t,3> a_cart = order_a[i];
		for(size_t j=0;j<shell_b.num_carts();j++)
		{
			std::array<size_t,3> b_cart = order_b[j];
			block(i,j) = overlap_integral(shell_a,a_cart, shell_b, b_cart);
		}
	}
	return block;
}

void compute_analytical_overlap(BasisSet bs, Eigen::MatrixXd &Smat)
{
	unsigned int row_idx = 0;
	for(auto&shell1:bs.basis)
	{
		unsigned int col_idx = 0;
		for(auto&shell2: bs.basis)
		{
			Smat.block(row_idx,col_idx,shell1.num_carts(),shell2.num_carts()) = shell_overlap(shell1,shell2);
            col_idx += shell2.num_carts();
		}
		row_idx += shell1.num_carts();
	}
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Example of using Bayesian Filter Class to solve a simple problem.
 *  A Non-linear filter with one state and constant noises
 */

// Use the Unscented filter scheme from Bayes++
#include "BayesFilter/unsFlt.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;

/*
 * Simple Prediction model
 */
class Simple_predict : public Linear_predict_model
{
public:
	Simple_predict() : Linear_predict_model(1,1)
	// Construct a constant model
	{
				// Stationary Prediction model (Identity)
		Fx(0,0) = 1.;
				// Constant Noise model with a large variance
		q[0] = 2.;
		G(0,0) = 1.;
	}
};

/*
 * Nonlinear Observation model
 * Observation is distance from origin squared
 */
class Nonlinear_observe : public Uncorrelated_additive_observe_model
{
public:
	Nonlinear_observe () : Uncorrelated_additive_observe_model(1), hx(1)
	// Construct a noise model
	{
				// Constant Observation Noise model with variance of one
		Zv[0] = 1.;
	}
	const FM::Vec& h(const FM::Vec& x) const
	{
		hx[0] = x[0]*x[0];
		return hx;
	}
	private:
		mutable FM::Vec hx;
};

/*
 * Filter a simple example
 */
int main()
{
	// Global setup for test output
	cout.flags(ios::fixed); cout.precision(4);

	// Construct simple Prediction and Observation models
	Simple_predict my_predict;
	Nonlinear_observe my_observe;

	// Use an 'Unscented' filter scheme with one state
	Unscented_scheme my_filter(1);

	// Setup the initial state and covariance
	Vec x_init (1); SymMatrix X_init (1, 1);
	x_init[0] = 10.;		// Start at 10 with no uncertainty
	X_init(0,0) = 0.;
	my_filter.init_kalman (x_init, X_init);

	cout << "Initial  " << my_filter.x << my_filter.X << endl;

	// Predict the filter forward
	my_filter.predict (my_predict);
	my_filter.update();		// Update the filter, so state and covariance are available

	cout << "Predict  " << my_filter.x << my_filter.X << endl;

	// Make an observation
	Vec z(1);
	z[0] = 11.;				// Observe that we should be at 11
	my_filter.observe (my_observe, z);
	my_filter.update();		// Update the filter to state and covariance are available

	cout << "Filtered " << my_filter.x << my_filter.X << endl;
	return 0;
}


#include <state_estimation/filters/kalman_filter.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void KalmanFilter::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    const Eigen::VectorXd Ax = system_model_->A() * filter_state_.x;
    const Eigen::VectorXd Bu = system_model_->B() * u;
    filter_state_.x = system_model_->addVectors(Ax, Bu);

    filter_state_.covariance =
        system_model_->A() * filter_state_.covariance * system_model_->A().transpose() +
        system_model_->P() * system_model_->Rp() * system_model_->P().transpose() +
        system_model_->V() * system_model_->Rc() * system_model_->V().transpose();

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF predicition update:" << std::endl
              << "A=" << std::endl
              << printMatrix(system_model_->A()) << std::endl
              << "B=" << std::endl
              << printMatrix(system_model_->B()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << filter_state_.x.transpose() << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void KalmanFilter::myCorrect(const Eigen::VectorXd& z,
                             measurement_models::LinearMeasurementModel* model, double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_C_T = filter_state_.covariance * model->C().transpose();
    const Eigen::MatrixXd K = cov_C_T * (model->C() * cov_C_T + model->covariance()).inverse();

    // Update the state and covariance with the measurement
    const Eigen::MatrixXd I =
        Eigen::MatrixXd::Identity(filter_state_.x.rows(), filter_state_.x.rows());
    const Eigen::VectorXd z_pred = model->C() * filter_state_.x;
    const Eigen::VectorXd dx = K * model->subtractVectors(z, z_pred);
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx);
    filter_state_.covariance = (I - K * model->C()) * filter_state_.covariance;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF measurement update:" << std::endl
              << "C=" << std::endl
              << printMatrix(model->C()) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


#include "helper/bearing_vector.h"

#include <Eigen/Core>
#include <Eigen/LU>

void create_bearing_vectors(const Mat33_t& rot_1, const Vec3_t& trans_1, const Mat33_t& rot_2, const Vec3_t& trans_2,
                            const eigen_alloc_vector<Vec3_t>& landmarks, Mat33_t& E_21,
                            eigen_alloc_vector<Vec3_t>& bearings_1, eigen_alloc_vector<Vec3_t>& bearings_2,
                            double noise_stddev) {
    const auto num_landmarks = landmarks.size();

    // E行列を求める

    const Mat33_t rot_21 = rot_2 * rot_1.inverse();
    const Vec3_t trans_21 = trans_2 - rot_21 * trans_1;

    Mat33_t skew_21;
    skew_21 << 0, -trans_21(2), trans_21(1),
               trans_21(2), 0, -trans_21(0),
               -trans_21(1), trans_21(0), 0;

    E_21 = skew_21 * rot_21;

    // bearing vectorsを求める

    bearings_1.resize(num_landmarks);
    bearings_2.resize(num_landmarks);
    for (unsigned int i = 0; i < num_landmarks; ++i) {
        const Vec3_t& lm = landmarks.at(i);

        const Vec3_t lm_in_1 = rot_1 * lm + trans_1;
        const Vec3_t lm_in_2 = rot_2 * lm + trans_2;

        bearings_1.at(i) = lm_in_1.normalized();
        bearings_2.at(i) = lm_in_2.normalized();
    }

    if (noise_stddev == 0.0) {
        return;
    }

    // ノイズを追加

    std::random_device rand_dev;
    std::mt19937 mt(rand_dev());
    std::normal_distribution<> rand(0, noise_stddev);

    for (unsigned int i = 0; i < num_landmarks; ++i) {
        bearings_1.at(i) += Vec3_t{rand(mt), rand(mt), rand(mt)};
        bearings_1.at(i).normalized();
        bearings_2.at(i) += Vec3_t{rand(mt), rand(mt), rand(mt)};
        bearings_2.at(i).normalized();
    }
}


/**
 *@file main.cpp
 */

#include <fstream>
#include <armadillo>
#include "HermitePolynomials.h"
#include "Solution1DHO.h"
#include "Orthonormality.h"

/**
 * @mainpage Calcul de l'équation schrödinger d'oscillateur harmonique à une dimension
 *
 * # Equation de Schrödinger
 * \f[\left(\frac{\hat{p}_{(z)}^2}{2m} + \frac{1}{2}m\omega^2\hat{z}^2\right)\psi_n = E_n\psi_n.\f]
 *
 * # Solution forme de la fonction d'onde
 * \f[\psi_n(z) = \frac{1}{\sqrt{2^n n!}}\left(\frac{m\omega}{\pi\hbar}\right)^{1/4}e^{-\frac{m\omega z^2}{2\hbar}}H_n\left(\sqrt{\frac{m\omega}{\hbar}} . z\right).\f]
 */

/**
 *La fonction principale
 *
 *Cette fonction a pour but de tester.
 *
 *@return la fonction toujours retourne 0.
 */

int main()
{
    int i,j;
    int a=-4, b=4, N=100;
    int n=0;

    Solution1DHO ex = Solution1DHO();

    arma::mat A = arma::linspace(-4,4,100);
    std::cout << "Tracer la fonction d\'onde pour n = " << "[" << a << " ," << b << "] pour N =" << N << std::endl<< std::endl;
    ex.solutionToFile(0, A.t());
    ex.drawSolution();

    n = 4;
    std::cout.precision(50);
    for (i = 1; i<n; i++)
    {
        for (j = 1; j<n; j++)
        {
            std::cout << "Test Orthonormality n = " << i << " m = " << j << " quadrature: " << Orthonormality::quadrature(i, j) << std::endl;
        }
    }
    std::cout << std::endl << std::endl;

    N = 1000000;
    n = 2;
    for (i = 0; i<=n; i++)
    {

        std::cout<< "La plage de valeur z: (-2,2) n: "<< i << std::endl;
        std::cout<< "Nombre d'échantillons: N = 1000000" << std::endl;
        std::cout<< "E" << i << " (1-5): ";
        arma::mat res;
        res = ex.energy(i, -2, 2, N);
        std::cout<< res.rows(0,4) << std::endl;

        std::cout<<  "E" << i << " (500100-500104): " ;
        std::cout<< res.rows(500099,500103) << std::endl;

        std::cout<< "E" << i << " (999995-1000000): " ;
        std::cout<< res.rows(999994,999999) << std::endl;

    }

    return 0;
}


#include "util.hpp"
#include <armadillo>

namespace util{
  /*
    For a discrete probability distribution Pi = p(i), returns the index
    of an element randomly selected by sampling the distribution.

    Examples:
    * sample_discrete({0.5, 0.5}) returns 0 or 1 with probability 0.5
    * sample_discrete({0.0, 0.0, 1.0}) returns 2 with probability 1.0

    Note:
    While there does exist std::discrete_distribution, I think that the
    below implementation is superior in that it does not require
    instantiating a new distribution for every round.
  */
  arma::uword sample_discrete(const arma::vec &p){
    /*
      Sanity checks; require:
      * Pi >= 0 forall i
      * Sum(Pi) == 1
      */
    std::string die = "";
    const double eps = 1e-12;
    if (p.has_nan()){die = "P is malformed; it contains NaN!";}
    if (arma::any(p < -1 * eps)){die = "P cannot have negative elements!";}
    if (std::abs(arma::sum(p) - 1) > eps){ die = "P is not normed!";}

    if (die != ""){
      p.t().print("P");
      throw std::logic_error(die);
    }

    const double zeta = arma::randu();
    return as_scalar(arma::find(arma::cumsum(p) > zeta, 1));
  }

  // return a uvec with filled with the indices [a,b) like Python's
  // range(); contrast arma::span(a,b) -> [a,b]
  arma::uvec range(arma::uword a, arma::uword b){
    if (a > b){
      throw std::logic_error("cannot compute range with a > b");
    }

    arma::uword N = b - a;
    arma::uvec R(N, arma::fill::zeros);
    for (arma::uword i = 0; i < N; i++){
      R(i) = i + a;
    }

    return R;
  }

  // variant for [0, n)
  arma::uvec range(arma::uword n){ return range(0, n); }

  /*
    std::hypot() for complex<double>

    returns sqrt(|a|^2 + |b|^2)

    I'm not sure what is the "best" sequence for the real() operations.
    The present choice---to call real() twice, once after each
    multiplication---was made because that's the first time the
    operands mathematically lie on the real line. Waiting longer seemed
    to invite the accumulation of small numerical deviations.
  */
  double hypot(std::complex<double> a, std::complex<double> b){
    return std::sqrt(std::real(std::conj(a)*a) + std::real(std::conj(b)*b) );
  }

  arma::vec center_of_mass(const arma::mat &R, const arma::vec &m){
    // R.B.: sum(A,1) gives the sum of the elements of each row of A
    return arma::sum(R.each_row() % m.t(), 1) / arma::sum(m);
  }

  /*
    For the (3xN) configuration space vectors A, B, compute the sum of
    the cross products of the vectors in each column: Sum(Ai X Bi)
  */
  arma::vec sum_cross(const arma::mat &A, const arma::mat &B){
    if ((A.n_cols != B.n_cols) || (3 != A.n_rows) || (3 != B.n_rows)){
      throw std::logic_error("Invalid dimension!");
    }

    arma::vec sum(3, arma::fill::zeros);

    for (arma::uword i = 0; i < A.n_cols; i++){
      sum += arma::cross(A.col(i), B.col(i));
    }

    return sum;
  }

  /*
    Implements Algorithm 3 from:
    Loring, T. A. Computing a logarithm of a unitary matrix with
    general spectrum. Numer. Linear Algebr. Appl. 2014, 21, 744−760

    It is required that the input matrix be approximately unitary.
  */
  arma::mat logmat_unitary(const arma::mat &U){
    return arma::real(logmat_unitary(
      arma::cx_mat(U, arma::zeros(arma::size(U)))
                                     ));
  }

  arma::cx_mat logmat_unitary(const arma::cx_mat &U){
    arma::cx_mat Q,T;
    if ((arma::norm(U.t() * U - arma::eye(arma::size(U))) > 0.75) ||
        !arma::schur(Q, T, U)){
      std::cerr << "Matrix deviates too much from unitary "
        "or Schur Decomposition failed; "
        "falling back to arma::logmat()" << std::endl;
      return arma::logmat(U);
    }

    arma::cx_mat D(arma::size(U), arma::fill::zeros);
    D.diag() = arma::log(T.diag() / arma::abs(T.diag()));
    arma::cx_mat L = Q * D * Q.t();

    return L;
  }

  /*
    Replace U with the closest unitary matrix according to the
    transformation U' = (UU^T)^(-1/2)U.

    For a thorough discussion, see:
    https://en.wikipedia.org/wiki/Kabsch_algorithm
    https://en.wikipedia.org/wiki/Polar_decomposition
  */
  void unitarize(arma::mat &U){
    // arma::cx_mat R; arma::cx_vec s;
    // arma::eig_gen(s, R, U*U.t());
    // U = arma::real(R * diagmat(arma::pow(s, -0.5)) * R.t()) * U;

    /*
      These algorithms are equivalent but the below has better numerical
      stability properties; namely the above returns 2*I for the
      identity (I) rather than I itself.
    */

    arma::mat W,V; arma::vec s;
    arma::svd(W,s,V,U);
    U = W*V.t();
  }
}


#include <iostream>
#include <cassert>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <CGAL/Triangulation_data_structure_2.h>
#include <boost/pending/disjoint_sets.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> Vb;
typedef CGAL::Triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Triangulation;

// Note: the order that patterns evaluated in is important.
// I don't fully understand why this specific order should work though.
const std::vector<std::vector<std::vector<int>>> patterns_by_k = {
    {},
    {},
    {{0, 2}},
    {{0, 1, 1}, {0, 3}, {0, 0, 2}},
    {{0, 1, 0, 1}, {0, 2, 1}, {0, 4}, {0, 0, 2}, {0, 0, 1, 1}, {0, 0, 0, 2}},
};

std::vector<int> determine_component_sizes(const Triangulation &triangulation, double s)
{
  const int n = triangulation.number_of_vertices();

  boost::disjoint_sets_with_storage<> disjoint_sets(n);
  for (auto it = triangulation.finite_edges_begin(); it != triangulation.finite_edges_end(); it++)
  {
    if (triangulation.segment(it).squared_length() < s)
    {
      int i1 = it->first->vertex((it->second + 1) % 3)->info();
      int i2 = it->first->vertex((it->second + 2) % 3)->info();
      disjoint_sets.union_set(i1, i2);
    }
  }

  std::vector<int> component_sizes(n, 0);
  for (int i = 0; i < n; i++)
  {
    component_sizes.at(disjoint_sets.find_set(i))++;
  }

  return component_sizes;
}

int count_max_families(const std::vector<int> &component_sizes, int k)
{
  const int max_counted_size = 4;
  assert(k <= max_counted_size);
  assert(k >= 1 && k <= 4);

  std::vector<int> counts_by_size(max_counted_size + 1);
  for (const int size : component_sizes)
  {
    counts_by_size.at(std::min(size, max_counted_size))++;
  }

  for (int i = 0; i <= max_counted_size; i++)
  {
    DEBUG(3, "counts_by_size.at(" << i << ") = " << counts_by_size.at(i));
  }

  int f = 0;
  for (int i = k; i <= max_counted_size; i++)
  {
    f += counts_by_size.at(i);
    counts_by_size.at(i) = 0;
  }

  for (const std::vector<int> &pattern : patterns_by_k.at(k))
  {
    int matches = std::numeric_limits<int>::max();
    for (int i = 0; i < int(pattern.size()); i++)
    {
      int required = pattern.at(i);
      assert(required >= 0);
      if (required > 0)
      {
        matches = std::min(matches, counts_by_size.at(i) / required);
      }
    }

    if (matches > 0)
    {
      for (int i = 0; i < int(pattern.size()); i++)
      {
        const int used_count = pattern.at(i) * matches;
        assert(counts_by_size.at(i) >= used_count);
        counts_by_size.at(i) -= used_count;
      }
      f += matches;
    }
  }

  DEBUG(3, "f " << f);

  return f;
}

long find_upper_bound(std::function<bool(long)> is_match)
{
  assert(is_match(0));
  int base = 0;
  while (is_match(1L << base))
  {
    base++;
  }

  long low = base == 0 ? 0 : 1L << (base - 1);
  long high = 1L << base;
  while (low < high)
  {
    long mid = (low + high) / 2;
    if (is_match(mid))
    {
      low = mid + 1;
    }
    else
    {
      high = mid;
    }
  }

  assert(is_match(low - 1) && !is_match(low));
  return low - 1;
}

void testcase()
{
  int n, k, f_0;
  double s_0;
  std::cin >> n >> k >> f_0 >> s_0;
  assert(n >= 2 && n <= 9e4 && k >= 1 && k <= 4 && f_0 >= 2 && k * f_0 <= n && s_0 >= 0 && s_0 <= (1L << 50));

  std::vector<std::pair<K::Point_2, int>> tent_locations;
  for (int i = 0; i < n; i++)
  {
    double x, y;
    std::cin >> x >> y;
    assert(abs(x) < (1 << 24) && abs(y) < (1 << 24));
    tent_locations.push_back(std::make_pair(K::Point_2(x, y), i));
  }

  Triangulation triangulation;
  triangulation.insert(tent_locations.begin(), tent_locations.end());

  double s = find_upper_bound([&triangulation, f_0, k](long s_query) {
    return count_max_families(determine_component_sizes(triangulation, s_query), k) >= f_0;
  });

  int f = count_max_families(determine_component_sizes(triangulation, s_0), k);

  std::cout << s << " " << f << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


#include <Eigen/Core>

Eigen::Vector3d reflect(const Eigen::Vector3d & in, const Eigen::Vector3d & n)
{
  ////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d r = in - 2 * (in.dot(n)) * n; // reflection direction vector
  return r.normalized();
  ////////////////////////////////////////////////////////////////////////////
}


#include "finiteelement.h"

#include <Eigen/Eigen>

FiniteElement::FiniteElement(const Eigen::MatrixXd& V, const Eigen::VectorXi& indices, const Eigen::VectorXi& faces, double shear, double bulk)
: Element(V, indices, faces), _volume(1.0), _shear(shear), _bulk(bulk)
{
}

FiniteElement::~FiniteElement()
{

}

void FiniteElement::compute(const Eigen::MatrixXd& V, const Eigen::MatrixXd& ext)
{
	const Eigen::MatrixXd x = extract(V);
	const Eigen::MatrixXd f = extract(ext);

	_F = compute_F(x);
	_E = compute_E(_F);
	_energy = compute_U(_E) - f.cwiseProduct(x - _X).sum();

	assert(_energy >= 0.0 && "Energy should always be positive.");

	const Tensor dFdx = compute_dFdx(x);
	const Eigen::MatrixXd dUdE = compute_dUdE(_E);
	const Eigen::MatrixXd dEdF = compute_dEdF(_F);
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);
	const Eigen::MatrixXd dUdx = compute_dUdx(dUdF, dFdx);
	_G = dUdx - f;

#ifdef COMPUTE_FINITE_DIFFERENCE
	_fG = diffTestG(x, 0.0001) - f;
#endif
}

double FiniteElement::diffTest(const Eigen::MatrixXd& V, double h)
{
	Eigen::MatrixXd x = extract(V);
	const Eigen::MatrixXd F = compute_F(x);
	const Eigen::MatrixXd E = compute_E(F);
	const double e = compute_U(E);

	// Compute first derivatives
	const Eigen::MatrixXd dUdE = compute_dUdE(E);
	const Eigen::MatrixXd dEdF = compute_dEdF(F);
	const Tensor dFdx = compute_dFdx(x);

	const Eigen::MatrixXd hdUdE = diffTestdUdE(E, h);
	const Eigen::MatrixXd hdEdF = diffTestdEdF(F, h);
	const Tensor hdFdx = diffTestdFdx(x, h);

	// Compute gradient
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);
	const Eigen::MatrixXd dUdx = compute_dUdx(dUdF, dFdx);

	const Eigen::MatrixXd hdUdF = diffTestdUdF(F, h);
	const Eigen::MatrixXd hdUdx = diffTestdUdx(x, h);

	// Compute second derivatives
	const Eigen::MatrixXd ddFddx = compute_ddFddx(x);
	const Eigen::MatrixXd ddEddF = compute_ddEddF(F);
	const Tensor ddUddE = compute_ddUddE(E);

	const Eigen::MatrixXd hddEddF = diffTestddEddF(F, h);
	const Tensor hddUddE = diffTestddUddE(E, h);

	// Compute Hessian
	const Eigen::MatrixXd ddUdEdF = compute_ddUdEdF(dEdF, ddUddE);
	const Eigen::MatrixXd ddUddF = compute_ddUddF(dEdF, ddUdEdF, ddEddF, dUdE);
	const Eigen::MatrixXd ddUdFdx = compute_ddUdFdx(dFdx, ddUddF);
	const Tensor ddUddx = compute_ddUddx(dFdx, ddUdFdx, ddFddx, dUdF);

	//const Tensor ddUdFdxT = diffTestddUdFdxT(x, h);

	const Eigen::MatrixXd hddUdEdF = diffTestddUdEdF(F, h);
	const Eigen::MatrixXd hddUddF = diffTestddUddF(F, h);
	const Eigen::MatrixXd hddUdFdx = diffTestddUdFdx(x, h);
	const Tensor hddUddx = diffTestddUddx(x, h);

	//std::cout << "...................." << std::endl;
	//std::cout << ddUddx.flatten() << std::endl;
	//std::cout << "- - - - - - - -" << std::endl;
	//std::cout << hddUddx.flatten() << std::endl;
	//std::cout << ddUddx.toString() << std::endl;
	//std::cout << "- - - - - - - -" << std::endl;
	//std::cout << hddUddx.toString() << std::endl;

	double err = 0.0;
	//err += (dFdx - hdFdx).squaredNorm();
	//err += (dEdF - hdEdF).squaredNorm();
	//err += (dUdE - hdUdE).squaredNorm();
	//err += (dUdF - hdUdF).squaredNorm();
	err += (dUdx - hdUdx).squaredNorm();
	//err += (ddEddF - hddEddF).squaredNorm();
	//err += (ddUddE - hddUddE).squaredNorm();
	//err += (ddUddF - hddUddF).squaredNorm();
	//err += (ddUdFdx - hddUdFdx).squaredNorm();
	//err += (ddUddx - hddUddx).squaredNorm();

	return err;
}

/////////////////////////////////////////////////////

Tensor FiniteElement::diffTestdFdx(const Eigen::MatrixXd& x, double h) const
{
	const Eigen::MatrixXd F = compute_F(x);

	Tensor fF(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hx = x;
			hx(i, j) += h;

			const Rotation hF = compute_F(hx);
			fF[i][j] = (hF - F) / h;
		}
	}
	return fF;
}

Eigen::MatrixXd FiniteElement::diffTestdEdF(const Eigen::MatrixXd& F, double h) const
{
	const Eigen::MatrixXd E = compute_E(F);

	Tensor fE(F.rows(), F.cols());
	for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < F.cols(); j++)
		{
			Eigen::MatrixXd hF = F;
			hF(i, j) += h;

			const Rotation hE = compute_E(hF);
			fE[i][j] = (hE - E) / h;
		}
	}
	return fE * Eigen::MatrixXd::Identity(F.rows(), F.cols());
}

Eigen::MatrixXd FiniteElement::diffTestdUdE(const Eigen::MatrixXd& E, double h) const
{
	const double e = compute_U(E);

	Eigen::MatrixXd fU = Eigen::MatrixXd::Zero(E.rows(), E.cols());
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			Eigen::MatrixXd hE = E;
			hE(i, j) += h;

			fU(i, j) = (compute_U(hE) - e) / h;
		}
	}
	return fU;
}

Eigen::MatrixXd FiniteElement::diffTestdUdF(const Eigen::MatrixXd& F, double h) const
{
	const Eigen::MatrixXd E = compute_E(F);
	const double e = compute_U(E);

	Eigen::MatrixXd fU = Eigen::MatrixXd::Zero(F.rows(), F.cols());
	for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < F.cols(); j++)
		{
			Eigen::MatrixXd hF = F;
			hF(i, j) += h;

			const Eigen::MatrixXd hE = compute_E(hF);
			fU(i, j) = (compute_U(hE) - e) / h;
		}
	}
	return fU;
}

Eigen::MatrixXd FiniteElement::diffTestdUdx(const Eigen::MatrixXd& x, double h) const
{
	const Eigen::MatrixXd F = compute_F(x);
	const Eigen::MatrixXd E = compute_E(F);
	const double e = compute_U(E);

	Eigen::MatrixXd fU = Eigen::MatrixXd::Zero(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hx = x;
			hx(i, j) += h;

			const Eigen::MatrixXd hF = compute_F(hx);
			const Eigen::MatrixXd hE = compute_E(hF);
			fU(i, j) = (compute_U(hE) - e) / h;
		}
	}
	return fU;
}

Eigen::MatrixXd FiniteElement::diffTestddEddF(const Eigen::MatrixXd& F, double h) const
{
	const Eigen::MatrixXd dEdF = compute_dEdF(F);

	Tensor fE(F.rows(), F.cols());
	for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < F.cols(); j++)
		{
			Eigen::MatrixXd hF = F;
			hF(i, j) += h;

			const Rotation hdEdF = compute_dEdF(hF);
			fE[i][j] = (hdEdF - dEdF) / h;
		}
	}
	return fE * Eigen::MatrixXd::Identity(F.rows(), F.cols());
}

Tensor FiniteElement::diffTestddUddE(const Eigen::MatrixXd& E, double h) const
{
	const Eigen::MatrixXd dUdE = compute_dUdE(E);

	Tensor fU(E.rows(), E.cols());
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			Eigen::MatrixXd hE = E;
			hE(i, j) += h;

			const Rotation hdUdE = compute_dUdE(hE);
			fU[i][j] = (hdUdE - dUdE) / h;
		}
	}
	return fU;
}

Eigen::MatrixXd FiniteElement::diffTestddUdEdF(const Eigen::MatrixXd& F, double h) const
{
	const Eigen::MatrixXd E = compute_E(F);
	const Eigen::MatrixXd dUdE = compute_dUdE(E);

	Tensor fU(F.rows(), F.cols());
	for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < F.cols(); j++)
		{
			Eigen::MatrixXd hF = F;
			hF(i, j) += h;

			const Eigen::MatrixXd hE = compute_E(hF);
			const Eigen::MatrixXd hdUdE = compute_dUdE(hE);
			fU(i, j) = (hdUdE - dUdE) / h;
		}
	}
	return fU * Eigen::MatrixXd::Identity(F.rows(), F.cols());
}

Eigen::MatrixXd FiniteElement::diffTestddUddF(const Eigen::MatrixXd& F, double h) const
{
	const Eigen::MatrixXd E = compute_E(F);
	const Eigen::MatrixXd dUdE = compute_dUdE(E);
	const Eigen::MatrixXd dEdF = compute_dEdF(F);
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);

	Tensor fU(F.rows(), F.cols());
	for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < F.cols(); j++)
		{
			Eigen::MatrixXd hF = F;
			hF(i, j) += h;

			const Eigen::MatrixXd hE = compute_E(hF);
			const Eigen::MatrixXd hdUdE = compute_dUdE(hE);
			const Eigen::MatrixXd hdEdF = compute_dEdF(hF);
			const Eigen::MatrixXd hdUdF = compute_dUdF(hdUdE, hdEdF);
			fU(i, j) = (hdUdF - dUdF) / h;
		}
	}
	return fU * Eigen::MatrixXd::Identity(F.rows(), F.cols());
}

Eigen::MatrixXd FiniteElement::diffTestddUdFdx(const Eigen::MatrixXd& x, double h) const
{
	const Eigen::MatrixXd F = compute_F(x);
	const Eigen::MatrixXd E = compute_E(F);
	const Eigen::MatrixXd dUdE = compute_dUdE(E);
	const Eigen::MatrixXd dEdF = compute_dEdF(F);
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);

	Tensor fU(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hx = x;
			hx(i, j) += h;

			const Eigen::MatrixXd hF = compute_F(hx);
			const Eigen::MatrixXd hE = compute_E(hF);
			const Eigen::MatrixXd hdUdE = compute_dUdE(hE);
			const Eigen::MatrixXd hdEdF = compute_dEdF(hF);
			const Eigen::MatrixXd hdUdF = compute_dUdF(hdUdE, hdEdF);
			fU(i, j) = (hdUdF - dUdF) / h;
		}
	}
	return fU * Eigen::MatrixXd::Identity(F.rows(), F.cols());
}

Tensor FiniteElement::diffTestddUdFdxT(const Eigen::MatrixXd& x, double h) const
{
	const Eigen::MatrixXd F = compute_F(x);
	const Eigen::MatrixXd E = compute_E(F);
	const Eigen::MatrixXd dUdE = compute_dUdE(E);
	const Eigen::MatrixXd dEdF = compute_dEdF(F);
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);

	Tensor fU(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hx = x;
			hx(i, j) += h;

			const Eigen::MatrixXd hF = compute_F(hx);
			const Eigen::MatrixXd hE = compute_E(hF);
			const Eigen::MatrixXd hdUdE = compute_dUdE(hE);
			const Eigen::MatrixXd hdEdF = compute_dEdF(hF);
			const Eigen::MatrixXd hdUdF = compute_dUdF(hdUdE, hdEdF);
			fU(i, j) = (hdUdF - dUdF) / h;
		}
	}
	return fU;
}

Tensor FiniteElement::diffTestddUddx(const Eigen::MatrixXd& x, double h) const
{
	const Eigen::MatrixXd F = compute_F(x);
	const Eigen::MatrixXd E = compute_E(F);
	const Eigen::MatrixXd dUdE = compute_dUdE(E);
	const Eigen::MatrixXd dEdF = compute_dEdF(F);
	const Tensor dFdx = compute_dFdx(x);
	const Eigen::MatrixXd dUdF = compute_dUdF(dUdE, dEdF);
	const Eigen::MatrixXd dUdx = compute_dUdx(dUdF, dFdx);

	Tensor fU(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hx = x;
			hx(i, j) += h;

			const Eigen::MatrixXd hF = compute_F(hx);
			const Eigen::MatrixXd hE = compute_E(hF);
			const Eigen::MatrixXd hdUdE = compute_dUdE(hE);
			const Eigen::MatrixXd hdEdF = compute_dEdF(hF);
			const Tensor hdFdx = compute_dFdx(hx);
			const Eigen::MatrixXd hdUdF = compute_dUdF(hdUdE, hdEdF);
			const Eigen::MatrixXd hdUdx = compute_dUdx(hdUdF, hdFdx);
			fU(i, j) = (hdUdx - dUdx) / h;
		}
	}
	return fU;
}

/////////////////////////////////////////////////////

Tensor FiniteElement::compute_dFdx(const Eigen::MatrixXd& x) const
{
	Tensor out(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			out[i][j] = compute_dFdx(x, i, j);
		}
	}
	return out;
}

Eigen::MatrixXd FiniteElement::compute_dUdx(const Eigen::MatrixXd& dUdF, const Tensor& dFdx) const
{
	return dFdx * dUdF;
}

Eigen::MatrixXd FiniteElement::compute_dUdF(const Eigen::MatrixXd& dUdE, const Eigen::MatrixXd& dEdF) const
{
	return dEdF.transpose() * dUdE;
}

Tensor FiniteElement::compute_ddUddx(const Tensor& dFdx, const Eigen::MatrixXd& ddUdFdx, const Eigen::MatrixXd& ddFddx, const Eigen::MatrixXd& dUdF) const
{
	return dFdx.mult(ddUdFdx);// +ddFddx.transpose() * dUdF; // dFdx was constant, second derivative should therefore be 0
}

Eigen::MatrixXd FiniteElement::compute_ddUdFdx(const Tensor& dFdx, const Eigen::MatrixXd& ddUddF) const
{
	return dFdx * ddUddF;
}

Eigen::MatrixXd FiniteElement::compute_ddUddF(const Eigen::MatrixXd& dEdF, const Eigen::MatrixXd& ddUdEdF, const Eigen::MatrixXd& ddEddF, const Eigen::MatrixXd& dUdE) const
{
	return dEdF.transpose() * ddUdEdF + ddEddF.transpose() * dUdE;
}

Eigen::MatrixXd FiniteElement::compute_ddUdEdF(const Eigen::MatrixXd& dEdF, const Tensor& ddUddE) const
{
	return ddUddE * dEdF.transpose();
}

/////////////////////////////////////////////////////


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

#include "Optimizer/ceres/LocalParameterization.h"
#include "Math.h"

namespace VISFS {
namespace Optimizer {

bool PoseLocalParameterization::Plus(const double * x, const double * delta, double * x_plus_delta) const {
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);
    Eigen::Map<const Eigen::Vector3d> dp(delta);
    Eigen::Quaterniond dq = deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (dq * _q).normalized();

    return true;
}

bool PoseLocalParameterization::ComputeJacobian(const double * x, double * jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

bool PointLocalParameterization::Plus(const double * x, const double * delta, double * x_plus_delta) const {
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    p = _p + dp;

    return true;
}

bool PointLocalParameterization::ComputeJacobian(const double * x, double * jacobian) const {
    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> j(jacobian);
    j.topRows<3>().setIdentity();

    return true;
}

}	// Optimizer
}	// VISFS


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using namespace boost::numeric::ublas;

#include <random>
#include <stdio.h>

void fill_u(matrix<double> & U, vector<double> & d) {
  U.clear();
  d.clear();

  std::default_random_engine generator;
  std::uniform_real_distribution<double> distribution(0.0,1.0);

  for (unsigned i=0; i < U.size1(); ++i) {
    U(i,i) = 1.0;
    for (unsigned j=i+1; j < U.size2(); ++j) {
      U(i,j) = distribution(generator);
    }
  }

  for (unsigned i=0; i < d.size(); ++i) {
    d(i) = distribution(generator);
  }

}

void udu(matrix<double> &M, matrix<double> &U, vector<double> &d, unsigned int state_size) {
  // this method stolen from pykalman.sqrt.bierman by Daniel Duckworth (BSD license)
  /*Construct the UDU' decomposition of a positive, semidefinite matrix M

    Parameters
    ----------
    M : [n, n] array
        Matrix to factorize

    Returns
    -------
    UDU : UDU_decomposition of size n
        UDU' representation of M
  */

  unsigned int n = state_size;

  // make M upper triangular: not sure if this is necessary?
  for (unsigned i=0; i < n; ++i) {
    for (unsigned j=0; j < i; ++j) {
      M(i,j) = 0;
    }
  }

  d.clear();
  U.clear();
  for (unsigned i=0; i < n; ++i) {
    U(i,i) = 1;
  }

  for (unsigned j=n; j >= 2; --j) {
    d(j - 1) = M(j - 1, j - 1);
    double alpha = 0.0;
    double beta = 0.0;
    if (d(j - 1) > 0) {
      alpha = 1.0 / d(j - 1);
    } else {
      if (fabs(d(j-1) > 1e-5) ) {
	printf("WARNING: nonpositive d[%d] %f in udu decomp\n", j-1, d(j-1));
	exit(-1);
      }
      d(j-1) = 0.0;
      alpha = 0.0;
    }
    for (unsigned k=1; k < j; ++k) {
      beta = M(k - 1, j - 1);
      U(k - 1, j - 1) = alpha * beta;

      // M[0:k, k - 1] = M[0:k, k - 1] - beta * U[0:k, j - 1]
      for (unsigned kk=0; kk < k; ++kk) {
	M(kk, k - 1) = M(kk, k - 1) - beta * U(kk, j - 1);
      }
    }
  }
  d(0) = M(0, 0);

  if (d(0) < 0) {
    printf("ERROR: udu decomposition on non-posdef matrix with M(0,0)=%f\n", M(0,0));
    exit(-1);
  }
}

void udu_ptr(matrix<double> &M_mat, matrix<double> &U_mat, vector<double> &d_vec, unsigned int state_size) {

  unsigned int n = M_mat.size1();
  double *M = &(M_mat(0,0));
  double *U = &(U_mat(0,0));
  double *d = &(d_vec(0));

  // this method stolen from pykalman.sqrt.bierman by Daniel Duckworth (BSD license)
  /*Construct the UDU' decomposition of a positive, semidefinite matrix M

    Parameters
    ----------
    M : [n, n] array
        Matrix to factorize

    Returns
    -------
    UDU : UDU_decomposition of size n
        UDU' representation of M
  */

  // make M upper triangular: not sure if this is necessary?
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned idx=i*n; idx < i*n+i; ++idx) {
      M[idx] = 0;
    }
  }

  // initialize U as the identity
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned idx=i*n; idx < i*n+state_size; ++idx) {
      U[idx] = 0;
    }
  }
  for (unsigned i=0; i < state_size; ++i) {
    U[i*n+i] = 1;
  }

  for (unsigned j=state_size-1; j >= 1; --j) {
    d[j] = M[j*n+j];
    double alpha = 0.0;
    double beta = 0.0;
    if (d[j] > 0) {
      alpha = 1.0 / d[j];
    } else {
      if (fabs(d[j] > 1e-5) ) {
	printf("WARNING: nonpositive d[%d] %f in udu decomp\n", j, d[j]);
	exit(-1);
      }
      d[j] = 0.0;
      alpha = 0.0;
    }
    for (unsigned k=0; k < j; ++k) {
      beta = M[k*n+ j ];
      U[k*n+ j] = alpha * beta;
      for (unsigned kk=0; kk <= k; ++kk) {
	M[kk*n+ k] = M[kk*n+k] - beta * U[kk*n+j];
      }
    }
  }
  d[0] = M[0];

  if (d[0] < 0) {
    printf("ERROR: udu decomposition on non-posdef matrix with M(0,0)=%f\n", M[0]);
    exit(-1);
  }
}

void compute_explicit_cov(matrix<double> &U,
			  vector<double> &d,
			  matrix<double> &mtmp,
			  matrix<double> &P) {

  unsigned int n = d.size();

  // construct the cov matrix
  for (unsigned i=0; i < n; ++i) {
    subrange(mtmp, 0, n, i, i+1) = subrange(U, 0, n, i, i+1);
    subrange(mtmp, 0, n, i, i+1) *= d(i);
  }
  noalias(subrange(P, 0, n, 0, n))		\
    = prod(subrange(mtmp, 0, n, 0, n),
	   trans(subrange(U, 0, n, 0, n)));
}

void cov_by_hand(matrix<double> &U,
		 vector<double> &d,
		 matrix<double> &mtmp,
		 matrix<double> &P) {
  unsigned int n = d.size();

  for (unsigned i=0; i < n; ++i) {
    for (unsigned j=0; j < n; ++j) {
      double accum = 0;
      for (unsigned k=0; k < n; ++k) {
	accum += U(i, k)* U(j,k) * d(k);
      }
      P(i,j) = accum;
    }
  }
}

#include <cblas.h>
void cov_atlas(matrix<double> &U,
	       vector<double> &d,
	       matrix<double> &mtmp,
	       matrix<double> &P,
	       int state_size,
	       int prev_state_size) {
  unsigned int n = d.size();
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned j=0; j < prev_state_size; ++j) {
      mtmp(i,j) = U(i,j) * d(j);
    }
  }

  cblas_dgemm(CblasRowMajor,
	      CblasNoTrans,
	      CblasTrans,
	      state_size,  prev_state_size, state_size, 1.0,
	      &(mtmp(0,0)), mtmp.size2(),
	      &(U(0,0)), U.size2(),
	      0.0, &(P(0,0)), P.size2());

  /*
Problem with the triangular case:
    After transition, U is not triangular in general. In fact it's not square in general...
   */
}

void write_mat_col(const char *fname, const matrix<double> & m) {

  FILE *f = fopen(fname, "w");
  if (f == NULL)
    {
      printf("Error opening file!\n");
      exit(1);
    }

  for(unsigned i=0; i < m.size1(); ++i) {
    for(unsigned j=0; j < m.size2(); ++j) {
      fprintf(f, "%.12f ", m(i,j));
    }
    fprintf(f, "\n");
  }
  fprintf(f, "\n");

  fclose(f);
}

bool matrix_eq(matrix<double> &m1, matrix<double> &m2, double tol) {
  for(unsigned i=0; i < m1.size1(); ++i) {
    for(unsigned j=0; j < m1.size2(); ++j) {
      if (fabs(m1(i,j) - m2(i,j)) > tol) {
	printf("unequal\n");
	return false;
      }
    }
  }
  printf("equal\n");
  return true;
}

int main() {

  int n = 150;
  int iters = 100;

  matrix<double> U(n, n);
  matrix<double> P(n, n);
  matrix<double> tmp(n, n);
  vector<double> d(n);
  fill_u(U, d);

  printf("filled\n");

  matrix<double> U_out(n, n);
  vector<double> d_out(n);

  matrix<double> P_true(n, n);
  matrix<double> P_hand(n, n);
  matrix<double> P_atlas(n, n);
  //compute_explicit_cov(U, d, tmp, P_true);
  //cov_by_hand(U, d, tmp, P_hand);
  //cov_atlas(U, d, tmp, P_atlas, n, n);
  //matrix_eq(P_true, P_atlas, 1e-8);

  /*
  write_mat_col("P_true.txt", P_true);
  write_mat_col("P_hand.txt", P_hand);
  write_mat_col("P_atlas.txt", P_atlas);
  */
  cov_atlas(U, d, tmp, P, n, n);
  P_true = P;
  for (unsigned i=0; i < iters; ++i) {
    udu_ptr(P, U_out, d_out, n);
    //P = P_true;
    //udu_ptr(&(P(0,0)), &(U_out(0,0)), &(d_out(0)),
    //	    n, n);

    //printf("udu\n");
    P = P_true;
  }

  bool eq = matrix_eq(U_out, U, 1e-8);

  return 0;

}


#include <iostream>
#include <Eigen\Dense>

using namespace Eigen;
using namespace std;

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

	// Constructor
	MatrixXf a(10, 15);
	Vector3d b(5, 6, 7);

	// Coefficient accessors
	{
		MatrixXd m(2, 2);
		m(0, 0) = 3;
		m(1, 0) = 2.5;
		m(0, 1) = -1;
		m(1, 1) = m(1, 0) + m(0, 1);
	}

	{
		// Comma-initialization
		Matrix3f m;
		m << 1, 2, 3,
			4, 5, 6,
			7, 8, 9;
	}
	{
		// Resizing
		MatrixXd m(2, 5);
		m.resize(4, 3);
		cout << "The matrix m is of size " << m.rows() << "x" << m.cols() << endl; // 4x3
		cout << "It has " << m.size() << " coefficients" << endl; // 12
		VectorXd v(2);
		v.resize(5);
		cout << "The vector v is of size " << v.size() << endl; // 5
		cout << "As a matrix, v is of size " << v.rows() << "x" << v.cols() << endl; //5x1

	}

	// assignment and resizing
	{
		MatrixXf a(2, 2); // size 2x2
		MatrixXf b(3, 3);
		a = b;
		cout << a.size() << endl; // 3x3
	}

	{
		// addition and subtraction
		Matrix2d a;
		a << 1, 2,
			 3, 4;
		MatrixXd b(2, 2);
		b << 2, 3,
			 1, 4;
		cout << "a = \n" << a << endl << "b = \n" << b << endl;
		cout << "a + b = \n" << a + b << endl;
		cout << "a - b = \n" << a - b << endl;
		cout << "Doing a += b." << endl;
		a += b;
		cout << "Now a = \n" << a << endl;
		Vector3d v(1, 2, 3);
		Vector3d w(1, 0, 0);
		cout << "-v + w - v = \b" << -v + w - v << endl;
	}
	{
		// scalar multiplication and division
		Matrix2d a;
		a << 1, 2,
			 3, 4;
		Vector3d v(1, 2, 3);
		cout << "a = \n" << a << endl << "v = \n" << v << endl;
		cout << "a * 2.5 = \n" << a * 2.5 << endl;
		cout << "0.1 * v = \n" << 0.1 * v << endl;
		cout << "Doing v*= 2; " << endl;
		v *= 2;
		cout << "Now v =\n" << v << endl;
	}

	{
		// transposition and conjugation
		MatrixXcf a = MatrixXcf::Random(2, 2);
		cout << "Here is the matrix a\n" << a << endl;
		cout << "Here is the matrix a^T\n" << a.transpose() << endl;
		cout << "Here is the conjugate of a\n" << a.conjugate() << endl;
		cout << "Here is the matrix a^*\n" << a.adjoint() << endl;
	}
	{
		// transpose and adjoint return a proxy object without doing the actual transpotition
		Matrix2i a; a << 1, 2, 3, 4;
		cout << "a = \n" << a << endl;
		Matrix2i b = a.transpose(); // this is ok
		cout << "b = \n" << b << endl;
		//a = a.transpose(); // !!! do NOT do this. This is the aliasing issue and automatically detected in debug.
		//cout << "a doesn't change after a = a.tranpose(). a = \n" << a << endl;
		// Should use the transposeInPlace()
		a.transposeInPlace();
		cout << "a.transposeInPlace() = \n" << a << endl;
	}
	{
		// Matrix multiplication
		Matrix2d mat;
		mat << 1, 2,
			   3, 4;
		Vector2d u(-1, 1), v(2, 0);
		cout << "Here is mat * mat: \n" << mat * mat << endl;
		cout << "Here is mat * u: \n" << mat * u << endl;
		cout << "Here is u^T * mat: \n" << u.transpose()*mat << endl;
		cout << "Here is u^T * v: \n" << u.transpose() * v << endl;
		cout << "Here is u * v^T: \n" << u * v.transpose() << endl;
		cout << "Let's multiply mat by itself " << endl;
		mat *= mat;
		cout << "Now mat is mat: \n" << mat << endl;
	}
	{
		// Dot product
		Vector3d v(1, 2, 3);
		Vector3d w(0, 1, 2);
		cout << "Dot product: " << v.dot(w) << endl;
		double dp = v.adjoint() * w;
		cout << "Dot product via a matrix product: " << dp << endl;
		cout << "Cross product: \n" << v.cross(w) << endl;
	}

	{
		// basic arithmetic reduction operations
		Eigen::Matrix2d mat;
		mat << 1, 2, 3, 4;
		cout << "mat = " << mat << endl;
		cout << "mat.sum()  = " << mat.sum() << endl;
		cout << "mat.prod() = " << mat.prod() << endl;
		cout << "mat.mean() = " << mat.mean() << endl;
		cout << "mat.minCoeff() = " << mat.minCoeff() << endl;
		cout << "mat.maxCoeff() = " << mat.maxCoeff() << endl;
		cout << "mat.trace() = " << mat.trace() << endl;

		Matrix3f m = Matrix3f::Random();
		std::ptrdiff_t i, j;
		float minOfM = m.minCoeff(&i, &j);
		cout << "m = \n" << m << endl;
		cout << "minOfM = " << minOfM << " at i " << i << " j " << j << endl;

		RowVector4i v = RowVector4i::Random();
		int maxOfV = v.maxCoeff(&i);
		cout << "v = \n" << v << endl;
		cout << "maxOfV = " << maxOfV << " at i = " << i << endl;

	}

	system("pause");
	return 0;
}


#include <iostream>
#include <fstream>
#include <cstdlib>
#include <cstdio>
#include <cmath>
#include <vector>
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib
#include "bayes_classifier.h"
using namespace std;
using namespace Eigen;

const int NO_ERRORS = 0;
//const char CORRECT = 'X';
//const char INCORRECT = '-';

typedef struct
{
    string actual_class;
    string classified_as;
    bool correctly_classified;
} ClassificationItem;

typedef struct
{
    int num_correct;
    int num_incorrect;
    int false_males;
    int false_females;
    vector< ClassificationItem > tested_data;
} Results;

int loadData( MatrixXd& class_1_data, MatrixXd& class_2_data,
              const char* data_filename, const int num_features );

VectorXd computeMeanVector( const MatrixXd& data );

MatrixXd computeCovarianceMatrix( const MatrixXd& data, const VectorXd mean_vector );

MatrixXd mergeDataMatrices( const MatrixXd& class_1_data, const MatrixXd& class_2_data );

Results classifyTestData( const MatrixXd& class_1_test_data,
                          const MatrixXd& class_2_test_data,
                          vector< BayesClassifier >& classifiers );

void outputResults( const char* output_filename, const Results& results );

int main( int argc, char** argv )
{
    int program_success = NO_ERRORS;
    MatrixXd class_1_data;
    MatrixXd class_2_data;
    MatrixXd test_data;
    BayesClassifier male_image_class;
    BayesClassifier female_image_class;
    vector< BayesClassifier > classifiers( 2 );
    Results results;

    if( argc == 1 )
    {
        cout << "Usage: project4  num_features_to_keep training_data_filename" << endl
             << "testing_filenam output_filename" << endl;
    }

    puts( "Loading Training Data..." );
    loadData( class_1_data, class_2_data, argv[2], atoi( argv[1] ) );

    puts( "Training Bayes Classifiers..." );
    classifiers[0].set_class_name( "Male Image" );
    classifiers[0].train_classifier( class_1_data );
    classifiers[0].set_prior_probability( 0.5 );

    classifiers[1].set_class_name( "Female Image" );
    classifiers[1].train_classifier( class_2_data );
    classifiers[1].set_prior_probability( 0.5 );

//double y=0;
//for( int i = 0; i < class_1_data.cols(); i++ )
//{
//    cout << class_1_data( 0, i ) << '\t';
//    y += class_1_data( 0, i );
//}

//cout << endl << ( y / (double) class_1_data.cols() ) << endl;
//  classifiers[0].reportClassifierInfo();
//    classifiers[1].reportClassifierInfo();

    puts( "Loading Testing Data..." );
    loadData( class_1_data, class_2_data, argv[3], atoi( argv[1] ) ); // re-using objects

    puts( "Classifying Test Data..." );
    results = classifyTestData( class_1_data, class_2_data, classifiers );

    puts( "Outputting results..." );
    outputResults( argv[4], results );

    return program_success;
}

int loadData( MatrixXd& class_1_data, MatrixXd& class_2_data,
              const char* data_filename, const int num_features )
{
    ifstream fin;
    vector< VectorXd > feature_data;
    VectorXd temp_vector;
    vector< int > labels;
    int label;
    double value;
    int num_in_class_1 = 0;
    int num_in_class_2 = 0;
    char buffer[1000];
    int i = 0;
    int j = 0;
    int k = 0;

    temp_vector.resize( num_features, 1 );

    fin.clear();
    fin.open( data_filename );

    fin >> label;
    while( fin.good() )
    {
        labels.push_back( label );
        if( label == -1 )
        {
            num_in_class_1++;
        }
        else if( label == +1 ) // possibly unnecessary test, only expect -1/+1
        {
            num_in_class_2++;
        }

        for( j = 0; j < num_features; j++ )
        {
            fin.getline( buffer, 999, ':' ); // eat the index information
            fin >> value;
            temp_vector( j ) = value;
        }

        feature_data.push_back( temp_vector );
        fin.getline( buffer, 999, '\n' ); // eat any excess features' values
        i++;

        fin >> label;
    }

    fin.close();

    assert( ( num_in_class_1 + num_in_class_2 ) == labels.size() );

    class_1_data.resize( num_features, num_in_class_1 );
    class_2_data.resize( num_features, num_in_class_2 );

    for( i = 0, j = 0, k = 0; i < feature_data.size(); i++ )
    {
        if( labels[i] == -1 )
        {
            class_1_data.col( j ) = feature_data[i].transpose().eval();
            j++;
        }
        else if( labels[i] == +1 )
        {
            class_2_data.col( k ) = feature_data[i];
            k++;
        }
    }

    return 0;
}

VectorXd computeMeanVector( const MatrixXd& data )
{
    VectorXd mean_vector;
    int j = 0;

    mean_vector.resize( data.rows(), 1 );
    mean_vector *= 0;

    for( j = 0; j < data.cols(); j++ )
    {
        mean_vector += data.col( j );
    }
    mean_vector /= data.cols();

    return mean_vector;
}

MatrixXd computeCovarianceMatrix( const MatrixXd& data, const VectorXd mean_vector )
{
    MatrixXd covariance_matrix;
    MatrixXd A_matrix;
    int j = 0;

    covariance_matrix.resize( mean_vector.size(), mean_vector.size() );
    A_matrix.resize( data.rows(), data.cols() );

    for( j = 0; j < data.cols(); j++ )
    {
        A_matrix.col( j ) = data.col( j ) - mean_vector;
    }

    covariance_matrix = A_matrix * A_matrix.transpose().eval();
    covariance_matrix *= data.cols();

    return covariance_matrix;
}

MatrixXd mergeDataMatrices( const MatrixXd& class_1_data, const MatrixXd& class_2_data )
{
    assert( class_1_data.rows() == class_2_data.rows() );

    MatrixXd merged_data;
    int j = 0;
    int k = 0;

    merged_data.resize( class_1_data.rows(), ( class_1_data.cols() + class_2_data.cols() ) );

    for( k = 0, j = 0; k < class_1_data.cols(); k++, j++ )
    {
        merged_data.col( j ) = class_1_data.col( k );
    }

    for( k = 0; k < class_2_data.cols(); k++, j++ )
    {
        merged_data.col( j ) = class_1_data.col( k );
    }

    return merged_data;
}

Results classifyTestData( const MatrixXd& class_1_test_data,
                          const MatrixXd& class_2_test_data,
                          vector< BayesClassifier >& classifiers )
{
    Results results;
        results.num_correct = 0;
        results.num_incorrect = 0;
        results.false_males = 0;
        results.false_females = 0;
        results.tested_data.resize( class_1_test_data.cols() + class_2_test_data.cols() );
    ClassificationItem temp;
    int i = 0;
    int j = 0;

    for( i = 0, j = 0; j < class_1_test_data.cols(); i++, j++ )
    {
        results.tested_data[i].actual_class = classifiers[0].class_name();
        results.tested_data[i].classified_as = BayesClassifier::assignToClass( class_1_test_data.col( j ), classifiers );
        results.tested_data[i].correctly_classified = ( results.tested_data[i].actual_class == results.tested_data[i].classified_as );

        if( results.tested_data[i].correctly_classified )
        {
            results.num_correct++;
        }
        else
        {
            results.num_incorrect++;

            if( results.tested_data[i].classified_as == classifiers[0].class_name() )
            {
                results.false_males++;
            }
            else if( results.tested_data[i].classified_as == classifiers[1].class_name() ) // likely could just be an else
            {
                results.false_females++;
            }
        }
    }

    for( j = 0; j < class_2_test_data.cols(); i++, j++ )
    {
        results.tested_data[i].actual_class = classifiers[1].class_name();
        results.tested_data[i].classified_as = BayesClassifier::assignToClass( class_2_test_data.col( j ), classifiers );
        results.tested_data[i].correctly_classified = ( results.tested_data[i].actual_class == results.tested_data[i].classified_as );

        if( results.tested_data[i].correctly_classified )
        {
            results.num_correct++;
        }
        else
        {
            results.num_incorrect++;

            if( results.tested_data[i].classified_as == classifiers[0].class_name() )
            {
                results.false_males++;
            }
            else if( results.tested_data[i].classified_as == classifiers[1].class_name() ) // likely could just be an else
            {
                results.false_females++;
            }
        }
    }

    return results;
}

void outputResults( const char* output_filename, const Results& results )
{
    ofstream fout;
    int i = 0;

    fout.clear();
    fout.open( output_filename );

    fout << "Accuracy:             " << (double) results.num_correct / (double) (results.num_correct + results.num_incorrect) << endl
         << "Number of Test Items: " << results.num_correct + results.num_incorrect << endl
         << "Number Correct:       " << results.num_correct << endl
         << "Number Incorrect:     " << results.num_incorrect << endl
         << "Number False Males:   " << results.false_males << endl
         << "Number False Females: " << results.false_females << endl
         << endl;

    fout << "Actual Class, Classified As, Correctly Classified (0 = No, 1 = Yes)" << endl;
    for( i = 0; i < results.tested_data.size(); i++ )
    {
        fout << results.tested_data[i].actual_class << ", " << results.tested_data[i].classified_as << ", " << results.tested_data[i].correctly_classified << endl;
    }

    fout.close();
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/models/exactbachelierimpliedvolatility.hpp>

#include <ql/math/comparison.hpp>

#include <boost/math/distributions/normal.hpp>

namespace QuantExt {

using namespace QuantLib;

namespace {

static boost::math::normal_distribution<double> normal_dist;
Real phi(const Real x) { return boost::math::pdf(normal_dist, x); }
Real Phi(const Real x) { return boost::math::cdf(normal_dist, x); }

Real PhiTilde(const Real x) { return Phi(x) + phi(x) / x; }

Real inversePhiTilde(const Real PhiTildeStar) {
    QL_REQUIRE(PhiTildeStar < 0.0, "inversePhiTilde(" << PhiTildeStar << "): negative argument required");
    Real xbar;
    if (PhiTildeStar < -0.001882039271) {
        Real g = 1.0 / (PhiTildeStar - 0.5);
        Real xibar = (0.032114372355 - g * g * (0.016969777977 - g * g * (2.6207332461E-3 - 9.6066952861E-5 * g * g))) /
                     (1.0 - g * g * (0.6635646938 - g * g * (0.14528712196 - 0.010472855461 * g * g)));
        xbar = g * (0.3989422804014326 + xibar * g * g);
    } else {
        Real h = std::sqrt(-std::log(-PhiTildeStar));
        xbar = (9.4883409779 - h * (9.6320903635 - h * (0.58556997323 + 2.1464093351 * h))) /
               (1.0 - h * (0.65174820867 + h * (1.5120247828 + 6.6437847132E-5 * h)));
    }
    Real q = (PhiTilde(xbar) - PhiTildeStar) / phi(xbar);
    Real xstar =
        xbar + 3.0 * q * xbar * xbar * (2.0 - q * xbar * (2.0 + xbar * xbar)) /
                   (6.0 + q * xbar * (-12.0 + xbar * (6.0 * q + xbar * (-6.0 + q * xbar * (3.0 + xbar * xbar)))));
    return xstar;
}

} // namespace

Real exactBachelierImpliedVolatility(Option::Type optionType, Real strike, Real forward, Real tte, Real bachelierPrice,
                                     Real discount) {

    Real theta = optionType == Option::Call ? 1.0 : -1.0;

    // compound bechelierPrice, so that effectively discount = 1

    bachelierPrice /= discount;

    // handle case strike = forward

    if (std::abs(strike - forward) < 1E-15) {
        return bachelierPrice / (std::sqrt(tte) * phi(0.0));
    }

    // handle case strike != forward

    Real timeValue = bachelierPrice - std::max(theta * (forward - strike), 0.0);

    if (std::abs(timeValue) < 1E-15)
        return 0.0;

    QL_REQUIRE(timeValue > 0.0, "exactBachelierImpliedVolatility(theta="
                                    << theta << ",strike=" << strike << ",forward=" << forward << ",tte=" << tte
                                    << ",price=" << bachelierPrice << "): option price implies negative time value ("
                                    << timeValue << ")");

    Real PhiTildeStar = -std::abs(timeValue / (strike - forward));
    Real xstar = inversePhiTilde(PhiTildeStar);
    Real impliedVol = std::abs((strike - forward) / (xstar * std::sqrt(tte)));

    return impliedVol;
}

} // namespace QuantExt


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

// Zdenek Prusa and Peter L. Soendergaard,
// Real-time spectrogram inversion using phase gradient heap integration
// Proceedings of DAFX 2016

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../public/STFT.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class RTPGHI
{

public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXcd = Eigen::ArrayXcd;

  void init(index fftSize)
  {
    mBins = fftSize / 2 + 1;
    mPrevMag = ArrayXd::Zero(mBins);
    mPrevLogMag = ArrayXd::Zero(mBins);
    mPrevPrevLogMag = ArrayXd::Zero(mBins);
    mPrevPhase = ArrayXd::Zero(mBins);
    mPrevPrevPhase = ArrayXd::Zero(mBins);
    mPrevPhaseDeltaT = ArrayXd::Zero(mBins);
    mBinIndices = ArrayXd::LinSpaced(mBins, 0, mBins - 1);
  }

  void processFrame(RealVectorView in, ComplexVectorView out, index winSize,
                    index fftSize, index hopSize, double tolerance)
  {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    using namespace std::complex_literals;
    double  gamma = 0.25645 * pow(winSize, 2); // assumes Hann window
    ArrayXd mag = asEigen<Array>(in);
    ArrayXd logMag = mag.max(epsilon).log();
    ArrayXd futureLogMag = logMag;
    ArrayXd currentLogMag = mPrevLogMag;
    ArrayXd prevLogMag = mPrevPrevLogMag;

    ArrayXd phaseDeltaT =
        getPhaseDeltaT(currentLogMag, gamma, fftSize, hopSize);
    ArrayXd phaseDeltaF =
        getPhaseDeltaF(prevLogMag, futureLogMag, gamma, fftSize, hopSize);
    double absTol =
        log(tolerance) + max(currentLogMag.maxCoeff(), prevLogMag.maxCoeff());
    ArrayXd todo = (currentLogMag > absTol).cast<double>();
    index   numTodo = todo.sum();
    ArrayXd phaseEst = pi + ArrayXd::Random(mBins) * pi;

    vector<pair<double, index>> heap;

    for (index i = 0; i < mBins; i++)
    {
      if (prevLogMag(i) > absTol) heap.push_back({prevLogMag(i), i});
    }
    make_heap(heap.begin(), heap.end());

    while (numTodo > 0 && heap.size() > 0)
    {
      pop_heap(heap.begin(), heap.end());
      index _m = heap.back().second;
      heap.pop_back();

      // use indices 0..mBins for prev frame
      // mBins ... 2 * mBins  for current frame
      if (_m < mBins && todo[_m] > 0)
      {
        index m = _m;
        phaseEst[m] =
            mPrevPhase[m] + 0.5 * (phaseDeltaT[m] + mPrevPhaseDeltaT[m]);
        heap.push_back({currentLogMag[m], m + mBins});
        push_heap(heap.begin(), heap.end());
        todo[m] = 0;
        numTodo--;
      }
      else if (_m >= mBins)
      {
        index m = _m - mBins;
        if (m < mBins - 1 && todo[m + 1] > 0)
        {
          phaseEst[m + 1] =
              phaseEst[m] + 0.5 * (phaseDeltaF[m] + phaseDeltaF[m + 1]);
          heap.push_back({currentLogMag[m + 1], _m + 1});
          push_heap(heap.begin(), heap.end());
          todo[m + 1] = 0;
          numTodo--;
        }
        if (m > 0 && todo[m - 1] > 0)
        {
          phaseEst[m - 1] =
              phaseEst[m] - 0.5 * (phaseDeltaF[m] + phaseDeltaF[m - 1]);
          heap.push_back({currentLogMag[m - 1], _m - 1});
          push_heap(heap.begin(), heap.end());
          todo[m - 1] = 0;
          numTodo--;
        }
      }
    }

    ArrayXd finalPhase =
        phaseEst - ArrayXd::LinSpaced(mBins, 0, 1) * pi * (winSize - 1) / 2;
    ArrayXcd result = mPrevMag * (1i * finalPhase).exp();
    mPrevPrevLogMag = mPrevLogMag;
    mPrevLogMag = logMag;
    mPrevPhase = phaseEst;
    mPrevPhaseDeltaT = phaseDeltaT;
    mPrevMag = mag;
    out = asFluid(result);
  }

private:
  Eigen::ArrayXd getPhaseDeltaT(Eigen::Ref<ArrayXd> logMag, double gamma,
                                index fftSize, index hopSize)
  {
    ArrayXd deltaT = ArrayXd::Zero(mBins);
    deltaT.segment(1, mBins - 2) =
        logMag.segment(2, mBins - 2) - logMag.segment(0, mBins - 2);
    deltaT = deltaT * 0.5 * hopSize * fftSize / gamma;
    deltaT = deltaT + twoPi * hopSize * mBinIndices / fftSize;
    return deltaT;
  }

  Eigen::ArrayXd getPhaseDeltaF(ArrayXd prevLogMag, ArrayXd nextLogMag,
                                double gamma, index fftSize, index hopSize)
  {
    return 0.5 * (nextLogMag - prevLogMag) * (-gamma / (hopSize * fftSize));
  }

  index   mBins;
  ArrayXd mBinIndices;
  ArrayXd mPrevMag;
  ArrayXd mPrevLogMag;
  ArrayXd mPrevPrevLogMag;
  ArrayXd mPrevPhase;
  ArrayXd mPrevPhaseDeltaT;
  ArrayXd mPrevPrevPhase;
};
} // namespace algorithm
} // namespace fluid


#include "zmpUtil.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using namespace Eigen;
using namespace std;

MatrixXd expm(const MatrixXd& A) {
  MatrixXd F;
  MatrixExponential<MatrixXd>(A).compute(F);
  return F;
}

ExponentialPlusPiecewisePolynomial<double> s1Trajectory(const TVLQRData &sys, const PiecewisePolynomial<double> &zmp_trajectory,const Ref<const MatrixXd> &S) {
  size_t n = static_cast<size_t>(zmp_trajectory.getNumberOfSegments());
  int d = zmp_trajectory.getSegmentPolynomialDegree(0);
  int k = d + 1;

  for (size_t i = 1; i < n; i++) {
    assert(zmp_trajectory.getSegmentPolynomialDegree(i) == d);
  }

  VectorXd dt(n);
  std::vector<double> breaks = zmp_trajectory.getSegmentTimes();

  for (size_t i = 0; i < n; i++) {
    dt(i) = breaks[i + 1] - breaks[i];
  }

  MatrixXd zmp_tf = zmp_trajectory.value(zmp_trajectory.getEndTime());
  PiecewisePolynomial<double> zbar_pp = zmp_trajectory - zmp_tf;

  Matrix2d R1i = sys.R1.inverse();
  MatrixXd NB = sys.N.transpose() + sys.B.transpose() * S; //2 x 4
  Matrix4d A2 = NB.transpose() * R1i * sys.B.transpose() - sys.A.transpose();
  MatrixXd B2 = 2 * (sys.C.transpose() - NB.transpose() * R1i * sys.D) * sys.Qy; //4 x 2
  Matrix4d A2i = A2.inverse();

  MatrixXd alpha = MatrixXd::Zero(4, n);

  vector<MatrixXd> beta;
  VectorXd s1dt;

  for (size_t i = 0; i < n ; i++) {
    beta.push_back(MatrixXd::Zero(4, k));
  }

  for (int j = static_cast<int>(n) - 1; j >= 0; j--) {

    auto poly_mat = zbar_pp.getPolynomialMatrix(j);
    size_t nq = poly_mat.rows();
    MatrixXd poly_coeffs = MatrixXd::Zero(nq, k);

    for (size_t x = 0; x < nq; x++) {
      poly_coeffs.row(x) = poly_mat(x).getCoefficients().transpose();
    }

    beta[j].col(k - 1) = -A2i * B2 * poly_coeffs.col(k - 1);

    for (int i = k - 2; i >= 0; i--) {
      beta[j].col(i) = A2i * ((i+1) * beta[j].col(i + 1) - B2 * poly_coeffs.col(i));
    }

    if (j == n - 1) {
      s1dt = VectorXd::Zero(4);
    } else {
      s1dt = alpha.col(j+1) + beta[j + 1].col(0);
    }

    VectorXd dtpow(k);
    for (size_t p = 0; p < k; p++) {
      dtpow(p) = pow(dt(j), static_cast<int>(p));
    }

    alpha.col(j) = expm(A2*dt(j)).inverse() * (s1dt - beta[j]*dtpow);
  }

  vector<PiecewisePolynomial<double>::PolynomialMatrix> polynomial_matrices;
  for (int segment = 0; segment < n ; segment++) {
    PiecewisePolynomial<double>::PolynomialMatrix polynomial_matrix(4, 1);
    for(int row = 0; row < 4; row++) {
      polynomial_matrix(row) = Polynomial<double>(beta[segment].row(row));
    }
    polynomial_matrices.push_back(polynomial_matrix);
  }

  PiecewisePolynomial<double> pp_part = PiecewisePolynomial<double>(polynomial_matrices, breaks);
  auto s1traj = ExponentialPlusPiecewisePolynomial<double>(Matrix4d::Identity(), A2, alpha, pp_part);
  return s1traj;
}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include<Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include<Eigen/SparseQR>
#include <Eigen/OrderingMethods>

#include <chrono>
#include <time.h>

#include "parameters.h"
#include "Set_2DAV_diags.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::SparseMatrix;
using Eigen::BiCGSTAB;
using Eigen::MatrixXd;
using Eigen::SimplicialCholeskyLDLT;

typedef Eigen::Triplet<double> Trp;  //TrpripleTrps are objecTrp s of form: row index, column index, value.  used to fill sparse matrices

int main()
{

    std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();  //start clock timer

    VectorXd x(num_elements), b(num_elements);
    SparseMatrix<double> A(num_elements,num_elements);
    //The creation of these 2 matrices is super slow!So
    MatrixXd epsilon = MatrixXd::Ones(num_cell+1,num_cell+1);  //NOTE: this is num_cell+1*num_cell+1 (NOT num_elements^2), 1 element per each physical grid pt. make matrix of ones
    MatrixXd netcharge = MatrixXd::Zero(num_cell+1,num_cell+1);  //make matrix of zeroes

    std::vector<double> main_diag (num_elements+1);
    std::vector<double> upper_diag(num_elements);
    std::vector<double> lower_diag(num_elements);
    std::vector<double> far_lower_diag(num_elements-N+1);
    std::vector<double> far_upper_diag(num_elements-N+1);

    std::vector<double> V(num_elements+1); //vector for solution, electric potential
    std::vector<double> rhs(num_elements+1);

    //fill A and b
    //Define boundary conditions and initial conditions
    double Va = 1.;
    double V_bottomBC = 0.;
    double V_topBC= Va/Vt;

    //Initial conditions
    double diff = (V_topBC - V_bottomBC)/num_cell;
    int index = 0;
    for (int j = 1;j<=N;j++){//  %corresponds to z coord
        index++;
        V[index] = diff*j;
        for (int i = 2;i<=N;i++){//  %elements along the x direction assumed to have same V
            index++;
            V[index] = V[index-1];
        }
    }

    //side BCs, insulating BC's
    std::vector<double> V_leftBC(N+1);
    std::vector<double> V_rightBC(N+1);

    for(int i = 1;i<=N;i++){
        V_leftBC[i] = V[(i-1)*N +  1];
        V_rightBC[i] = V[i*N];
    }

    //Set up matrix equation
    main_diag = set_main_Vdiag(epsilon, main_diag);
    upper_diag = set_upper_Vdiag(epsilon, upper_diag);
    lower_diag = set_lower_Vdiag(epsilon, lower_diag);
    far_upper_diag = set_far_upper_Vdiag(epsilon, far_upper_diag);
    far_lower_diag = set_far_lower_Vdiag(epsilon, far_lower_diag);

    //setup rhs of Poisson eqn.
    int index2 = 0;
    for(int j = 1;j<=N;j++){
        if(j==1){
            for(int i = 1;i<=N;i++){
                index2++;                //THIS COULD BE MODIFIES TO a switch ,case statement--> might be cleaner
                if(i==1){
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_leftBC[1] + V_bottomBC);
                }else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_rightBC[1] + V_bottomBC);
                else
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_bottomBC;
            }
        }else if(j==N){
            for(int i = 1; i<=N;i++){
                index2++;
                if(i==1)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_leftBC[N] + V_topBC);
                else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_rightBC[N] + V_topBC);
                else
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_topBC;
            }
        }else{
            for(int i = 1;i<=N;i++){
                index2++;
                if(i==1)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_leftBC[j];
                else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_rightBC[j];
                else
                    rhs[index2] = netcharge(i,j);
            }
        }
    }

   //setup the triplet list for sparse matrix

    std::vector<Trp> triplet_list(5*num_elements);   //approximate the size that need         // list of non-zeros coefficients in triplet form(row index, column index, value)
    int trp_cnt = 0;
    for(int i = 1; i< main_diag.size(); i++){
        b(i-1) = rhs[i];   //fill VectorXd  rhs of the equation
          triplet_list[trp_cnt] = {i-1, i-1, main_diag[i]};
          trp_cnt++;
        //triplet_list.push_back(Trp(i-1,i-1,main_diag[i]));;   //triplets for main diag
    }
    for(int i = 1;i< upper_diag.size();i++){
        triplet_list[trp_cnt] = {i-1, i, upper_diag[i]};
        trp_cnt++;
        //triplet_list.push_back(Trp(i-1, i, upper_diag[i]));  //triplets for upper diagonal
     }
     for(int i = 1;i< lower_diag.size();i++){
         triplet_list[trp_cnt] = {i, i-1, lower_diag[i]};
         trp_cnt++;
        // triplet_list.push_back(Trp(i, i-1, lower_diag[i]));
     }
     for(int i = 1;i< far_upper_diag.size();i++){
         triplet_list[trp_cnt] = {i-1, i-1+N, far_upper_diag[i]};
         trp_cnt++;
         //triplet_list.push_back(Trp(i-1, i-1+N, far_upper_diag[i]));
         triplet_list[trp_cnt] = {i-1+N, i-1, far_lower_diag[i]};
         trp_cnt++;
         //triplet_list.push_back(Trp(i-1+N, i-1, far_lower_diag[i]));
      }

    A.setFromTriplets(triplet_list.begin(), triplet_list.end());    //A is our sparse matrix

    //using conjugate gradient--> this is faster than BiCGSTAB
/*
    Eigen::ConjugateGradient<SparseMatrix<double>, Eigen::UpLoType::Lower|Eigen::UpLoType::Upper > cg;
    cg.compute(A);
   x = cg.solve(b);
    std::cout << "#iterations:     " << cg.iterations() << std::endl;
     std::cout << "estimated error: " << cg.error()      << std::endl;

*/

  /*  //using sparse LU
     Eigen::SparseLU<SparseMatrix<double>>  sLU;
    sLU.analyzePattern(A);
    sLU.factorize(A);
    x = sLU.solve(b);
    */

   //using Sparse Cholesky  (THIS SEEMS TO BE THE FASTEST for the 2D Poisson problem)
    Eigen::SimplicialLDLT<SparseMatrix<double>, Eigen::UpLoType::Lower, Eigen::AMDOrdering<int>> SCholesky; //Note using NaturalOrdering is much much slower
    SCholesky.analyzePattern(A);
    SCholesky.factorize(A);
    x = SCholesky.solve(b);

    //NOTE, for repeat solves, I might not need to keep factorizing! Can just subtitute in the new rhs every time, and use solve()

    /*
    //using SparseQR  (is very slow, compared to others for large matrices).
    Eigen::SparseQR<SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SQR;  //NOTE: this ordering parameter is needed for this to work
    SQR.analyzePattern(A);
    SQR.factorize(A);
    x = SQR.solve(b);
    */

    //using BiCGSTAB: this is probably useful for non-symmetric matrices, i.e. not Poisson equation which is symmetric. In that case, Cholesky might not work.

    BiCGSTAB<SparseMatrix<double>, Eigen::IncompleteLUT<double>> solver;  //BiCGStab solver object
    solver.compute(A);  //this computes the preconditioner
    x = solver.solve(b);
    std::cout << "#iterations:     " << solver.iterations() << std::endl;
    std::cout << "estimated error: " << solver.error()      << std::endl;

    std::cout << x <<std::endl;  //they have an overloaded << for seeing result!

    std::chrono::high_resolution_clock::time_point finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time = std::chrono::duration_cast<std::chrono::duration<double>>(finish-start);
    std::cout << "1 Va CPU time = " << time.count() << std::endl;

    return 0;
}


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

#include "semimprk.h"

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

#include "../../../lecturecodes/helperfiles/polyfit.h"

namespace SemImpRK {

/* SAM_LISTING_BEGIN_0 */
double CvgRosenbrock() {
  double cvgRate = 0.0;
  // Use polyfit to estimate the rate of convergence
  // for SolveRosenbrock.
#if SOLUTION
  // Final time
  const double T = 10.0;
  // Timestep sizes h=2^{-k} for which discretization erorrs should be computed
  const Eigen::ArrayXd K = Eigen::ArrayXd::LinSpaced(7, 4, 10);
  // Initial state vector
  Eigen::Vector2d y0(1., 1.);

  // Parameter and useful matrix for f
  const double lambda = 1;
  Eigen::Matrix2d R;
  R << 0.0, -1.0, 1.0, 0.0;
  // Lambda functions giving the right-hand side function f
  // and its Jacobian df
  auto f = [&R, &lambda](Eigen::Vector2d y) {
    return R * y + lambda * (1.0 - y.squaredNorm()) * y;
  };
  auto df = [&lambda](Eigen::Vector2d y) {
    double x = 1 - y.squaredNorm();
    Eigen::Matrix2d J;
    J << lambda * x - 2 * lambda * y(0) * y(0), -1 - 2 * lambda * y(1) * y(0),
        1 - 2 * lambda * y(1) * y(0), lambda * x - 2 * lambda * y(1) * y(1);
    return J;
  };

  // timestep size h=2^{-12} (=> M = T*2^{12}) used to compute a reference
  // solution
  const int M_ref = T * std::pow(2, 12);
  // Reference solution
  std::vector<Eigen::VectorXd> solref = SolveRosenbrock(f, df, y0, M_ref, T);
  // Arrays for recording error norms and numbers of timesteps
  Eigen::ArrayXd Error(K.size());
  Eigen::ArrayXd M(K.size());
  // Loop over all temporal meshes
  for (unsigned int i = 0; i < K.size(); ++i) {
    // h = 2^{-k} => M = T*h = T*2^k
    M(i) = T * std::pow(2, K[i]);

    // Compute solution
    std::vector<Eigen::VectorXd> sol = SolveRosenbrock(f, df, y0, M(i), T);

    // Compute error
    double maxerr = 0;
    for (unsigned int j = 0; j < sol.size(); ++j) {
      maxerr = std::max(maxerr, (sol[j] - solref[j * M_ref / M(i)]).norm());
    }
    Error(i) = maxerr;
  }
  // Print Error table
  std::cout << std::setw(15) << "M" << std::setw(16) << "maxerr \n";
  for (int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << Error(i)
              << std::endl;
  }
  // Estimate convergence rate using linear regression provided by the auxiliary
  // function polyfit()
  cvgRate = -polyfit(M.log(), Error.log(), 1)(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return cvgRate;
}
/* SAM_LISTING_END_0 */

}  // namespace SemImpRK