/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    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.
*/

#ifndef GEO_UTILS_HPP
#define GEO_UTILS_HPP

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

#include <Eigen/Eigen>

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

namespace geo_utils
{

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    inline bool findInterior(const Eigen::MatrixX4d &hPoly,
                             Eigen::Vector3d &interior)
    {
        const int m = hPoly.rows();

        Eigen::MatrixX4d A(m, 4);
        Eigen::VectorXd b(m);
        Eigen::Vector4d c, x;
        const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm();
        A.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm;
        A.rightCols<1>().setConstant(1.0);
        b = -hPoly.rightCols<1>().array() / hNorm;
        c.setZero();
        c(3) = -1.0;

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

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

    inline bool overlap(const Eigen::MatrixX4d &hPoly0,
                        const Eigen::MatrixX4d &hPoly1,
                        const double eps = 1.0e-6)

    {
        const int m = hPoly0.rows();
        const int n = hPoly1.rows();
        Eigen::MatrixX4d A(m + n, 4);
        Eigen::Vector4d c, x;
        Eigen::VectorXd b(m + n);
        A.leftCols<3>().topRows(m) = hPoly0.leftCols<3>();
        A.leftCols<3>().bottomRows(n) = hPoly1.leftCols<3>();
        A.rightCols<1>().setConstant(1.0);
        b.topRows(m) = -hPoly0.rightCols<1>();
        b.bottomRows(n) = -hPoly1.rightCols<1>();
        c.setZero();
        c(3) = -1.0;

        const double minmaxsd = sdlp::linprog<4>(c, A, b, x);

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

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

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

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // proposed epsilon is 1.0e-6
    inline void enumerateVs(const Eigen::MatrixX4d &hPoly,
                            const Eigen::Vector3d &inner,
                            Eigen::Matrix3Xd &vPoly,
                            const double epsilon = 1.0e-6)
    {
        const Eigen::VectorXd b = -hPoly.rightCols<1>() - hPoly.leftCols<3>() * inner;
        const Eigen::Matrix<double, 3, -1, Eigen::ColMajor> A =
            (hPoly.leftCols<3>().array().colwise() / b.array()).transpose();

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

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // proposed epsilon is 1.0e-6
    inline bool enumerateVs(const Eigen::MatrixX4d &hPoly,
                            Eigen::Matrix3Xd &vPoly,
                            const double epsilon = 1.0e-6)
    {
        Eigen::Vector3d inner;
        if (findInterior(hPoly, inner))
        {
            enumerateVs(hPoly, inner, vPoly, epsilon);
            return true;
        }
        else
        {
            return false;
        }
    }

} // namespace geo_utils

#endif


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

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

// This file was modified by Oracle on 2016-2020.
// Modifications copyright (c) 2016-2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>
#include <type_traits>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/make.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/static_assert.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Dimension.",
        std::integral_constant<std::size_t, Dimension>);
};

template <>
struct cross_product<2>
{
    template <typename P1, typename P2, typename ResultP>
    static void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();
        assert_dimension<ResultP, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        set<0>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

template <>
struct cross_product<3>
{
    template <typename P1, typename P2, typename ResultP>
    static void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        set<0>(result, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(result, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }

    template <typename ResultP, typename P1, typename P2>
    static constexpr ResultP apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        return traits::make<ResultP>::apply(
                get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2),
                get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2),
                get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

*/

template
<
    typename ResultP, typename P1, typename P2,
    std::enable_if_t
        <
            dimension<ResultP>::value != 3
         || ! traits::make<ResultP>::is_specialized,
            int
        > = 0
>
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<ResultP>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P2>) );

    ResultP result;
    detail::cross_product<dimension<ResultP>::value>::apply(p1, p2, result);
    return result;
}

template
<
    typename ResultP, typename P1, typename P2,
    std::enable_if_t
        <
            dimension<ResultP>::value == 3
         && traits::make<ResultP>::is_specialized,
            int
        > = 0
>
// workaround for VS2015
#if !defined(_MSC_VER) || (_MSC_VER >= 1910)
constexpr
#endif
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<ResultP>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P1>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P2>));

    return detail::cross_product<3>::apply<ResultP>(p1, p2);
}

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

\qbk{[heading Examples]}
\qbk{[cross_product] [cross_product_output]}
*/
template
<
    typename P,
    std::enable_if_t
        <
            dimension<P>::value != 3
         || ! traits::make<P>::is_specialized,
            int
        > = 0
>
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    P result;
    detail::cross_product<dimension<P>::value>::apply(p1, p2, result);
    return result;
}

template
<
    typename P,
    std::enable_if_t
        <
            dimension<P>::value == 3
         && traits::make<P>::is_specialized,
            int
        > = 0
>
// workaround for VS2015
#if !defined(_MSC_VER) || (_MSC_VER >= 1910)
constexpr
#endif
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    return detail::cross_product<3>::apply<P>(p1, p2);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP


// Statistical Computing //
// Load typical packages

#include<cmath>
#include<iostream>
#include<iomanip>

// Typiccal C libraries
#include<cmath>
#include<cstdlib>

// C++ Data Structures
#include<set>
#include<map>
#include<vector>

// Include Eigen Package for Matrices
#include <Eigen/Core>

// Load Matrix615 header file to read in from file
#include "Matrix615.h"

using namespace std;
// we avoid using namespace Eigen to be able to clearly see where we are calling the Eigen Package

// Driver Code
int main(int argc, char* argv[]) {

	// Steps corresponding to the commented code is associated with the following documents:

	// Tentative Read Matrix using Matrix615.h

	Matrix615<double> read_Design; // throw-away Matrices to read in data
	Matrix615<double> read_Y;
	Matrix615<double> read_ID;

	// Read the data in
	read_Design.readFromFile(argv[3]); // t
	read_Y.readFromFile(argv[2]);
	read_ID.readFromFile(argv[1]);

	// Step (1): Store the design matrix, response, and Id into an Eigen Matrix
	Eigen::MatrixXd	design_X;
	Eigen::MatrixXd outcome_Y;

	read_Design.cloneToEigen(design_X);
	read_Y.cloneToEigen(outcome_Y);

	// cout << Weights.rows() << endl << endl;
	// cout << Weights << endl;
	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	Weights.setOnes();

	if (int (argc) == 5) {
		Matrix615<double> read_Weights;
		read_Weights.readFromFile(argv[4]);

		Eigen::MatrixXd Weights;
		read_Weights.cloneToEigen(Weights);
	}

	// else {
	// 	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	// 	Weights.setOnes();
	// }

	// cout << design_X.rows() << " " << design_X.cols() << endl << endl;
	// cout << design_X << endl << endl << endl;
	// cout << outcome_Y.rows() << " " << outcome_Y.cols() << endl << endl;
	// cout << outcome_Y << endl << endl << endl;

	// Step (2): Count number of unique ID's, patients
	Eigen::MatrixXd ID_Mat;
	read_ID.cloneToEigen(ID_Mat);

	std::set<int> unique_ID; // to dynamically count n
	std::map<int, int> id_map; // to dynamically count m
	std::map<int, double> weight_map;

	for (int i = 0; i < int(ID_Mat.rows()); ++i) {
		unique_ID.insert(  ID_Mat(i,0) );
		id_map[ID_Mat(i, 0)] += 1;
		weight_map[ID_Mat(i, 0)] = Weights(i, 0);
	}

	int n = int( unique_ID.size() );
	// int m = int( id_map[ID_Mat(1,0)] );

	// q = (p + 1) : # number of paramters to be estimated using GEE
	int q = int ( design_X.cols() );

	// Correlation Structure

	// Step : Weights

	// Step (3): Initialize betas *********** Should we incorporate the beta's that come from a fast linear regression or no?

	Eigen::MatrixXd Betas_new(q , 1); // dimensions (p+1)  x 1
	Betas_new.setZero();

	// cout << "Betas ***********************" << endl;
	// cout << Betas_new << endl << endl;

	// Step (4): Initialize rho and phi (rho: off diagonals of correlation structure & phi: )
	double rho = 0.0;
	// double phi = 0.0;

	// Step (5): Set up variables for iteration convergence check

	// Should we make this dynamic?

	double diff_beta = 1; // updated difference in beta estimation {beta (new) - beta (old)}

	double diff_threshold = 0.00000010; // threshold for the update difference in betas

	int iteration_threshold = 1000; // threshold on how many iterations there will be

	// Step (6): Initialize the iteration
	int iteration_count = 0;

	// Step (7): Assign appropriate value to n*
	// double n_star = (0.5) * double(n) * double(m) * double(m - 1);

	// double n_star = (0.5) * double(n) * double(5) * double(5 - 1);

	// cout << " n* is " << n_star << endl;

	double n_star_sum = 0.0;

	// Step (8): Start the GEE Estimation

	Eigen::MatrixXd Betas_updated = Betas_new;

	Eigen::MatrixXd sandwhich_Mat;

	// cout << Betas_updated << endl << endl;

	while ( (diff_beta > diff_threshold) && (iteration_count < iteration_threshold) ) {
		// (1)
		Betas_updated = Betas_new;

		// (2) Vector EE ((p+1) x 1)
		Eigen::MatrixXd EE(q , 1);
		EE.setZero();

		// (3) Matrix GI ( (p+1) x (p+1) )
		// First derivative of EE; Model-based Variance
		Eigen::MatrixXd GI(q , q);
		GI.setZero();

		// (4) Matrix G ( (p+1) x (p+1) )
		// Meat of the Sandwhich Estimator
		Eigen::MatrixXd G(q , q);
		G.setZero();

		// (5) Initialize phi(sum) and tau(sum)
		double phi_sum = 0.0;
		double tau_sum = 0.0;

		// (6) Initialize start and end
		int start = 0;
		int end = -1;

		// inner loop in while loop Time-Complexity: O(n^2)??

		for (int i = 0; i < n; ++i) {

			int m = id_map[i + 1];
			start = end + 1;
			end = start + m - 1;

			//update n*
			n_star_sum = n_star_sum + (0.5) * double(m) * double(double(m) - 1.0);

			// assign mu_i ( m x (p+1) ) for ith person
			// Eigen.block(starting_row = , starting_col = , dim_row = , dim_col = )
			Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

			// assign r_i (m x 1)
			Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

			// Loop to update phi-sum
			for (int j = 0; j < int(m); ++j) phi_sum = phi_sum + (r_i(j,0) * r_i(j,0));

			// Loop to update tau-sum
			for (int j = 0; j < int(m - 1); ++j) {
				for (int k = (j + 1); k < m; ++k) tau_sum = tau_sum + r_i(j,0) * r_i(k, 0);
			}

			// create R matrix (m x m) for each observation/patient
			Eigen::MatrixXd R(m, m);
			R.setConstant(rho); // off-diagonals will be rho
			for (int d = 0; d < int(m); ++d) R(d, d) = 1; //diagonals will be 1

			// update EE ((p+1) x 1)
			EE = EE + ((design_X.block(start, 0, m, q).transpose()) * (R.inverse() * (weight_map[i+1] * r_i) )); // weight_map

			// update GI ((p+1) x (p+1)) or (q x q)
			GI = GI + ((design_X.block(start, 0, m, q).transpose()) * R.inverse() *  design_X.block(start, 0, m, q));

			// update G ((p+1) x (p+1)) or (q x q)
			G = G + ((design_X.block(start, 0, m, q).transpose()) *
				( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));
		}

		// Update beta using either Newton Raphson or Gradient Boosting *** we can add an if statement
		Betas_new = Betas_updated + ( GI.inverse() * EE);

		// calculate the difference in the betas
		diff_beta = (Betas_new - Betas_updated).norm();

		// update rho
		rho = ( (( double(n) - double(q) ) * tau_sum) / ( (double(n_star_sum) - double(q) ) * phi_sum) );

		sandwhich_Mat = GI.inverse() * G * GI.inverse();
		// increment iteration count
		iteration_count += 1;
	}

	cout << endl << "Estimated Betas are: " << endl;
	cout << Betas_new.transpose() << endl << endl << endl;

	// cout << "The sandwhich matrix is: " << endl << endl;
	// cout << sandwhich_Mat << endl << endl;

	cout << "The beta Robust S.E.'s are: " << endl << endl;
	cout << sandwhich_Mat.diagonal().array().sqrt() << endl << endl;

	return 0;

}


