// MSVC has a bug where it chokes on complex Eigen expressions

#ifdef _MSC_VER
#define EIGEN_STRONG_INLINE inline
#endif

#include "MidedgeAngleSinFormulation.h"
#include "../GeometryDerivatives.h"
#include "../MeshConnectivity.h"
#include <iostream>
#include <random>
#include <Eigen/Geometry>

static double edgeTheta(
    const MeshConnectivity& mesh,
    const Eigen::MatrixXd& curPos,
    int edge,
    Eigen::Matrix<double, 1, 12>* derivative, // edgeVertex, then edgeOppositeVertex
    Eigen::Matrix<double, 12, 12>* hessian)
{
    if (derivative)
        derivative->setZero();
    if (hessian)
        hessian->setZero();
    int v0 = mesh.edgeVertex(edge, 0);
    int v1 = mesh.edgeVertex(edge, 1);
    int v2 = mesh.edgeOppositeVertex(edge, 0);
    int v3 = mesh.edgeOppositeVertex(edge, 1);
    if (v2 == -1 || v3 == -1)
        return 0; // boundary edge

    Eigen::Vector3d q0 = curPos.row(v0);
    Eigen::Vector3d q1 = curPos.row(v1);
    Eigen::Vector3d q2 = curPos.row(v2);
    Eigen::Vector3d q3 = curPos.row(v3);

    Eigen::Vector3d n0 = (q0 - q2).cross(q1 - q2);
    Eigen::Vector3d n1 = (q1 - q3).cross(q0 - q3);
    Eigen::Vector3d axis = q1 - q0;
    Eigen::Matrix<double, 1, 9> angderiv;
    Eigen::Matrix<double, 9, 9> anghess;

    double theta = angle(n0, n1, axis, (derivative || hessian) ? &angderiv : NULL, hessian ? &anghess : NULL);

    if (derivative)
    {
        derivative->block<1, 3>(0, 0) += angderiv.block<1, 3>(0, 0) * crossMatrix(q2 - q1);
        derivative->block<1, 3>(0, 3) += angderiv.block<1, 3>(0, 0) * crossMatrix(q0 - q2);
        derivative->block<1, 3>(0, 6) += angderiv.block<1, 3>(0, 0) * crossMatrix(q1 - q0);

        derivative->block<1, 3>(0, 0) += angderiv.block<1, 3>(0, 3) * crossMatrix(q1 - q3);
        derivative->block<1, 3>(0, 3) += angderiv.block<1, 3>(0, 3) * crossMatrix(q3 - q0);
        derivative->block<1, 3>(0, 9) += angderiv.block<1, 3>(0, 3) * crossMatrix(q0 - q1);
    }

    if (hessian)
    {
        Eigen::Matrix3d vqm[3];
        vqm[0] = crossMatrix(q0 - q2);
        vqm[1] = crossMatrix(q1 - q0);
        vqm[2] = crossMatrix(q2 - q1);
        Eigen::Matrix3d wqm[3];
        wqm[0] = crossMatrix(q0 - q1);
        wqm[1] = crossMatrix(q1 - q3);
        wqm[2] = crossMatrix(q3 - q0);

        int vindices[3] = { 3, 6, 0 };
        int windices[3] = { 9, 0, 3 };

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                hessian->block<3, 3>(vindices[i], vindices[j]) += vqm[i].transpose() * anghess.block<3, 3>(0, 0) * vqm[j];
                hessian->block<3, 3>(vindices[i], windices[j]) += vqm[i].transpose() * anghess.block<3, 3>(0, 3) * wqm[j];
                hessian->block<3, 3>(windices[i], vindices[j]) += wqm[i].transpose() * anghess.block<3, 3>(3, 0) * vqm[j];
                hessian->block<3, 3>(windices[i], windices[j]) += wqm[i].transpose() * anghess.block<3, 3>(3, 3) * wqm[j];
            }

            hessian->block<3, 3>(vindices[i], 3) += vqm[i].transpose() * anghess.block<3, 3>(0, 6);
            hessian->block<3, 3>(3, vindices[i]) += anghess.block<3, 3>(6, 0) * vqm[i];
            hessian->block<3, 3>(vindices[i], 0) += -vqm[i].transpose() * anghess.block<3, 3>(0, 6);
            hessian->block<3, 3>(0, vindices[i]) += -anghess.block<3, 3>(6, 0) * vqm[i];

            hessian->block<3, 3>(windices[i], 3) += wqm[i].transpose() * anghess.block<3, 3>(3, 6);
            hessian->block<3, 3>(3, windices[i]) += anghess.block<3, 3>(6, 3) * wqm[i];
            hessian->block<3, 3>(windices[i], 0) += -wqm[i].transpose() * anghess.block<3, 3>(3, 6);
            hessian->block<3, 3>(0, windices[i]) += -anghess.block<3, 3>(6, 3) * wqm[i];

        }

        Eigen::Vector3d dang1 = angderiv.block<1, 3>(0, 0).transpose();
        Eigen::Vector3d dang2 = angderiv.block<1, 3>(0, 3).transpose();

        Eigen::Matrix3d dang1mat = crossMatrix(dang1);
        Eigen::Matrix3d dang2mat = crossMatrix(dang2);

        hessian->block<3, 3>(6, 3) += dang1mat;
        hessian->block<3, 3>(0, 3) -= dang1mat;
        hessian->block<3, 3>(0, 6) += dang1mat;
        hessian->block<3, 3>(3, 0) += dang1mat;
        hessian->block<3, 3>(3, 6) -= dang1mat;
        hessian->block<3, 3>(6, 0) -= dang1mat;

        hessian->block<3, 3>(9, 0) += dang2mat;
        hessian->block<3, 3>(3, 0) -= dang2mat;
        hessian->block<3, 3>(3, 9) += dang2mat;
        hessian->block<3, 3>(0, 3) += dang2mat;
        hessian->block<3, 3>(0, 9) -= dang2mat;
        hessian->block<3, 3>(9, 3) -= dang2mat;
    }

    return theta;
}

