#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]
   */


// SPDX-License-Identifier: MIT
// Copyright (c) 2020 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#ifndef SCICPP_CORE_INTERPOLATE
#define SCICPP_CORE_INTERPOLATE

#include "scicpp/core/functional.hpp"
#include "scicpp/core/macros.hpp"
#include "scicpp/core/meta.hpp"
#include "scicpp/linalg/utils.hpp"

#include <Eigen/Core>
#include <unsupported/Eigen/Splines>
#include <utility>

namespace scicpp::interpolate {

// Interpolation with degree zero doesn't work
enum InterpKind : int { /* ZERO = 0, */ SLINEAR = 1, QUADRATIC = 2, CUBIC = 3 };

namespace detail {

// https://stackoverflow.com/questions/29822041/eigen-spline-interpolation-how-to-get-spline-y-value-at-arbitray-point-x

template <InterpKind kind>
class SplineFunction {
  public:
    template <class EigenVector>
    SplineFunction(const EigenVector &x, const EigenVector &y)
        : x_min(x.minCoeff()), x_max(x.maxCoeff()),
          // Spline fitting here. X values are scaled down to [0, 1] for this.
          m_spline(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(
              y.transpose(),
              std::min<int>(int(x.rows() - 1), kind),
              scaled_values(x))) {
        scicpp_require(x.size() == y.size());
        scicpp_require(x_max > x_min);
    }

    template <typename T>
    auto operator()(T x) const {
        // x values need to be scaled down in extraction as well.
        return m_spline(scaled_value(x))(0);
    }

  private:
    template <typename T>
    auto scaled_value(T x) const {
        return (x - T(x_min)) / (T(x_max) - T(x_min));
    }

    template <class EigenVector>
    auto scaled_values(const EigenVector &v) const {
        return v.unaryExpr([this](double x) { return scaled_value(x); })
            .transpose();
    }

    double x_min, x_max;
    Eigen::Spline<double, 1> m_spline;
};

} // namespace detail

template <InterpKind kind = SLINEAR>
struct interp1d {
    template <typename Array1, typename Array2>
    interp1d(const Array1 &x, const Array2 &y)
        : s(linalg::to_eigen_array(x), linalg::to_eigen_array(y)) {}

    template <typename T>
    auto operator()(T &&x) const {
        if constexpr (meta::is_iterable_v<T>) {
            return map(s, std::forward<T>(x));
        } else {
            return s(x);
        }
    }

  private:
    detail::SplineFunction<kind> s;
};

} // namespace scicpp::interpolate

#endif // SCICPP_CORE_INTERPOLATE


#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 <Eigen/Dense>
#include <map>

namespace costmap
{

struct TerrainData
{
    Eigen::Vector3d meanPosition;
    Eigen::Vector3d centroidPosition;
    Eigen::Vector3d normal;
    double curvature;
    Eigen::Matrix3d covarianceMatrix;
    std::map<std::pair<double, double>, double> heightMap;
    std::map<double, std::map<double, double>> costMap;
    double minHeight;
    double resolution;
};
}


//
//  cal_ke.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/7/18.
//
//

#ifndef cal_ke_hpp
#define cal_ke_hpp

#include <stdio.h>
#include <Eigen/Eigen>

using namespace Eigen;

void cal_ke(MatrixXd coord ,double E, double nu, MatrixXd &ke);

#endif /* cal_ke_hpp */


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_PREC_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_PREC_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/err/check_pos_definite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/**
 * Return a multivariate normal random variate with the given location
 * and precision using the specified random number generator.
 *
 * mu can be either an Eigen::VectorXd, an Eigen::RowVectorXd, or a
 * std::vector of either of those types.
 *
 * @tparam T_loc Type of location paramater
 * @tparam RNG Type of pseudo-random number generator
 * @param mu (Sequence of) location parameter(s)
 * @param S Precision matrix
 * @param rng random number generator
 * @throw std::domain_error if S is not positive definite, or
 * std::invalid_argument if the length of (each) mu is not equal to
 * the number of rows and columns in S
 */