#pragma once

#include <Eigen/Dense>

class CubicKernel
{
public:
	 double getRadius() { return m_radius; }
	 void setRadius(double val)
	{
		m_radius = val;
		const double pi = static_cast<double>(M_PI);

		const double h3 = m_radius*m_radius*m_radius;
		m_k = 8.0 / (pi*h3);
		m_l = 48.0 / (pi*h3);
		m_W_zero = W(Eigen::Vector3d::Zero());
	}

public:
	double W(Eigen::Vector3d const& r)
	{
		double res = 0.0;
		const double rl = r.norm();
		const double q = rl/m_radius;
		if (q <= 1.0)
		{
			if (q <= 0.5)
			{
				const double q2 = q*q;
				const double q3 = q2*q;
				res = m_k * (6.0*q3-6.0*q2+1.0);
			}
			else
			{
				auto _1mq = 1.0 - q;
				res = m_k * (2.0*_1mq*_1mq*_1mq);
			}
		}
		return res;
	}

	Eigen::Vector3d gradW(const Eigen::Vector3d &r)
	{
		using namespace Eigen;
		Vector3d res;
		const double rl = r.norm();
		const double q = rl / m_radius;
		if (q <= 1.0)
		{
			if (rl > 1.0e-6)
			{
				const Vector3d gradq = r * ((double) 1.0 / (rl*m_radius));
				if (q <= 0.5)
				{
					res = m_l*q*((double) 3.0*q - (double) 2.0)*gradq;
				}
				else
				{
					const double factor = 1.0 - q;
					res = m_l*(-factor*factor)*gradq;
				}
			}
		}
		else
			res.setZero();

		return res;
	}

	double W_zero()
	{
		return m_W_zero;
	}

private:
	double m_radius;
	double m_k;
	double m_l;
	double m_W_zero;
};


#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

namespace vpp
{

  // http://www.cse.psu.edu/~rcollins/CSE486/lecture19_6pp.pdf
  // Page 5.
  inline vfloat2 epipole_left(const Eigen::Matrix3f& F)
  {
    // F e_l = 0.
    Eigen::EigenSolver<Eigen::Matrix3d> es(F.cast<double>().transpose() * F.cast<double>());

    auto values = es.eigenvalues();
    auto vectors = es.eigenvectors();
    vdouble2 epipole;
    double min_ev = FLT_MAX;
    for (int i = 0; i < values.size(); i++)
      if (values[i].real() < min_ev)
      {
        min_ev = values[i].real();
        auto vc = vectors.col(i);
        epipole[0] = vc[0].real();
        epipole[1] = vc[1].real();
        epipole /= double(vc[2].real());
      }

    return epipole.cast<float>();
  }

  // http://www.cse.psu.edu/~rcollins/CSE486/lecture19_6pp.pdf
  // Page 5.
  inline vfloat2 epipole_right(const Eigen::Matrix3f& F)
  {
    // e_r F  = 0.
    Eigen::EigenSolver<Eigen::Matrix3d> es(F.cast<double>() * F.cast<double>().transpose());

    auto values = es.eigenvalues();
    auto vectors = es.eigenvectors();
    vdouble2 epipole;
    double min_ev = FLT_MAX;
    for (int i = 0; i < values.size(); i++)
      if (values[i].real() < min_ev)
      {
        min_ev = values[i].real();
        auto vc = vectors.col(i);
        epipole[0] = vc[0].real();
        epipole[1] = vc[1].real();
        epipole /= double(vc[2].real());
      }

    return epipole.cast<float>();
  }
}


/**
 * @file crossprod.cc
 * @brief NPDE homework CrossProd code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "crossprod.h"

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

namespace CrossProd {

/* SAM_LISTING_BEGIN_0 */
void tab_crossprod() {
  // TO DO (13-1.e): solve the cross-product ODE with the implicit RK method
  // defined in solve_imp_mid. Tabulate the norms of the results at all steps.
  double T = 10.;
  int N = 128;
  // set data
  double c = 1.;
  Eigen::Vector3d y0(1., 1., 1.);
  Eigen::Vector3d a(1., 0., 0.);

  // define rhs
  auto f = [a, c](Eigen::Vector3d y) -> Eigen::Vector3d {
    return a.cross(y) + c * y.cross(a.cross(y));
  };
  // define Jacobian of rhs
  auto Jf = [a, c](Eigen::Vector3d y) -> Eigen::Matrix3d {
    Eigen::Matrix3d temp;
    temp << -c * (a(1) * y(1) + a(2) * y(2)),
        c * (2 * a(0) * y(1) - a(1) * y(0)) - a(2),
        a(1) + c * (2 * a(0) * y(2) - a(2) * y(0)),
        a(2) - c * (a(0) * y(1) - 2 * a(1) * y(0)),
        -c * (a(0) * y(0) + a(2) * y(2)),
        c * (2 * a(1) * y(2) - a(2) * y(1)) - a(0),
        -a(1) - c * (a(0) * y(2) - 2 * a(2) * y(0)),
        a(0) - c * (a(1) * y(2) - 2 * a(2) * y(1)),
        -c * (a(0) * y(0) + a(1) * y(1));
    return temp;
  };

  std::vector<Eigen::VectorXd> res_imp = solve_imp_mid(f, Jf, T, y0, N);

  std::cout << "1. Implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))"
            << std::endl;

  for (int i = 0; i < N + 1; ++i) {
    std::cout << std::setw(10) << T * i / N << std::setw(15)
              << res_imp[i].norm() << std::endl;
  }
  /* SAM_LISTING_END_0 */

  /* SAM_LISTING_BEGIN_1 */
  // TO DO (13-1.g): solve the cross-product ODE with the implicit RK method
  // defined in solve_lin_mid. Tabulate the norms of the results at all steps.
  std::vector<Eigen::VectorXd> res_lin = solve_lin_mid(f, Jf, T, y0, N);

  std::cout << "\n2. Linear implicit midpoint method" << std::endl;
  std::cout << std::setw(10) << "t" << std::setw(15) << "norm(y(t))"
            << std::endl;
  for (int i = 0; i < N + 1; ++i) {
    std::cout << std::setw(10) << T * i / N << std::setw(15)
              << res_lin[i].norm() << std::endl;
  }
  /* SAM_LISTING_END_1 */
}

}  // namespace CrossProd


#include "problemes.h"
#include "arithmetique.h"
#include "graphe.h"

#include <fstream>
#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

typedef std::pair<nombre, nombre> paire;

ENREGISTRER_PROBLEME(107, "Minimal network") {
    // The following undirected network consists of seven vertices and twelve edges with a total weight of 243.
    //
    //
    // The same network can be represented by the matrix below.
    //
    //      	A	B	C	D	E	F	G
    //      A	-	16	12	21	-	-	-
    //      B	16	-	-	17	20	-	-
    //      C	12	-	-	28	-	31	-
    //      D	21	17	28	-	18	19	23
    //      E	-	20	-	18	-	-	11
    //      F	-	-	31	19	-	-	27
    //      G	-	-	-	23	11	27	-
    //
    // However, it is possible to optimise the network by removing some edges and still ensure that all points on the
    // network remain connected. The network which achieves the maximum saving is shown below. It has a weight of 93,
    // representing a saving of 243 − 93 = 150 from the original network.
    //
    // Using network.txt (right click and 'Save Link/Target As...'), a 6K text file containing a network with forty
    // vertices, and given in matrix form, find the maximum saving which can be achieved by removing redundant edges
    // whilst ensuring that the network remains connected.
    std::ifstream ifs("data/p107_network.txt");

    graphe::Kruskal::aretes A;
    nombre i = 0;
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> strings;
        boost::split(strings, ligne, boost::is_any_of(","));

        nombre j = 0;
        for (auto &s: strings) {
            if (s != "-" && i < j)
                A.emplace_back(i, j, std::stoull(s));
            ++j;
        }
        ++i;
    }

    graphe::Kruskal kruskal(A);
    auto arbre_mini = kruskal.algorithme();

    auto somme_poids = [](const nombre &r, const graphe::Kruskal::arete &a) { return r + std::get<2>(a); };

    nombre resultat = std::accumulate(A.begin(), A.end(), 0ull, somme_poids) -
                      std::accumulate(arbre_mini.begin(), arbre_mini.end(), 0ull, somme_poids);
    return std::to_string(resultat);
}


// Created by dinies on 03/07/2018.

//Collection of static functions and useful types to perform geometric computations
#pragma once

#include <unistd.h>
#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace MultiRobot{
  class MyMath{

  public:
    // static Eigen::Isometry2d v2t(const std::vector<double> &t_vec);
    // static std::vector<double> t2v(const Eigen::Isometry2d& t_transf);

    static Eigen::Isometry2d v2t(const Eigen::Vector3d &t_vec);
    static Eigen::Vector3d t2v(const Eigen::Isometry2d& t_transf);

    static void rotate2D( Eigen::Vector2d &t_point, const double t_angle_rad );

    static std::vector<double> vecSum(const std::vector<double> &t_first,const std::vector<double> &t_second);

    static std::vector<double> vecMultEleWise(const std::vector<double> &t_first,const std::vector<double> &t_second);

    static double boxMinusAngleRad(const double t_ref,const double t_actual);
    static double boxPlusAngleRad(const double t_ref,const double t_actual);
    static double computeAvg(const double t_prevAvg,const int t_numEntries,const double t_newValue);
  };
}


#include <QatGenericFunctions/Variable.h>
#include <QatGenericFunctions/Parameter.h>
#include <QatGenericFunctions/Square.h>
#include <QatGenericFunctions/Sqrt.h>
#include <QatGenericFunctions/Exp.h>
#include <QatDataAnalysis/Hist1D.h>
#include <QatDataAnalysis/Table.h>
#include <QatDataModeling/TableLikelihoodFunctional.h>
#include <QatDataModeling/MinuitMinimizer.h>
#include <QatPlotWidgets/PlotView.h>
#include <QatPlotWidgets/MultipleViewWindow.h>
#include <QatPlotting/PlotStream.h>
#include <QatPlotting/PlotHist1D.h>
#include <QatPlotting/PlotFunction1D.h>
#include <QatPlotting/PlotProfile.h>
#include <QatPlotting/PlotKey.h>
#include <Eigen/Dense>
#include <QApplication>
#include <QMainWindow>
#include <QAction>
#include <QToolBar>
#include <QFont>
#include <cmath>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <random>

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange);

typedef std::vector<double> Data;
typedef std::vector<double> XList;
typedef std::vector<double> YList;

Data readData(const std::string& file);
Hist1D binData(const Data& data);
Table tableData(const Data& data);

const std::string dataFileName = "../data01.dat";
const std::string coarseResultFile = "coarse-result.dat";
const double nBin = 400;
const bool verbose = false;
const int SUCCESS = 3;
const size_t nTrial = 100;
const double wScale = 0.05;
const double nEffTrial = 1 * nTrial;
const double Precision = 32;