static Eigen::Vector3d secondFundamentalFormEntries(
    const MeshConnectivity& mesh,
    const Eigen::MatrixXd& curPos,
    const Eigen::VectorXd& edgeThetas,
    int face,
    Eigen::Matrix<double, 3, 21>* derivative,
    std::vector<Eigen::Matrix<double, 21, 21> >* hessian)
{
    if (derivative)
        derivative->setZero();
    if (hessian)
    {
        hessian->resize(3);
        for (int i = 0; i < 3; i++)
            (*hessian)[i].setZero();
    }

    Eigen::Vector3d II;
    for (int i = 0; i < 3; i++)
    {
        Eigen::Matrix<double, 1, 9> hderiv;
        Eigen::Matrix<double, 9, 9> hhess;
        double altitude = triangleAltitude(mesh, curPos, face, i, (derivative || hessian) ? &hderiv : NULL, hessian ? &hhess : NULL);

        int edge = mesh.faceEdge(face, i);
        Eigen::Matrix<double, 1, 12> thetaderiv;
        Eigen::Matrix<double, 12, 12> thetahess;
        double theta = edgeTheta(mesh, curPos, edge, (derivative || hessian) ? &thetaderiv : NULL, hessian ? &thetahess : NULL);

        double orient = mesh.faceEdgeOrientation(face, i) == 0 ? 1.0 : -1.0;
        double alpha = 0.5 * theta + orient * edgeThetas[edge];

        double sinAlpha = sin(alpha);
        double cosAlpha = cos(alpha);

        II[i] = 2.0 * altitude * sinAlpha;

        if (derivative)
        {
            int hv0 = i;
            int hv1 = (i + 1) % 3;
            int hv2 = (i + 2) % 3;
            derivative->block<1, 3>(i, 3 * hv0) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 0);
            derivative->block<1, 3>(i, 3 * hv1) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 3);
            derivative->block<1, 3>(i, 3 * hv2) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 6);

            int av0, av1, av2, av3;
            if (mesh.faceEdgeOrientation(face, i) == 0)
            {
                av0 = (i + 1) % 3;
                av1 = (i + 2) % 3;
                av2 = i;
                av3 = 3 + i;
            }
            else
            {
                av0 = (i + 2) % 3;
                av1 = (i + 1) % 3;
                av2 = 3 + i;
                av3 = i;
            }
            derivative->block<1, 3>(i, 3 * av0) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 0);
            derivative->block<1, 3>(i, 3 * av1) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 3);
            derivative->block<1, 3>(i, 3 * av2) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 6);
            derivative->block<1, 3>(i, 3 * av3) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 9);
            (*derivative)(i, 18 + i) += 2.0 * altitude * cosAlpha * orient;
        }

        if (hessian)
        {
            int hv[3];
            hv[0] = i;
            hv[1] = (i + 1) % 3;
            hv[2] = (i + 2) % 3;
            for (int j = 0; j < 3; j++)
            {
                for (int k = 0; k < 3; k++)
                {
                    (*hessian)[i].block<3, 3>(3 * hv[j], 3 * hv[k]) += 2.0 * sinAlpha * hhess.block<3, 3>(3 * j, 3 * k);
                }
            }

            int av[4];
            if (mesh.faceEdgeOrientation(face, i) == 0)
            {
                av[0] = (i + 1) % 3;
                av[1] = (i + 2) % 3;
                av[2] = i;
                av[3] = 3 + i;
            }
            else
            {
                av[0] = (i + 2) % 3;
                av[1] = (i + 1) % 3;
                av[2] = 3 + i;
                av[3] = i;
            }

            for (int k = 0; k < 3; k++)
            {
                for (int j = 0; j < 4; j++)
                {
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * hv[k]) += cosAlpha * thetaderiv.block<1, 3>(0, 3 * j).transpose() * hderiv.block<1, 3>(0, 3 * k);
                    (*hessian)[i].block<3, 3>(3 * hv[k], 3 * av[j]) += cosAlpha * hderiv.block<1, 3>(0, 3 * k).transpose() * thetaderiv.block<1, 3>(0, 3 * j);
                }
                (*hessian)[i].block<1, 3>(18 + i, 3 * hv[k]) += 2.0 * cosAlpha * orient * hderiv.block<1, 3>(0, 3 * k);
                (*hessian)[i].block<3, 1>(3 * hv[k], 18 + i) += 2.0 * cosAlpha * orient * hderiv.block<1, 3>(0, 3 * k).transpose();
            }

            for (int k = 0; k < 4; k++)
            {
                for (int j = 0; j < 4; j++)
                {
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * av[k]) += altitude * cosAlpha * thetahess.block<3, 3>(3 * j, 3 * k);
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * av[k]) += -0.5 * altitude * sinAlpha * thetaderiv.block<1, 3>(0, 3 * j).transpose() * thetaderiv.block<1, 3>(0, 3 * k);
                }
                (*hessian)[i].block<1, 3>(18 + i, 3 * av[k]) += -1.0 * altitude * sinAlpha * orient * thetaderiv.block<1, 3>(0, 3 * k);
                (*hessian)[i].block<3, 1>(3 * av[k], 18 + i) += -1.0 * altitude * sinAlpha * orient * thetaderiv.block<1, 3>(0, 3 * k).transpose();
            }

            (*hessian)[i](18 + i, 18 + i) += -2.0 * altitude * sinAlpha;
        }
    }

    return II;
}

Eigen::Matrix2d MidedgeAngleSinFormulation::secondFundamentalForm(
    const MeshConnectivity &mesh,
    const Eigen::MatrixXd &curPos,
    const Eigen::VectorXd &extraDOFs,
    int face,
    Eigen::MatrixXd *derivative,
    std::vector<Eigen::MatrixXd> *hessian) const
{
    if (derivative)
    {
        derivative->resize(4, 21);
        derivative->setZero();
    }
    if (hessian)
    {
        hessian->resize(4);
        for (int i = 0; i < 4; i++)
        {
            (*hessian)[i].resize(21, 21);
            (*hessian)[i].setZero();
        }
    }

    Eigen::Matrix<double, 3, 21> IIderiv;
    std::vector < Eigen::Matrix<double, 21, 21> > IIhess;

    Eigen::Vector3d II = secondFundamentalFormEntries(mesh, curPos, extraDOFs, face, derivative ? &IIderiv : NULL, hessian ? &IIhess : NULL);

    Eigen::Matrix2d result;
    result << II[0] + II[1], II[0], II[0], II[0] + II[2];

    if (derivative)
    {
        derivative->row(0) += IIderiv.row(0);
        derivative->row(0) += IIderiv.row(1);

        derivative->row(1) += IIderiv.row(0);
        derivative->row(2) += IIderiv.row(0);

        derivative->row(3) += IIderiv.row(0);
        derivative->row(3) += IIderiv.row(2);
    }
    if (hessian)
    {
        (*hessian)[0] += IIhess[0];
        (*hessian)[0] += IIhess[1];

        (*hessian)[1] += IIhess[0];
        (*hessian)[2] += IIhess[0];

        (*hessian)[3] += IIhess[0];
        (*hessian)[3] += IIhess[2];
    }

    return result;
}

static void testSecondFundamentalFormEntries(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
    double eps = 1e-6;
    MeshConnectivity mesh(F);
    int nfaces = mesh.nFaces();
    int nedges = mesh.nEdges();
    Eigen::VectorXd thetas(nedges);
    thetas.setConstant(0.3);
    int ntests = 100;

    std::random_device rd;
    std::mt19937 rng(rd());
    std::uniform_int_distribution<int> uni(0, nfaces);

    std::cout << "Testing " << ntests << " random faces" << std::endl;

    for(int i=0; i<ntests; i++)
    {
        int face = uni(rng);
        Eigen::Matrix<double, 3, 21> deriv;
        std::vector<Eigen::Matrix<double, 21, 21> > hess;
        Eigen::Vector3d b = secondFundamentalFormEntries(mesh, V, thetas, face, &deriv, &hess);

        for(int j=0; j<3; j++)
        {
            for(int k=0; k<3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(mesh.faceVertex(face, j), k) += 1e-6;
                Eigen::Matrix<double, 3, 21> derivpert;
                Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, Vpert, thetas, face, &derivpert, NULL);
                Eigen::Vector3d findiff = (bpert-b)/1e-6;
                Eigen::Vector3d exact = deriv.col(3*j+k);
                std::cout << "q" << j << "[" << k <<"]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

                Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
                for(int l=0; l<3; l++)
                {
                    std::cout << " hess[" << l << "]: " << hess[l].col(3*j+k).transpose() << " / " << findiffhess.row(l) << std::endl;;
                }
            }
            int edge = mesh.faceEdge(face, j);
            int ofaceidx = 0;
            if(mesh.edgeFace(edge, ofaceidx) == face)
                ofaceidx = 1;
            if(mesh.edgeFace(edge, ofaceidx) == -1)
                continue;
            int pidx = mesh.edgeOppositeVertex(edge, ofaceidx);
            for(int k=0; k<3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(pidx, k) += 1e-6;
                Eigen::Matrix<double, 3, 21> derivpert;
                Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, Vpert, thetas, face, &derivpert, NULL);
                Eigen::Vector3d findiff = (bpert-b)/1e-6;
                Eigen::Vector3d exact = deriv.col(9 + 3*j+k);
                std::cout << "p" << j << "[" << k <<"]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

                Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
                for(int l=0; l<3; l++)
                {
                    std::cout << " hess[" << l << "]: " << hess[l].col(9 + 3*j+k).transpose() << " / " << findiffhess.row(l) << std::endl;;
                }
            }
            Eigen::VectorXd thetapert(thetas);
            thetapert[edge] += 1e-6;
            Eigen::Matrix<double, 3, 21> derivpert;
            Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, V, thetapert, face, &derivpert, NULL);
            Eigen::Vector3d findiff = (bpert-b)/1e-6;
            Eigen::Vector3d exact = deriv.col(18+j);
            std::cout << "theta[" << j << "]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

            Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
            for(int l=0; l<3; l++)
            {
                std::cout << " hess[" << l << "]: " << hess[l].col(18 + j).transpose() << " / " << findiffhess.row(l) << std::endl;;
            }
        }
    }
}