template <typename T_loc, class RNG>
inline typename StdVectorBuilder<true, Eigen::VectorXd, T_loc>::type
multi_normal_prec_rng(const T_loc &mu, const Eigen::MatrixXd &S, RNG &rng) {
  using boost::normal_distribution;
  using boost::variate_generator;

  static const char *function = "multi_normal_prec_rng";

  check_positive(function, "Precision matrix rows", S.rows());
  check_finite(function, "Precision matrix", S);
  check_symmetric(function, "Precision matrix", S);

  Eigen::LLT<Eigen::MatrixXd> llt_of_S = S.llt();
  check_pos_definite(function, "precision matrix argument", llt_of_S);

  vector_seq_view<T_loc> mu_vec(mu);
  check_positive(function, "number of location parameter vectors",
                 mu_vec.size());
  size_t size_mu = mu_vec[0].size();

  size_t N = mu_vec.size();

  for (size_t i = 1; i < N; i++) {
    int size_mu_new = mu_vec[i].size();
    check_size_match(function,
                     "Size of one of the vectors of "
                     "the location variable",
                     size_mu_new,
                     "Size of another vector of the "
                     "location variable",
                     size_mu);
  }

  for (size_t i = 0; i < N; i++) {
    check_finite(function, "Location parameter", mu_vec[i]);
  }

  check_size_match(function, "Rows of location parameter", size_mu, "Rows of S",
                   S.rows());

  StdVectorBuilder<true, Eigen::VectorXd, T_loc> output(N);

  variate_generator<RNG &, normal_distribution<>> std_normal_rng(
      rng, normal_distribution<>(0, 1));

  for (size_t n = 0; n < N; ++n) {
    Eigen::VectorXd z(S.cols());
    for (int i = 0; i < S.cols(); i++)
      z(i) = std_normal_rng();

    output[n] = Eigen::VectorXd(mu_vec[n]) + llt_of_S.matrixU().solve(z);
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#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);
}


#include "math_unit_test.hpp"
#include <boost/math/fft/algorithms.hpp>

using namespace boost::math::fft;

void test_is_prime()
{
  CHECK_EQUAL(false,detail::is_prime(1));
  CHECK_EQUAL(true,detail::is_prime(2));
  CHECK_EQUAL(true,detail::is_prime(3));
  CHECK_EQUAL(false,detail::is_prime(4));
  CHECK_EQUAL(true,detail::is_prime(5));
  CHECK_EQUAL(false,detail::is_prime(6));
  CHECK_EQUAL(true,detail::is_prime(7));
  CHECK_EQUAL(false,detail::is_prime(8));
  CHECK_EQUAL(false,detail::is_prime(9));
  CHECK_EQUAL(false,detail::is_prime(10));

  // factorial primes
  CHECK_EQUAL(true,detail::is_prime(719));
  CHECK_EQUAL(true,detail::is_prime(5039));
  CHECK_EQUAL(true,detail::is_prime(39916801));
  CHECK_EQUAL(true,detail::is_prime(479001599));
}
void test_primitive_root()
{
  for(auto p : std::vector<long>{3,5,7,11,13,17,23,29})
  {
    const long r = detail::primitive_root(p);
    std::cerr << "p = " << p << ", root = " << r << '\n';
    const long phi = p-1;

    long r_p = r;
    for(int i=1;i<phi;++i)
    {
      CHECK_EQUAL(false,r_p == 1);
      r_p = (r_p * r) % p;
    }
      CHECK_EQUAL(true,r_p == 1);
  }
}

void test_power2()
{
  CHECK_EQUAL(false,detail::is_power2(-4));
  CHECK_EQUAL(false,detail::is_power2(-3));
  CHECK_EQUAL(false,detail::is_power2(-2));
  CHECK_EQUAL(false,detail::is_power2(-1));
  CHECK_EQUAL(false,detail::is_power2(0));
  CHECK_EQUAL(false,detail::is_power2(3));
  CHECK_EQUAL(false,detail::is_power2(5));
  CHECK_EQUAL(false,detail::is_power2(6));
  CHECK_EQUAL(false,detail::is_power2(7));

  for(int i=9;i<16;++i)
    CHECK_EQUAL(false,detail::is_power2(i));

  CHECK_EQUAL(true,detail::is_power2(1));
  CHECK_EQUAL(true,detail::is_power2(2));
  CHECK_EQUAL(true,detail::is_power2(4));
  CHECK_EQUAL(true,detail::is_power2(8));
  CHECK_EQUAL(true,detail::is_power2(16));

  for(int i=-20;i<=0;++i)
    CHECK_EQUAL(0,detail::lower_bound_power2(i));

  CHECK_EQUAL(1,detail::lower_bound_power2(1));
  CHECK_EQUAL(2,detail::lower_bound_power2(2));
  CHECK_EQUAL(2,detail::lower_bound_power2(3));
  CHECK_EQUAL(4,detail::lower_bound_power2(4));
  CHECK_EQUAL(4,detail::lower_bound_power2(5));
  CHECK_EQUAL(4,detail::lower_bound_power2(6));
  CHECK_EQUAL(4,detail::lower_bound_power2(7));
  for(int i=8;i<16;++i)
    CHECK_EQUAL(8,detail::lower_bound_power2(i));
  for(int i=16;i<32;++i)
    CHECK_EQUAL(16,detail::lower_bound_power2(i));

  for(int i=-20;i<=0;++i)
    CHECK_EQUAL(1,detail::upper_bound_power2(i));

  CHECK_EQUAL(1,detail::upper_bound_power2(1));
  CHECK_EQUAL(2,detail::upper_bound_power2(2));
  CHECK_EQUAL(4,detail::upper_bound_power2(3));
  CHECK_EQUAL(4,detail::upper_bound_power2(4));
  CHECK_EQUAL(8,detail::upper_bound_power2(5));
  CHECK_EQUAL(8,detail::upper_bound_power2(6));
  CHECK_EQUAL(8,detail::upper_bound_power2(7));
  CHECK_EQUAL(8,detail::upper_bound_power2(8));
  for(int i=9;i<=16;++i)
    CHECK_EQUAL(16,detail::upper_bound_power2(i));
  for(int i=17;i<=32;++i)
    CHECK_EQUAL(32,detail::upper_bound_power2(i));
  CHECK_EQUAL(0,detail::upper_bound_power2(std::numeric_limits<int>::max()));
  CHECK_EQUAL(0,detail::upper_bound_power2((1<<30)+1));
  CHECK_EQUAL(1<<30,detail::upper_bound_power2(1<<30));
  CHECK_EQUAL(1<<30,detail::upper_bound_power2((1<<30) - 1));
}

int main()
{
  test_power2();
  test_is_prime();
  test_primitive_root();
  return boost::math::test::report_errors();
}


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


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

double amips_param;
double energy_min;
double energy_max = 10;
double c1_param;
double c2_param;
double d1_param;

Vector3d color_map(double v)
{
	Vector3d c;

    c[0] = c[1] = c[2] = 1;

	double dv;

	if (v < energy_min)
		v = energy_min;
	if (v > energy_max)
		v = energy_max;
	dv = energy_max - energy_min;

	if (v < (energy_min + 0.25 * dv)) {
		c[0] = 0;
		c[1] = 4 * (v - energy_min) / dv;
	}
	else if (v < (energy_min + 0.5 * dv)) {
		c[0] = 0;
		c[2] = 1 + 4 * (energy_min + 0.25 * dv - v) / dv;
	}
	else if (v < (energy_min + 0.75 * dv)) {
		c[0] = 4 * (v - energy_min - 0.5 * dv) / dv;
		c[2] = 0;
	}
	else {
		c[1] = 1 + 4 * (energy_min + 0.75 * dv - v) / dv;
		c[2] = 0;
	}

	return c;
}

void set_energy_min(int type)
{

    switch(type)
    {
        case 0: //arap
            energy_min = 0;
            break;

        case 1: // mips
            energy_min = 2;
            break;

        case 2: // iso
            energy_min = 4;
            break;

        case 3: // amips
            energy_min = exp(amips_param * 2);
            break;

        case 4: // conf
            energy_min = 1;
            break;

        case 5:
            energy_min = -c1_param - 2 * c2_param;
            break;
    }

}

double energy_value(int type, Vector2d S)
{

    double value = 0;
    double J;
    double l1;
    double l2;

    switch(type)
    {
        case 0: //arap
            value = (S[0] - 1) * (S[0] - 1) + (S[1] - 1) * (S[1] - 1);
            break;

        case 1: // mips
            value = S[0] / S[1] + S[1] / S[0];
            break;

        case 2: // iso
            value = S[0] * S[0] + 1.0 / (S[0] * S[0]) + S[1] * S[1] + 1.0 / (S[1] * S[1]);
            break;

        case 3: // amips
            value = exp(amips_param * (S[0] / S[1] + S[1] / S[0]));
            break;

        case 4: // conf
            value = S[0] / S[1];
            value *= value;
            break;

        case 5: // gmr
            J = S[0] * S[1];
            l1 = S[0] * S[0] + S[1] * S[1];
            l2 = S[0] * S[0] * S[1] * S[1];

            value = c1_param * (pow(J, -2.0 / 3.0) * l1 - 3) + c2_param * (pow(J, -4.0 / 3.0) * l2 - 3) + d1_param * (J - 1) * (J - 1);
            break;

        case 6: // olg

            value = max(S[0], 1.0 / S[1]);
            break;
    }

    return value;

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;
    const int *dims;
    double *tri_num, *X_g_inv, *tri_areas, *obj_tri, *q_target, *type, *amips_s, *c1, *c2, *d1;
    double *output;

    tri_num = mxGetPr(prhs[0]);
    X_g_inv = mxGetPr(prhs[1]);
    tri_areas = mxGetPr(prhs[2]);
    obj_tri = mxGetPr(prhs[3]);
    q_target = mxGetPr(prhs[4]);
    type = mxGetPr(prhs[5]);
    amips_s = mxGetPr(prhs[6]);
    c1 = mxGetPr(prhs[7]);
    c2 = mxGetPr(prhs[8]);
    d1 = mxGetPr(prhs[9]);

    int tri_n = tri_num[0];
    int energy_type = type[0];
    amips_param = amips_s[0];
    c1_param = c1[0];
    c2_param = c2[0];
    d1_param = d1[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(tri_n, 1, mxREAL);

    output = mxGetPr(output_mex);

    int tri[3];
    double tri_area;

    Matrix2d B, X_f, A;
    Vector2d S;
    Matrix2d U, V;

    for(int i = 0; i < tri_n; i++)
    {
        //mexPrintf("%d %d %d\n", (int)obj_tri[i], (int)obj_tri[i + tri_n], (int)obj_tri[i + 2 * tri_n]);
        tri[0] = obj_tri[i] - 1;
        tri[1] = obj_tri[i + tri_n] - 1;
        tri[2] = obj_tri[i + 2 * tri_n] - 1;
        tri_area = tri_areas[i];

        B(0, 0) = X_g_inv[i];
        B(0, 1) = X_g_inv[i + tri_n * 2];
        B(1, 0) = X_g_inv[i + tri_n];
        B(1, 1) = X_g_inv[i + tri_n * 3];

        X_f(0, 0) = q_target[2 * tri[1]] - q_target[2 * tri[0]];
        X_f(1, 0) = q_target[2 * tri[1] + 1] - q_target[2 * tri[0] + 1];
        X_f(0, 1) = q_target[2 * tri[2]] - q_target[2 * tri[0]];
        X_f(1, 1) = q_target[2 * tri[2] + 1] - q_target[2 * tri[0] + 1];

        A = X_f * B;

        if(A.determinant() <= 0)
        {
            //mexPrintf("element inverted");
        }

        JacobiSVD<Matrix2d> svd(A, ComputeFullU | ComputeFullV);

        S = svd.singularValues();
        U = svd.matrixU();
        V = svd.matrixV();

        output[i] = energy_value(energy_type, S);

    }

    return;

}


#include "maxwellJuttner.h"

#include <fmath/mathFunctions.h>
#include <fmath/physics.h>
#include <boost/math/special_functions/bessel.hpp>

double f_norm(double g, double norm_temp)
{
	double beta = sqrt(1.0-1.0/P2(g));

	return P2(g)*beta*exp(-g/norm_temp);

}

double maxwellRel(double gamma, double norm_temp, double norm)
{

	double beta = sqrt(1.0-1.0/(gamma*gamma));

	double K2 =  boost::math::cyl_bessel_k(2, 1.0/norm_temp); //bessk(2, 1.0/norm_temp);

	double dist_g = gamma*gamma*beta*exp(-gamma/norm_temp) / (K2*norm_temp);

	return  dist_g*norm;

}


#ifndef BOOST_METAPARSE_GETTING_STARTED_7_HPP
#define BOOST_METAPARSE_GETTING_STARTED_7_HPP

// Automatically generated header file

// Definitions before section 6.2
#include "6_2.hpp"

// Definitions of section 6.2
#include <boost/mpl/minus.hpp>

template <class L, char Op, class R> struct eval_binary_op;

template <class L, class R> struct eval_binary_op<L, '+', R> : boost::mpl::plus<L, R>::type {};

template <class L, class R> struct eval_binary_op<L, '-', R> : boost::mpl::minus<L, R>::type {};

// query:
//    eval_binary_op<boost::mpl::int_<11>, '+', boost::mpl::int_<2>>::type

// query:
//    eval_binary_op<boost::mpl::int_<13>, '-', boost::mpl::int_<2>>::type

template <class S, class Item>
 struct binary_op :
   eval_binary_op<
     S,
     boost::mpl::at_c<Item, 0>::type::value,
     typename boost::mpl::at_c<Item, 1>::type
   >
   {};

// query:
//    binary_op<boost::mpl::int_<11>, boost::mpl::vector<boost::mpl::char_<'+'>, boost::mpl::int_<2>>>::type

using exp_parser13 =
 build_parser<
   foldl_start_with_parser<
     sequence<one_of<plus_token, minus_token>, int_token>,
     int_token,
     boost::mpl::quote2<binary_op>
   >
 >;

// query:
//    exp_parser13::apply<BOOST_METAPARSE_STRING("1 + 2 - 3")>::type

#endif


/**
    @file Project_1.cpp

    @author Terence Henriod

    Project 1: Bayesion Minimum Error Classification

    @brief The driver program for use of a Bayesian Minimum Error Classifier.

    @version Original Code 1.00 (3/8/2014) - T. Henriod

    UNOFFICIALLY:
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    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
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

Compilation notes:
g++ -I /home/thenriod/Desktop/cpp_libs/Eigen_lib/ Project_1.cpp

*/

/*==============================================================================
=======     HEADER FILES     ===================================================
==============================================================================*/
#include <cmath>
#include <iostream>

#include "bayes_classifier.cpp"
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

using namespace std;

/*==============================================================================
=======     USER DEFINED TYPES     =============================================
==============================================================================*/

/*==============================================================================
=======     CONSTANTS / MACROS     =============================================
==============================================================================*/

/*==============================================================================
=======     GLOBAL VARIABLES     ===============================================
==============================================================================*/
  // none

/*==============================================================================
=======     FUNCTION PROTOTYPES     ============================================
==============================================================================*/

/*==============================================================================
=======     MAIN FUNCTION     ==================================================
==============================================================================*/

/**
main

The main driver

@param

@return

@pre
-#

@post
-#

@code
@endcode
*/

int main( int argc, char** argv )
{
  // variables
  BayesClassifier problem_solver;
  double part_A_prior_probability = 0.5;
  double part_B_prior_probability = 0.3;
  Eigen::Vector2d part_one_mean_one;
    part_one_mean_one << 1.502, 1.484;
  Eigen::Vector2d part_one_mean_two;
    part_one_mean_two << 2.499, 2.497;
  Eigen::Vector2d part_two_mean_one;
    part_two_mean_one << 1, 2;
  Eigen::Vector2d part_two_mean_two;
    part_two_mean_two << 1, 4;
  Eigen::Matrix2d part_one_covariance_one;
    part_one_covariance_one << 1.099, 0,
                               0, 1.099;
  Eigen::Matrix2d part_one_covariance_two;
    part_one_covariance_two << 1.813, 0,
                               0, 1.813;
  Eigen::Matrix2d part_two_covariance_one;
    part_two_covariance_one << 1, 0,
                               0, 1;
  Eigen::Matrix2d part_two_covariance_two;
    part_two_covariance_two << 3, 0,
                               0, 2;

  // setup for problem 1
  problem_solver.setMean( part_one_mean_one, CLASS_ONE );
  problem_solver.setMean( part_one_mean_two, CLASS_TWO );
  problem_solver.setCovariance( part_one_covariance_one, CLASS_ONE );
  problem_solver.setCovariance( part_one_covariance_two, CLASS_TWO );
  problem_solver.setPriorProbabilities( part_A_prior_probability );
  problem_solver.setAssumptionCase( CASE_ONE );

  // solve 1.a
//  problem_solver.performAnalysis( "P1_data.txt", "test_1A_output.txt" );
problem_solver.performAnalysis( "Bebis11.txt", "Bebis1A.txt" );

  // solve 1.b
  problem_solver.setPriorProbabilities( part_B_prior_probability );
//  problem_solver.performAnalysis( "P1_data.txt", "test_1B_output.txt" );
problem_solver.performAnalysis( "Bebis11.txt", "Bebis1B.txt" );

  // setup for problem 2
  problem_solver.setMean( part_two_mean_one, CLASS_ONE );
  problem_solver.setMean( part_two_mean_two, CLASS_TWO );
  problem_solver.setCovariance( part_two_covariance_one, CLASS_ONE );
  problem_solver.setCovariance( part_two_covariance_two, CLASS_TWO );
  problem_solver.setPriorProbabilities( part_A_prior_probability );
  problem_solver.setAssumptionCase( CASE_THREE );

  // solve 2.a
//  problem_solver.performAnalysis( "P2_data.txt", "test_2A_output.txt" );
problem_solver.performAnalysis( "Bebis21.txt", "Bebis2A.txt" );

  // solve 2.b
  problem_solver.setPriorProbabilities( part_B_prior_probability );
//  problem_solver.performAnalysis( "P2_data.txt", "test_2B_output.txt" );
problem_solver.performAnalysis( "Bebis21.txt", "Bebis2B.txt" );

  // end program
  return 0;
}

/*==============================================================================
=======     FUNCTION IMPLEMENTATIONS     =======================================
==============================================================================*/


#define BOOST_TEST_MODULE test_opencellcad

#include <boost/test/unit_test.hpp>

#include <carl/core/MultivariatePolynomial.h>
#include <carl/core/Variable.h>
#include <carl/formula/model/ran/RealAlgebraicPoint.h>

#include <smtrat-mcsat/explanations/onecellcad/OpenCAD.h>

/**
  * References:
  * [1] Christopher W. Brown. 2013. Constructing a single open cell in a
  * cylindrical algebraic decomposition. In Proceedings of the 38th
  * International Symposium on Symbolic and Algebraic Computation (ISSAC '13).
  * ACM
  */

namespace {
  using std::cout;
  using std::endl;
  using std::optional;

  using smtrat::Rational;
  using namespace smtrat::onecellcad;
  using carl::Variable;
  using MultiPoly = carl::MultivariatePolynomial<Rational>;
	using RAN = carl::RealAlgebraicNumber<Rational>;
	using RANPoint = carl::RealAlgebraicPoint<Rational>;

struct VariableFixture {
  Variable x = carl::freshRealVariable("x");
  Variable y = carl::freshRealVariable("y");
  Variable z = carl::freshRealVariable("z");
};

BOOST_FIXTURE_TEST_CASE(polylevel, VariableFixture) {
  BOOST_TEST_MESSAGE("Test polyLevel");

  std::vector<Variable> variableOrder {x,y,z};
  BOOST_CHECK(levelOf(MultiPoly(1),variableOrder) == 0);
  BOOST_CHECK(levelOf(MultiPoly(x)*Rational(0),variableOrder) == 0);
  BOOST_CHECK(levelOf(MultiPoly(x*y)*Rational(0),variableOrder) == 0);
  BOOST_CHECK(levelOf(MultiPoly(x),variableOrder) == 1);
  BOOST_CHECK(levelOf(MultiPoly(y),variableOrder) == 2);
  BOOST_CHECK(levelOf(MultiPoly(x*y),variableOrder) == 2);
  BOOST_CHECK(levelOf(MultiPoly(z),variableOrder) == 3);
  BOOST_CHECK(levelOf(MultiPoly(x*z),variableOrder) == 3);
  BOOST_CHECK(levelOf(MultiPoly(x*y*z),variableOrder) == 3);
}

BOOST_FIXTURE_TEST_CASE(cell2d, VariableFixture) {
  BOOST_TEST_MESSAGE("Test 2D example from [1]");
  MultiPoly p = MultiPoly(x*x) + MultiPoly(y*y) - Rational(1) ;
  MultiPoly q = MultiPoly(y*y)*Rational(2) - MultiPoly(x*x) * (MultiPoly(x)*Rational(2) + Rational(3)) ;
  MultiPoly r = MultiPoly(y) + MultiPoly(x)*Rational(0.5) - Rational(0.5) ;

  std::vector<MultiPoly> polys {p,q,r};
  RANPoint alpha { RAN(Rational(-1)/3), RAN(Rational(1)/3) };
  std::vector<Variable> variableOrder {x,y};

  optional<OpenCADCell> c = createOpenCADCell(polys, alpha, variableOrder);

  BOOST_CHECK(c);
}

} // namespace


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE getGravitationalAttraction

#include <boost/test/unit_test.hpp>
#include "math/constant.hpp"
#include "math/getGravitationalAttraction.hpp"

BOOST_AUTO_TEST_CASE(test_getGravitationalAttraction_1)
{
    BOOST_CHECK_MESSAGE(static_cast<long int>(my::math::ga::getGravitationalAttraction<long double>(10000, 20000, 30000)) == static_cast<long int>(0),
        static_cast<long int>(my::math::ga::getGravitationalAttraction<long double>(10000, 20000, 30000)) << " instead: " << static_cast<long int>(0));
}
BOOST_AUTO_TEST_CASE(test_getGravitationalAttraction_2)
{
    BOOST_CHECK_MESSAGE(static_cast<long int>(my::math::ga::getGravitationalAttraction<long double>(0, 0, 1)) == static_cast<long int>(0),
        static_cast<long int>(my::math::ga::getGravitationalAttraction<long double>(0, 0, 1)) << " instead: " << static_cast<long int>(0));
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/lapack.h>
#include <boost/lexical_cast.hpp>

typedef rokko::localized_vector<double> vector_t;
typedef rokko::localized_matrix<std::complex<double>, rokko::matrix_col_major> matrix_t;

int main(int argc, char *argv[]) {
  int info;
  int m = 6;
  int n = 4;
  if (argc > 2) {
    m = boost::lexical_cast<int>(argv[1]);
    n = boost::lexical_cast<int>(argv[2]);
  }
  std::cout << "m = " << m << "\nn = " << n << std::endl;
  int k = std::min(m, n);

  // generate random martix
  matrix_t mat = matrix_t::Random(m, n);
  std::cout << "Input random matrix A:\n" << mat << std::endl;

  // singular value decomposition
  matrix_t a = mat; // 'a' will be destroyed by dgesvd
  vector_t s(k), superb(k);
  matrix_t u(m, k), vt(k, n);
  info = LAPACKE_zgesvd(LAPACK_COL_MAJOR, 'S', 'S', m, n, &a(0, 0), m, &s(0),
                         &u(0, 0), m, &vt(0, 0), k, &superb(0));
  std::cout << "U:\n" << u << std::endl;
  std::cout << "S:\n" << s << std::endl;
  std::cout << "Vt:\n" << vt << std::endl;

  // check correctness of SVD
  matrix_t smat = matrix_t::Zero(k, k);
  for (int i = 0; i < k; ++i) smat(i, i) = s(i);
  matrix_t check = u * smat * vt;
  std::cout << "U * S * Vt:\n" << check << std::endl;
  std::cout << "| A - U * S * Vt | = " << (mat - check).norm() << std::endl;
}