// Coarse Search
const double fMin = 0.01;
const double fMax = 0.99;
const double mu1Min = 80;
const double mu1Max = 120;
const double mu2Min = mu1Min;
const double mu2Max = mu1Max;
const double gamma1Min = 0.01;
const double gamma1Max = 1;
const double gamma2Min = gamma1Min;
const double gamma2Max = gamma1Max;
const double pNBins = 200;
const std::string searchType = "Coarse";
const std::string fileExt = searchType + ".png";

int main(int argc, char **argv)
{
    QApplication app(argc, argv);
    MultipleViewWindow window;

    Genfun::Square qPow2;
    Genfun::Sqrt qSqrt;
    Genfun::Exp qExp;

    const Data data = readData(dataFileName);
    Hist1D dataHist = binData(data);
    Table dataTable = tableData(data);
    TableLikelihoodFunctional objFunc(dataTable);
    Genfun::Variable X = dataTable.symbol("E");

//  Prepare View
    const double viewXMin = dataHist.binLowerEdge(0) - 10;
    const double viewXMax = dataHist.binUpperEdge(nBin-1) + 10;
    const double viewYMin = 1e-2;
    const double viewYMax = dataHist.maxContents() * 3;
    PRectF viewRange(viewXMin, viewXMax, viewYMin, viewYMax);
    PlotView *view = createPlotView("Spectrum", "Energy / eV", "", viewRange);
    view->setLogY(true);
    window.add(view, "Fit");

    PlotKey legend(viewXMin, viewYMax);
    legend.setFont(QFont("Times", 25, QFont::Bold));
    view->add(&legend);

// Plot Data
    PlotHist1D dataHistPlot(dataHist);
    {
        PlotHist1D::Properties prop;
        prop.plotStyle = PlotHist1D::Properties::SYMBOLS;
        prop.symbolSize = 10;
        prop.pen.setWidth(1.9);
        dataHistPlot.setProperties(prop);
    }
    view->add(&dataHistPlot);
    legend.add(&dataHistPlot, "Data");

//  Lorentz Fit
    Genfun::Parameter pMu1("Mu1", 80, dataHist.min(), dataHist.max());
    Genfun::Parameter pMu2("Mu2", 120, dataHist.min(), dataHist.max());
    Genfun::Parameter pGamma1("Gamma1", 0.5, 0, 10*std::sqrt(dataHist.variance()));
    Genfun::Parameter pGamma2("Gamma2", 0.5, 0, 10*std::sqrt(dataHist.variance()));
    Genfun::Parameter pF("fraction", 0.5, 0, 1);

    Genfun::GENFUNCTION LorentzPDF1 = 1 / (M_PI * pGamma1 * (1 + qPow2((X - pMu1)/pGamma1)));
    Genfun::GENFUNCTION LorentzPDF2 = 1 / (M_PI * pGamma2 * (1 + qPow2((X - pMu2)/pGamma2)));
    Genfun::GENFUNCTION Lorentz = dataHist.sum() * (pF * LorentzPDF1 + (1-pF) * LorentzPDF2);

    std::cout << searchType + " Search" << std::endl;

    std::random_device rndDev;
    std::mt19937_64 rndEng(rndDev());
    std::uniform_real_distribution<double> mu1Dist(mu1Min, mu1Max);
    std::uniform_real_distribution<double> mu2Dist(mu2Min, mu2Max);
    std::uniform_real_distribution<double> gamma1Dist(gamma1Min, gamma1Max);
    std::uniform_real_distribution<double> gamma2Dist(gamma2Min, gamma2Max);
    std::uniform_real_distribution<double> fDist(fMin, fMax);
    Hist1D mu1Hist("Mu1", pNBins, mu1Min, mu1Max);
    Hist1D mu2Hist("Mu2", pNBins, mu2Min, mu2Max);
    Hist1D gamma1Hist("Gamma1", pNBins, gamma1Min, gamma1Max);
    Hist1D gamma2Hist("Gamma2", pNBins, gamma2Min, gamma2Max);
    Hist1D fHist("Fraction", pNBins, fMin, fMax);
    double count = nTrial;
    while (count > 0)
    {
        double mu10 = mu1Dist(rndEng);
        double mu20 = mu2Dist(rndEng);
        double gamma10 = gamma1Dist(rndEng);
        double gamma20 = gamma2Dist(rndEng);
        double f0 = fDist(rndEng);

        pMu1.setValue(mu10);
        pMu2.setValue(mu20);
        pGamma1.setValue(gamma10);
        pGamma2.setValue(gamma20);
        pF.setValue(f0);

        MinuitMinimizer lorentzMinimizer(verbose);
        lorentzMinimizer.addParameter(&pMu1);
        lorentzMinimizer.addParameter(&pMu2);
        lorentzMinimizer.addParameter(&pGamma1);
        lorentzMinimizer.addParameter(&pGamma2);
        lorentzMinimizer.addParameter(&pF);
        lorentzMinimizer.addStatistic(&objFunc, &Lorentz);
        lorentzMinimizer.minimize();

        if (lorentzMinimizer.getStatus() == SUCCESS)
        {
            --count;

            Eigen::MatrixXd errMat = lorentzMinimizer.getErrorMatrix();
            double weight = wScale / std::sqrt(errMat.trace());

            if (pMu1.getValue() <= pMu2.getValue())
            {
                mu1Hist.accumulate(pMu1.getValue(), weight);
                mu2Hist.accumulate(pMu2.getValue(), weight);
                gamma1Hist.accumulate(pGamma1.getValue(), weight);
                gamma2Hist.accumulate(pGamma2.getValue(), weight);
                fHist.accumulate(pF.getValue(), weight);
            }else
            {
                mu1Hist.accumulate(pMu2.getValue(),weight);
                mu2Hist.accumulate(pMu1.getValue(), weight);
                gamma1Hist.accumulate(pGamma2.getValue(), weight);
                gamma2Hist.accumulate(pGamma1.getValue(), weight);
                fHist.accumulate(1 - pF.getValue(), weight);
            }
        }
    }

    std::cout << "Mu1 :" << mu1Hist.mean() << " +/- " << std::sqrt(mu1Hist.variance()) << std::endl;
    std::cout << "Mu2 :" << mu2Hist.mean() << " +/- " << std::sqrt(mu2Hist.variance()) << std::endl;
    std::cout << "Gamma1 :" << gamma1Hist.mean() << " +/- " << std::sqrt(gamma1Hist.variance()) << std::endl;
    std::cout << "Gamma2 :" << gamma2Hist.mean() << " +/- " << std::sqrt(gamma2Hist.variance()) << std::endl;
    std::cout << "Fraction :" << fHist.mean() << " +/- " << std::sqrt(fHist.variance()) << std::endl;

    std::ofstream oFile(coarseResultFile);
    oFile << std::setprecision(Precision) << mu1Hist.mean() << "\t";
    oFile << std::setprecision(Precision) << std::sqrt(mu1Hist.variance()) << std::endl;
    oFile << std::setprecision(Precision) << mu2Hist.mean() << "\t";
    oFile << std::setprecision(Precision) << std::sqrt(mu2Hist.variance()) << std::endl;
    oFile << std::setprecision(Precision) << gamma1Hist.mean() << "\t";
    oFile << std::setprecision(Precision) << std::sqrt(gamma1Hist.variance()) << std::endl;
    oFile << std::setprecision(Precision) << gamma2Hist.mean() << "\t";
    oFile << std::setprecision(Precision) << std::sqrt(gamma2Hist.variance()) << std::endl;
    oFile << std::setprecision(Precision) << fHist.mean() << "\t";
    oFile << std::setprecision(Precision) << std::sqrt(fHist.variance()) << std::endl;
    oFile.close();

    pMu1.setValue(mu1Hist.mean());
    pMu2.setValue(mu2Hist.mean());
    pGamma1.setValue(gamma1Hist.mean());
    pGamma2.setValue(gamma2Hist.mean());
    pF.setValue(fHist.mean());

    PlotFunction1D LorentzPlot(Lorentz);
    {
        PlotFunction1D::Properties prop;
        prop.pen.setWidth(2);
        prop.pen.setColor(Qt::red);
        LorentzPlot.setProperties(prop);
    }
    view->add(&LorentzPlot);
    legend.add(&LorentzPlot, "Lorentz");
    view->save("Lorentz" + fileExt);

    PlotView *mu1View = createPlotView("Mu1", "", "", PRectF(mu1Min, mu2Max, 0, nEffTrial));
    PlotHist1D mu1Plot(mu1Hist);
    mu1View->add(&mu1Plot);
    window.add(mu1View, "Mu1");
    mu1View->save("Mu1" + fileExt);

    PlotView *mu2View = createPlotView("Mu2", "", "", PRectF(mu2Min, mu2Max, 0, nEffTrial));
    PlotHist1D mu2Plot(mu2Hist);
    mu2View->add(&mu2Plot);
    window.add(mu2View, "Mu2");
    mu2View->save("Mu2" + fileExt);

    PlotView *gamma1View = createPlotView("Gamma1", "", "", PRectF(gamma1Min, gamma2Max, 0, nEffTrial));
    PlotHist1D gamma1Plot(gamma1Hist);
    gamma1View->add(&gamma1Plot);
    window.add(gamma1View, "Gamma1");
    gamma1View->save("Gamma1" + fileExt);

    PlotView *gamma2View = createPlotView("Gamma2", "", "", PRectF(gamma2Min, gamma2Max, 0, nEffTrial));
    PlotHist1D gamma2Plot(gamma2Hist);
    gamma2View->add(&gamma2Plot);
    window.add(gamma2View, "Gamma2");
    gamma2View->save("Gamma2" + fileExt);

    PlotView *fView = createPlotView("Fraction", "", "", PRectF(fMin, fMax, 0, nEffTrial));
    PlotHist1D fPlot(fHist);
    fView->add(&fPlot);
    window.add(fView, "Fraction");
    fView->save("Fraction" + fileExt);

/*
	QToolBar *toolBar = window.addToolBar("Tools");

    QAction *quitAction = toolBar->addAction("Quit (q)");
    quitAction->setShortcut(QKeySequence("q"));
	QObject::connect(quitAction, SIGNAL(triggered()), &app, SLOT(quit()));

    QAction *saveAction = toolBar->addAction("Save as (s)");
    saveAction->setShortcut(QKeySequence("s"));
    QObject::connect(saveAction, SIGNAL(triggered()), view, SLOT(save()));
*/
	window.show();
	app.exec();

    return 0;
}

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

Data readData(const std::string& file)
{
    std::ifstream dataFile(file);
    double dataIn;
    Data data;

    while (dataFile >> dataIn)
    {
        data.push_back(dataIn);
    }

    return data;
}

Hist1D binData(const Data& data)
{
    std::pair<Data::const_iterator,Data::const_iterator> MIN_MAX_Iter = std::minmax_element(data.begin(), data.end());
    double dataMin = *(MIN_MAX_Iter.first);
    double dataMax = *(MIN_MAX_Iter.second);

    std::cout << "Data Num: " << data.size() << std::endl;
    std::cout << "Data Min: " << dataMin << std::endl;
    std::cout << "Data Max: " << dataMax << std::endl;

    Hist1D dataHist(nBin, std::floor(dataMin-1), std::ceil(dataMax+1));
    for (size_t i = 0; i < data.size(); i++)
    {
        dataHist.accumulate(data[i]);
    }

    std::cout << "Data Mean: " << dataHist.mean() << std::endl;
    std::cout << "Data Variance: " << dataHist.variance() << std::endl;
    std::cout << "Data Sigma: " << std::sqrt(dataHist.variance()) << std::endl;
    std::cout << std::endl;

    return dataHist;
}