static void testThetas(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
    double eps = 1e-6;
    MeshConnectivity mesh(F);
    int nfaces = mesh.nFaces();
    int nedges = mesh.nEdges();
    int ntests = 100;

    std::random_device rd;
    std::mt19937 rng(rd());
    std::uniform_int_distribution<int> uni(0, nedges);

    std::cout << "Testing " << ntests << " random edges" << std::endl;

    for (int i = 0; i < ntests; i++)
    {
        int edge = uni(rng);
        Eigen::Matrix<double, 1, 12> deriv;
        Eigen::Matrix<double, 12, 12> hess;
        double theta = edgeTheta(mesh, V, edge, &deriv, &hess);

        int v[2];
        v[0] = mesh.edgeVertex(edge, 0);
        v[1] = mesh.edgeVertex(edge, 1);

        int vo[2];
        vo[0] = mesh.edgeOppositeVertex(edge, 0);
        vo[1] = mesh.edgeOppositeVertex(edge, 1);

        for (int j = 0; j < 2; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(v[j], k) += 1e-6;
                Eigen::Matrix<double, 1, 12> derivpert;
                double thetapert = edgeTheta(mesh, Vpert, edge, &derivpert, NULL);
                double findiff = (thetapert - theta) / 1e-6;
                double exact = deriv(0, 3 * j + k);
                std::cout << "edgeVert" << j << "[" << k << "]: " << exact << " / " << findiff << std::endl;

                Eigen::Matrix<double, 1, 12> findiffhess = (derivpert-deriv)/1e-6;
                std::cout << " hess: " << hess.col(3 * j + k).transpose() << " / " << findiffhess << std::endl;
            }

            if (vo[j] != -1)
            {
                for (int k = 0; k < 3; k++)
                {
                    Eigen::MatrixXd Vpert(V);
                    Vpert(vo[j], k) += 1e-6;
                    Eigen::Matrix<double, 1, 12> derivpert;
                    double thetapert = edgeTheta(mesh, Vpert, edge, &derivpert, NULL);
                    double findiff = (thetapert - theta) / 1e-6;
                    double exact = deriv(0, 6 + 3 * j + k);
                    std::cout << "edgeOppVert" << j << "[" << k << "]: " << exact << " / " << findiff << std::endl;

                    Eigen::Matrix<double, 1, 12> findiffhess = (derivpert-deriv)/1e-6;
                    std::cout << " hess: " << hess.col(6 + 3 * j + k).transpose() << " / " << findiffhess << std::endl;
                }
            }
        }
    }
}

int MidedgeAngleSinFormulation::numExtraDOFs() const
{
    return 1;
}

void MidedgeAngleSinFormulation::initializeExtraDOFs(Eigen::VectorXd &extraDOFs, const MeshConnectivity &mesh, const Eigen::MatrixXd &curPos) const
{
    extraDOFs.resize(mesh.nEdges());
    extraDOFs.setZero();
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2009 Andrea Odetti

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_longstaff_schwartz_multi_path_pricer_hpp
#define quantlib_longstaff_schwartz_multi_path_pricer_hpp

#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/math/functional.hpp>
#include <ql/methods/montecarlo/pathpricer.hpp>
#include <ql/methods/montecarlo/multipath.hpp>
#include <ql/methods/montecarlo/lsmbasissystem.hpp>
#include <ql/experimental/mcbasket/pathpayoff.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Longstaff-Schwarz path pricer for early exercise options
    /*! References:

        Francis Longstaff, Eduardo Schwartz, 2001. Valuing American Options
        by Simulation: A Simple Least-Squares Approach, The Review of
        Financial Studies, Volume 14, No. 1, 113-147

        \ingroup mcarlo

        \test the correctness of the returned value is tested by
              reproducing results available in web/literature
    */
    class LongstaffSchwartzMultiPathPricer : public PathPricer<MultiPath> {
      public:

        LongstaffSchwartzMultiPathPricer(
            const boost::shared_ptr<PathPayoff>& ,
            const std::vector<Size> &,
            const std::vector<Handle<YieldTermStructure> > &,
            const Array &,
            Size ,
            LsmBasisSystem::PolynomType );

        Real operator()(const MultiPath& multiPath) const;
        virtual void calibrate();

      protected:
        struct PathInfo {
            PathInfo(Size numberOfTimes);

            Size pathLength() const;

            Array                   payments;
            Array                   exercises;
            std::vector<Array>      states;
        };

        PathInfo transformPath(const MultiPath& path) const;

        bool  calibrationPhase_;

        const boost::shared_ptr<PathPayoff> payoff_;

        boost::scoped_array<Array> coeff_;
        boost::scoped_array<Real> lowerBounds_;

        const std::vector<Size> timePositions_;
        const std::vector<Handle<YieldTermStructure> > forwardTermStructures_;
        const Array dF_;

        mutable std::vector<PathInfo> paths_;
        const   std::vector<boost::function1<Real, Array> > v_;
    };

}

#endif


#include "plane.h"
#include <vector>
#include <math.h>
#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;
using namespace std;

bool line3d::ClosestPoint(const line3d &L1, const line3d &L2, Vector3d &Result1, Vector3d &Result2)
{
	Vector3d NewDir = L1.D.cross(L2.D);
	float Length = NewDir.norm();
	if (Length == 0.0f) {
		return false;
	}

	plane P1 = plane::fromPointVectors(L1.P0, NewDir, L1.D);
	plane P2 = plane::fromPointVectors(L2.P0, NewDir, L2.D);

	Result1 = P2.IntersectLine(L1);
	Result2 = P1.IntersectLine(L2);
	return true;
}

float line3d::Dist(const line3d &L1, const line3d &L2)
{
	Vector3d Cross = L1.D.cross(L2.D);
	Vector3d v = L2.P0 - L1.P0;

	return abs(v.dot(Cross)) / Cross.norm();
}

float line3d::DistSq(const line3d &L1, const line3d &L2)
{
	Vector3d Cross = L1.D.cross(L2.D);

	Vector3d v = L2.P0 - L1.P0;
	float Dot = v.dot(Cross);
	return (Dot * Dot) / Cross.squaredNorm();
}

float line3d::DistToPoint(const Vector3d &P) const
{
	float t0 = D.dot(P - P0) / D.dot(D);

	Vector3d v = P0 + t0 * D;

	// TODO: Replace this with the eigen equivalent
	float x = P.x() + v.x();
	float y = P.y() + v.y();
	float z = P.z() + v.z();

	float distanceLine = sqrt(x*x + y*y + z*z);
	return distanceLine;
}

//-----------------------------------------------------------------------------

plane::plane()
{

}

float plane::get_x(float y, float z)
{
	return -(b*y + c*z + d) / a;
}

float plane::get_y(float x, float z)
{
	return -(a*x + c*z + d) / b;
}

float plane::get_z(float x, float y)
{
	return -(a*x + b*y + d) / c;
}

plane::plane(const plane &P)
{
	a = P.a;
	b = P.b;
	c = P.c;
	d = P.d;
}

plane::plane(float _a, float _b, float _c, float _d)
{
	a = _a;
	b = _b;
	c = _c;
	d = _d;
}

plane::plane(const Vector3d &NormalizedNormal, float _d)
{
	a = NormalizedNormal(0);
	b = NormalizedNormal(1);
	c = NormalizedNormal(2);
	d = _d;
}

plane plane::fromPointNormal(const Vector3d &Pt, const Vector3d &Normal)
{
	plane Result;
	Vector3d NormalizedNormal = Normal;
	NormalizedNormal.normalize();

	Result.a = NormalizedNormal(0);
	Result.b = NormalizedNormal(1);
	Result.c = NormalizedNormal(2);
	Result.d = -Pt.dot(NormalizedNormal);
	return Result;
}

plane plane::fromPointVectors(const Vector3d &Pt, const Vector3d &V1, const Vector3d &V2)
{
	Vector3d Normal = V1.cross(V2);
	return fromPointNormal(Pt, Normal);
}

plane plane::Normalize()
{
	plane Result;
	float Distance = sqrtf(a * a + b * b + c * c);
	Result.a = a / Distance;
	Result.b = b / Distance;
	Result.c = c / Distance;
	Result.d = d / Distance;
	return Result;
}