Table tableData(const Data& data)
{
    Table dataTable("Data Table");

    for (size_t i = 0; i < data.size(); i++)
    {
        dataTable.add("E", data[i]);
        dataTable.capture();
    }

    return dataTable;
}

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

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange)
{
    PlotView *view_ptr = new PlotView(viewRange);
    view_ptr->setFixedWidth(1200);
    view_ptr->setFixedHeight(800);

    PlotStream titleStream(view_ptr->titleTextEdit());
    titleStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << title
                << PlotStream::EndP();

    PlotStream xLabelStream(view_ptr->xLabelTextEdit());
    xLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << xLabel
                << PlotStream::EndP();

    PlotStream yLabelStream(view_ptr->yLabelTextEdit());
    yLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << yLabel
                << PlotStream::EndP();

    return view_ptr;
}


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

using namespace Eigen;
using namespace std;

void main() {

	{
		Matrix2f m;
		m << 1, 2, 3, 4;
		cout << m << endl;

		RowVectorXd vec1(3);
		vec1 << 1, 2, 3;
		cout << "vec1 : " << vec1 << endl;

		RowVectorXd vec2(4);
		vec2 << 1, 4, 9, 16;
		cout << "vec2 : " << vec2 << endl;

		RowVectorXd joined(7);
		joined << vec1, vec2;
		cout << "joined = " << joined << endl;

		MatrixXf matA(2, 2); matA << 1, 2, 3, 4;
		MatrixXf matB(4, 4);
		matB << matA, matA / 10, matA / 10, matA;
		cout << "matA: \n" << matA << endl;
		cout << "matB: \n" << matB << endl;
	}
	{
		Matrix3f m;
		m.row(0) << 1, 2, 3;
		m.block(1, 0, 2, 2) << 4, 5, 7, 8;
		m.col(2).tail(2) << 6, 9;
		cout << m << endl;
	}
	{
		cout << "A fixed-size array:\n";
		Array33f a1 = Array33f::Zero();
		cout << a1 << endl;

		cout << "A one-dimensional dynamic-size array: \n";
		ArrayXf a2 = ArrayXf::Zero(3);
		cout << a2 << endl;

		cout << "A two-dimensional dynamic-size array:\n";
		ArrayXXf a3 = ArrayXXf::Zero(3, 4);
		cout << a3 << endl;
	}
	{
		float M_PI = 3.14;
		ArrayXXf table(10, 4);
		table.col(0) = ArrayXf::LinSpaced(10, 0, 90);
		table.col(1) = M_PI / 180 * table.col(0);
		table.col(2) = table.col(1).sin();
		table.col(3) = table.col(1).cos();
		std::cout << "  Degrees   Radians      Sine    Cosine\n";
		std::cout << table << std::endl;
	}

	system("pause");
}


// Copyright Paul A. Bristow 2017, 2018
// Copyright John Z. Maddock 2017

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
//  copy at http ://www.boost.org/LICENSE_1_0.txt).

/*! \brief Graph showing differences of Lambert W function double from nearest representable values.

\details

*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;
using boost::math::lambert_wm1;
#include <boost/math/special_functions.hpp>
using boost::math::isfinite;
#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

// For higher precision computation of Lambert W.
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/next.hpp> // For float_distance.
using boost::math::float_distance;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>
#include <utility>
using std::pair;
#include <map>
using std::map;
#include <set>
using std::multiset;
#include <limits>
using std::numeric_limits;
#include <cmath> // exp

/*!
*/

int main()
{
  try
  {
    std::cout << "Lambert W errors graph." << std::endl;
    using boost::multiprecision::cpp_bin_float_50;
    using boost::multiprecision::cpp_bin_float_quad;

    typedef cpp_bin_float_quad HPT; // High precision type.

    using boost::math::float_distance;
    using boost::math::policies::precision;
    using boost::math::policies::digits10;
    using boost::math::policies::digits2;
    using boost::math::policies::policy;

    std::cout.precision(std::numeric_limits<double>::max_digits10);

    //[lambert_w_graph_1

    //] [/lambert_w_graph_1]
    {
      std::map<const double, double> w0s;   // Lambert W0 branch values, default double precision, digits2 = 53.
      std::map<const double, double> w0s_50;   // Lambert W0 branch values digits2 = 50.

      int max_distance = 0;
      int total_distance = 0;
      int count = 0;
      const int bits = 7;
      double min_z = -0.367879; // Close to singularity at -0.3678794411714423215955237701614608727 -exp(-1)
      //double min_z = 0.06; // Above 0.05 switch point.
      double max_z = 99.99;
      double step_z = 0.05;

      for (HPT z = min_z; z < max_z; z += step_z)
      {
        double zd = static_cast<double>(z);
        double w0d = lambert_w0(zd); // double result from same default.
        HPT w0_best = lambert_w0<HPT>(z);
        double w0_best_d = static_cast<double>(w0_best); // reference result.
       // w0s[zd] = (w0d - w0_best_d); // absolute difference.
        // w0s[z] = 100 * (w0 - w0_best) / w0_best; // difference relative % .
        w0s[zd] = float_distance<double>(w0d, w0_best_d); // difference in bits.
        double fd = float_distance<double>(w0d, w0_best_d);
        int distance = static_cast<int>(fd);
        int abs_distance = abs(distance);

         // std::cout << count << " " << zd << " " << w0d << " " << w0_best_d
         //   << ", Difference = " << w0d - w0_best_d << ", % = " << (w0d - w0_best_d) / w0d << ", Distance = " << distance << std::endl;

        total_distance += abs_distance;
        if (abs_distance > max_distance)
        {
          max_distance = abs_distance;
        }
        count++;
      } // for z
      std::cout << "points " << count << std::endl;
      std::cout.precision(3);
      std::cout << "max distance " << max_distance << ", total distances = " << total_distance
        << ", mean distance " << (float)total_distance / count << std::endl;

      typedef std::map<const double, double>::const_iterator Map_Iterator;

   /* for (std::map<const double, double>::const_iterator it = w0s.begin(); it != w0s.end(); ++it)
      {
        std::cout  << " " << *(it) << "\n";
      }
  */
      svg_2d_plot data_plot_0; // <-0.368, -46> <-0.358, -4> <-0.348, 1>...

      data_plot_0.title("Lambert W0 function differences from 'best' for double.")
        .title_font_size(11)
        .x_size(400)
        .y_size(200)
        .legend_on(false)
        //.legend_font_weight(1)
        .x_label("z")
        .y_label("W0 difference (bits)")
        //.x_label_on(true)
        //.y_label_on(true)
        //.xy_values_on(false)
        .x_range(-1, 100.)
        .y_range(-4., +4.)
        .x_major_interval(10.)
        .y_major_interval(2.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        .x_label_font_size(9)
        .y_label_font_size(9)
        //.x_values_on(true)
        //.y_values_on(true)
        .y_values_rotation(horizontal)
        //.plot_window_on(true)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(3) // Needed to avoid stepping on curves.
        //.coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        //.background_border_color(black);
        ;

      data_plot_0.plot(w0s, "W0 branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(0.2);
      //data_plot.plot(wm1s, "W-1 branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plot_0.write("./lambert_w0_errors_graph");

    } // end W0 branch plot.
    { // Repeat for Lambert W-1 branch.

      std::map<const double, double> wm1s;   // Lambert W-1 branch values.
      std::map<const double, double> wm1s_50;   // Lambert Wm1 branch values digits2 = 50.

      int max_distance = 0;
      int total_distance = 0;
      int count = 0;
      const int bits = 7;
      double min_z = -0.367879; // Close to singularity at -0.3678794411714423215955237701614608727 -exp(-1)
                                //double min_z = 0.06; // Above 0.05 switch point.
      double max_z = -0.0001;
      double step_z = 0.001;

      for (HPT z = min_z; z < max_z; z += step_z)
      {
        if (z > max_z)
        {
          break;
        }
        double zd = static_cast<double>(z);
        double wm1d = lambert_wm1(zd); // double result from same default.
        HPT wm1_best = lambert_wm1<HPT>(z);
        double wm1_best_d = static_cast<double>(wm1_best); // reference result.
                                                         // wm1s[zd] = (wm1d - wm1_best_d); // absolute difference.
                                                         // wm1s[z] = 100 * (wm1 - wm1_best) / wm1_best; // difference relative % .
        wm1s[zd] = float_distance<double>(wm1d, wm1_best_d); // difference in bits.
        double fd = float_distance<double>(wm1d, wm1_best_d);
        int distance = static_cast<int>(fd);
        int abs_distance = abs(distance);

         //std::cout << count << " " << zd << " " << wm1d << " " << wm1_best_d
         //  << ", Difference = " << wm1d - wm1_best_d << ", % = " << (wm1d - wm1_best_d) / wm1d << ", Distance = " << distance << std::endl;

        total_distance += abs_distance;
        if (abs_distance > max_distance)
        {
          max_distance = abs_distance;
        }
        count++;

      } // for z
      std::cout << "points " << count << std::endl;
      std::cout.precision(3);
      std::cout << "max distance " << max_distance << ", total distances = " << total_distance
        << ", mean distance " << (float)total_distance / count << std::endl;

      typedef std::map<const double, double>::const_iterator Map_Iterator;

      /* for (std::map<const double, double>::const_iterator it = wm1s.begin(); it != wm1s.end(); ++it)
      {
      std::cout  << " " << *(it) << "\n";
      }
      */
      svg_2d_plot data_plot_m1; // <-0.368, -46> <-0.358, -4> <-0.348, 1>...

      data_plot_m1.title("Lambert W-1 function differences from 'best' for double.")
        .title_font_size(11)
        .x_size(400)
        .y_size(200)
        .legend_on(false)
        //.legend_font_weight(1)
        .x_label("z")
        .y_label("W-1 difference (bits)")
        .x_range(-0.39, +0.0001)
        .y_range(-4., +4.)
        .x_major_interval(0.1)
        .y_major_interval(2.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        .x_label_font_size(9)
        .y_label_font_size(9)
        //.x_values_on(true)
        //.y_values_on(true)
        .y_values_rotation(horizontal)
        //.plot_window_on(true)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(3) // Needed to avoid stepping on curves.
                            //.coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        //.background_border_color(black);
        ;
        data_plot_m1.plot(wm1s, "W-1 branch").line_color(darkblue).shape(none).line_on(true).bezier_on(false).line_width(0.2);
        data_plot_m1.write("./lambert_wm1_errors_graph");
    }
  }
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}  // int main()

   /*
   //[lambert_w_errors_graph_1_output
   Lambert W errors graph.
   points 2008
   max distance 46, total distances = 717, mean distance 0.357

   points 368
   max distance 23, total distances = 329, mean distance 0.894

   //] [/lambert_w_errors_graph_1_output]
   */


#ifndef STAN_MATH_PRIM_FUN_CONSTANTS_HPP
#define STAN_MATH_PRIM_FUN_CONSTANTS_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/inv.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

// TODO(anyone) Use constexpr when moving to C++17

/**
 * Return the base of the natural logarithm.
 *
 * @return Base of natural logarithm.
 */
static constexpr double e() { return boost::math::constants::e<double>(); }

/**
 * Return the Euler's gamma constant.
 *
 * @return Euler's Gamma.
 */
static constexpr double egamma() {
  return boost::math::constants::euler<double>();
}

/**
 * Return the value of pi.
 *
 * @return Pi.
 */
static constexpr double pi() { return boost::math::constants::pi<double>(); }

/**
 * Smallest positive value.
 */
static constexpr double EPSILON = std::numeric_limits<double>::epsilon();

/**
 * Positive infinity.
 */
static constexpr double INFTY = std::numeric_limits<double>::infinity();

/**
 * Negative infinity.
 */
static constexpr double NEGATIVE_INFTY = -INFTY;

/**
 * (Quiet) not-a-number value.
 */
static constexpr double NOT_A_NUMBER = std::numeric_limits<double>::quiet_NaN();

/**
 * Twice the value of \f$ \pi \f$,
 * \f$ 2\pi \f$.
 */
static constexpr double TWO_PI = boost::math::constants::two_pi<double>();

/**
 * The natural logarithm of 0,
 * \f$ \log 0 \f$.
 */
static constexpr double LOG_ZERO = -INFTY;

/**
 * The natural logarithm of machine precision \f$ \epsilon \f$,
 * \f$ \log \epsilon \f$.
 */
const double LOG_EPSILON = std::log(EPSILON);

/**
 * The natural logarithm of \f$ \pi \f$,
 * \f$ \log \pi \f$.
 */
const double LOG_PI = std::log(pi());

/**
 * The natural logarithm of 2,
 * \f$ \log 2 \f$.
 */
static constexpr double LOG_TWO = boost::math::constants::ln_two<double>();

/**
 * The natural logarithm of 0.5,
 * \f$ \log 0.5 = \log 1 - \log 2 \f$.
 */
static constexpr double LOG_HALF = -LOG_TWO;

/**
 * The natural logarithm of 2 plus the natural logarithm of \f$ \pi \f$,
 * \f$ \log(2\pi) \f$.
 */
const double LOG_TWO_PI = LOG_TWO + LOG_PI;

/**
 * The value of one quarter the natural logarithm of \f$ \pi \f$,
 * \f$ \log(\pi) / 4 \f$.
 */
const double LOG_PI_OVER_FOUR = 0.25 * LOG_PI;

/**
 * The natural logarithm of the square root of \f$ \pi \f$,
 * \f$ \log(sqrt{\pi}) \f$.
 */
const double LOG_SQRT_PI = std::log(boost::math::constants::root_pi<double>());

/**
 * The natural logarithm of 10,
 * \f$ \log 10 \f$.
 */
static constexpr double LOG_TEN = boost::math::constants::ln_ten<double>();

/**
 * The value of the square root of 2,
 * \f$ \sqrt{2} \f$.
 */
static constexpr double SQRT_TWO = boost::math::constants::root_two<double>();

/**
 * The value of the square root of \f$ \pi \f$,
 * \f$ \sqrt{\pi} \f$.
 */
static constexpr double SQRT_PI = boost::math::constants::root_pi<double>();

/**
 * The value of the square root of \f$ 2\pi \f$,
 * \f$ \sqrt{2\pi} \f$.
 */
static constexpr double SQRT_TWO_PI
    = boost::math::constants::root_two_pi<double>();

/**
 * The square root of 2 divided by the square root of \f$ \pi \f$,
 * \f$ \sqrt{2} / \sqrt{\pi} \f$.
 */
static constexpr double SQRT_TWO_OVER_SQRT_PI = SQRT_TWO / SQRT_PI;

/**
 * The value of 1 over the square root of 2,
 * \f$ 1 / \sqrt{2} \f$.
 */
static constexpr double INV_SQRT_TWO
    = boost::math::constants::one_div_root_two<double>();

/**
 * The value of 1 over the square root of \f$ \pi \f$,
 * \f$ 1 / \sqrt{\pi} \f$.
 */
static constexpr double INV_SQRT_PI
    = boost::math::constants::one_div_root_pi<double>();

/**
 * The value of 1 over the square root of \f$ 2\pi \f$,
 * \f$ 1 / \sqrt{2\pi} \f$.
 */
static constexpr double INV_SQRT_TWO_PI
    = boost::math::constants::one_div_root_two_pi<double>();

/**
 * The value of 2 over the square root of \f$ \pi \f$,
 * \f$ 2 / \sqrt{\pi} \f$.
 */
static constexpr double TWO_OVER_SQRT_PI
    = boost::math::constants::two_div_root_pi<double>();

/**
 * The value of half the natural logarithm 2,
 * \f$ \log(2) / 2 \f$.
 */
static constexpr double HALF_LOG_TWO = 0.5 * LOG_TWO;

/**
 * The value of half the natural logarithm \f$ 2\pi \f$,
 * \f$ \log(2\pi) / 2 \f$.
 */
const double HALF_LOG_TWO_PI = 0.5 * LOG_TWO_PI;

/**
 * The value of minus the natural logarithm of the square root of \f$ 2\pi \f$,
 * \f$ -\log(\sqrt{2\pi}) \f$.
 */
const double NEG_LOG_SQRT_TWO_PI = -std::log(SQRT_TWO_PI);

/**
 * Largest rate parameter allowed in Poisson RNG
 */
const double POISSON_MAX_RATE = std::pow(2.0, 30);

/**
 * Return positive infinity.
 *
 * @return Positive infinity.
 */
static constexpr inline double positive_infinity() { return INFTY; }

/**
 * Return negative infinity.
 *
 * @return Negative infinity.
 */
static constexpr inline double negative_infinity() { return NEGATIVE_INFTY; }

/**
 * Return (quiet) not-a-number.
 *
 * @return Quiet not-a-number.
 */
static constexpr inline double not_a_number() { return NOT_A_NUMBER; }

/**
 * Returns the difference between 1.0 and the next value
 * representable.
 *
 * @return Minimum positive number.
 */
static constexpr inline double machine_precision() { return EPSILON; }

/**
 * Returns the natural logarithm of ten.
 *
 * @return Natural logarithm of ten.
 */
static constexpr inline double log10() { return LOG_TEN; }

/**
 * Returns the square root of two.
 *
 * @return Square root of two.
 */
static constexpr inline double sqrt2() { return SQRT_TWO; }

}  // namespace math
}  // namespace stan

#endif


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_GNOMONIC_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_GNOMONIC_INTERSECTION_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/formulas/gnomonic_spheroid.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two geodesics using spheroidal gnomonic projection
       as proposed by Karney.
\author See
    - Charles F.F Karney, Algorithms for geodesics, 2011
      https://arxiv.org/pdf/1109.4448.pdf
    - GeographicLib forum thread: Intersection between two geodesic lines
      https://sourceforge.net/p/geographiclib/discussion/1026621/thread/21aaff9f/
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    template <typename, bool, bool, bool, bool> class Direct
>
class gnomonic_intersection
{
public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        return apply(lon_a1, lat_a1, lon_a2, lat_a2, lon_b1, lat_b1, lon_b2, lat_b2, lon, lat, spheroid);
    }

    template <typename Spheroid>
    static inline bool apply(CT const& lona1, CT const& lata1,
                             CT const& lona2, CT const& lata2,
                             CT const& lonb1, CT const& latb1,
                             CT const& lonb2, CT const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        typedef gnomonic_spheroid<CT, Inverse, Direct> gnom_t;

        lon = (lona1 + lona2 + lonb1 + lonb2) / 4;
        lat = (lata1 + lata2 + latb1 + latb2) / 4;
        // TODO: consider normalizing lon

        for (int i = 0; i < 10; ++i)
        {
            CT xa1, ya1, xa2, ya2;
            CT xb1, yb1, xb2, yb2;
            CT x, y;
            double lat1, lon1;

            bool ok = gnom_t::forward(lon, lat, lona1, lata1, xa1, ya1, spheroid)
                   && gnom_t::forward(lon, lat, lona2, lata2, xa2, ya2, spheroid)
                   && gnom_t::forward(lon, lat, lonb1, latb1, xb1, yb1, spheroid)
                   && gnom_t::forward(lon, lat, lonb2, latb2, xb2, yb2, spheroid)
                   && intersect(xa1, ya1, xa2, ya2, xb1, yb1, xb2, yb2, x, y)
                   && gnom_t::inverse(lon, lat, x, y, lon1, lat1, spheroid);

            if (! ok)
            {
                return false;
            }

            if (math::equals(lat1, lat) && math::equals(lon1, lon))
            {
                break;
            }

            lat = lat1;
            lon = lon1;
        }

        // NOTE: true is also returned if the number of iterations is too great
        //       which means that the accuracy of the result is low
        return true;
    }

private:
    static inline bool intersect(CT const& xa1, CT const& ya1, CT const& xa2, CT const& ya2,
                                 CT const& xb1, CT const& yb1, CT const& xb2, CT const& yb2,
                                 CT & x, CT & y)
    {
        typedef model::point<CT, 3, cs::cartesian> v3d_t;

        CT const c0 = 0;
        CT const c1 = 1;

        v3d_t const va1(xa1, ya1, c1);
        v3d_t const va2(xa2, ya2, c1);
        v3d_t const vb1(xb1, yb1, c1);
        v3d_t const vb2(xb2, yb2, c1);

        v3d_t const la = cross_product(va1, va2);
        v3d_t const lb = cross_product(vb1, vb2);
        v3d_t const p = cross_product(la, lb);

        CT const z = get<2>(p);

        if (math::equals(z, c0))
        {
            // degenerated or collinear segments
            return false;
        }

        x = get<0>(p) / z;
        y = get<1>(p) / z;

        return true;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_GNOMONIC_INTERSECTION_HPP


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (3.1415926535897932384626433)

namespace Stokes2D3D {

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

/*
 * def AEW(xi,rvec):
 r=np.sqrt(rvec.dot(rvec))
 A = 2*(xi*np.exp(-(xi**2)*(r**2))/(np.sqrt(np.pi)*r**2)+ss.erfc(xi*r)/(2*r**3))
 *(r*r*np.identity(3)+np.outer(rvec,rvec)) -
 4*xi/np.sqrt(np.pi)*np.exp(-(xi**2)*(r**2))*np.identity(3)
 return A
 *
 * */
inline Eigen::Matrix3d AEW(const double xi, const Eigen::Vector3d &rvec) {
    const double r = rvec.norm();
    Eigen::Matrix3d A =
        2 *
            (xi * exp(-(xi * xi) * (r * r)) / (sqrt(PI314) * r * r) +
             erfc(xi * r) / (2 * r * r * r)) *
            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
        4 * xi / sqrt(PI314) * exp(-(xi * xi) * (r * r)) *
            Eigen::Matrix3d::Identity();
    return A;
}

inline double lbda(double k, double xi, double z) {
    return exp(-k * k / (4 * xi * xi) - (xi * xi) * (z * z));
}

inline double thetaplus(double k, double xi, double z) {
    return exp(k * z) * ERFC(k / (2 * xi) + xi * z);
}

inline double thetaminus(double k, double xi, double z) {
    return exp(-k * z) * ERFC(k / (2 * xi) - xi * z);
}

inline double J00(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J10(double k, double xi, double z) {
    return PI314 * (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (4 * k);
}

inline double J20(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) / (4 * k * k * xi) +
           PI314 *
               ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k * k * k) +
                (thetaminus(k, xi, z) - thetaplus(k, xi, z)) * z / (8 * k * k) -
                (thetaplus(k, xi, z) + thetaminus(k, xi, z)) /
                    (16 * k * (xi * xi)));
}

inline double J12(double k, double xi, double z) {
    return PI314 * (-thetaplus(k, xi, z) - thetaminus(k, xi, z)) * k / 4 +
           sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J22(double k, double xi, double z) {
    return PI314 * ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) * k /
                        (16 * xi * xi) +
                    (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k) +
                    (thetaplus(k, xi, z) - thetaminus(k, xi, z)) * z / 8) -
           sqrt(PI314) * lbda(k, xi, z) / (4 * xi);
}

inline double K11(double k, double xi, double z) {
    return PI314 * ((thetaminus(k, xi, z) - thetaplus(k, xi, z))) / 4;
}

inline double K12(double k, double xi, double z) {
    return PI314 *
           ((thetaplus(k, xi, z) - thetaminus(k, xi, z)) / (16 * xi * xi) +
            (thetaminus(k, xi, z) + thetaplus(k, xi, z)) * z / (8 * k));
}

inline void QI(const Eigen::Vector3d &kvec, double xi, double z,
               Eigen::Matrix3d &QI) {
    // 3*3 tensor
    // kvec: np.array([k1,k2,0])
    double knorm = sqrt(kvec[0] * kvec[0] + kvec[1] * kvec[1]);
    QI = 2 * (J00(knorm, xi, z) / (4 * xi * xi) + J10(knorm, xi, z)) *
         Eigen::Matrix3d::Identity();
}

inline void Qkk(const Eigen::Vector3d &kvec, double xi, double z,
                Eigen::Matrix3d &Qreal, Eigen::Matrix3d &Qimg) {
    double k1 = kvec[0];
    double k2 = kvec[1];
    double knorm = sqrt(k1 * k1 + k2 * k2);
    auto j10 = J10(knorm, xi, z);
    auto j20 = J20(knorm, xi, z);
    auto j12 = J12(knorm, xi, z);
    auto j22 = J22(knorm, xi, z);

    auto k11 = K11(knorm, xi, z);
    auto k12 = K12(knorm, xi, z);
    Qreal.setZero();
    Qreal(0, 0) = k1 * k1;
    Qreal(1, 1) = k2 * k2;
    Qreal(0, 1) = k1 * k2;
    Qreal(1, 0) = k1 * k2;

    Qreal *= (j10 / (4 * (xi * xi)) + j20);
    Qreal(2, 2) = (j12 / (4 * xi * xi) + j22);
    Qreal *= -2;

    Qimg.setZero();
    Qimg(0, 2) = k1;
    Qimg(1, 2) = k2;
    Qimg(2, 0) = k1;
    Qimg(2, 1) = k2;
    // Qimg=np.array([[0,0,k1],[0,0,k2],[k1,k2,0]])*( k11/(4*xi**2) + k12 )
    Qimg *= (k11 / (4 * xi * xi) + k12);
    Qimg *= -2;
}

// inline Eigen::Matrix3d uFk0(double xi, double zmn) {
//	Eigen::Matrix3d wavek0;
//	wavek0 = -(4.0 / 1) * (PI314 * (zmn) * ERF(zmn * xi) + sqrt(PI314) / (2
//* xi) * exp(-zmn * zmn * xi * xi));
//	return wavek0;
//
//}

inline void GkernelEwald(const Eigen::Vector3d &rvecIn, Eigen::Matrix3d &Gsum) {
    const double xi = 2;
    Eigen::Vector3d rvec = rvecIn;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]); // reset to a periodic cell

    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 5;
    if (r < 1e-14) {
        auto Gself = -4 * xi / sqrt(PI314) *
                     Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                if (i == 0 && j == 0) {
                    continue;
                }
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
    }

    // k
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    double zmn = rvec[2];
    Eigen::Vector3d rhomn = rvec;
    rhomn[2] = 0;
    Eigen::Matrix3d Qreal;
    Eigen::Matrix3d Qimg;
    Eigen::Matrix3d QImat;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            Eigen::Vector3d kvec(2 * PI314 * i, 2 * PI314 * j, 0);
            if (i == 0 and j == 0) {
                continue;
            }
            Qkk(kvec, xi, zmn, Qreal, Qimg);
            QI(kvec, xi, zmn, QImat);
            wave = wave + (QImat + Qreal) * cos(kvec.dot(rhomn)) -
                   (Qimg)*sin(kvec.dot(rhomn));
        }
    }
    wave *= 4;

    // k=0
    Eigen::Matrix3d waveK0;
    waveK0.setZero();
    /*
     *   I2fn=force
     I2fn[2]=0
     wavek0=-(4/1)*(np.pi*(zmn)*ss.erf(zmn*xi)+np.sqrt(np.pi)/(2*xi)*np.exp(-zmn**2*xi**2))*I2fn
     *
     * */
    waveK0 = -(4 / 1.0) *
             (PI314 * (zmn)*ERF(zmn * xi) +
              sqrt(PI314) / (2 * xi) * exp(-zmn * zmn * xi * xi)) *
             Eigen::Matrix3d::Identity();
    waveK0(2, 2) = 0;

    Gsum = real + wave + waveK0;
}