plane plane::fromPoints(const Vector3d &V0, const Vector3d &V1, const Vector3d &V2)
{
	Vector3d t0 = V1 - V0;
	Vector3d t1 = V2 - V0;
	Vector3d Normal = t0.cross(t1);
	Normal.normalize();
	return fromPointNormal(V0, Normal);
}

Vector3d plane::IntersectLine(const line3d &Line) const
{
	return IntersectLine(Line.P0, Line.P0 + Line.D);
}

Vector3d plane::IntersectLine(const Vector3d &V1, const Vector3d &V2) const
{
	Vector3d Diff = V1 - V2;
	float Denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (Denominator == 0.0f) {
		return (V1 + V2) * 0.5f;
	}
	float u = (a * V1(0) + b * V1(1) + c * V1(2) + d) / Denominator;

	return (V1 + u * (V2 - V1));
}

Vector3d plane::IntersectLine(const Vector3d &V1, const Vector3d &V2, bool &Hit) const
{
	Hit = true;
	Vector3d Diff = V2 - V1;
	float denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (denominator == 0) { Hit = false; return V1; }
	float u = (a * V1(0) + b * V1(1) + c * V1(2) + d) / denominator;

	return (V1 + u * (V2 - V1));
}

float plane::IntersectLineRatio(const Vector3d &V1, const Vector3d &V2)
{
	Vector3d Diff = V2 - V1;
	float Denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (Denominator == 0.0f) {
		return 0.0f;
	}
	return (a * V1(0) + b * V1(1) + c * V1(2) + d) / -Denominator;
}

float plane::signedDistance(const Vector3d &Pt) const
{
	return (a * Pt(0) + b * Pt(1) + c * Pt(2) + d);
}

float plane::unsignedDistance(const Vector3d &Pt) const
{
	return abs(a * Pt(0) + b * Pt(1) + c * Pt(2) + d);
}

Vector3d plane::ClosestPoint(const Vector3d &Point)
{
	return (Point - Normal() * signedDistance(Point));
}

bool plane::planePlaneIntersection(const plane &P1, const plane &P2, line3d &L)
{
	float Denominator = P1.a * P2.b - P1.b * P2.a;
	if (Denominator == 0.0f) {
		// this case should be handled by switching axes...
		return false;
	}
	L.P0 = Vector3d((P2.d * P1.b - P1.d * P2.b) /
		Denominator, (P1.d * P2.a - P2.d * P1.a) /
		Denominator, 0.0f);

	L.D = P1.Normal().cross(P2.Normal());
	if (L.D.norm() == 0.0f) {
		return false;
	}
	L.D.normalize();

	return true;
}

float plane::Dot(const plane &P, const Vector4d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2) + P.d * V(3);
}

float plane::DotCoord(const plane &P, const Vector3d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2) + P.d;
}

float plane::DotNormal(const plane &P, const Vector3d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2);
}


/**
 * @file mtgp64-calc-poly.cpp
 *
 * @brief calculate characteristic polynomial for 64bit mtgp.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (Hiroshima University)
 *
 * Copyright (c) 2012 Mutsuo Saito, Makoto Matsumoto, Hiroshima
 * University and University of Tokyo. All rights reserved.
 *
 * The new BSD License is applied to this software, see LICENSE.txt
 */
#include <stdint.h>
#include <inttypes.h>
#include <time.h>
#include <string.h>
#include <string>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>
#include "mtgp64-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp64-fast.h"

using namespace std;
using namespace NTL;

/**
 * calculate the characteristic polynomial for given 64-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] poly calculated characteristic polynomial.
 * @param[in] mtgp64 generator
 */
void mtgp64_calc_characteristic(GF2X& poly, mtgp64_fast_t * mtgp64)
{
    vec_GF2 seq;
    int mexp = mtgp64->params.mexp;
    seq.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
	seq[i] = mtgp64_genrand_uint64(mtgp64) & 1;
    }
    MinPolySeq(poly, seq, mexp);
}

/**
 * calculate the characteristic polynomial for given 64-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] str calculated characteristic polynomial in string format.
 * @param[in] mtgp64 generator
 */
void mtgp64_calc_characteristic(string& str, mtgp64_fast_t * mtgp64)
{
    GF2X poly;
    mtgp64_calc_characteristic(poly, mtgp64);
    polytostring(str, poly);
}

#if defined(MAIN)
/**
 * main function for executable.
 * @param[in] argc number of arguments.
 * @param[in] argv an array of arguments.
 * @return 0 if normal, other abnormal.
 */
int main(int argc, char *argv[]) {
    int mexp;
    int no;
    uint32_t seed = 1;
    mtgp64_params_fast_t *params;
    mtgp64_fast_t mtgp64;
    int rc;

    if (argc <= 2) {
	printf("%s: mexp no.\n", argv[0]);
	return 1;
    }
    mexp = strtol(argv[1], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 2;
    }
    no = strtol(argv[2], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 3;
    }
    switch (mexp) {
    case 23209:
	params = mtgp64_params_fast_23209;
	break;
    case 44497:
	params = mtgp64_params_fast_44497;
	break;
    case 110503:
	params = mtgp64_params_fast_110503;
	break;
    default:
	printf("%s: mexp no.\n", argv[0]);
	printf("mexp shuould be 23209, 44497 or 110503\n");
	return 4;
    }
    if (no >= 128 || no < 0) {
	printf("%s: mexp no.\n", argv[0]);
	printf("no must be between 0 and 127\n");
	return 5;
    }
    params += no;
    rc = mtgp64_init(&mtgp64, params, seed);
    if (rc) {
	printf("failure in mtgp64_init\n");
	return -1;
    }
    mtgp64_print_idstring(&mtgp64, stdout);
    string s;
    mtgp64_calc_characteristic(s, &mtgp64);
    printf("%s\n", s.c_str());
    mtgp64_free(&mtgp64);
    return 0;
}
#endif


#include <math.h>
#include <stdio.h>

#include <boost/math/quaternion.hpp>

static double inertia_z = 6.55929728e-05;
static double frequency =3.0;

static double stiffness= inertia_z * pow(2*M_PI*frequency,2);
static double angle_init = -0.2300258882727588;
//static double angle_init = -0.8;

extern "C" void externalForces(double t, double *f, unsigned int size_z, double *z)
{
  f[0]=0;
  f[1]=0;
  f[2]=-10;
}
extern "C" void externalMoment(double t,double *m, unsigned int size_z, double *z)
{
  m[0]=0;
  m[1]=0;
  m[2]=0;
}

extern "C" void internalForces(double t, double *q, double *v, double *f, unsigned int size_z,double *z)
{
  // Simple spring in z direction.
  f[0]=0;
  f[1]=0;
  f[2]=0.;
  // printf("internalForcesB1 :\n");
  // printf("f[0] = %e\t f[1] = %e\t, f[2]=%e\n",f[0],f[1],f[2]);
}

extern "C" void internalForcesB1_Jacq(double t, double *q, double *v, double *jac, unsigned int size_z,double *z)
{
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  jac[2+2*3]=1e4;
  // printf("internalForcesB1_Jacq :\n");
  // printf("jac[2+2*3] = %e\n", jac[2+2*3]);
}

extern "C" void internalMomentsBalanceWheel(double t, double *q, double *v, double *m, unsigned int size_z,double *z)
{
  //  printf("internalMomentsB1 :\n");
  // Simple torsional spring around z axis
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  double angle = 2*asin(q[6]);
  //printf("angle = %e\n", angle);
  //printf("stiffness = %e \n", stiffness);
  m[0]=0.0;
  m[1]=0.;
  m[2]=stiffness * (angle-angle_init);
  //printf("m[0] = %e\t m[1] = %e\t, m[2]=%e\n",m[0],m[1],m[2]);
}

extern "C" void internalMomentsBalanceWheel_Jacq(double t, double *q, double *v, double *jac, unsigned int size_z,double *z)
{
  //printf("internalMomentsB1_Jacq :\n");
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  //double angle = 2*asin(q[6]);
  // printf("angle = %e\n", angle);
  jac[2+6*3]=stiffness * 2.0 / sqrt(1 - q[6]*q[6]) ;
  // printf("jac[3+3*3] = %e\n", jac[3+3*3]);
}