inline void Gkernel(const Eigen::Vector3d &target,
                    const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        answer = Eigen::Matrix3d::Zero();
        return;
    }
    auto part2 = rst * rst.transpose() / (rnorm * rnorm * rnorm);
    auto part1 = Eigen::Matrix3d::Identity() / rnorm;
    answer = part1 + part2;
}

// Out of Layer 1
inline void GkernelEwaldO1(const Eigen::Vector3d &rvec,
                           Eigen::Matrix3d &GsumO1) {
    Eigen::Matrix3d Gfree = Eigen::Matrix3d::Zero();
    GkernelEwald(rvec, GsumO1);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            Gkernel(rvec, Eigen::Vector3d(i, j, 0), Gfree);
            GsumO1 -= Gfree;
        }
    }
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);
    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {
        -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {
        -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(
        pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    //	for (int i = 0; i < pointLEquiv.size() / 3; i++) {
    //		std::cout << pointLEquiv[3 * i] << " " << pointLEquiv[3 * i + 1]
    //<< " " << pointLEquiv[3 * i + 2] << " "
    //				<< std::endl;
    //	}
    //
    //	for (int i = 0; i < pointLCheck.size() / 3; i++) {
    //		std::cout << pointLCheck[3 * i] << " " << pointLCheck[3 * i + 1]
    //<< " " << pointLCheck[3 * i + 2] << " "
    //				<< std::endl;
    //	}

    // const int imageN = 100; // images to sum
    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN);
    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            Gkernel(Cpoint, Lpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        //		std::cout<<"debug:"<<Mpoint<<std::endl;
        // assemble linear system
        Eigen::MatrixXd f(3 * checkN, 3);
        for (int k = 0; k < checkN; k++) {
            Eigen::Matrix3d temp = Eigen::Matrix3d::Zero();
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            // use 3D Ewald subtract the first layer
            GkernelEwaldO1(Cpoint - Mpoint, temp);
            f.block<3, 3>(3 * k, 0) = temp;
        }

        M2L.block(0, 3 * i, 3 * equivN, 3) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    /*
     * pointForce=[(np.array([1.0,0,0]),np.array([0.1,0.55,0.2]))
     ,(np.array([-1.0,1.0,1.0]),np.array([0.5,0.1,0.3]))
     ,(np.array([0.0,0.0,-1.0]),np.array([0.8,0.5,0.7]))]
     * */
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePoint(3);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValue(3);
    forcePoint[0] = Eigen::Vector3d(0.1, 0.5, 0.5);
    forceValue[0] = Eigen::Vector3d(1, 0, 0);
    forcePoint[1] = Eigen::Vector3d(0.9, 0.5, 0.5);
    forceValue[1] = Eigen::Vector3d(-1, 0, 0);
    forcePoint[2] = Eigen::Vector3d(0.0, 0.0, 0.0);
    forceValue[2] = Eigen::Vector3d(0, 0, 0);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d temp = Eigen::Vector3d::Zero();
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < forcePoint.size(); p++) {
            Gkernel(Cpoint, forcePoint[p], G);
            temp = temp + G * (forceValue[p]);
        }
        f.block<3, 1>(3 * k, 0) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            Gkernel(Cpoint, Mpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    // impose net charge equal
    double fx = 0, fy = 0, fz = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[3 * i];
        fy += Msource[3 * i + 1];
        fz += Msource[3 * i + 2];
    }
    std::cout << "fx svd before correction: " << fx << std::endl;
    std::cout << "fy svd before correction: " << fy << std::endl;
    std::cout << "fz svd before correction: " << fz << std::endl;
    double fnetx = 0;
    double fnety = 0;
    double fnetz = 0;
    for (size_t p = 0; p < forcePoint.size(); p++) {
        fnetx += (forceValue[p][0]);
        fnety += (forceValue[p][1]);
        fnetz += (forceValue[p][2]);
    }

    std::cout << "Msource: " << Msource << std::endl;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forcePointExt(0);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        forceValueExt(0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        for (int i = -DIRECTLAYER; i < DIRECTLAYER + 1; i++) {
            for (int j = -DIRECTLAYER; j < DIRECTLAYER + 1; j++) {
                forcePointExt.push_back(Eigen::Vector3d(i, j, 0) +
                                        forcePoint[p]);
                forceValueExt.push_back(forceValue[p]);
            }
        }
    }

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0.4, 0.5, 0.5);
    Eigen::Vector3d Usample(0, 0, 0);
    Eigen::Vector3d UsampleSP(0, 0, 0);
    Eigen::Matrix3d G;
    for (size_t p = 0; p < forcePointExt.size(); p++) {
        Gkernel(samplePoint, forcePointExt[p], G);
        Usample = Usample + G * (forceValueExt[p]);
    }
    std::cout << "Usample Direct:" << Usample << std::endl;
    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        Eigen::Vector3d Fpoint(M2Lsource[3 * p], M2Lsource[3 * p + 1],
                               M2Lsource[3 * p + 2]);
        Gkernel(samplePoint, Lpoint, G);
        UsampleSP = UsampleSP + G * (Fpoint);
    }

    std::cout << "Usample M2L:" << UsampleSP << std::endl;
    std::cout << "Usample M2L total:" << UsampleSP + Usample << std::endl;

    Eigen::Vector3d UsampleDirect = 0 * Usample;
    for (size_t p = 0; p < forcePoint.size(); p++) {
        GkernelEwald(samplePoint - forcePoint[p], G);
        UsampleDirect += G * (forceValue[p]);
    }
    std::cout << "Usample Ewald:" << UsampleDirect << std::endl;

    std::cout << "error" << UsampleSP + Usample - UsampleDirect << std::endl;

    return 0;
}

} // namespace Stokes2D3D

#undef DIRECTLAYER
#undef PI314


#include "../include/Jabc.hpp"
#include <armadillo>
#include <iostream>
#include <cmath>

using namespace std;
using namespace arma;

void transform_xlogx_mat(cx_mat &X)
{
  cx_mat U, V;
  vec s, sp;

  svd_econ(U, s, V, X);

  sp = s;

  for (int i=0; i<s.size(); i++)
    {
      sp(i) = -2.0*s(i) * log(s(i));
    }

  transform_xlogx_vec(s);
  X = U * diagmat(s) * V.t();
}

void transform_xlogx_vec(vec &s)
{
  for (int i=0; i<s.size(); i++)
    {
      s(i) = -2.0*s(i) * log(s(i));
    }
}

int log2_int(int n)
{
  int k=-1;
  while (n>0)
    {
      n/=2;
      k++;
    }
  return k;
}

// |\psi\rangle \to K_X|\psi\rangle, where X is the first k qubits.
// (Note that this is equal to K_{\bar{X}})|\psi\rangle).
// n: Number of qubits
// Input format: psi should be a column vector.
void apply_modular_op(int k, int n, cx_mat &psi)
{
  psi.reshape(1<<k, 1<<(n-k));
  transform_xlogx_mat(psi);
  psi.reshape(1<<n,1);
}

// Given |\psi>_{ABCD}, return \log \rho_{AB} |\psi>_{ABCD}
// a, b, c: Number of bits in A, B, C.
// Format: First a bits are A. Next b bits are B. The next c bits are C.
void transform_ab(int a, int b, int c, cx_mat &psi)
{
  int dim = psi.size();
  int n = log2_int(dim);
  apply_modular_op(a+b, n, psi);
}

// Given |\psi>_{ABCD}, return \log \rho_{BC} |\psi>_{ABCD}
// a, b, c: Number of bits in A, B, C.
// Format: First a bits are A. Next b bits are B. The next c bits are C.
void transform_bc(int a, int b, int c, cx_mat &psi)
{
  int dim = psi.size();
  int n = log2_int(dim);
  psi.reshape(1<<a, 1<<(n-a));
  psi = psi.st();
  psi.reshape(1<<n,1);
  apply_modular_op(b+c, n, psi);
  psi.reshape(1<<(n-a), 1<<a);
  psi = psi.st();
  psi.reshape(1<<n,1);
}

double Jabc(int a, int b, int c, cx_mat &psi)
{
  cx_mat psi_cpy = psi;

  transform_ab(a,b,c,psi);
  transform_bc(a,b,c,psi_cpy);

  cx_mat result = psi_cpy.t() * psi;

  cout << "result" << endl;
  cout << result << endl;

  cx_double inner_product = result(0,0);
  return inner_product.imag() / 2;
}


#include "MeshMorph.h"
#include <cstring>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;

#define RBF_KERNEL RBF_multiquadric
//#define RBF_KERNEL RBF_inv_quadric
//#define RBF_KERNEL RBF_inv_multiquadric

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

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

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

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

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

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

	return true;
}

MeshMorph::MeshMorph() : Wx(0),Wy(0),Wz(0)
{
}

MeshMorph::~MeshMorph(void)
{
}

void MeshMorph::init(float* vertexData_, unsigned short vertexNum_, unsigned short* featureIndices_, unsigned short featureNum_)
{
	vertexNum = vertexNum_;
	vertexData = new float[3 * vertexNum];
	memcpy(vertexData, vertexData_, vertexNum * 3 * sizeof(float));
	featureNum = featureNum_;
	featureIndices = new unsigned short[featureNum];
	memcpy(featureIndices, featureIndices_, featureNum * sizeof(unsigned short));

	//std::cout << RBFmInv << std::endl<< std::endl;

	//unsigned short* indices = new unsigned short[featureNum_];
	//for(unsigned short i = 0; i < featureNum_; ++i)
	//{
	//	indices[i] = i;
	//}
	//solveRBF(indices, featureNum_, newFeaturePos_);
	//delete indices;
}

float MeshMorph::dist2(float* p1, float* p2)
{
	return (p1[0]-p2[0]) * (p1[0]-p2[0]) + (p1[1]-p2[1]) * (p1[1]-p2[1]) + (p1[2]-p2[2]) * (p1[2]-p2[2]);
}

void  MeshMorph::getMorphedData(float* out, float* newFeaturePos_)
{
	unsigned short* indices = new unsigned short[featureNum];
	for(unsigned short i = 0; i < featureNum; ++i)
	{
		indices[i] = i;
	}
	solveRBF(indices, featureNum, newFeaturePos_);
	delete indices;
	for(int i = 0; i < vertexNum; ++i)
	{
		out[i*3 + 0] = 0;
		out[i*3 + 1] = 0;
		out[i*3 + 2] = 0;
		for(int j = 0; j < featureNum; ++j)
		{
			float d2 = dist2(&vertexData[i*3], &vertexData[featureIndices[j]*3]);
			out[i*3 + 0] += RBF_KERNEL(d2, minDist2[j]) * Wx[j];
			out[i*3 + 1] += RBF_KERNEL(d2, minDist2[j]) * Wy[j];
			out[i*3 + 2] += RBF_KERNEL(d2, minDist2[j]) * Wz[j];
		}
	}
}

void MeshMorph::getMorphedData(float* out, unsigned short* vertexIndices, unsigned short vertexNum_, unsigned short* featureIndices_, unsigned short featureNum_, float* newFeaturePos_, bool morphAllVertices)
{
	solveRBF(featureIndices_, featureNum_, newFeaturePos_);
	if(morphAllVertices)
	{
		//Copy all vertex positions
		memcpy(out, vertexData, vertexNum * 3 * sizeof(float));
		for(int i = 0; i < vertexNum_; ++i)
		{
			out[vertexIndices[i]*3 + 0] = 0;
			out[vertexIndices[i]*3 + 1] = 0;
			out[vertexIndices[i]*3 + 2] = 0;
			for(int j = 0; j < featureNum_; ++j)
			{
				float d2 = dist2(&vertexData[vertexIndices[i]*3], &vertexData[featureIndices[featureIndices_[j]]*3]);
				out[vertexIndices[i]*3 + 0] += RBF_KERNEL(d2, minDist2[j]) * Wx[j];
				out[vertexIndices[i]*3 + 1] += RBF_KERNEL(d2, minDist2[j]) * Wy[j];
				out[vertexIndices[i]*3 + 2] += RBF_KERNEL(d2, minDist2[j]) * Wz[j];
			}
		}
	}
	else
	{
		//only output the morphed vertices
		for(int i = 0; i < vertexNum_; ++i)
		{
			out[i*3 + 0] = 0;
			out[i*3 + 1] = 0;
			out[i*3 + 2] = 0;
			for(int j = 0; j < featureNum_; ++j)
			{
				float d2 = dist2(&vertexData[vertexIndices[i]*3], &vertexData[featureIndices[featureIndices_[j]]*3]);
				out[i*3 + 0] += RBF_KERNEL(d2, minDist2[j]) * Wx[j];
				out[i*3 + 1] += RBF_KERNEL(d2, minDist2[j]) * Wy[j];
				out[i*3 + 2] += RBF_KERNEL(d2, minDist2[j]) * Wz[j];
			}
		}
	}
}

void MeshMorph::solveRBF(unsigned short* featureIndices_, unsigned short featureNum_, float* newFeaturePos_)
{
	matrix<float> RBFm(featureNum_, featureNum_);
	minDist2 = new float[featureNum_];
	for(int i = 0; i < featureNum_; ++i)
	{
		minDist2[i] = FLT_MAX;
		for(int j = 0; j < featureNum_; ++j)
		{
			float d2 = dist2(&vertexData[featureIndices[featureIndices_[i]] * 3], &vertexData[featureIndices[featureIndices_[j]] * 3]);
			RBFm(i, j) = d2;
			if(i != j && d2 < minDist2[i])
				minDist2[i] = d2;
		}
	}
	//std::cout << RBFm << std::endl << std::endl;
	for(int i = 0; i < featureNum_; ++i)
	{
		for(int j = 0; j < featureNum_; ++j)
		{
			RBFm(i, j) = RBF_KERNEL(RBFm(i, j), minDist2[i]);
		}
	}

	//std::cout << RBFm << std::endl<< std::endl;

	RBFmInv.resize(featureNum_, featureNum_);

	InvertMatrix(RBFm, RBFmInv);

	if(Wx != 0) delete Wx;
	if(Wy != 0) delete Wy;
	if(Wz != 0) delete Wz;
	Wx = new float[featureNum_];
	Wy = new float[featureNum_];
	Wz = new float[featureNum_];

	for(int i = 0; i < featureNum_; ++i)
	{
		Wx[i] = 0;
		Wy[i] = 0;
		Wz[i] = 0;
		for(int j = 0; j < featureNum_; ++j)
		{
			Wx[i] += RBFmInv(j,i) * newFeaturePos_[featureIndices_[j]*3];
			Wy[i] += RBFmInv(j,i) * newFeaturePos_[featureIndices_[j]*3+1];
			Wz[i] += RBFmInv(j,i) * newFeaturePos_[featureIndices_[j]*3+2];
		}
	}
}

inline float MeshMorph::RBF_multiquadric(float dist2, float eps)
{
	return sqrt(eps + dist2);
}
inline float MeshMorph::RBF_inv_quadric(float dist2, float eps)
{
	return 1.0 / (eps + dist2);//sqrt(eps + dist2 > (100*eps) ? 100 * eps : dist2);
}
inline float MeshMorph::RBF_inv_multiquadric(float dist2, float eps)
{
	return 1.0 / sqrt(eps * 10 + dist2);
}


/* boost histogram.cpp graphical verification of distribution functions
 *
 * Copyright Jens Maurer 2000
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: histogram.cpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * This test program allows to visibly examine the results of the
 * distribution functions.
 */

#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <cmath>
#include <string>
#include <boost/random.hpp>

void plot_histogram(const std::vector<int>& slots, int samples,
                    double from, double to)
{
  int m = *std::max_element(slots.begin(), slots.end());
  const int nRows = 20;
  std::cout.setf(std::ios::fixed|std::ios::left);
  std::cout.precision(5);
  for(int r = 0; r < nRows; r++) {
    double y = ((nRows - r) * double(m))/(nRows * samples);
    std::cout << std::setw(10) << y << "  ";
    for(unsigned int col = 0; col < slots.size(); col++) {
      char out = ' ';
      if(slots[col]/double(samples) >= y)
        out = 'x';
      std::cout << out;
    }
    std::cout << std::endl;
  }
  std::cout << std::setw(12) << " "
            << std::setw(10) << from;
  std::cout.setf(std::ios::right, std::ios::adjustfield);
  std::cout << std::setw(slots.size()-10) << to << std::endl;
}

// I am not sure whether these two should be in the library as well

// maintain sum of NumberGenerator results
template<class NumberGenerator,
  class Sum = typename NumberGenerator::result_type>
class sum_result
{
public:
  typedef NumberGenerator base_type;
  typedef typename base_type::result_type result_type;
  explicit sum_result(const base_type & g) : gen(g), _sum(0) { }
  result_type operator()() { result_type r = gen(); _sum += r; return r; }
  base_type & base() { return gen; }
  Sum sum() const { return _sum; }
  void reset() { _sum = 0; }
private:
  base_type gen;
  Sum _sum;
};

// maintain square sum of NumberGenerator results
template<class NumberGenerator,
  class Sum = typename NumberGenerator::result_type>
class squaresum_result
{
public:
  typedef NumberGenerator base_type;
  typedef typename base_type::result_type result_type;
  explicit squaresum_result(const base_type & g) : gen(g), _sum(0) { }
  result_type operator()() { result_type r = gen(); _sum += r*r; return r; }
  base_type & base() { return gen; }
  Sum squaresum() const { return _sum; }
  void reset() { _sum = 0; }
private:
  base_type gen;
  Sum _sum;
};

template<class RNG>
void histogram(RNG base, int samples, double from, double to,
               const std::string & name)
{
  typedef squaresum_result<sum_result<RNG, double>, double > SRNG;
  SRNG gen((sum_result<RNG, double>(base)));
  const int nSlots = 60;
  std::vector<int> slots(nSlots,0);
  for(int i = 0; i < samples; i++) {
    double val = gen();
    if(val < from || val >= to)    // early check avoids overflow
      continue;
    int slot = int((val-from)/(to-from) * nSlots);
    if(slot < 0 || slot > (int)slots.size())
      continue;
    slots[slot]++;
  }
  std::cout << name << std::endl;
  plot_histogram(slots, samples, from, to);
  double mean = gen.base().sum() / samples;
  std::cout << "mean: " << mean
            << " sigma: " << std::sqrt(gen.squaresum()/samples-mean*mean)
            << "\n" << std::endl;
}