extern "C" void externalMomentEscapeWheel(double t,double *m, unsigned int size_z, double *z)
{
  m[0]=0;
  m[1]=0;
  m[2]=-0.001;
}

extern "C" void prescribedvelocityB1(double time, unsigned int sizeofprescribedvelocity, double *pv)
{
  /* the plugin implements v(t) = C + A cos(omega *t) */

  double C = -150.0 ;
  // double omega = M_PI / 2.0;
  // double A = 10.0;

  //pv[0] =  A * cos(omega * time*100.0);
  pv[0] =  C;
  //printf("prescribed velocity = %e\n", pv[0]);
}


#ifndef STAN_MATH_FWD_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP
#define STAN_MATH_FWD_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP

#include <stan/math/fwd/meta.hpp>
#include <stan/math/fwd/core.hpp>

#include <boost/math/special_functions/digamma.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>

namespace stan {
namespace math {

template <typename T>
inline fvar<T> binomial_coefficient_log(const fvar<T>& x1, const fvar<T>& x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1.val_ < cutoff) || (x1.val_ - x2.val_ < cutoff)) {
    return fvar<T>(binomial_coefficient_log(x1.val_, x2.val_),
                   x1.d_ * digamma(x1.val_ + 1) - x2.d_ * digamma(x2.val_ + 1)
                       - (x1.d_ - x2.d_) * digamma(x1.val_ - x2.val_ + 1));
  } else {
    return fvar<T>(
        binomial_coefficient_log(x1.val_, x2.val_),
        x2.d_ * log(x1.val_ - x2.val_)
            + x2.val_ * (x1.d_ - x2.d_) / (x1.val_ - x2.val_)
            + x1.d_ * log(x1.val_ / (x1.val_ - x2.val_))
            + (x1.val_ + 0.5) / (x1.val_ / (x1.val_ - x2.val_))
                  * (x1.d_ * (x1.val_ - x2.val_) - (x1.d_ - x2.d_) * x1.val_)
                  / ((x1.val_ - x2.val_) * (x1.val_ - x2.val_))
            - x1.d_ / (12.0 * x1.val_ * x1.val_) - x2.d_
            + (x1.d_ - x2.d_)
                  / (12.0 * (x1.val_ - x2.val_) * (x1.val_ - x2.val_))
            - digamma(x2.val_ + 1) * x2.d_);
  }
}

template <typename T>
inline fvar<T> binomial_coefficient_log(const fvar<T>& x1, double x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1.val_ < cutoff) || (x1.val_ - x2 < cutoff)) {
    return fvar<T>(
        binomial_coefficient_log(x1.val_, x2),
        x1.d_ * digamma(x1.val_ + 1) - x1.d_ * digamma(x1.val_ - x2 + 1));
  } else {
    return fvar<T>(binomial_coefficient_log(x1.val_, x2),
                   x2 * x1.d_ / (x1.val_ - x2)
                       + x1.d_ * log(x1.val_ / (x1.val_ - x2))
                       + (x1.val_ + 0.5) / (x1.val_ / (x1.val_ - x2))
                             * (x1.d_ * (x1.val_ - x2) - x1.d_ * x1.val_)
                             / ((x1.val_ - x2) * (x1.val_ - x2))
                       - x1.d_ / (12.0 * x1.val_ * x1.val_)
                       + x1.d_ / (12.0 * (x1.val_ - x2) * (x1.val_ - x2)));
  }
}

template <typename T>
inline fvar<T> binomial_coefficient_log(double x1, const fvar<T>& x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1 < cutoff) || (x1 - x2.val_ < cutoff)) {
    return fvar<T>(
        binomial_coefficient_log(x1, x2.val_),
        -x2.d_ * digamma(x2.val_ + 1) - x2.d_ * digamma(x1 - x2.val_ + 1));
  } else {
    return fvar<T>(binomial_coefficient_log(x1, x2.val_),
                   x2.d_ * log(x1 - x2.val_) + x2.val_ * -x2.d_ / (x1 - x2.val_)
                       - x2.d_
                       - x2.d_ / (12.0 * (x1 - x2.val_) * (x1 - x2.val_))
                       + x2.d_ * (x1 + 0.5) / (x1 - x2.val_)
                       - digamma(x2.val_ + 1) * x2.d_);
  }
}
}  // namespace math
}  // namespace stan
#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
 *
 *  This is an example illustrating the use the general purpose non-linear
 *  optimization routines from the dlib C++ Library.
 *
 *  The library provides implementations of many popular algorithms such as L-BFGS
 *  and BOBYQA.  These algorithms allow you to find the minimum or maximum of a
 *  function of many input variables.  This example walks though a few of the ways
 *  you might put these routines to use.
 *
 */

#include <iostream>
#include <dlib/optimization.h>
#include <dlib/global_optimization.h>

#ifdef __cplusplus
extern "C" {
#endif
#include "ViennaRNA/utils/basic.h"
#ifdef __cplusplus
}
#endif

#include "wrap_dlib.h"

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// In dlib, most of the general purpose solvers optimize functions that take a
// column vector as input and return a double.  So here we make a typedef for a
// variable length column vector of doubles.  This is the type we will use to
// represent the input to our objective functions which we will be minimizing.
typedef matrix<double, 0, 1> column_vector;

/*
 *  function to minimize to obtain equlibrium concentrations
 *  of multistrand systems. We use the transformation
 *
 *  L_a = lambda_a + ln Z_a
 *
 *  such that h(L) reads
 *
 *  h(L) = -\sum_a (c_a L_a - exp(L_a)) + sum_k K_k exp(sum_b L_b A_{b,k}
 *
 *  with total concentration c_a of strand a, equilibrium constant
 *  K_k of strand k, and membership matrix A[b][k] denoting the number
 *  of strands b in complex k
 *
 *  Note, here we minimize h(L) due to implementation issues whereas
 *  in our publication we've written h'(L) = -h(L) to effectively
 *  maximize the function instead.
 */
PRIVATE double
h(const column_vector&  L,
  const double          *eq_constants,
  const double          *concentration_strands_tot,
  const unsigned int    **A,
  size_t                strands,
  size_t                complexes)
{
  double h, hh, *K, maxK;

  K = (double *)vrna_alloc(sizeof(double) * complexes);
  h = 0.;
  maxK = (double)(-INF);

  for (size_t a = 0; a < strands; a++) {
//    printf("L[%u] = %g\n", a, L(a));
    maxK = (maxK < L(a)) ? L(a) : maxK;
  }

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);

    for (size_t a = 0; a < strands; a++) {
      K[k] += L(a) *
              (double)A[a][k];
    }

    maxK = (maxK < K[k]) ? K[k] : maxK;
  }

  for (size_t a = 0; a < strands; a++)
    h -= concentration_strands_tot[a] *
         L(a);

//  printf("h = %g\n", h);
  hh = 0;

  for (size_t a = 0; a < strands; a++)
    hh += exp(L(a) - maxK);

  for (size_t k = 0; k < complexes; k++)
    hh += exp(K[k] - maxK);

  h += exp(maxK + log(hh));
//  printf("h = %g\n", h);

  free(K);

  return h;
}

/*
 *  Get gradient of h(L)
 */
PRIVATE const column_vector
h_derivative(const column_vector& L,
             const double         *eq_constants,
             const double         *concentration_strands_tot,
             const unsigned int   **A,
             size_t               strands,
             size_t               complexes)
{
  double        *K, *maxK;
  column_vector g(strands);

  K     = (double *)vrna_alloc(sizeof(double) * complexes);
  maxK  = (double *)vrna_alloc(sizeof(double) * strands);

  for (size_t a = 0; a < strands; a++)
    maxK[a] = L(a);

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);
    for (size_t a = 0; a < strands; a++)
      K[k] += L(a) *
              (double)A[a][k];

    for (size_t a = 0; a < strands; a++)
      if (A[a][k] > 0)
        maxK[a] = (maxK[a] < K[k] + log((double)A[a][k])) ?
                  K[k] + log((double)A[a][k]) :
                  maxK[a];
  }

  for (size_t a = 0; a < strands; a++) {
    g(a) = -concentration_strands_tot[a];

    double hh = exp(L(a) - maxK[a]);
    for (size_t k = 0; k < complexes; k++)
      if (A[a][k] > 0)
        hh += exp(log((double)A[a][k]) +
                  K[k] -
                  maxK[a]);

    g(a) += exp(maxK[a] + log(hh));
//    printf("g(%u) = %g\n", a, g(a));
  }

  free(K);
  free(maxK);

  return g;
}