template<class PRNG, class Dist>
inline boost::variate_generator<PRNG&, Dist> make_gen(PRNG & rng, Dist d)
{
  return boost::variate_generator<PRNG&, Dist>(rng, d);
}

template<class PRNG>
void histograms()
{
  PRNG rng;
  using namespace boost;
  histogram(make_gen(rng, uniform_smallint<>(0, 5)), 100000, -1, 6,
            "uniform_smallint(0,5)");
  histogram(make_gen(rng, uniform_int<>(0, 5)), 100000, -1, 6,
            "uniform_int(0,5)");
  histogram(make_gen(rng, uniform_real<>(0,1)), 100000, -0.5, 1.5,
            "uniform_real(0,1)");
  histogram(make_gen(rng, bernoulli_distribution<>(0.2)), 100000, -0.5, 1.5,
            "bernoulli(0.2)");
  histogram(make_gen(rng, binomial_distribution<>(4, 0.2)), 100000, -1, 5,
            "binomial(4, 0.2)");
  histogram(make_gen(rng, triangle_distribution<>(1, 2, 8)), 100000, 0, 10,
            "triangle(1,2,8)");
  histogram(make_gen(rng, geometric_distribution<>(5.0/6.0)), 100000, 0, 10,
            "geometric(5/6)");
  histogram(make_gen(rng, exponential_distribution<>(0.3)), 100000, 0, 10,
            "exponential(0.3)");
  histogram(make_gen(rng, cauchy_distribution<>()), 100000, -5, 5,
            "cauchy");
  histogram(make_gen(rng, lognormal_distribution<>(3, 2)), 100000, 0, 10,
            "lognormal");
  histogram(make_gen(rng, normal_distribution<>()), 100000, -3, 3,
            "normal");
  histogram(make_gen(rng, normal_distribution<>(0.5, 0.5)), 100000, -3, 3,
            "normal(0.5, 0.5)");
  histogram(make_gen(rng, poisson_distribution<>(1.5)), 100000, 0, 5,
            "poisson(1.5)");
  histogram(make_gen(rng, poisson_distribution<>(10)), 100000, 0, 20,
            "poisson(10)");
  histogram(make_gen(rng, gamma_distribution<>(0.5)), 100000, 0, 0.5,
            "gamma(0.5)");
  histogram(make_gen(rng, gamma_distribution<>(1)), 100000, 0, 3,
            "gamma(1)");
  histogram(make_gen(rng, gamma_distribution<>(2)), 100000, 0, 6,
            "gamma(2)");
}

int main()
{
  histograms<boost::mt19937>();
  // histograms<boost::lagged_fibonacci607>();
}


#pragma once
#include "coordinate_transform.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>

//! Computes the H^1 differences between
//! u1 (considered as coefficients for the shape functions)
//! and u2grad.
double computeH1Difference(const Eigen::MatrixXd &vertices,
                           const Eigen::MatrixXi &triangles,
                           const Eigen::VectorXd &u1,
                           const std::function<Eigen::Vector2d(double, double)> &u2grad) {
	const int numberOfElements = triangles.rows();

	double error = 0;
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const int i0 = indexSet(0);
		const int i1 = indexSet(1);
		const int i2 = indexSet(2);

		const auto &a = vertices.row(i0);
		const auto &b = vertices.row(i1);
		const auto &c = vertices.row(i2);

		auto            coordinateTransform = makeCoordinateTransform(b - a, c - a);
		auto            volumeFactor        = std::abs(coordinateTransform.determinant());
		Eigen::Matrix2d elementMap          = coordinateTransform.inverse().transpose();

		error += integrate([&](double x, double y) {
			Eigen::Vector2d z = coordinateTransform * Eigen::Vector2d(x, y) + Eigen::Vector2d(a(0), a(1));

			Eigen::Vector2d approximate_grad = u1(i0) * elementMap * gradientLambda(0, x, y) + u1(i1) * elementMap * gradientLambda(1, x, y) + u1(i2) * elementMap * gradientLambda(2, x, y);
			Eigen::Vector2d difference_grad  = u2grad(z(0), z(1)) - approximate_grad;
			return difference_grad.dot(difference_grad) * volumeFactor;
		});
	}

	return std::sqrt(error);
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "mexHelpers.cpp"

using namespace Eigen;

void computeMeshTranformationCoeffsFullDim(const MatrixXd& F, const MatrixXd& V, SparseMatrix<double> &T, VectorXd& areas)
{
	// init
	int n_tri = F.rows();
	int d_simplex = F.cols();
	int n_vert = V.rows();
	int dim = V.cols();
	int T_rows = n_tri*dim*dim;
	int T_cols = n_vert*dim;
	int T_nnz = n_tri*dim*dim*d_simplex;

	// prepare centering matrix
	MatrixXd B = MatrixXd::Identity(d_simplex, d_simplex);
	B = B.array() - (1.0 / d_simplex);

	// prepare output matrix
	T.resize(T_cols, T_rows);
	T.reserve(VectorXi::Constant(T_rows, d_simplex));
	areas.resize(n_tri);

	// calculate differential coefficients for each element
	MatrixXd currV(d_simplex, dim);
	MatrixXd currT(dim, d_simplex);
	int curr_row = 0;
	for (int ii = 0; ii < n_tri; ii++)
	{
		// calculate current element
		for (int jj = 0; jj < d_simplex; jj++)
		{
			currV.row(jj) = V.row(F(ii, jj));
		}
		currV = B*currV; // center
		currT = currV.fullPivLu().solve(B); // solver

		// fill into the correct places of T
		for (int cd = 0; cd < dim; cd++)
		for (int cr = 0; cr < dim; cr++)
		{
			for (int cc = 0; cc < d_simplex; cc++)
				T.insert(F(ii, cc) + (cd*n_vert), curr_row) = currT(cr, cc);
			curr_row += 1;
		}

		// calculate area
		areas(ii) = (currV.bottomRows(dim).rowwise() - currV.row(0)).determinant() / 2;
	}

	// compress
	T.makeCompressed();
	T = T.transpose();
}

void orth(const MatrixXd &A, MatrixXd &Q)
{

	//perform svd on A = U*S*V' (V is not computed and only the thin U is computed)
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU);
	Eigen::MatrixXd U = svd.matrixU();
	const Eigen::VectorXd S = svd.singularValues();

	//get rank of A
	int m = A.rows();
	int n = A.cols();
	double tol = std::max(m, n) * S.maxCoeff() *  2.2204e-16;
	int r = 0;
	for (int i = 0; i < S.rows(); ++r, ++i)
	{
		if (S[i] < tol)
			break;
	}

	//keep r first columns of U
	Q = U.block(0, 0, U.rows(), r);
}

void compute2dEmbedding(const MatrixXd& V, MatrixXd& A)
{
	// given a nXn matrix whose columns are the vertices of a (n-1)-D simplex,
	// returns the transformation A, s.t A*V gives embedding in (n-1)-D

	MatrixXd ctrV(V.rows() - 1, V.cols());
	ctrV = -V.bottomRows(V.rows() - 1);
	ctrV.rowwise() += V.row(0);
	ctrV.transpose();
	orth(ctrV, A);
	//if (((ctrV*A).determinant()) < 0)
	//	A.col(0).swap(A.col(1));
	A.transpose();
}

void embedTriangle(const MatrixXd& V, MatrixXd& flatV, double& area)
{
	VectorXd v1 = V.row(1) - V.row(0);
	VectorXd v2 = V.row(2) - V.row(0);

	double norm_v1 = v1.norm();
	double norm_v2 = v2.norm();
	double cos_theta = v1.dot(v2) / (norm_v1*norm_v2);
	double sin_theta = sqrt(1 - cos_theta*cos_theta);

	flatV << 0, 0,
		norm_v1, 0,
		norm_v2*cos_theta, norm_v2*sin_theta;

	area = norm_v1*norm_v2*sin_theta / 2;
}

void computeMeshTranformationCoeffsFlatenning(const MatrixXd& F, const MatrixXd& V, SparseMatrix<double> &T, VectorXd& areas)
{
	// init
	int n_tri = F.rows();
	int d_simplex = F.cols();
	int n_vert = V.rows();
	int dim = V.cols();
	int d_diff = dim - 1;
	int T_rows = n_tri*d_diff*d_diff;
	int T_cols = n_vert*d_diff;
	int T_nnz = n_tri*d_diff*d_diff*d_simplex;

	assert(d_simplex == 3 && dim == 3);

	// prepare centering matrix
	MatrixXd B = MatrixXd::Identity(d_simplex, d_simplex);
	B = B.array() - (1.0 / d_simplex);

	// prepare output matrix
	T.resize(T_cols, T_rows);
	T.reserve(VectorXi::Constant(T_rows, d_simplex));
	areas.resize(n_tri);

	// calculate differential coefficients for each element
	MatrixXd currV(d_simplex, dim);
	MatrixXd currT(dim, d_simplex);
	MatrixXd RFlat(dim, d_diff);
	MatrixXd currVFlat(d_simplex, d_diff);
	int curr_row = 0;
	for (int ii = 0; ii < n_tri; ii++)
	{
		// calculate current element
		for (int jj = 0; jj < d_simplex; jj++)
		{
			currV.row(jj) = V.row(F(ii, jj));
		}
		// transform to plane
		embedTriangle(currV, currVFlat, areas(ii)); // this only works for triangles
		// compute
		currVFlat = B*currVFlat; // center
		currT = currVFlat.fullPivLu().solve(B); // solver

		// fill into the correct places of T
		for (int cd = 0; cd < d_diff; cd++)
		for (int cr = 0; cr < d_diff; cr++)
		{
			for (int cc = 0; cc < d_simplex; cc++)
				T.insert(F(ii, cc) + (cd*n_vert), curr_row) = currT(cr, cc);
			curr_row += 1;
		}
	}

	// compress
	T.makeCompressed();
	T = T.transpose();
}

void mexFunction(int nlhs, mxArray *plhs[],
	int nrhs, const mxArray*prhs[])
{
	// assign input
	int n_tri = mxGetM(prhs[0]); // # rows of F
	int d_simplex = mxGetN(prhs[0]); // # cols of F
	int n_vert = mxGetM(prhs[1]); // # rows of V
	int dim = mxGetN(prhs[1]); // # cols of V
	const Map<MatrixXd, Aligned> Fmatlab(mxGetPr(prhs[0]), n_tri, d_simplex);
	const Map<MatrixXd, Aligned> V(mxGetPr(prhs[1]), n_vert, dim);

	// update index numbers to 0-base
	MatrixXd F (Fmatlab);
	F = F.array() - 1;

	// compute
	SparseMatrix<double> T;
	VectorXd areas;
	if (d_simplex == 3 && dim == 2)
	{
		// Planar triangulation
		computeMeshTranformationCoeffsFullDim(F, V, T, areas);
	}
	else if (d_simplex == 4 && dim == 3)
	{
		// Tet mesh
		computeMeshTranformationCoeffsFullDim(F, V, T, areas);
	}
	else if (d_simplex == 3 && dim == 3)
	{
		// 3D surface
		computeMeshTranformationCoeffsFlatenning(F, V, T, areas);
	}
	else
		mexErrMsgIdAndTxt("MATLAB:invalidInputs", "Invalid input dimensions or mesh type not supported");

	// assign outputs
	mapSparseMatrixToMex(T, &(plhs[0]));
	mapDenseMatrixToMex(areas, &(plhs[1]));
}