/*
 *  Get Hessian of h(L)
 */
PRIVATE matrix<double>
h_hessian(const column_vector&  L,
          const double          *eq_constants,
          const unsigned int    **A,
          size_t                strands,
          size_t                complexes)
{
  double                  *K, **xs;

  PRIVATE matrix<double>  H(strands, strands);

  K = (double *)vrna_alloc(sizeof(double) * complexes);
  xs = (double **)vrna_alloc(sizeof(double *) * strands);

  for (size_t a = 0; a < strands; a++) {
    xs[a] = (double *)vrna_alloc(sizeof(double) * strands);
    for (size_t b = 0; b < strands; b++)
      xs[a][b] = (a == b) ? L(a) : (double)(-INF);
  }

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);
    for (size_t a = 0; a < strands; a++)
      K[k] += L(a) *
              (double)A[a][k];

    for (size_t a = 0; a < strands; a++)
      for (size_t b = 0; b < strands; b++) {
        if ((A[a][k] > 0) && (A[b][k] > 0))
          xs[a][b] = (xs[a][b] < K[k] + log((double)A[a][k]) + log((double)A[b][k])) ?
                      K[k] + log((double)A[a][k]) + log((double)A[b][k]) :
                      xs[a][b];
      }
  }

  for (size_t a = 0; a < strands; a++) {
    for (size_t b = 0; b < strands; b++) {
      double hh = (a == b) ? exp(L(a) - xs[a][b]) : 0.;

      for (size_t k = 0; k < complexes; k++)
        if ((A[a][k] > 0) && (A[b][k] > 0))
          hh += exp(log((double)A[a][k]) +
                    log((double)A[b][k]) +
                    K[k] -
                    xs[a][b]);

      H(a,b) = exp(xs[a][b] + log(hh));
//      H(b,a) = H(a,b);
//      printf("H(%u,%u) = %g\n", a, b, H(a,b));
    }
  }

  free(K);
  for (size_t a = 0; a < strands; a++)
    free(xs[a]);
  free(xs);

  return H;
}

class h_model
{
/*!
 *  This object is a "function model" which can be used with the
 *  find_min_trust_region() routine.
 * !*/

public:
typedef ::column_vector column_vector;
typedef matrix<double> general_matrix;

const double *eq_constants;
const double *concentration_strands_tot;
const unsigned int **A;
size_t strands;
size_t complexes;

double
operator()(const column_vector& x) const
{
  return h(x, eq_constants, concentration_strands_tot, A, strands, complexes);
}

void
init(const double       *eq_constants,
     const double       *concentration_strands_tot,
     const unsigned int **A,
     size_t             strands,
     size_t             complexes)
{
  this->eq_constants              = eq_constants;
  this->concentration_strands_tot = concentration_strands_tot;
  this->A                         = A;
  this->strands                   = strands;
  this->complexes                 = complexes;
}

void
get_derivative_and_hessian(const column_vector& x,
                           column_vector&       der,
                           general_matrix&      hess) const
{
  der   = h_derivative(x, eq_constants, concentration_strands_tot, A, strands, complexes);
  hess  = h_hessian(x, eq_constants, A, strands, complexes);
}
};

/*
 *  Get concentrations of single strands from
 *  a given vector L (that minimizes h(L))
 */
PRIVATE double *
conc_single_strands(const column_vector&  L,
                    size_t                strands)
{
  double *c = (double *)vrna_alloc(sizeof(double) * strands);

  for (size_t a = 0; a < strands; a++)
    c[a] = exp(L(a));

  return c;
}

/*
 *  Get concentrations of complexes from
 *  a given vector L (that minimizes h(L))
 */
PRIVATE double *
conc_complexes(const column_vector& L,
               const double         *eq_const,
               const unsigned int   **A,
               size_t               strands,
               size_t               complexes)
{
  double *c;

  c = (double *)vrna_alloc(sizeof(double) * complexes);

  for (size_t k = 0; k < complexes; k++) {
    c[k] = log(eq_const[k]);

    for (size_t a = 0; a < strands; a++)
      c[k] += (double)A[a][k] * L(a);

    c[k] = exp(c[k]);
  }

  return c;
}

double *
vrna_equilibrium_conc(const double        *eq_constants,
                      double              *concentration_strands,
                      const unsigned int  **A,
                      size_t              num_strands,
                      size_t              num_complexes)
{
  double        *r = NULL;

  column_vector starting_point;

  h_model       h;

  h.init(eq_constants,
         concentration_strands,
         A,
         num_strands,
         num_complexes);

  starting_point.set_size(num_strands);

  for (size_t a = 0; a < num_strands; a++)
    starting_point(a) = 0.;

  find_min_trust_region(objective_delta_stop_strategy(1e-18),
                        h,
                        starting_point,
                        1   // initial trust region radius
                        );

  double *conc_monomers = conc_single_strands(starting_point, num_strands);

  for (size_t a = 0; a < num_strands; a++)
    concentration_strands[a] = conc_monomers[a];

  r = conc_complexes(starting_point, eq_constants, A, num_strands, num_complexes);

  free(conc_monomers);

  return r;
}


#include <iostream>
#include <complex>
#include <cstdlib>
#include <Eigen/Eigen>

int main(int argc, char const *argv[])
{
  Eigen::Matrix<float, 2, 2> a = Eigen::Matrix<float, 2, 2>::Zero();
  a(1,1) = 1;
  Eigen::Matrix<std::complex<double>, 2, 2> b, c;
  b << std::complex<double>(), std::complex<double>(1,1), std::complex<double>(2,2), std::complex<double>(3,3);
  c = a.cast<std::complex<double>>()*b;
  std::cout << "a = " << std::endl << a << std::endl;
  std::cout << "b = " << std::endl << b << std::endl;
  std::cout << "c = " << std::endl << c << std::endl;
  return EXIT_SUCCESS;
}


// Copyright (c) 2020 Chris Richardson & Matthew Scroggs
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "brezzi-douglas-marini.h"
#include "core/dof-permutations.h"
#include "core/element-families.h"
#include "core/mappings.h"
#include "core/moments.h"
#include "core/polyset.h"
#include "core/quadrature.h"
#include "lagrange.h"
#include "nedelec.h"
#include <Eigen/Dense>
#include <numeric>
#include <vector>

using namespace basix;

//----------------------------------------------------------------------------
FiniteElement basix::create_bdm(cell::type celltype, int degree)
{
  if (celltype != cell::type::triangle and celltype != cell::type::tetrahedron)
    throw std::runtime_error("Unsupported cell type");

  const int tdim = cell::topological_dimension(celltype);

  const cell::type facettype
      = (tdim == 2) ? cell::type::interval : cell::type::triangle;

  // The number of order (degree) scalar polynomials
  const int npoly = polyset::dim(celltype, degree);
  const int ndofs = npoly * tdim;

  // Create coefficients for order (degree-1) vector polynomials
  Eigen::MatrixXd wcoeffs = Eigen::MatrixXd::Identity(ndofs, ndofs);

  // Dual space
  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, ndofs);

  // quadrature degree
  int quad_deg = 5 * degree;

  // Add rows to dualmat for integral moments on facets
  const int facet_count = tdim + 1;
  const int facet_dofs = polyset::dim(facettype, degree);

  dual.block(0, 0, facet_count * facet_dofs, ndofs)
      = moments::make_normal_integral_moments(
          create_dlagrange(facettype, degree), celltype, tdim, degree,
          quad_deg);

  const int internal_dofs = ndofs - facet_count * facet_dofs;

  // Add rows to dualmat for integral moments on interior
  if (degree > 1)
  {
    // Interior integral moment
    dual.block(facet_count * facet_dofs, 0, internal_dofs, ndofs)
        = moments::make_dot_integral_moments(
            create_nedelec(celltype, degree - 1), celltype, tdim, degree,
            quad_deg);
  }

  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);

  int perm_count = 0;
  for (int i = 1; i < tdim; ++i)
    perm_count += topology[i].size() * i;

  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));
  if (tdim == 2)
  {
    Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree + 1);
    Eigen::ArrayXXd edge_dir
        = dofperms::interval_reflection_tangent_directions(degree + 1);
    for (int edge = 0; edge < facet_count; ++edge)
    {
      const int start = edge_ref.size() * edge;
      for (int i = 0; i < edge_ref.size(); ++i)
      {
        base_permutations[edge](start + i, start + i) = 0;
        base_permutations[edge](start + i, start + edge_ref[i]) = 1;
      }
      Eigen::MatrixXd directions = Eigen::MatrixXd::Identity(ndofs, ndofs);
      directions.block(edge_dir.rows() * edge, edge_dir.cols() * edge,
                       edge_dir.rows(), edge_dir.cols())
          = edge_dir;
      base_permutations[edge] *= directions;
    }
  }
  else if (tdim == 3)
  {
    Eigen::ArrayXi face_ref = dofperms::triangle_reflection(degree + 1);
    Eigen::ArrayXi face_rot = dofperms::triangle_rotation(degree + 1);

    for (int face = 0; face < facet_count; ++face)
    {
      const int start = face_ref.size() * face;
      for (int i = 0; i < face_rot.size(); ++i)
      {
        base_permutations[6 + 2 * face](start + i, start + i) = 0;
        base_permutations[6 + 2 * face](start + i, start + face_rot[i]) = 1;
        base_permutations[6 + 2 * face + 1](start + i, start + i) = 0;
        base_permutations[6 + 2 * face + 1](start + i, start + face_ref[i])
            = -1;
      }
    }
  }

  // BDM has facet_dofs dofs on each facet, and ndofs-facet_count*facet_dofs in
  // the interior
  std::vector<std::vector<int>> entity_dofs(topology.size());
  for (int i = 0; i < tdim - 1; ++i)
    entity_dofs[i].resize(topology[i].size(), 0);
  entity_dofs[tdim - 1].resize(topology[tdim - 1].size(), facet_dofs);
  entity_dofs[tdim] = {internal_dofs};

  Eigen::MatrixXd coeffs = compute_expansion_coefficients(wcoeffs, dual);

  return FiniteElement(element::family::BDM, celltype, degree, {tdim}, coeffs,
                       entity_dofs, base_permutations, {}, {},
                       mapping::type::contravariantPiola);
}
//-----------------------------------------------------------------------------


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

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <algorithm>
#include <chrono>
#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))

namespace Laplace3D3D {

using EVec3 = Eigen::Vector3d;
using EVec4 = Eigen::Vector4d;
constexpr double eps = 1e-10;

// real and wave sum of 2D Laplace kernel Ewald

inline double freal(double xi, double r) { return std::erfc(xi * r) / r; }

inline double frealp(double xi, double r) {
    return -(2. * exp(-r * r * (xi * xi)) * xi) / (sqrt(M_PI) * r) - std::erfc(r * xi) / (r * r);
}

// xm: target, xn: source
inline double realSum(const double xi, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < eps) {
        return 0;
    }
    return freal(xi, rnorm);
}

inline double potEwald(const EVec3 &xm, const EVec3 &xn) {
    const double xi = 2; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    // real sum
    int rLim = 4;
    double Kreal = 0;
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            for (int k = -rLim; k <= rLim; k++) {
                Kreal += realSum(xi, target, source - EVec3(i, j, k));
            }
        }
    }

    // wave sum
    int wLim = 4;
    double Kwave = 0;
    EVec3 rmn = target - source;
    const double xi2 = xi * xi;
    const double rmnnorm = rmn.norm();
    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            for (int k = -wLim; k <= wLim; k++) {
                if (i == 0 && j == 0 && k == 0) {
                    continue;
                }
                EVec3 kvec = EVec3(i, j, k) * (2 * PI314);
                double k2 = kvec.dot(kvec);
                Kwave += 4 * PI314 * cos(kvec.dot(rmn)) * exp(-k2 / (4 * xi2)) / k2;
            }
        }
    }

    double Kself = rmnnorm < 1e-10 ? -2 * xi / sqrt(PI314) : 0;

    return Kreal + Kwave + Kself - PI314 / xi2;
}

inline void realGradSum(double xi, const EVec3 &target, const EVec3 &source, EVec3 &v) {
    EVec3 rvec = target - source;
    double rnorm = rvec.norm();
    if (rnorm < eps) {
        v.setZero();
    } else {
        v = (frealp(xi, rnorm) / rnorm) * rvec;
    }
}

inline void gradkernel(const EVec3 &target, const EVec3 &source, EVec3 &answer) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;
    answer = -rst / rnorm3;
}

// grad of Laplace potential, without 1/4pi prefactor, periodic of -r_k/r^3
inline void gradEwald(const EVec3 &target_, const EVec3 &source_, EVec3 &answer) {
    EVec3 target = target_;
    EVec3 source = source_;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    double xi = 0.54;

    // real sum
    int rLim = 10;
    EVec3 Kreal = EVec3::Zero();
    for (int i = -rLim; i < rLim + 1; i++) {
        for (int j = -rLim; j < rLim + 1; j++) {
            for (int k = -rLim; k < rLim + 1; k++) {
                EVec3 v = EVec3::Zero();
                realGradSum(xi, target, source + EVec3(i, j, k), v);
                Kreal += v;
            }
        }
    }

    // wave sum
    int wLim = 10;
    EVec3 rmn = target - source;
    double xi2 = xi * xi;
    EVec3 Kwave(0., 0., 0.);
    for (int i = -wLim; i < wLim + 1; i++) {
        for (int j = -wLim; j < wLim + 1; j++) {
            for (int k = -wLim; k < wLim + 1; k++) {
                if (i == 0 && j == 0 && k == 0)
                    continue;
                EVec3 kvec = EVec3(i, j, k) * (2 * M_PI);
                double k2 = kvec.dot(kvec);
                double knorm = kvec.norm();
                Kwave += -kvec * (sin(kvec.dot(rmn)) * exp(-k2 / (4 * xi2)) / k2);
            }
        }
    }

    answer = Kreal + Kwave;
}

inline double pot(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < eps ? 0 : 1 / rnorm;
}

inline EVec4 gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    EVec4 pgrad = EVec4::Zero();
    double rnorm = rst.norm();
    if (rnorm < eps) {
        pgrad.setZero();
    } else {
        pgrad[0] = 1 / rnorm;
        double rnorm3 = rnorm * rnorm * rnorm;
        pgrad.block<3, 1>(1, 0) = -rst / rnorm3;
    }
    return pgrad;
}

inline EVec4 gKernelEwald(const EVec3 &target, const EVec3 &source) {
    EVec4 pgrad = EVec4::Zero();
    pgrad[0] = potEwald(target, source);
    EVec3 grad;
    gradEwald(target, source, grad);
    pgrad[1] = grad[0];
    pgrad[2] = grad[1];
    pgrad[3] = grad[2];
    return pgrad;
}

inline EVec4 gKernelNF(const EVec3 &target, const EVec3 &source, int N = DIRECTLAYER) {
    EVec4 gNF = EVec4::Zero();
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                EVec4 gFree = gKernel(target, source + EVec3(i, j, k));
                gNF += gFree;
            }
        }
    }
    return gNF;
}

// Out of Direct Sum Layer, far field part
inline EVec4 gKernelFF(const EVec3 &target, const EVec3 &source) {
    EVec4 fEwald = gKernelEwald(target, source);
    fEwald -= gKernelNF(target, source);
    return fEwald;
}

inline double potNF(const EVec3 &target, const EVec3 &source, int N = DIRECTLAYER) {
    double gNF = 0;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                gNF += pot(target, source + EVec3(i, j, k));
            }
        }
    }
    return gNF;
}

// Out of Direct Sum Layer, far field part
inline double potFF(const EVec3 &target, const EVec3 &source) {
    double fEwald = potEwald(target, source);
    fEwald -= potNF(target, source);
    return fEwald;
}

/**
 * \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);

    std::cout << std::scientific << std::setprecision(18);

    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    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

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(equivN, equivN); // Laplace, 1->1

    // Aup for solving MEquiv
    Eigen::MatrixXd Aup(checkN, equivN);
    Eigen::MatrixXd AuppinvU(Aup.cols(), Aup.rows());
    Eigen::MatrixXd AuppinvVT(Aup.cols(), Aup.rows());
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Aup(k, l) = pot(Cpoint, Lpoint);
        }
    }
    pinv(Aup, AuppinvU, AuppinvVT);

    // condition number
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Aup);
    double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1);
    std::cout << cond << std::endl;
    std::cout << "s:" << svd.singularValues() << std::endl;

    // Adown for solving LEquiv
    Eigen::MatrixXd Adown(checkN, equivN);
    Eigen::MatrixXd AdownpinvU(Adown.cols(), Adown.rows());
    Eigen::MatrixXd AdownpinvVT(Adown.cols(), Adown.rows());
    for (int k = 0; k < checkN; k++) {
        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]);
            Adown(k, l) = pot(Cpoint, Lpoint);
        }
    }
    pinv(Adown, AdownpinvU, AdownpinvVT);

#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]);
        const EVec3 Npoint(0.5, 0.5, 0.5);
        Eigen::VectorXd f(checkN);
        f.setZero();
        for (int k = 0; k < checkN; k++) {
            EVec3 Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            f[k] = potFF(Cpoint, Mpoint) - potFF(Cpoint, Npoint);
        }

        M2L.col(i) = (AdownpinvU.transpose() * (AdownpinvVT.transpose() * f));
    }
    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();

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

    std::cout << "Precomputing time:" << duration / 1e6 << std::endl;

    // Test
    EVec3 center(0.6, 0.5, 0.5);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> chargePoint(2);
    std::vector<double> chargeValue(2);
    chargePoint[0] = center + EVec3(0.1, 0, 0);
    chargeValue[0] = 1;
    chargePoint[1] = center + EVec3(-0.1, 0., 0.);
    chargeValue[1] = -1;
    // chargePoint[2] = center + EVec3(-0.2, 0., 0.);
    // chargeValue[2] = 1;

    // solve M
    Eigen::VectorXd f(checkN);
    for (int k = 0; k < checkN; k++) {
        double temp = 0;
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < chargePoint.size(); p++) {
            temp = temp + pot(Cpoint, chargePoint[p]) * (chargeValue[p]);
        }
        f[k] = temp;
    }
    Eigen::VectorXd Msource = (AuppinvU.transpose() * (AuppinvVT.transpose() * f));

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

    std::cout << "backward error: " << f - Aup * Msource << std::endl;

    // check dipole moment
    {
        EVec3 dipole = EVec3::Zero();
        for (int i = 0; i < chargePoint.size(); i++) {
            dipole += chargeValue[i] * chargePoint[i];
        }
        std::cout << "charge dipole " << dipole.transpose() << std::endl;
    }
    {
        EVec3 dipole = EVec3::Zero();
        for (int i = 0; i < equivN; i++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
            dipole += Mpoint * Msource[i];
        }
        std::cout << "Mequiv dipole " << dipole.transpose() << std::endl;
    }

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

    for (int is = 0; is < 5; is++) {

        Eigen::Vector3d samplePoint = EVec3::Random() * 0.2 + EVec3(0.5, 0.5, 0.5);

        EVec4 UFFL2T = EVec4::Zero();
        EVec4 UFFS2T = EVec4::Zero();
        EVec4 UFFM2T = EVec4::Zero();

#pragma omp sections
        {
#pragma omp section
            for (int p = 0; p < chargePoint.size(); p++) {
                UFFS2T += gKernelFF(samplePoint, chargePoint[p]) * chargeValue[p];
            }

#pragma omp section
            for (int p = 0; p < equivN; p++) {
                Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1], pointLEquiv[3 * p + 2]);
                UFFL2T += gKernel(samplePoint, Lpoint) * M2Lsource[p];
            }

#pragma omp section
            for (int p = 0; p < equivN; p++) {
                Eigen::Vector3d Mpoint(pointMEquiv[3 * p], pointMEquiv[3 * p + 1], pointMEquiv[3 * p + 2]);
                UFFM2T += gKernelFF(samplePoint, Mpoint) * Msource[p];
            }
        }
        std::cout << std::scientific << std::setprecision(10);
        std::cout << "-----------------------------------------------" << std::endl;
        std::cout << "samplePoint:" << samplePoint.transpose() << std::endl;
        std::cout << "UFF S2T: " << UFFS2T.transpose() << std::endl;
        std::cout << "UFF M2T: " << UFFM2T.transpose() << std::endl;
        std::cout << "UFF L2T: " << UFFL2T.transpose() << std::endl;
        std::cout << "Error M2T: " << (UFFM2T - UFFS2T).transpose() << std::endl;
        std::cout << "Error L2T: " << (UFFL2T - UFFS2T).transpose() << std::endl;
    }

    return 0;
}

} // namespace Laplace3D3D

#undef DIRECTLAYER
#undef PI314


/**
 * Some additional useful distributions (only bare operator()).
 */
#ifndef SKYLARK_DISTRIBUTIONS_HPP
#define SKYLARK_DISTRIBUTIONS_HPP

#include <boost/random.hpp>

namespace skylark {
namespace utility {

/**
 * Levy distribution
 */
template< typename ValueType >
struct standard_levy_distribution_t {

    // TODO not really sure this is the standard, or implemented correctly

    typedef ValueType result_type;

    standard_levy_distribution_t() {

    }

    template< typename URNG >
    ValueType operator()(URNG &prng) const {
        boost::random::gamma_distribution<ValueType> dist(0.5, 2);
        result_type y = static_cast<ValueType>(dist(prng));
        return (1.0 / y);
    }
    void reset() {}

};

/**
 * Radamachar distribution - +1 and -1 with equal probability.
 */
template< typename ValueType >
struct rademacher_distribution_t {

    typedef ValueType result_type;

    template< typename URNG >
    ValueType operator()(URNG &prng) const {
        double probabilities[] = { 0.5, 0.0, 0.5 };
        boost::random::discrete_distribution<> dist(probabilities);
        return static_cast<ValueType>(dist(prng)) - 1.0;
    }
    void reset() {}
};

/**
 * Uniform distribution
 */
template <typename ValueType> struct uniform_distribution_t {
    typedef ValueType result_type;
};

/**
 * Uniform distribution specialization for double's
 */
template <> struct uniform_distribution_t <double> {

    typedef double result_type;

    boost::random::uniform_real_distribution<double> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(double low, double high) :
      distribution(low, high) {}

    template< typename URNG >
    double operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for int's
 */
template <> struct uniform_distribution_t <int> {

    typedef int result_type;

    boost::random::uniform_int_distribution<int> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(int low, int high) :
      distribution(low, high) {}

    template< typename URNG >
    int operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for size_t
 */
template <> struct uniform_distribution_t <size_t> {

    typedef int result_type;

    boost::random::uniform_int_distribution<size_t> distribution;

    uniform_distribution_t() {}

    uniform_distribution_t(size_t low, size_t high) :
      distribution(low, high) {}

    template< typename URNG >
    int operator()(URNG &urng) const {
        return distribution(urng);
    }
    void reset() {}
};

/**
 * Uniform distribution specialization for bool's
 */
template <> struct uniform_distribution_t <bool> {

    typedef bool result_type;

    boost::random::uniform_int_distribution<int> distribution;

    uniform_distribution_t() {}

    template< typename URNG >
    bool operator()(URNG &urng) const {
        return (1==distribution(urng));
    }
    void reset() {}
};

} // namespace utility
} // namespace skylark

#endif // SKYLARK_DISTRIBUTIONS_HPP