#include <iostream>
#include <iomanip>
#include <chrono>
#include <limits>

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

#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

#include "poseocv.h"
#include "math.hh"
#include "display.h"

namespace poseocv
{
   void homography_pose(const cv::Mat& intrinsics, const std::vector<cv::Point3d>& train_img_pts,
                        const std::vector<cv::Point3d>& query_img_pts, std::vector<cv::Mat>& rotations,
                        std::vector<cv::Mat>& translations, std::vector<cv::Mat>& normals, bool isRANSAC)
//---------------------------------------------------------------------------------------------
   {
      cv::Mat H;
      rotations.clear(); translations.clear();
      std::vector<cv::Point2f> train_pts, query_pts;
      for (size_t i = 0; i < std::min(train_img_pts.size(), query_img_pts.size()); i++)
      {
         const cv::Point3d tpt = train_img_pts[i];
         train_pts.emplace_back(tpt.x, tpt.y);
         const cv::Point3d qpt = query_img_pts[i];
         query_pts.emplace_back(qpt.x, qpt.y);
      }
      if ((isRANSAC) && (train_img_pts.size() > 3))
         H = cv::findHomography(train_pts, query_pts, cv::RANSAC);
      else
         H = cv::findHomography(train_pts, query_pts);
      if (H.empty()) return;
      cv::decomposeHomographyMat(H, intrinsics, rotations, translations, normals);
   }

   bool pose_PnP(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point3d>& image_pts,
                 const cv::Mat& intrinsics, cv::Mat& rotation_vec, cv::Mat& translations3x1, cv::Mat* R,
                 long& total_time, bool is_refine, int flags, PnPRANSACParameters* RANSACParams)
//--------------------------------------------------------------------------------------------------
   {
      std::vector<cv::Point2f> points2d;
      std::vector<cv::Point3f> points3d;
      std::size_t n = std::min(world_pts.size(), image_pts.size());
      for (size_t i = 0; i < n; i++)
      {
         if ((i >= 4) &&
             ((flags == cv::SOLVEPNP_P3P) || (flags == cv::SOLVEPNP_AP3P)))
         {
//         std::cout << "poseocv::pose_PnP WARNING: Abbreviated input point count to " << points2d.size() << std::endl;
            break;
         }
         cv::Point3d pt = image_pts[i];
         points2d.emplace_back(pt.x, pt.y);
         pt = world_pts[i];
         points3d.emplace_back(pt.x, pt.y, pt.z);
      }
      if ( (RANSACParams != nullptr) && (n > 4) )
      {
         std::vector<unsigned char> inliers(points3d.size());
         auto start = std::chrono::high_resolution_clock::now();
         if (cv::solvePnPRansac(points3d, points2d, intrinsics, cv::noArray(), rotation_vec, translations3x1,
                                is_refine, RANSACParams->iterationsCount, RANSACParams->reprojectionError,
                                RANSACParams->confidence, inliers, flags))
         {
            auto diff = std::chrono::high_resolution_clock::now() - start;
            total_time = std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count();
//         for (size_t j = 0; j < points3d.size(); j++)
//         {
//            if (!inliers[j])
//               fprintf(stdout, "outlier (%.5f, %.5f) (%.5f, %.5f, %.5f)\n", points2d[j].x, points2d[j].y, points3d[j].x,
//                       points3d[j].y, points3d[j].z);
//         }
            if (R != nullptr)
               cv::Rodrigues(rotation_vec, *R);
            return true;
         }
         else
            total_time = -1;
      }
      else
      {
         auto start = std::chrono::high_resolution_clock::now();
         try
         {
            if (cv::solvePnP(points3d, points2d, intrinsics, cv::noArray(), rotation_vec, translations3x1, is_refine,
                             flags))
            {
               auto diff = std::chrono::high_resolution_clock::now() - start;
               total_time = std::chrono::duration_cast<std::chrono::nanoseconds>(diff).count();
               if (R != nullptr)
                  cv::Rodrigues(rotation_vec, *R);
               return true;
            }
            else
               total_time = -1;
         }
         catch (...)
         {
            return false;
         }
      }
      return false;
   }

   void display_PnP(const std::vector<cv::Point3d>& world_points,
                    const std::vector<cv::Point3d>& image_points,
                    const cv::Mat& intrinsics, bool isMinimal, const cv::Mat* query_img,
                    bool show_reprojection, bool save_reprojection, bool is_time)
//----------------------------------------------------------------------------------------------------
   {
      cv::Mat rotation_vec, translations3x1, R;
      std::array<std::pair<std::string, int>, 5> methods {std::make_pair("Iterative", cv::SOLVEPNP_ITERATIVE),
                                                          std::make_pair("P3P", cv::SOLVEPNP_P3P),
                                                          std::make_pair("EPnP", cv::SOLVEPNP_EPNP),
                                                          std::make_pair("DLS", cv::SOLVEPNP_DLS),
                                                          std::make_pair("AP3P", cv::SOLVEPNP_AP3P)
      };
      for (auto pp : methods)
      {
         bool is_pose;
         std::vector<cv::Point3d> world_pts, image_pts, new_world_pts, new_image_pts;
         if (world_points.size() != image_points.size())
         {
            size_t n = std::min(world_points.size(), image_points.size());
            std::copy(world_points.begin(), world_points.begin() + n, std::back_inserter(world_pts));
            std::copy(image_points.begin(), image_points.begin() + n, std::back_inserter(image_pts));
         }
         else
         {
            world_pts = world_points;
            image_pts = image_points;
         }
         switch (pp.second)
         {
            case cv::SOLVEPNP_AP3P:
            case cv::SOLVEPNP_P3P:
               if (world_pts.size() > 4)
               {
                  std::copy(world_pts.begin(), world_pts.begin() + 4, std::back_inserter(new_world_pts));
                  std::copy(image_pts.begin(), image_pts.begin() + 4, std::back_inserter(new_image_pts));
               }
         }
         long time_ns;
         for (int i = 0; i < 1; i++)
         {
            bool is_ransac = (i > 0);
            PnPRANSACParameters RANSAC_params, *pRANSAC_params = nullptr;
            if (is_ransac)
            {
               std::cout << "RANSAC:";
               RANSAC_params.iterationsCount = 1000;
               pRANSAC_params = &RANSAC_params;
            }
            try
            {
               if (new_world_pts.size() > 0)
                  is_pose = pose_PnP(new_world_pts, new_image_pts, intrinsics, rotation_vec, translations3x1, &R,
                                     time_ns, false, pp.second, pRANSAC_params);
               else
                  is_pose = pose_PnP(world_pts, image_pts, intrinsics, rotation_vec, translations3x1, &R,
                                     time_ns, false, pp.second, pRANSAC_params);
            }
            catch (std::exception& e)
            {
//            std::cerr << "   display_PnP " << pp.first << " Exception: " << e.what() << std::endl;
               std::cout << "    No PnP solution (exception)" << std::endl;
               continue;
            }
            if (is_pose)
            {
               std::cout << "    ***** " << pp.first << " (" << time_ns << "ns )" << std::endl;
               cv::Mat TT = translations3x1.t();
               Eigen::Matrix3d ER;
               cv::cv2eigen(R, ER);
               Eigen::AngleAxisd aa(ER);
               Eigen::Quaterniond QQ(ER);
               if (!isMinimal)
               {
                  std::cout << "    Axis: " << aa.axis().transpose() << " Angle: " << mut::radiansToDegrees(aa.angle())
                            << std::endl;
                  std::cout << "    Quaternion: [" << QQ.w() << ", (" << QQ.vec().transpose() << ")" << std::endl;
               }

               const cv::Vec3f& euler = mut::rotation2Euler(R);
               std::cout << std::fixed << std::setprecision(4) << "Rotation Roll,Pitch,Yaw: ["
                         << mut::radiansToDegrees(euler[0]) << ","
                         << mut::radiansToDegrees(euler[1]) << "," << mut::radiansToDegrees(euler[2]) << " ]\u00B0 ("
                         << euler[0] << "," << euler[1] << "," << euler[2] << ") radians ("
                         << mut::radiansToDegrees(euler[0]) << "\\textdegree,"
                         << mut::radiansToDegrees(euler[1]) << "\\textdegree," << mut::radiansToDegrees(euler[2])
                         << "\\textdegree)" << std::endl;
               std::cout << std::fixed << std::setprecision(4) << "    Translation: " << TT.at<double>(0, 0) << ","
                         << TT.at<double>(0, 1) << "," << TT.at<double>(0, 2) << std::endl;
               cv::Mat empty_img;
               if (query_img == nullptr)
               {
                  query_img = &empty_img;
                  show_reprojection = false;
               }
               Eigen::Quaterniond Q(aa);
               Eigen::Vector3d t;
               cv::cv2eigen(translations3x1, t);
               std::string name;
               char* pname = nullptr;
               if ((save_reprojection) && (query_img != nullptr))
               {
                  name = "reproject-" + pp.first + ".jpg";
                  pname = const_cast<char*>(name.c_str());
               }
               double max_error, mean_error, stddev_error;
               show_projection(*query_img, world_pts, image_pts, intrinsics, Q, t[0], t[1], t[2],
                               max_error, mean_error, stddev_error, show_reprojection, pname);
               std::cout << std::setprecision(4) << "    Max Error " << max_error << ", mean error " << mean_error
                         << " Std Deviation " << stddev_error << std::endl;
            }
            else
               std::cout << "    No PnP solution" << std::endl;
            std::cout << "---------------------------------------------" << std::endl;
         }
      }
   }
}


/***************************************************************************
 *   Copyright (C) 2006 by Nicola Bellotto                                 *
 *   nbellotto@lincoln.ac.uk                                                    *
 *                                                                         *
 *   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 2 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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "models.h"
#include <bayes_tracking/BayesFilter/matSup.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <float.h>

using namespace Bayesian_filter;
using namespace Models;

//*************************************************************************
//                      CV PREDICTION MODEL
//*************************************************************************

CVModel::CVModel(Float wxSD, Float wySD) :
        Linrz_predict_model(x_size, q_size),
        Sampled_predict_model(),
        fx(x_size),
        genn(rnd),
        xp(x_size),
        n(q_size), rootq(q_size),
        m_wxSD(wxSD), m_wySD(wySD)
{
    first_init = true;
    init();
}

void CVModel::init()
{
    Fx.clear();
    // x
    Fx(0,0) = 1.;
    Fx(0,1) = dt;
    Fx(0,2) = 0.;
    Fx(0,3) = 0.;
    // dx
    Fx(1,0) = 0.;
    Fx(1,1) = 1.;
    Fx(1,2) = 0.;
    Fx(1,3) = 0.;
    // y
    Fx(2,0) = 0.;
    Fx(2,1) = 0.;
    Fx(2,2) = 1.;
    Fx(2,3) = dt;
    // dy
    Fx(3,0) = 0.;
    Fx(3,1) = 0.;
    Fx(3,2) = 0.;
    Fx(3,3) = 1.;
    // noise
    q[0] = sqr(m_wxSD); // cov(w_x)
    q[1] = sqr(m_wySD); // cov(w_y)
    G.clear();
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
}

const FM::Vec& CVModel::f(const FM::Vec& x) const
{  // human model
    fx[0] = x[0] + x[1] * dt;  // x
    fx[1] = x[1];              // dx
    fx[2] = x[2] + x[3] * dt;  // y
    fx[3] = x[3];              // dy
    return fx;
}

void CVModel::update(double dt)
{
    // time interval
    this->dt = dt;

    //     | 0.5dt^2     0    |
    // G = |    dt       0    |
    //     |    0     0.5dt^2 |
    //     |    0        dt   |
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
}

void CVModel::updateJacobian(const FM::Vec& x) {
    //      | 1  dt 0  0 |
    // Fx = | 0  1  0  0 |
    //      | 0  0  1  dt|
    //      | 0  0  0  1 |
    Fx(0,1) = dt;
    Fx(2,3) = dt;
}

const Vec& CVModel::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   *  Generate Gaussian correlated samples
   * Precond: init_GqG, automatic on first use
   */
{
    if (first_init)
        init_GqG();
    // Predict state using supplied functional predict model
    xp = f(x);
    // Additive random noise
    CVModel::genn.normal(n);            // independant zero mean normal
    // multiply elements by std dev
    for (FM::DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
        *ni *= rootq[ni.index()];
    }
    FM::noalias(xp) += FM::prod(this->G,n);         // add correlated noise
    return xp;
}

void CVModel::init_GqG() const
/* initialise predict given a change to q,G
   *  Implementation: Update rootq
   */
{
    first_init = false;
    for (FM::Vec::const_iterator qi = this->q.begin(); qi != this->q.end(); ++qi) {
        if (*qi < 0)
            error (Numeric_exception("Negative q in init_GqG"));
        rootq[qi.index()] = std::sqrt(*qi);
    }
}

//*************************************************************************
//                  2D CARTESIAN SUBTRACTION OBSERVATION MODEL
//*************************************************************************

CartesianModel::CartesianModel(Float xSD, Float ySD) :
        Linrz_correlated_observe_model(x_size, z_size),
        Likelihood_observe_model(z_size),
        z_pred(z_size),
        li(z_size)
{
    // Hx = | 1  0  0  0 |
    //      | 0  0  1  0 |
    Hx.clear();
    Hx(0,0) = 1.;
    Hx(1,2) = 1.;
    // noise
    Z.clear();
    Z(0,0) = sqr(xSD);
    Z(1,1) = sqr(ySD);
}

Bayes_base::Float
CartesianModel::Likelihood_correlated::L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const
/*
 * Definition of likelihood given an additive Gaussian observation model:
 *  p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z));
 *  L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size
 * Precond: Observation Information: z,Z_inv,detZterm
 */
{
    if (!zset)
        Bayes_base::error (Logic_exception ("BGSubModel used without Lz set"));
    // Normalised innovation
    zInnov = z;
    model.normalise (zInnov, zp);
    FM::noalias(zInnov) -= zp;

    Float logL = scaled_vector_square(zInnov, Z_inv);
    using namespace std;
    return exp(Float(-0.5)*(logL + z.size()*log(2*M_PI) + logdetZ));   // normalized likelihood
}

void CartesianModel::Likelihood_correlated::Lz (const Correlated_additive_observe_model& model)
/* Set the observation zz and Z about which to evaluate the Likelihood function
 * Postcond: Observation Information: z,Z_inv,detZterm
 */
{
    zset = true;
    // Compute inverse of Z and its reciprocal condition number
    Float detZ;
    Float rcond = FM::UdUinversePD (Z_inv, detZ, model.Z);
    model.rclimit.check_PD(rcond, "Z not PD in observe");
    // detZ > 0 as Z PD
    using namespace std;
    logdetZ = log(detZ);
}

Bayes_base::Float
CartesianModel::Likelihood_correlated::scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V)
/*
 * Compute covariance scaled square inner product of a Vector: v'*V*v
 */
{
    return FM::inner_prod(v, FM::prod(V,v));
}

const FM::Vec& CartesianModel::h(const FM::Vec& x) const
{
    z_pred[0] = x[0];
    z_pred[1] = x[2];
    return z_pred;
};

void CartesianModel::updateJacobian(const FM::Vec& x) {
    // nothing to do
}

void CartesianModel::normalise(FM::Vec& z_denorm, const FM::Vec& z_from) const {
}

//*************************************************************************
//                      CV 3D PREDICTION MODEL
//*************************************************************************

CVModel3D::CVModel3D(Float wxSD, Float wySD, Float wzSD) :
        Linrz_predict_model(x_size, q_size),
        Sampled_predict_model(),
        fx(x_size),
        genn(rnd),
        xp(x_size),
        n(q_size), rootq(q_size),
        m_wxSD(wxSD), m_wySD(wySD),m_wzSD(wzSD)
{
    first_init = true;
    init();
}

void CVModel3D::init()
{
    Fx.clear();
    // x
    Fx(0,0) = 1.;
    Fx(0,1) = dt;
    Fx(0,2) = 0.;
    Fx(0,3) = 0.;
    Fx(0,4) = 0.;
    Fx(0,5) = 0.;
    // dx
    Fx(1,0) = 0.;
    Fx(1,1) = 1.;
    Fx(1,2) = 0.;
    Fx(1,3) = 0.;
    Fx(1,4) = 0.;
    Fx(1,5) = 0.;
    // y
    Fx(2,0) = 0.;
    Fx(2,1) = 0.;
    Fx(2,2) = 1.;
    Fx(2,3) = dt;
    Fx(2,4) = 0.;
    Fx(2,5) = 0.;
    // dy
    Fx(3,0) = 0.;
    Fx(3,1) = 0.;
    Fx(3,2) = 0.;
    Fx(3,3) = 1.;
    Fx(3,4) = 0.;
    Fx(3,5) = 0.;
    // z
    Fx(4,0) = 0.;
    Fx(4,1) = 0.;
    Fx(4,2) = 0.;
    Fx(4,3) = 0.;
    Fx(4,4) = 1.;
    Fx(4,5) = dt;
    // dz
    Fx(5,0) = 0.;
    Fx(5,1) = 0.;
    Fx(5,2) = 0.;
    Fx(5,3) = 0.;
    Fx(5,4) = 0.;
    Fx(5,5) = 1.;

    // noise
    q[0] = sqr(m_wxSD); // cov(w_x)
    q[1] = sqr(m_wySD); // cov(w_y)
    q[2] = sqr(m_wzSD); // cov(w_z)
    G.clear();
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
    G(4,2) = 0.5*sqr(dt);
    G(5,2) = dt;
}

const FM::Vec& CVModel3D::f(const FM::Vec& x) const
{  // human model
    fx[0] = x[0] + x[1] * dt;  // x
    fx[1] = x[1];              // dx
    fx[2] = x[2] + x[3] * dt;  // y
    fx[3] = x[3];              // dy
    fx[4] = x[4] + x[5] * dt;  // z
    fx[5] = x[5];              // dz
    return fx;
}

void CVModel3D::update(double dt)
{
    // time interval
    this->dt = dt;

    //     | 0.5dt^2     0        0    |
    //     |    dt       0        0    |
    //     |    0     0.5dt^2     0    |
    // G = |    0        dt       0    |
    //     |    0        0     0.5dt^2 |
    //     |    0        0         dt  |
    G(0,0) = 0.5*sqr(dt);
    G(1,0) = dt;
    G(2,1) = 0.5*sqr(dt);
    G(3,1) = dt;
    G(4,2) = 0.5*sqr(dt);
    G(5,2) = dt;
}

void CVModel3D::updateJacobian(const FM::Vec& x) {
    //      | 1  dt 0  0  0  0 |
    // Fx = | 0  1  0  0  0  0 |
    //      | 0  0  1  dt 0  0 |
    //      | 0  0  0  1  0  0 |
    //      | 0  0  0  0  1  dt|
    //      | 0  0  0  0  0  1 |
    Fx(0,1) = dt;
    Fx(2,3) = dt;
    Fx(4,5) = dt;
}

const Vec& CVModel3D::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   *  Generate Gaussian correlated samples
   * Precond: init_GqG, automatic on first use
   */
{
    if (first_init)
        init_GqG();
    // Predict state using supplied functional predict model
    xp = f(x);
    // Additive random noise
    CVModel3D::genn.normal(n);            // independent zero mean normal
    // multiply elements by std dev
    for (FM::DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
        *ni *= rootq[ni.index()];
    }
    FM::noalias(xp) += FM::prod(this->G,n);         // add correlated noise
    return xp;
}

void CVModel3D::init_GqG() const
/* initialise predict given a change to q,G
   *  Implementation: Update rootq
   */
{
    first_init = false;
    for (FM::Vec::const_iterator qi = this->q.begin(); qi != this->q.end(); ++qi) {
        if (*qi < 0)
            error (Numeric_exception("Negative q in init_GqG"));
        rootq[qi.index()] = std::sqrt(*qi);
    }
}

//*************************************************************************
//                      STATIC 3D PREDICTION MODEL
//*************************************************************************

StaticModel3D::StaticModel3D() :
        CVModel3D(0., 0., 0.),
        fx(x_size)
//        genn(rnd),
//        xp(x_size)
//        n(q_size), rootq(q_size)
//        m_wxSD(wxSD), m_wySD(wySD),m_wzSD(wzSD)
{
//    first_init = true;
    init();
}

void StaticModel3D::init()
{
    //      | 1  0  0  0  0  0 | x
    // Fx = | 0  0  0  0  0  0 | vx
    //      | 0  0  1  0  0  0 | y
    //      | 0  0  0  0  0  0 | vy
    //      | 0  0  0  0  1  0 | z
    //      | 0  0  0  0  0  0 | vz

    Fx.clear();

    Fx(0,0) = 1.;
    Fx(2,2) = 1.;
    Fx(4,4) = 1.;

    q.clear();
//    q[0] = 0.; // cov(w_x)
//    q[1] = 0.; // cov(w_y)
//    q[2] = 0.; // cov(w_z)
    G.clear(); // G = zero matrix

}

const FM::Vec& StaticModel3D::f(const FM::Vec& x) const
{  // static object model
    fx[0] = x[0];  // x
    fx[1] = 0;     // dx
    fx[2] = x[2];  // y
    fx[3] = 0;     // dy
    fx[4] = x[4];  // z
    fx[5] = 0;     // dz
    return fx;
}

void StaticModel3D::update(double dt)
{
    // time interval
    this->dt = dt;
//  G = zero matrix (no noise in static state)
}

void StaticModel3D::updateJacobian(const FM::Vec& x) {
    //      | 1  0  0  0  0  0 | x
    // Fx = | 0  0  0  0  0  0 | vx
    //      | 0  0  1  0  0  0 | y
    //      | 0  0  0  0  0  0 | vy
    //      | 0  0  0  0  1  0 | z
    //      | 0  0  0  0  0  0 | vz
    // No need to update the Jacobian since the function is linear and independent from state
}

const Vec& StaticModel3D::fw(const FM::Vec& x) const
/*
   * Definition of sampler for additive noise model given state x
   */
{
    return f(x);
}

//*************************************************************************
//                 3D  CARTESIAN SUBTRACTION OBSERVATION MODEL
//*************************************************************************

CartesianModel3D::CartesianModel3D(Float xSD, Float ySD, Float zSD) :
        Linrz_correlated_observe_model(x_size, z_size),
        Likelihood_observe_model(z_size),
        z_pred(z_size),
        li(z_size)
{
    // Hx = | 1  0  0  0  0  0 |
    //      | 0  0  1  0  0  0 |
    //      | 0  0  0  0  1  0 |
    Hx.clear();
    Hx(0,0) = 1.;
    Hx(1,2) = 1.;
    Hx(2,4) = 1.;
    // noise
    Z.clear();
    Z(0,0) = sqr(xSD);
    Z(1,1) = sqr(ySD);
    Z(2,2) = sqr(zSD);
}

Bayes_base::Float
CartesianModel3D::Likelihood_correlated::L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const
/*
 * Definition of likelihood given an additive Gaussian observation model:
 *  p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z));
 *  L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size
 * Precond: Observation Information: z,Z_inv,detZterm
 */
{
    if (!zset)
        Bayes_base::error (Logic_exception ("BGSubModel used without Lz set"));
    // Normalised innovation
    zInnov = z;
    model.normalise (zInnov, zp);
    FM::noalias(zInnov) -= zp;

    Float logL = scaled_vector_square(zInnov, Z_inv);
    using namespace std;
    return exp(Float(-0.5)*(logL + z.size()*log(2*M_PI) + logdetZ));   // normalized likelihood
}

void CartesianModel3D::Likelihood_correlated::Lz (const Correlated_additive_observe_model& model)
/* Set the observation zz and Z about which to evaluate the Likelihood function
 * Postcond: Observation Information: z,Z_inv,detZterm
 */
{
    zset = true;
    // Compute inverse of Z and its reciprocal condition number
    Float detZ;
    Float rcond = FM::UdUinversePD (Z_inv, detZ, model.Z);
    model.rclimit.check_PD(rcond, "Z not PD in observe");
    // detZ > 0 as Z PD
    using namespace std;
    logdetZ = log(detZ);
}

Bayes_base::Float
CartesianModel3D::Likelihood_correlated::scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V)
/*
 * Compute covariance scaled square inner product of a Vector: v'*V*v
 */
{
    return FM::inner_prod(v, FM::prod(V,v));
}

const FM::Vec& CartesianModel3D::h(const FM::Vec& x) const
{
    z_pred[0] = x[0];
    z_pred[1] = x[2];
    z_pred[2] = x[4];
    return z_pred;
};

void CartesianModel3D::updateJacobian(const FM::Vec& x) {
    // nothing to do
}

void CartesianModel3D::normalise(FM::Vec& z_denorm, const FM::Vec& z_from) const {
}


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "GaussSeq.h"
#include "ARSeq.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;

    // set precision of output
//    std::streamsize precision = std::numeric_limits<cpp_bin_float_50>::digits10;
//    std::cout << std::setprecision(10);

    // testing
    std::map<int, my_float> coeff {
        {3, 0.5},
        {20, 20},
    };
    ARSeq seq(coeff, 1);
    seq.print_lags();
    std::vector<my_float> v;
    for (auto i = 0; i < 20; i++) {
        v.push_back(0);
    }
    v[2] = 2;
    v[19] = 3;

    seq.seed_prev_vals(v);

    seq.print_past_vals();
    for (auto i = 0; i < 100; i++) {
        seq.next();
    }
    seq.print_past_vals();

    return 0;
}


#pragma once

// C++ standard library
#include <utility>

// Armadillo
#include <armadillo>

namespace mant {
  namespace itd {
    const double heliocentricGravitationalConstant = 1.32712440018e20;
    const double solarMass = 1.98892e30;

    double stumpffFunction(
        const double parameter,
        const arma::uword type);

    double timeOfFlight(
        const double universalVariable,
        const arma::vec::fixed<3>& departurePosition,
        const arma::vec::fixed<3>& arrivalPosition,
        const bool useProgradeTrajectory);

    double sphereOfInfluenceRadius(
        const double semimajorAxis,
        const double mass);

    std::pair<arma::vec::fixed<3>, arma::vec::fixed<3>> positionAndVelocityOnOrbit(
        const double modifiedJulianDate,
        const arma::vec::fixed<7>& keplerianElements);

    arma::vec::fixed<3> gravityAssist(
        const arma::vec::fixed<3>& satelliteVelocity,
        const arma::vec::fixed<3>& planetPosition,
        const arma::vec::fixed<3>& planetVelocity,
        const double standardGravitationalParameter,
        const double periapsis);

    std::pair<arma::vec::fixed<3>, arma::vec::fixed<3>> lambert(
        const arma::vec::fixed<3>& departurePosition,
        const arma::vec::fixed<3>& arrivalPosition,
        const bool useProgradeTrajectory,
        const arma::uword numberOfRevolutions,
        const double transferTime);
  }
}


#include <iostream>
#include <fstream>
#include <vector>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/iterator.h>
#include <CGAL/Timer.h>

#include <boost/version.hpp>
#if BOOST_VERSION < 107200
#include <boost/progress.hpp>
using boost::progress_display;
#else
#include <boost/timer/progress_display.hpp>
using boost::timer::progress_display;
#endif

#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/K_neighbor_search.h>
#include <CGAL/Search_traits_3.h>

#include <ANN/ANN.h>
#include <ANN/ANNperf.h>

#include<sfcnn.hpp>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3 Point_3;
typedef CGAL::Random_points_in_cube_3<Point_3> Random_points_iterator;
typedef CGAL::Counting_iterator<Random_points_iterator> N_Random_points_iterator;

typedef CGAL::Search_traits_3<Kernel> TreeTraits;
typedef CGAL::Sliding_midpoint<TreeTraits> Splitter;
typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> OK_search;

template <class Output_iterator>
int read_points(std::string filename,Output_iterator out)
{
  std::ifstream file(filename.c_str());
  int nb_pts;
  Point_3 p;
  file >> nb_pts;
  for (int i=0;i<nb_pts;++i){
    file >> p;
    *out++=p;
  }
  return nb_pts;
}

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

  if (argc!=3 && argc!=4){
    std::cerr << "Usage: main nb_input_point nb_neighbors_needed\n";
    std::cerr << "or     main Input.xyz Queries.xyz nb_neighbors_needed\n";
    exit(EXIT_FAILURE);
  }

  int nb_input_point,k,nb_queries;
  std::vector<Point_3> points;
  std::vector<Point_3> queries;

  if (argc==3){
    nb_input_point=atoi(argv[1]);
    k=atoi(argv[2]);
    nb_queries=1;
    points.reserve(nb_input_point);
    Random_points_iterator rpit(1.0);
    points.insert(points.begin(),N_Random_points_iterator(rpit,0), N_Random_points_iterator(nb_input_point));
    queries.push_back(Point_3(0,0,0));
  }
  else{
    k=atoi(argv[3]);
    nb_input_point=read_points(argv[1],std::back_inserter(points));
    nb_queries=read_points(argv[2],std::back_inserter(queries));
  }

  std::cout << "Looking for " << k << " nearest neighbors amongst " << nb_input_point << " points, with "<< nb_queries <<" query points.\n";

  std::random_shuffle(points.begin(),points.end());

  double STANN_time=0,ANN_time=0,OK_time=0;

  CGAL::Timer time;
//Building trees
//--STANN
  time.start();
  sfcnn<Point_3, 3, double> NN(&points[0], nb_input_point);
  time.stop();
  double STANN_tree=time.time();
  time.reset();
//--ANN
  ANNpointArray    dataPts;     // data points
  ANNpoint         queryPt;     // query point
  ANNidxArray      nnIdx;       // near neighbor indices
  ANNdistArray     dists;       // near neighbor distances
  ANNkd_tree*      kdTree;      // search structure
  dataPts = annAllocPts(nb_input_point, 3);      // allocate data points

  queryPt = annAllocPt(3);          // allocate query point
  nnIdx = new ANNidx[k];            // allocate near neigh indices
  dists = new ANNdist[k];            // allocate near neighbor dists
  //set points
  for (int i=0;i<nb_input_point;++i){
     dataPts[i][0]=points[i][0];
     dataPts[i][1]=points[i][1];
     dataPts[i][2]=points[i][2];
  }
  time.start();
  kdTree = new ANNkd_tree(dataPts,nb_input_point,3);
  time.stop();
  double ANN_tree=time.time();
  time.reset();
//--OK_search
  time.start();
  Splitter splitter(10); //bucket size can be changed here
  OK_search::Tree ok_tree(points.begin(),points.end(),splitter);
  ok_tree.build();
  time.stop();
  double OK_tree=time.time();
  time.reset();

  progress_display show_progress( nb_queries );

//running NN algorithms
  for (std::vector<Point_3>::const_iterator it=queries.begin();it!=queries.end();++it)
  {
    const Point_3& query=*it;

  //STANN
    CGAL::Timer time;
    std::vector<long unsigned int> answer;
    time.start();
    NN.ksearch(query, k, answer);
    time.stop();
    STANN_time+=time.time();
    time.reset();
  //ANN
    queryPt[0]=query[0];
    queryPt[1]=query[1];
    queryPt[2]=query[2];

    time.start();
    kdTree->annkSearch(            // search
                    queryPt,            // query point
                    k,                // number of near neighbors
                    nnIdx,              // nearest neighbors (returned)
                    dists,              // distance (returned)
                    0);              // error bound

    time.stop();
    ANN_time+=time.time();
    time.reset();

    //~ ANNkdStats stats;
    //~ kdTree->getStats(stats);
    //~ std::cout << "====ANN stats ====\n";
    //~ std::cout << "dimension of space " << stats.dim << "\n";
    //~ std::cout << "no. of points " << stats.n_pts << "\n";
    //~ std::cout << "bucket size " << stats.bkt_size << "\n";
    //~ std::cout << "no. of leaves (including trivial) " << stats.n_lf << "\n";
    //~ std::cout << "no. of trivial leaves (no points) " << stats.n_tl << "\n";
    //~ std::cout << "no. of splitting nodes " << stats.n_spl << "\n";
    //~ std::cout << "no. of shrinking nodes (for bd-trees) " << stats.n_shr << "\n";
    //~ std::cout << "depth of tree " << stats.depth << "\n";
    //~ std::cout << "sum of leaf aspect ratios " << stats.sum_ar << "\n";
    //~ std::cout << "average leaf aspect ratio " << stats.avg_ar << "\n";
    //~ std::cout << "==================\n";

    //~ std::cout << "====ANN tree  ====\n";
    //~ kdTree->Print(ANNtrue,std::cout);
    //~ std::cout << "==================\n";

  //Ortho-k-NN
    std::vector<std::pair<Point_3,double> > ok_result;
    ok_result.reserve(k);
    time.start();
    OK_search ok_search(ok_tree, query,k);
    time.stop();
    std::copy(ok_search.begin(),ok_search.end(),std::back_inserter(ok_result));
    OK_time+=time.time();

    //~ std::cout << std::endl; ok_tree.statistics(std::cout); std::cout << std::endl;

    //~ std::cout << "====CGAL tree ====\n";
    //~ ok_tree.print();
    //~ std::cout << "==================\n";

    for (int i = 0; i < k; i++) {      //check results are the same
      if ( nnIdx[i]!=answer[i] ){
        std::cerr << "STANN and ANN produced different results\n";
        exit(EXIT_FAILURE);
      }
      if ( ok_result[i].first!=points[answer[i]] ){
        std::cerr << "CGAL::OK_search and STANN (ANN) produced different results\n";
        exit(EXIT_FAILURE);
      }
    }
    ++show_progress;
  }

  std::cout << "Time to build trees\n";
  std::cout << "STANN ANN OK_search\n";
  std::cout << STANN_tree << " " << ANN_tree << " " << OK_tree << "\n";
  std::cout << "Time spent for all queries\n";
  std::cout << "STANN ANN OK_search\n";
  std::cout << STANN_time << " " << ANN_time << " " << OK_time << "\n";

  delete [] nnIdx;              // clean things up
  delete [] dists;
  delete kdTree;
  annClose();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_PHI2_HPP
#define BOOST_GEOMETRY_PROJECTIONS_PHI2_HPP

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry { namespace projections {
namespace detail {

template <typename T>
inline T pj_phi2(T const& ts, T const& e)
{
    static const T TOL = 1.0e-10;
    static const int N_ITER = 15;

    T eccnth, Phi, con, dphi;
    int i;

    eccnth = .5 * e;
    Phi = geometry::math::half_pi<T>() - 2. * atan (ts);
    i = N_ITER;
    do {
        con = e * sin (Phi);
        dphi = geometry::math::half_pi<T>() - 2. * atan (ts * pow((1. - con) /
           (1. + con), eccnth)) - Phi;
        Phi += dphi;
    } while ( geometry::math::abs(dphi) > TOL && --i);
    if (i <= 0)
        BOOST_THROW_EXCEPTION( projection_exception(-18) );
    return Phi;
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

#ifndef BOOST_MATH_FFT_GSLBACKEND_HPP
#define BOOST_MATH_FFT_GSLBACKEND_HPP

#include <complex>

#if defined(__GNUC__)
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#endif

#include <boost/math/fft/dft_api.hpp>

namespace boost { namespace math {
namespace fft { namespace detail {

    #if defined(__GNUC__)
    template<class T, class A>
    class gsl_backend;

    template<class T, class A>
    class gsl_rfft_backend;

    template<class Allocator_t>
    class gsl_backend< std::complex<double>, Allocator_t >
    {
    public:
      using value_type     = std::complex<double>;
      using allocator_type = Allocator_t;

    private:
      using real_value_type    = double;
      using complex_value_type = std::complex<real_value_type>;
      enum plan_type { forward_plan, backward_plan };

      std::size_t my_size;
      allocator_type my_alloc;

      // complex fft
      gsl_fft_complex_wavetable *wtable;
      gsl_fft_complex_workspace *wspace;

      void execute(plan_type p, const complex_value_type* in, complex_value_type* out) const
      {
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+size(),out);
        }

        if(p==forward_plan)
        gsl_fft_complex_forward(
          reinterpret_cast<real_value_type*>(std::addressof(*out)),
          1, my_size, wtable, wspace);
        else
        gsl_fft_complex_backward(
          reinterpret_cast<real_value_type*>(std::addressof(*out)),
          1, my_size, wtable, wspace);
      }
      void free()
      {
        gsl_fft_complex_wavetable_free(wtable);
        gsl_fft_complex_workspace_free(wspace);
      }
      void alloc()
      {
        wtable = gsl_fft_complex_wavetable_alloc(size());
        wspace = gsl_fft_complex_workspace_alloc(size());
      }
   public:

      gsl_backend(std::size_t n, const allocator_type& = allocator_type{}):
          my_size{n}
      {
        alloc();
      }

      ~gsl_backend()
      {
        free();
      }
      std::size_t size() const {return my_size;}

      void resize(std::size_t new_size)
      {
        if(size()!=new_size)
        {
          free();
          my_size = new_size;
          alloc();
        }
      }

      void forward(const complex_value_type* in, complex_value_type* out) const
      {
        execute(forward_plan,in,out);
      }
      void backward(const complex_value_type* in, complex_value_type* out) const
      {
        execute(backward_plan,in,out);
      }
    };

    template<class Allocator_t>
    class gsl_rfft_backend< double, Allocator_t >
    {
    public:
      // using value_type     = double;
      using allocator_type = Allocator_t;

    private:
      enum plan_type { forward_plan, backward_plan };
      using real_value_type    = double;

      template<class U>
      using vector_t = std::vector<U, typename std::allocator_traits<allocator_type>::template rebind_alloc<U> >;

      std::size_t my_size;
      allocator_type my_alloc;

      gsl_fft_real_wavetable        *real_wtable;
      gsl_fft_halfcomplex_wavetable *halfcomplex_wtable;
      gsl_fft_real_workspace        *real_wspace;

      void pack_halfcomplex(real_value_type* out) const
      // precondition:
      // -> size(out) >= N
      {
        const std::size_t N = size();
        vector_t<real_value_type> tmp(out,out+N);
        out[0]=tmp[0];
        for(unsigned int i=1,j=1;j<N;++i,j+=2)
        {
          out[j] = tmp[i];
          if(j+1<N)
            out[j+1] = -tmp[N-i];
        }
      }
      void unpack_halfcomplex(real_value_type* out) const
      // precondition:
      // -> size(out) >= N
      {
        const std::size_t N = size();
        vector_t<real_value_type> tmp(out,out+N);
        out[0]=tmp[0];
        for(unsigned int i=1,j=1;j<N;++i,j+=2)
        {
          out[i] = tmp[j];
          if(j+1<N)
            out[N-i] = -tmp[j+1];
        }
      }
      template<plan_type p>
      void execute(const real_value_type* in,
                   real_value_type* out,
                   const typename std::enable_if<p==forward_plan>::type* = nullptr) const
      {
        const std::size_t N = size();
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+N,out);
        }
        gsl_fft_real_transform(
          out,1, N, real_wtable, real_wspace);
        unpack_halfcomplex(out);
      }
      template<plan_type p>
      void execute(const real_value_type* in,
                   real_value_type* out,
                   const typename std::enable_if<p==backward_plan>::type* = nullptr) const
      {
        const std::size_t N = size();
        if(in!=out)
        {
          // we avoid this extra step for in-place transforms
          // notice that if in==out, the following code has
          // undefined-behavior
          std::copy(in,in+N,out);
        }
        pack_halfcomplex(out);
        gsl_fft_halfcomplex_transform(
          out,1, N, halfcomplex_wtable, real_wspace);
      }

      void free()
      {
        gsl_fft_real_wavetable_free(real_wtable);
        gsl_fft_halfcomplex_wavetable_free(halfcomplex_wtable);
        gsl_fft_real_workspace_free(real_wspace);
      }
      void alloc()
      {
        const std::size_t N = size();
        real_wtable        = gsl_fft_real_wavetable_alloc(N);
        halfcomplex_wtable = gsl_fft_halfcomplex_wavetable_alloc(N);
        real_wspace        = gsl_fft_real_workspace_alloc(N);
      }
   public:

      gsl_rfft_backend(std::size_t n, const allocator_type& A= allocator_type{}):
          my_size{n},
          my_alloc{A}
      {
        alloc();
      }

      ~gsl_rfft_backend()
      {
        free();
      }
      constexpr std::size_t size() const {return my_size;}
      constexpr std::size_t unique_complex_size() const {return my_size/2 + 1;}

      void resize(std::size_t new_size)
      {
        if(size()!=new_size)
        {
          free();
          my_size = new_size;
          alloc();
        }
      }

      void real_to_halfcomplex(const real_value_type* in, real_value_type* out) const
      {
        execute<forward_plan>(in,out);
      }
      void halfcomplex_to_real(const real_value_type* in, real_value_type* out) const
      {
        execute<backward_plan>(in,out);
      }
    };
    #endif
  } // namespace detail

  #if defined(__GNUC__)
  template<class RingType = std::complex<double>, class Allocator_t = std::allocator<RingType> >
  using gsl_dft = detail::complex_dft<detail::gsl_backend,RingType,Allocator_t>;

  template<class T = double, class Allocator_t = std::allocator<T> >
  using gsl_rdft = detail::real_dft<detail::gsl_rfft_backend,T,Allocator_t>;

  using gsl_transform = transform< gsl_dft<> >;
  using gsl_real_transform = transform< gsl_rdft<> >;
  #endif

} // namespace fft
} // namespace math
} // namespace boost

#endif // BOOST_MATH_FFT_GSLBACKEND_HPP


#include <iostream>
#include <omp.h>
#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <unsupported/Eigen/SparseExtra> // For reading MatrixMarket files
#include <unsupported/Eigen/IterativeSolvers>

#include <amgcl/adapter/eigen.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/bicgstabl.hpp>
#include <amgcl/solver/idrs.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/ilu0.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/profiler.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/eigen.hpp>
#include <amgcl/backend/eigen.hpp>

#include "../include/config.hpp"

#ifdef USE_CUDA
#include <amgcl/backend/cuda.hpp>
#include <amgcl/relaxation/cusparse_ilu0.hpp>
#endif
#include "cxxopts.hpp"

struct LinearSystem
{
    Eigen::VectorXd b;
    Eigen::VectorXd x0;
    Eigen::SparseMatrix<double, Eigen::RowMajor> A;
};

struct Convoptions
{
    int iterations;
    double tolerance;
};

struct returnvalue
{
    int iterations;
    double error;
    double error_exact;
};

template <class T>
returnvalue RunEigen_Solver(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    prof.tic("setup_" + name);
    T Solver;
    Solver.setTolerance(opt.tolerance);
    Solver.setMaxIterations(opt.iterations);
    Solver.compute(Axb.A);
    prof.toc("setup_" + name);
    prof.tic("solve_" + name);
    Eigen::VectorXd result_eigen = Solver.solveWithGuess(Axb.b, Axb.x0);
    prof.toc("solve_" + name);
    returnvalue result;
    result.iterations = Solver.iterations();
    result.error = Solver.error();
    result.error_exact=(Axb.A*result_eigen-Axb.b).norm()/Axb.b.norm();
    return result;
}

template <class T>
returnvalue RunAMGCLEigen_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    Eigen::VectorXd x = Axb.x0;
    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(Axb.A, prm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(Axb.A, Axb.b, x);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}

template <class T>
returnvalue RunAMGCL_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

    std::vector<double> x0 = std::vector<double>(Axb.x0.data(), Axb.x0.data() + Axb.x0.size());
    std::vector<double> b = std::vector<double>(Axb.b.data(), Axb.b.data() + Axb.b.size());
    size_t n = Axb.A.rows();
    const int *ptr = Axb.A.outerIndexPtr();
    const int *col = Axb.A.innerIndexPtr();
    const double *val = Axb.A.valuePtr();
    amgcl::backend::crs<double> A_amgcl(std::make_tuple(n,
                                                        amgcl::make_iterator_range(ptr, ptr + n + 1),
                                                        amgcl::make_iterator_range(col, col + ptr[n]),
                                                        amgcl::make_iterator_range(val, val + ptr[n])));

    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(A_amgcl, prm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(A_amgcl, b, x0);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    Eigen::Map<Eigen::VectorXd> x=Eigen::Map<Eigen::VectorXd>(x0.data(),x0.size());

    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}

#ifdef USE_CUDA

template <class T>
returnvalue RunAMGCLCUDA_backend(amgcl::profiler<> &prof, Convoptions opt, const LinearSystem &Axb, const std::string &name)
{

	thrust::device_vector<double> X = std::vector<double>(Axb.x0.data(), Axb.x0.data() + Axb.x0.size());
	thrust::device_vector<double> F = std::vector<double>(Axb.b.data(), Axb.b.data() + Axb.b.size());
size_t n = Axb.A.rows();

    amgcl::backend::cuda<double>::params bprm;
    cusparseCreate(&bprm.cusparse_handle);
    typename T::params prm;
    prm.solver.tol = opt.tolerance;
    prm.solver.maxiter = opt.iterations;
    prof.tic("setup_" + name);
    T solve(Axb.A, prm,bprm);
    prof.toc("setup_" + name);
    returnvalue result;
    prof.tic("solve_" + name);
    std::tie(result.iterations, result.error) = solve(F, X);
    prof.toc("solve_" + name);
    std::cout << solve << std::endl;
    thrust::host_vector<double> X_cpu=X;
    Eigen::Map<Eigen::VectorXd> x=Eigen::Map<Eigen::VectorXd>(X_cpu.data(),X_cpu.size());
    result.error_exact=(Axb.A*x-Axb.b).norm()/Axb.b.norm();
    return result;
}
#endif

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

    cxxopts::Options options("SparseSolverBench", "Benchmarking various sparse linear solvers");

    options.add_options()("t,threads", "How many threads to use", cxxopts::value<int>()->default_value("1"))("s,tolerance", "Tolerance to which solvers should converge", cxxopts::value<double>()->default_value("1e-12"))("i,iterations", "max number of iterations", cxxopts::value<int>()->default_value("1000"))("A,SparseMatrix", "MM Format File for Sparse Matrix", cxxopts::value<std::string>())("b,Rightside", "MM Format File for Vector", cxxopts::value<std::string>())("x,initialguess", "MM Format File for InitialGuess", cxxopts::value<std::string>())("h,help", "Print usage");

    auto result = options.parse(argc, argv);

    if (result.count("help"))
    {
        std::cout << options.help() << std::endl;
        exit(0);
    }

    std::string intial_guess_filename = "";
    if (result.count("initialguess"))
    {
        intial_guess_filename = result["initialguess"].as<std::string>();
    }

    Convoptions opt;

    int threads = result["threads"].as<int>();
    opt.tolerance = result["tolerance"].as<double>();
    opt.iterations = result["iterations"].as<int>();
    std::string A_filename = result["SparseMatrix"].as<std::string>();
    std::string b_filename = result["Rightside"].as<std::string>();

    std::cout << "Running profiler with " << threads << " threads\n"
              << "Required tolerance " << opt.tolerance << " num iterations " << opt.iterations << "\nA:" << A_filename << "\nb" << b_filename << std::endl;
    if (intial_guess_filename.size())
    {
        std::cout << "x0:" << intial_guess_filename << std::endl;
    }
    omp_set_num_threads(threads);

    amgcl::profiler<> prof;
    LinearSystem Axb;

    prof.tic("read_input");
    Eigen::loadMarket(Axb.A, A_filename);

    Eigen::loadMarketVector(Axb.b, b_filename);
    Axb.x0 = Eigen::VectorXd::Zero(Axb.A.rows());
    if (intial_guess_filename.size())
    {
        Eigen::loadMarketVector(Axb.x0, intial_guess_filename);
    }
    prof.toc("read_input");

    std::vector<std::string> names;
    std::vector<returnvalue> results;

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::builtin<double>>>
        Solver_bicgstab;
names.push_back("amgcl_bicgstab");
  results.push_back(RunAMGCL_backend<Solver_bicgstab>(prof, opt, Axb, names.back()));

    typedef amgcl::make_solver<
        amgcl::relaxation::as_preconditioner<
        amgcl::backend::builtin<double>,
        amgcl::relaxation::ilu0>,
        amgcl::solver::bicgstab<amgcl::backend::builtin<double>>>
        Solver_bicgstab_ilut;
  names.push_back("amgcl_bicgstab_ilut");
  results.push_back(RunAMGCL_backend<Solver_bicgstab_ilut>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstabl<amgcl::backend::builtin<double>>>
        Solver_bicgstabl;
   names.push_back("amgcl_bicgstabl");
   results.push_back(RunAMGCL_backend<Solver_bicgstabl>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::idrs<amgcl::backend::builtin<double>>>
        Solver_idrs;

   names.push_back("amgcl_idrs");
   results.push_back(RunAMGCL_backend<Solver_idrs>(prof, opt, Axb, names.back()));

    // Setup the solver:
    typedef amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::eigen<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::eigen<double>>>
        Solver2;

   names.push_back("amgcl_bicgstab_eigen");
   results.push_back(RunAMGCLEigen_backend<Solver2>(prof, opt, Axb, names.back()));

  names.push_back("eigen_bicgstab");
results.push_back(RunEigen_Solver<Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

    names.push_back("eigen_bicgstabl");
    results.push_back(RunEigen_Solver<Eigen::BiCGSTABL<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

    names.push_back("eigen_idrstab");
    results.push_back(RunEigen_Solver<Eigen::IDRStab<Eigen::SparseMatrix<double, Eigen::RowMajor>>>(prof, opt, Axb, names.back()));

#ifdef USE_CUDA

    typedef amgcl::make_solver<
        amgcl::amg<
        amgcl::backend::cuda<double>,
       amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0>,
        amgcl::solver::bicgstab<amgcl::backend::cuda<double>>>
        Solver_cuda_bicgstab;
    names.push_back("amgcl_cuda_bicgstab");
    results.push_back(RunAMGCLCUDA_backend<Solver_cuda_bicgstab>(prof, opt, Axb, names.back()));

    typedef amgcl::make_solver<
            amgcl::relaxation::as_preconditioner<
            amgcl::backend::cuda<double>,
            amgcl::relaxation::ilu0>,
            amgcl::solver::bicgstab<amgcl::backend::cuda<double>>>
            Solver_cuda_bicgstab_ilut;
        names.push_back("amgcl_cuda_bicgstab_ilut");
        results.push_back(RunAMGCLCUDA_backend<Solver_cuda_bicgstab_ilut>(prof, opt, Axb, names.back()));
    #endif

    std::cout << "Eigen uses " << Eigen::nbThreads() << " threads" << std::endl;
    for(int i=0;i<names.size();i++){
        std::cout << names[i]<<"\t iter:" << results[i].iterations << "\t error " << results[i].error << "\t error_exact " << results[i].error_exact <<std::endl;
    }

    std::cout << prof << std::endl;
}


#pragma once

#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

#include "hermite_to_nodal.hpp"
#include "polar_to_hermite.hpp"

#include <omp.h>
#include <Eigen/Dense>
#include <cassert>
#include <memory>
#include <string>

namespace boltzmann {

template <typename PolarBasis = SpectralBasisFactoryKS::basis_type>
class Polar2Nodal
{
 public:
  typedef PolarBasis polar_basis_t;
  typedef SpectralBasisFactoryHN::basis_type hermite_basis_t;
  typedef Polar2Hermite<polar_basis_t, hermite_basis_t> p2h_t;
  typedef Hermite2Nodal<hermite_basis_t> h2n_t;

 public:
  Polar2Nodal() { /* empty */}

  /**
   *  @brief Transform coefficients btw Polar-Laguerre and nodal basis. The
   *         nodes of the nodal basis are located at the underlying
   *         Gauss-Hermite quadrature points.
   *
   *  @param polar_basis Polar-Laguerre basis object
   *  @param a exp weight factor in quad rule, e.g. for basis functions which
   *           decay like \f$exp(-r^/2)\f$, @param a is 1.0
   */
  Polar2Nodal(const polar_basis_t &polar_basis, double a = 1.0);

 public:
  void init(const polar_basis_t &polar_basis, double a);
  std::shared_ptr<p2h_t> get_p2h() const { return p2h_; }
  std::shared_ptr<h2n_t> get_h2n() const { return h2n_; }
  /// size Polar-Laguerre basis
  int N() const { return N_; }
  /// max. polynomial degree
  int K() const { return K_; }

  template <typename DERIVED1, typename DERIVED2>
  void to_nodal(Eigen::DenseBase<DERIVED1> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_polar(Eigen::DenseBase<DERIVED1> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  bool is_initialized() const { return is_initialized_; }

 private:
  std::shared_ptr<p2h_t> p2h_;
  std::shared_ptr<h2n_t> h2n_;
  int N_;
  int K_;
  bool is_initialized_ = false;

  thread_local static ArrayBuffer<> buf_;
};

template <typename PolarBasis>
thread_local ArrayBuffer<> Polar2Nodal<PolarBasis>::buf_;

template <typename PolarBasis>
Polar2Nodal<PolarBasis>::Polar2Nodal(const polar_basis_t &polar_basis, double a)
{
  this->init(polar_basis, a);
}

//  -------------------------------------------------------------------------------------
template <typename PolarBasis>
void
Polar2Nodal<PolarBasis>::init(const polar_basis_t &polar_basis, double a)
{
  K_ = spectral::get_max_k(polar_basis) + 1;
  N_ = polar_basis.n_dofs();

  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K_, 2);

  assert(hermite_basis.n_dofs() == polar_basis.n_dofs());

  // make_unique not available in c++11
  // p2h_ = std::make_unique<p2h_t>(polar_basis, hermite_basis);
  // h2n_ = std::make_unique<h2n_t>(hermite_basis, K);
  typedef Eigen::MatrixXd mat_t;
  p2h_ = std::make_shared<p2h_t>(polar_basis, hermite_basis);
  const int K = K_;
  if (a == 1.0) {
    h2n_ = std::make_shared<h2n_t>(
        hermite_basis, K_, [K](mat_t &m1, mat_t &m2) { H2N_1d<>::create(m1, m2, K); });
  } else {
    h2n_ = std::make_shared<h2n_t>(
        hermite_basis, K_, [K, a](mat_t &m1, mat_t &m2) { H2NG_1d::create(m1, m2, K, a); });
  }

  is_initialized_ = true;
}

// //
// --------------------------------------------------------------------------------------
// template<typename PolarBasis>
// void Polar2Nodal<PolarBasis>::
// to_nodal(numeric_t* dst, const numeric_t* src, bool transpose) const
// {
//   typedef Eigen::Map<Eigen::VectorXd> mvec_t;
//   auto buffer = buf_.get<mvec_t>(N_);

//  if(!transpose) {
//     p2h_->to_hermite(buffer, src);
//     h2n_->to_nodal(dst, buffer);
//   } else {
//     throw std::runtime_error(std::string(__FILE__)
//                              + ":" + std::to_string(__LINE__) + " not
//                              implemented");
//   }
// }

// template<typename PolarBasis>
// void Polar2Nodal<PolarBasis>::
// to_polar(numeric_t* dst, const numeric_t* src, bool transpose) const
// {
//   auto buffer = buf_.get<Eigen::VectorXd>(N_);

//   if(!transpose) {
//     h2n_->to_hermite(buffer, src);
//     p2h_->to_polar(dst, buffer);
//   } else {
//     throw std::runtime_error(std::string(__FILE__)
//                              + ":" + std::to_string(__LINE__) + " not
//                              implemented");
//   }
// }

template <typename PolarBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Nodal<PolarBasis>::to_nodal(Eigen::DenseBase<DERIVED1> &dst,
                                  const Eigen::DenseBase<DERIVED2> &src,
                                  bool transpose) const
{
  auto buffer = buf_.get<Eigen::VectorXd>(N_);
  BOOST_ASSERT(is_initialized_);

  if (!transpose) {
    p2h_->to_hermite(buffer, src);
    h2n_->to_nodal(dst, buffer);
  } else {
    throw std::runtime_error(std::string(__FILE__) + ":" + std::to_string(__LINE__) +
                             " not implemented");
  }
}

template <typename PolarBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Nodal<PolarBasis>::to_polar(Eigen::DenseBase<DERIVED1> &dst,
                                  const Eigen::DenseBase<DERIVED2> &src,
                                  bool transpose) const
{
  BOOST_ASSERT(is_initialized_);
  auto buffer = buf_.get<Eigen::VectorXd>(N_);

  if (!transpose) {
    h2n_->to_hermite(buffer, src);
    p2h_->to_polar(dst, buffer);
  } else {
    throw std::runtime_error(std::string(__FILE__) + ":" + std::to_string(__LINE__) +
                             " not implemented");
  }
}
}  // end namespace boltzmann


#pragma once

#include "algorithms/util/PeakDetection.hpp"
#include "algorithms/public/DataSetIdSequence.hpp"
#include "algorithms/util/DistanceFuncs.hpp"
#include "algorithms/public/KDTree.hpp"
#include "algorithms/public/KMeans.hpp"
#include "algorithms/public/MelBands.hpp"
#include "algorithms/util/AlgorithmUtils.hpp"
#include "algorithms/util/FluidEigenMappings.hpp"
#include "algorithms/util/MedianFilter.hpp"
#include "algorithms/util/SpectralEmbedding.hpp"
#include "data/TensorTypes.hpp"
#include "data/FluidDataSet.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <random>

namespace fluid {
namespace algorithm {

class GraphPlayUtils {

public:
  using  MatrixXd = Eigen::MatrixXd;
  using  VectorXd = Eigen::VectorXd;
  using DataSet = FluidDataSet<std::string, double, 1>;

  GraphPlayUtils(){
    using namespace std;
    random_device rd;
    mGen = mt19937(rd());
    mDis = uniform_real_distribution<> (0.0, 1.0);
    mFilter.init(5);
  }

  index randInt(index N){
    return static_cast<index>(mDis(mGen) * N);
  }

  double rand(){
    return mDis(mGen);
  }

  Eigen::ArrayXXd computeDM(RealMatrixView mag, index numBands,
    double sampleRate, index windowSize, index fftSize, index dist){
    using namespace Eigen;
    using namespace _impl;
    MelBands melBands = MelBands(numBands, fftSize);
    melBands.init(20, 5000, numBands, mag.cols(), sampleRate, windowSize);
    RealMatrix melSpec = RealMatrix(mag.rows(), numBands);
    for(index i = 0; i < mag.rows(); i++){
      melBands.processFrame(mag.row(i), melSpec.row(i), true, false, false);
    }
    MatrixXd tmp = asEigen<Matrix>(melSpec);
    return DistanceMatrix(tmp, dist);
  }

  void onsetDetection(Eigen::Ref<Eigen::ArrayXd> odf,
                      Eigen::Ref<Eigen::ArrayXXd> transitions, index offset = 2){
    for(index i = 0; i < odf.size(); i++){
      odf(i) = odf(i) - mFilter.processSample(odf(i));
    }
    auto onsets = mPD.process(odf, 0, 0.1, false, false);
    for(index i = 0; i < onsets.size(); i++){
      index pos = onsets[i].first;
      index start = std::max(index(0), pos - offset);
      index end = std::min(transitions.rows() - 1, pos + offset + 1);
      transitions.block(start, 0, end - start, transitions.cols()).setZero();
      transitions.block(0, start, transitions.rows(), end - start).setZero();
    }
  }

  FluidTensor<index, 1> kmeans(RealMatrixView data, index nClusters){
      algorithm::DataSetIdSequence seq("", 0, 0);
      index minClusterSize = 10;
      DataSet tmpDS = DataSet(data.cols());
      for(index i = 0; i < data.rows(); i++){
        tmpDS.add(seq.next(), data.row(i));
      }
      mKMeans.clear();
      mKMeans.train(tmpDS, nClusters, 100);
      FluidTensor<index, 1> clusters(data.rows());
      mKMeans.getAssignments(clusters);
      RealMatrix means(mKMeans.size(), mKMeans.dims());
      mKMeans.getMeans(means);
      for(index i = 0; i < nClusters; i++){
        index cSize = mKMeans.getClusterSize(i);
        //std::cout<<cSize<<std::endl;
        if(cSize > 0 && cSize < minClusterSize){
          RealMatrix distances(1, mKMeans.size());
          RealMatrix mean(1, mKMeans.dims());
          mean.row(0) =  means.col(i);
          mKMeans.getDistances(mean, distances);
          index closest = std::min_element(distances.begin(), distances.end()) - distances.begin();
          for(index j = 0; j < clusters.size(); j++)
            if(clusters(j) == i) clusters(j) = closest;
        }
      }
      //std::ofstream ofs ("clusters.mat", std::ofstream::out);ofs << clusters;ofs.close();
      return clusters;
  }

  void writeMatrix(Eigen::Ref<Eigen::MatrixXd> mat, std::string name){
    //std::ofstream ofs (name+".mat", std::ofstream::out);ofs << mat;ofs.close();
  }

  FluidTensor<index, 1>  spectralClustering(Eigen::Ref<Eigen::ArrayXXd> dm, index numClusters = 0){
    using namespace Eigen;
    index maxClusters = numClusters > 0? numClusters : std::min(index(50), dm.rows());
    index nPoints = dm.rows();
    MatrixXd tmpRp = (dm.array() < 0.25).cast<double>();
    MatrixXd weightedGraph = (1 - dm) * tmpRp.array();
    SpectralEmbedding spectralEmbedding;
    spectralEmbedding.train(weightedGraph.sparseView(), maxClusters);
    if(numClusters == 0 ){
      VectorXd eigenValues  =  spectralEmbedding.eigenValues();
      ArrayXd diff = (
        eigenValues.segment(1, eigenValues.size() - 2) -
        eigenValues.segment(0, eigenValues.size() - 2));
        VectorXd::Index maxIndex;
        double maxVal = diff.maxCoeff(&maxIndex);
        numClusters = std::min(2*(maxIndex + 1), maxClusters);
    }
    MatrixXd eigenVectors = spectralEmbedding.eigenVectors().block(0,0, nPoints, numClusters);
    eigenVectors.rowwise().normalize();
    return kmeans( _impl::asFluid(eigenVectors), numClusters);
  }

private:
  MedianFilter mFilter;
  PeakDetection mPD;
  KMeans mKMeans;
  std::mt19937 mGen;
  std::uniform_real_distribution<> mDis;
};
} // namespace algorithm
} // namespace fluid


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

#ifndef GAUSSFILTERTYPES_H
#define GAUSSFILTERTYPES_H

#include <boost/math/distributions/normal.hpp>
#include "pwiz/utility/findmf/base/filter/utilities/gauss.hpp"

namespace ralab{
  namespace base{
    namespace filter{

      /*! \brief generate the gauss filter function for filtering of peaks with fwhm (full width at half max)

                        \post accumulate(gauss) == 1.
                        \return accumulate(gauss) == 1.
                        */
      template <typename TReal>
      TReal getGaussianFilter
      (
          std::vector<TReal> & gauss, //!<[out] Gaussian for filtering
          TReal fwhm = 20 //!<[in] full width at half max in points
          )
      {
        std::vector<TReal> x;
        ralab::base::base::seq<TReal>( -ceil(TReal(2*fwhm)), ceil(TReal(2*fwhm)) , x);
        TReal sigma = fwhm/2.35;
        //generate response
        return utilities::getGaussWorker(sigma, gauss, x);
      }

      /*! \brief generate the gauss filter function for filtering of peaks with fwhm (full width at half max)

                        \post accumulate(gauss) == 1.
                        \return accumulate(gauss) == 1.
                        */
      template <typename TReal>
      TReal getGaussianFilterQuantile
      (
          std::vector<TReal> & gauss, //!<[out] Gaussian for filtering
          TReal fwhm = 20, //!<[in] full width at half max in points
          TReal quantile = 0.01 //!< would mean that the generated distribution covers at least 99.8 of mass
          )
      {
        if( quantile >= 0.5)
          {
            throw std::logic_error("quantile >= 0.5");
          }
        std::vector<TReal> x;

        TReal sigma = fwhm/2.35;
        boost::math::normal_distribution<TReal> nd_(0,sigma);
        TReal quant = floor(boost::math::quantile(nd_,quantile));
        ralab::base::base::seq( quant , -quant , x);
        return utilities::getGaussWorker(sigma, gauss, x);

      }

      /*! \brief generate first derivative Gauss

                        \post accumulate(gauss1d) == 0.
                        \post accumulate(fabs(gauss1d)) == 1.
                        */

      template <typename TReal>
      TReal getGaussian1DerFilter(
          std::vector<TReal> & gauss1d, //!<[out] Gaussian for filtering
          TReal fwhm = 20 //!<[in] full width at half max in points
          )
      {
        std::vector<TReal> x;
        ralab::base::base::seq( - ceil(TReal(2*fwhm)), ceil(TReal(2*fwhm)) , x);
        TReal sigma = fwhm/2.35;
        //generate response
        return utilities::getGaussian1DerWorker(sigma, gauss1d, x);
      }

      template <typename TReal>
      TReal getGaussian1DerFilterQuantile(
          std::vector<TReal> & gauss1d, //!<[out] Gaussian for filtering
          TReal fwhm = 20, //!<[in] full width at half max in points
          TReal quantile = 0.1
          )
      {
        if( quantile >= 0.5)
          {
            throw std::logic_error("quantile >= 0.5");
          }
        std::vector<TReal> x;
        TReal sigma = fwhm/2.35;
        boost::math::normal_distribution<TReal> nd_(0,sigma);
        TReal quant = floor(boost::math::quantile(nd_,quantile));
        ralab::base::base::seq( quant , -quant , x);
        //generate response
        return utilities::getGaussian1DerWorker(sigma, gauss1d, x);
      }
    }//filter
  }//base
}//ralab

#endif


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief core homography template implementations
 */

#include "homography.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/LU>

namespace kwiver {
namespace vital {

namespace //anonymous
{

/// Private helper method for point transformation via homography matrix
template < typename T >
Eigen::Matrix< T, 2, 1 >
h_map_point( Eigen::Matrix< T, 3, 3 > const& h, Eigen::Matrix< T, 2, 1 > const& p )
{
  Eigen::Matrix< T, 3, 1 > out_pt = h * Eigen::Matrix< T, 3, 1 > ( p[0], p[1], 1.0 );

  if ( fabs( out_pt[2] ) <= Eigen::NumTraits< T >::dummy_precision() )
  {
    VITAL_THROW(point_maps_to_infinity);
  }
  return Eigen::Matrix< T, 2, 1 > ( out_pt[0] / out_pt[2], out_pt[1] / out_pt[2] );
}

} // end anonymous namespace

/// Construct an identity homography
template < typename T >
homography_< T >
::homography_()
  : h_( matrix_t::Identity() )
{
}

/// Construct from a provided transformation matrix
template < typename T >
homography_< T >
::homography_( Eigen::Matrix< T, 3, 3 > const& mat )
  : h_( mat )
{
}

/// Conversion Copy constructor -- float specialization
template < >
template < >
homography_< float >
::homography_( homography_< float > const& other )
  : h_( other.get_matrix() )
{
}

/// Conversion Copy constructor -- double specialization
template < >
template < >
homography_< double >
::homography_( homography_< double > const& other )
  : h_( other.get_matrix() )
{
}

/// Construct from a generic homography
template < typename T >
homography_< T >
::homography_( homography const& base )
  : h_( base.matrix().template cast< T > () )
{
}

/// Construct from a generic homography -- double specialization
template < >
homography_< double >
::homography_( homography const& base )
  : h_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template < typename T >
transform_2d_sptr
homography_< T >
::clone() const
{
  return std::make_shared< homography_< T > >( *this );
}

/// Get a double-typed copy of the underlying matrix transformation
template < typename T >
Eigen::Matrix< double, 3, 3 >
homography_< T >
::matrix() const
{
  return this->h_.template cast< double > ();
}

/// Specialization for homographies with native double type
template < >
Eigen::Matrix< double, 3, 3 >
homography_< double >
::matrix() const
{
  return this->h_;
}

/// Normalize homography transformation in-place
template < typename T >
homography_sptr
homography_< T >
::normalize() const
{
  matrix_t norm = this->get_matrix();

  if ( fabs( norm( 2, 2 ) ) >= Eigen::NumTraits< T >::dummy_precision() )
  {
    norm /= norm( 2, 2 );
  }
  return std::make_shared< homography_< T > >( norm );
}

/// Inverse the homography transformation returning a new transformation
template < typename T >
homography_sptr
homography_< T >
::inverse() const
{
  matrix_t inv;
  bool isvalid;

  this->h_.computeInverseWithCheck( inv, isvalid );
  if ( ! isvalid )
  {
    VITAL_THROW(non_invertible);
  }
  return std::make_shared< homography_< T > >( inv );
}

/// Map a 2D double-type point using this homography
template < typename T >
Eigen::Matrix< double, 2, 1 >
homography_< T >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  // Explicitly refer to templated version of method so as to not infinitely
  // recurse.
  Eigen::Matrix< double, 3, 3 > m = h_.template cast< double > ();

  return h_map_point( m, p );
}

/// Map a 2D double-type point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Get the underlying matrix transformation
template < typename T >
typename homography_< T >::matrix_t &
homography_< T >
::get_matrix()
{
  return this->h_;
}

/// Get a const new copy of the underlying matrix transformation.
template < typename T >
typename homography_< T >::matrix_t const &
homography_< T >
::get_matrix() const
{
  return this->h_;
}

/// Map a 2D point using this homography -- generic version
template < typename T >
Eigen::Matrix< T, 2, 1 >
homography_< T >
::map_point( Eigen::Matrix< T, 2, 1 > const& p ) const
{
  return h_map_point< T > ( h_.template cast< T > (), p );
}

/// Map a 2D point using this homography -- float specialization
template < >
Eigen::Matrix< float, 2, 1 >
homography_< float >
::map_point( Eigen::Matrix< float, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Map a 2D point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map_point( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Custom f2f_homography multiplication operator.
template < typename T >
homography_< T >
homography_< T >
::operator*( homography_< T > const& rhs ) const
{
  return homography_< T > ( h_ * rhs.h_ );
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// homography_<T> output stream operator
template < typename T >
std::ostream&
operator<<( std::ostream& s, homography_< T > const& h )
{
  s << h.get_matrix();
  return s;
}

/// Output stream operator for \p homography instances
std::ostream&
operator<<( std::ostream& s, homography const& h )
{
  s << h.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_HOMOGRAPHY( T )              \
  template class homography_< T >;               \
  template VITAL_EXPORT std::ostream&            \
  operator<<( std::ostream&,                     \
              homography_< T > const& )

INSTANTIATE_HOMOGRAPHY( float );
INSTANTIATE_HOMOGRAPHY( double );
#undef INSTANTIATE_HOMOGRAPHY
/// \endcond

} } // end vital namespace


//!
//! Contains the implementation of some statistical diagnostic tests for MCMC.
//!
//! \file infer/diagnostics.hpp
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Dense>

namespace stateline
{
  namespace mcmc
  {
    //! Convergence test using estimated potential scale reduction (EPSR).
    //! It is a convergence metric that takes into account the variance of the
    //! means between chains and the variance of the samples within each chain.
    //!
    class EpsrConvergenceCriteria
    {
      public:
        //! Initialise the convergence criteria.
        //!
        //! \param numChains The number of chains to be tested for convergence.
        //! \param numDims The number of dimensions in each state.
        //!
        EpsrConvergenceCriteria(int numChains, int numDims) :
          M_(Eigen::ArrayXXd::Zero(numDims, numChains)),
          S_(Eigen::ArrayXXd::Zero(numDims, numChains)),
          numSamples_(Eigen::ArrayXi::Zero(numChains))
        {
        }

        //! Update the convergence statistics for a new sample in a particular chain.
        //!
        //! \param id The chain which has the new sample.
        //! \param sample The new sample to update the convergence statistics with.
        //!
        void update(uint id, const Eigen::VectorXd &sample)
        {
          // See http://www.johndcook.com/standard_deviation.html
          int n = numSamples_(id) + 1;

          Eigen::ArrayXd x = sample.array();

          // Update the running mean and variance
          Eigen::ArrayXd newM = M_.col(id) + (x - M_.col(id)) / n;
          S_.col(id) = S_.col(id) + (x - M_.col(id)) * (x - newM);
          M_.col(id) = newM;

          numSamples_(id) = n;
        }

        //! Compute the estimated potential scale factor. A low value indicates
        //! that the chains are converging.
        //!
        //! \return A vector containing the scale factors for each dimension.
        //!
        Eigen::ArrayXd rHat() const
        {
          // Number of samples (use the length of the shortest chain)
          int n = numSamples_.minCoeff();

          // Number of chains
          int m = numSamples_.rows();

          // Find the overall mean of the chains
          Eigen::ArrayXXd overallMean = M_.rowwise().mean().replicate(1, M_.cols());

          // Calculate the between chain variance
          Eigen::ArrayXd b = (n / (m - 1.0)) * (M_ - overallMean).matrix().rowwise().squaredNorm().array();

          // Calculate the within chain variance
          Eigen::ArrayXd w = ((1.0 / m) * S_ / (n - 1.0)).rowwise().sum();

          // Compute the weighted average of the between chain variance and within chain variance
          Eigen::ArrayXd vHat = ((n - 1.0) / n) * w + (1.0 / n) * b;

          // Compute the potential scale reduction
          return (vHat / (w + 1e-30)).sqrt();
        }

        //! Check if all the chains have converged. The chains have converged if the
        //! potential scale reduction factor is below 1.1 for all dimensions.
        //!
        //! \return Whether all the chains have converged.
        //!
        bool hasConverged() const
        {
          return (rHat() < 1.1).all();
        }

      private:
        Eigen::ArrayXXd M_;
        Eigen::ArrayXXd S_;
        Eigen::ArrayXi numSamples_;
    };
  }
}


/* ScaFES
 * Copyright (c) 2017-2018, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file HeatEqnFDMInitFile.hpp
 *
 *  @brief Implementation of n-dimensional heat equation problem on unit hybercube
 *         with init function and config file parser.
 */

#include "ScaFES.hpp"
#include "analyticalFunctions.hpp"

#include <boost/property_tree/ini_parser.hpp>

/*******************************************************************************
 ******************************************************************************/
/**
 * \class HeatEqnFDM
 *  @brief Class for discretized heat equation problem.
 *
 * \section heatEqnFDM 3D Heat Equation Problem on Unit Cube
 *
 *
 * \subsection mathdescr Mathematical Description
 * Given:
 *

 *
 Time interval \f[[t_S; t_E] \mbox{ with } 0 \le t_S < t_E,\f]

 *
 domain \f[\Omega := (0,1)^3,\f]

 *
 source \f[f: \bar{\Omega} \times (t_S;t_E] \to R, \quad
        f(x,t) := 0,\f]

 *
 boundary condition \f[g: \partial\Omega \times (t_S;t_E] \to R, \quad
         g(x,t) := 0,\f]

 *
 initial condition \f[\widetilde{y}: \bar{\Omega} \to R, \quad
        \widetilde{y}(x) :=  \prod_{i=0}^{d-1} x_i \cdot
                    (x_i - 1/2)^2 \cdot (1 - x_i),\f]

 *

 * Find \f[y: \bar{\Omega} \times [t_S; t_E] \to {R}\f] such that
 * \f{eqnarray*}{
 * \partial_t y - \Delta y & =&  f  \quad \mbox{in }
 *          \Omega \times (t_S;t_E], \\
 *                      y & =&  g \quad \mbox{on }
 *          \partial\Omega \times (t_S;t_E], \\
 *             y(\cdot,t_S) & =& \widetilde{y}  \quad \mbox{in } \bar{\Omega}.
 * \f}
 *
 * \subsection mathdiscretization Discretization of the problem
 * \subsubsection discrTD Discretization of the time interval and the domain
 * Let the time interval [t_S;t_E] be uniformly discretised with
 * \f[t_l := \{ t_S + l \cdot \tau\}_l \f]
 * with time step size tau > 0.
 *
 * Let the domain Omega be uniformly discretised with
 * \f[ x_{(i,j,k)} :=  \{ (i \cdot h_0, j \cdot h_1, k \cdot h_2) \}_{(i,j,k)}
 * \f]
 * with grid sizes h_p>0 for all p.
 *

 *
G_I: Set of all interior grid nodes,

 *
G_B: Set of all boundary grid nodes,

 *
G := G_I with G_B: Set of all grid nodes

 *

 *
 * \subsubsection discrHeatEqn Discretization of the heat equation
 * Define the following vectors:
 * \f{eqnarray*}{
 * Y^{(l)}_{(i,j,k)} &:=& y(x_{(i,j,k)},t_l), \\
 * F^{(l)}_{(i,j,k)} &:=& f(x_{(i,j,k)},t_l), \\
 * G^{(l)}_{(i,j,k)} &:=& g(x_{(i,j,k)},t_l), \\
 * \widetilde{Y}_{(i,j,k)} &:=& \widetilde{y}(x_{(i,j,k)}) \\
 * && \quad \mbox{for all } t_l \in \tau_h, x_{(i,j,k)} \in \Omega_h.
 * \f}
 * Discretise derivatives in space using the symmetric difference quotients
 * (Finite Difference Method with 7-point stencil) and in time using the
 * forward difference quotient (explicit Euler scheme):
 * Discretise in space (Finite Difference Method with 7-point stencil)
 * and in time (explicit Euler scheme):
 * \f{eqnarray*}{
 * Y^{(l+1)}_{(i,j,k)} &=&  Y^{(l)}_{(i,j,k)}
 *                   + \big( Y^{(l)}_{(i-1,j,k)} - 2 \cdot Y^{(l)}_{(i,j,k)}
 *               + Y^{(l)}_{(i+1,j,k)} \big) \cdot \tau / h_0^2   \\
 * &&
 *               \quad  \quad \quad  \quad  + \big(  Y^{(l)}_{(i,j-1,k)} - 2 \cdot Y^{(l)}_{(i,j,k)}
 *               + Y^{(l)}_{(i,j+1,k)} \big) \cdot \tau / h_1^2   \\
 * &&            \quad \quad  \quad \quad  + \big( Y^{(l)}_{(i,j,k-1)}  - 2 \cdot Y^{(l)}_{(i,j,k)}
 *               + Y^{(l)}_{(i,j,k+1)} \big) \cdot \tau / h_2^2  \\
 * &&  \quad \quad \quad\quad + \tau \cdot F^{(l)}_{(i,j,k)}  \quad\quad
 * \forall l, \, \forall {(i,j,k)} \in \cal{G}_I, \\
 * Y^{(l+1)}_{(i,j,k)} &=&  G^{(l+1)}_{(i,j,k)}
 *               \quad   \quad \forall l, \, \forall {(i,j,k)} \in \cal{G}_B, \\
 *            Y^{(0)}_{(i,j,k)} & =&  \widetilde{Y}_{(i,j,k)}\quad \quad
 * \forall {(i,j,k)} \in \cal{G}.
 * \f}
*/
template<typename CT, std::size_t DIM>
class HeatEqnFDM : public ScaFES::Problem<HeatEqnFDM<CT,DIM>, CT, DIM> {
   private:
   using PTree = boost::property_tree::ptree;
   const PTree ptree;

   public:
    /** Coefficient a. */
    const double A;

    /** Coefficient alpha. */
    const double ALPHA;

    /** Coefficient c. */
    const double C;

    /** Coefficient lambda. */
    const double LAMBDA;

    /** Coefficient rho. */
    const double RHO;

   public:
    /** All fields which are related to the underlying problem
     * are added in terms of an entry of the parameters of
     * type \c std::vector.
     * @param params Set of ScaFES parameters.
     * @param gg Global grid.
     * @param useLeapfrog Should the leap frog scheme be used?
     * @param nameDatafield Name of the fields.
     * @param stencilWidth Stencil width of the fields.
     * @param isKnownDf Is the data field are known or unknown one?
     * @param nLayers Number of layers at the global boundary.
     * @param defaultValue Default value of fields.
     * @param writeToFile How often should the data field be written to file.
     * @param computeError Should the Linf error between the numerical
     *                     and exact solution be computed?
     * @param geomparamsInit Initial guess of geometrical parameters.
     */
    HeatEqnFDM(ScaFES::Parameters const& params,
               ScaFES::GridGlobal<DIM> const& gg,
               bool useLeapfrog,
               std::vector<std::string> const& nameDatafield,
               std::vector<int> const& stencilWidth,
               std::vector<bool> const& isKnownDf,
               const PTree& ptree_,
               std::vector<int> const& nLayers = std::vector<int>(),
               std::vector<CT> const& defaultValue = std::vector<CT>(),
               std::vector<ScaFES::WriteHowOften> const& writeToFile
                 = std::vector<ScaFES::WriteHowOften>(),
               std::vector<bool> const& computeError = std::vector<bool>(),
               std::vector<CT> const& geomparamsInit = std::vector<CT>() )
        : ScaFES::Problem<HeatEqnFDM<CT, DIM>, CT, DIM>(params, gg, useLeapfrog,
                                                        nameDatafield, stencilWidth,
                                                        isKnownDf, nLayers,
                                                        defaultValue, writeToFile,
                                                        computeError, geomparamsInit),
        ptree(ptree_),
        A(ptree.get<CT>("Parameters.a")),
        ALPHA(ptree.get<CT>("Parameters.alpha")),
        C(ptree.get<CT>("Parameters.c")),
        LAMBDA(ptree.get<CT>("Parameters.lambda")),
        RHO(ptree.get<CT>("Parameters.rho"))
        { }

    /** Evaluates all fields at one given global inner grid node.
     *  @param vNew Set of all fields.
     *  @param idxNode Index of given grid node.
     */
    void evalInner(std::vector< ScaFES::DataField<CT, DIM> >& vNew,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& timestep) {
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        double t = this->time(timestep);

        /* Vector for f. */
        vNew[0](idxNode) = this->RHO * this->C * timeLinSpaceLindTime<CT,DIM>(x);
        vNew[0](idxNode) -= this->LAMBDA * timeLinSpaceLinSumOfdSpace2ndOrder<CT,DIM>(x, t);
        /* Vector for g. */
        vNew[1](idxNode) = timeLinSpaceLinFunc<CT,DIM>(x, t);
        /* Vector for y. */
        vNew[2](idxNode) = timeLinSpaceLinFunc<CT,DIM>(x, t);
    }

    /** Evaluates all fields at one given global border grid node.
     *  @param vNew Set of all fields.
     *  @param idxNode Index of given grid node.
     *  @param timestep Given time step.
     */
    void evalBorder(std::vector< ScaFES::DataField<CT, DIM> >& vNew,
                    ScaFES::Ntuple<int,DIM> const& idxNode,
                    int const& timestep) {
        this->evalInner(vNew, idxNode, timestep);
    }

    /** Initializes all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void initInner(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                   std::vector<TT> const& /*vOld*/,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& timestep) {
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        double t_s = this->time(timestep);

        vNew[0](idxNode) = timeLinSpaceLinFunc<CT,DIM>(x, t_s);
    }

    /** Initializes all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param vOld Set of all given fields.
     *  @param idxNode Index of given grid node.
     *  @param timestep Given time step.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                    std::vector<TT> const& vOld,
                    ScaFES::Ntuple<int,DIM> const& idxNode,
                    int const& timestep) {
        this->template initInner<TT>(vNew, vOld, idxNode, timestep);
    }

    /** Updates all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateInner(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                     std::vector<ScaFES::DataField<TT,DIM>> const& vOld,
                     ScaFES::Ntuple<int,DIM> const& idxNode,
                     int const& /*timestep*/) {
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            vNew[0](idxNode) += this->tau() * A * (
                     vOld[0](this->connect(idxNode, 2*pp))
                     + vOld[0](this->connect(idxNode, 2*pp+1))
                     - 2.0 * vOld[0](idxNode) )
                     / (this->gridsize(pp) * this->gridsize(pp));
        }
        vNew[0](idxNode) += this->tau() * (1.0/(RHO*C)) * this->knownDf(0, idxNode);
    }

    /** Updates all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateBorder(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                      std::vector<ScaFES::DataField<TT,DIM>>const& /*vOld*/,
                      ScaFES::Ntuple<int,DIM> const& idxNode,
                      int const& /*timestep*/) {
        vNew[0](idxNode) = this->knownDf(1, idxNode);
    }

    /** Updates (2nd cycle) all unknown fields at one given global inner grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateInner2(std::vector<ScaFES::DataField<TT,DIM>>&,
                      std::vector<ScaFES::DataField<TT,DIM>> const&,
                      ScaFES::Ntuple<int,DIM> const&,
                      int const&) { }

    /** Updates (2nd cycle) all unknown fields at one given global border
     *  grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateBorder2(std::vector<ScaFES::DataField<TT,DIM>>&,
                       std::vector<ScaFES::DataField<TT,DIM>>const&,
                       ScaFES::Ntuple<int,DIM> const&,
                       int const&) { }
};


//
// Created by tom on 09/01/2021.
//

#include <sstream>
#include <fstream>
#include <iostream>

#include <Eigen/Dense>
#include "utils.h"

vector<string> utils::split(const string &s, char delim) {
    /*********************************************************
     *  Split a string by a delimiter into a vector of strings
     ********************************************************/
    stringstream stream(s);
    string item;
    vector<string> elems;
    while (getline(stream, item, delim)) {
        if (!item.empty()){
            elems.push_back(item);
        }
    }
    return elems;
}

Eigen::MatrixXd utils::symmetric_matrix_from_file(const string &filename){
    /*******************************************************
     *  Extract a symmetric matrix from a file, where the
     *  file is formatted as:
     *
     *  idx1  idx2 value
     ******************************************************/
    string line, item;
    ifstream input_file(filename);

    std::vector<long> idxs1;
    std::vector<long> idxs2;
    std::vector<double> values;

    // Iterate through the file line by line
    while (getline(input_file, line, '\n')) {

        // Ignore any blank lines
        if (line.empty()) {
            continue;
        }
        vector<string> items = utils::split(line, ' ');
        if (items.size() != 3){
            throw runtime_error("Overlap matrix file not correctly formatted."
                                "Expecting lines in the format: idx1  idx2 value");
        }
        idxs1.push_back(stol(items[0]));
        idxs2.push_back(stol(items[1]));
        values.push_back(stod(items[2]));

    }
    long max_idx1 = *max_element(begin(idxs1), end(idxs1));
    long min_idx1 = *min_element(begin(idxs1), end(idxs1));
    long size = max_idx1 - min_idx1 + 1;     // Allows for indexing from 0 or 1 in the file

    Eigen::MatrixXd matrix(size, size);
    for (int n = 0; n < values.size(); n++){

        long i = idxs1[n] - min_idx1;
        long j = idxs2[n] - min_idx1;

        matrix(i, j) = values[n];
        // And because the matrix is symmetric the flipped element can also be set
        matrix(j, i) = values[n];
    }

    return matrix;
}

Eigen::VectorXd utils::ee_from_file(const string &filename, const int &length){
    /*******************************************************
     *  Extract a 1D vector of values for 4 index electron
     *  repulsion integrals from a file
     *
     *  idx1  idx2 idx3 idx4  value
     ******************************************************/
    // Set up the vector we're going to populate
    int M = (length*(length+1))/2;
    int max_ijkl = (M*(M+1))/2;
    Eigen::VectorXd ee_vector(max_ijkl+1);

    string line;
    int i, j, k, l;
    double value;

    ifstream input_file(filename);

    // Iterate through the file line by line
    while (getline(input_file, line, '\n')) {

        // Ignore any blank lines
        if (line.empty()) {
            continue;
        }
        vector<string> items = utils::split(line, ' ');
        if (items.size() != 5){
            throw runtime_error("Unexpected line length");
        }

        // Indexes from 1
        i = stoi(items[0]) - 1;
        j = stoi(items[1]) - 1;
        k = stoi(items[2]) - 1;
        l = stoi(items[3]) - 1;

        value = stod(items[4]);

        if (i < j){
            swap(i, j);
        }
        if (k < l){
            swap(k, l);
        }

        int ij = i * (i + 1) / 2 + j;
        int kl = k * (k + 1) / 2 + l;

        if (ij < kl){
            swap(ij, kl);
        }
        int idx = ij*(ij+1)/2+kl;

        if (idx >= ee_vector.size()){
            throw runtime_error("Index not settable, exceeded the "
                                "size of the array");
        }
        ee_vector(idx) = value;
    }
    return ee_vector;
}


#include <gsl/gsl_integration.h>

#include <algorithm>

#include <Eigen/Dense>

#include "GaussLegendre2d.hh"
#include "TypesFunctions.hh"

using namespace Eigen;

void GaussLegendre2d::init() {
  size_t npoints = 0;
  for (size_t n: m_xorders) {
    npoints += n;
  }
  m_xpoints.resize(npoints);
  m_xweights.resize(npoints);
  size_t k = 0;
  for (size_t i = 0; i < m_xorders.size(); ++i) {
    size_t n = m_xorders[i];
    auto *t = gsl_integration_glfixed_table_alloc(n);

    double a = m_xedges[i];
    double b = m_xedges[i+1];

    for (size_t j = 0; j < t->n; ++j) {
      gsl_integration_glfixed_point(a, b, j, &m_xpoints[k], &m_xweights[k], t);
      k++;
    }
    gsl_integration_glfixed_table_free(t);
  }

  auto *t = gsl_integration_glfixed_table_alloc(m_yorder);
  m_ypoints.resize(t->n);
  m_yweights.resize(t->n);
  for (size_t i = 0; i < t->n; ++i) {
    gsl_integration_glfixed_point(m_ymin, m_ymax, i,
                                  &m_ypoints[i], &m_yweights[i], t);
  }
  gsl_integration_glfixed_table_free(t);

  transformation_("points")
    .output("x")
    .output("y")
    .output("xedges")
    .output("xhist")
    .types([](GaussLegendre2d *obj, TypesFunctionArgs& fargs) {
        auto& rets=fargs.rets;
        rets[0] = DataType().points().shape(obj->m_xpoints.size());
        rets[1] = DataType().points().shape(obj->m_ypoints.size());
        rets[2] = DataType().points().shape(obj->m_xedges.size());
        rets[3] = DataType().hist().edges(obj->m_xedges);
      })
    .func([](GaussLegendre2d *obj, FunctionArgs& fargs) {
        auto& rets=fargs.rets;
        rets[0].x = Eigen::Map<const Eigen::ArrayXd>(&obj->m_xpoints[0], obj->m_xpoints.size());
        rets[1].x = Eigen::Map<const Eigen::ArrayXd>(&obj->m_ypoints[0], obj->m_ypoints.size());
        rets[2].x = Eigen::Map<const Eigen::ArrayXd>(&obj->m_xedges[0], obj->m_xedges.size());
        rets.untaint();
        rets.freeze();
      })
    .finalize()
    ;
}

GaussLegendre2dHist::GaussLegendre2dHist(const GaussLegendre2d *base)
  : m_base(base)
{
  transformation_("hist")
    .input("f")
    .output("hist")
    .types(TypesFunctions::ifSame, [](GaussLegendre2dHist *obj, TypesFunctionArgs& fargs) {
        fargs.rets[0] = DataType().hist().bins(obj->m_base->m_xorders.size()).edges(obj->m_base->m_xedges);
      })
    .func([](GaussLegendre2dHist *obj, FunctionArgs& fargs) {
        auto& args=fargs.args;
        auto& rets=fargs.rets;
        size_t shape[2];
        shape[0] = obj->m_base->m_xpoints.size();
        shape[1] = obj->m_base->m_ypoints.size();
        for (size_t k = 0; k < rets.size(); ++k) {
          Map<const ArrayXXd, Aligned> pts(args[k].x.data(), shape[0], shape[1]);
          auto &xw = obj->m_base->m_xweights, &yw = obj->m_base->m_yweights;
          ArrayXd prod = (pts.rowwise()*yw.transpose()).rowwise().sum()*xw;
          auto *data = prod.data();
          for (size_t i = 0; i < obj->m_base->m_xorders.size(); ++i) {
            size_t n = obj->m_base->m_xorders[i];
            rets[k].x(i) = std::accumulate(data, data+n, 0.0);
            data += n;
          }
        }
      })
    ;
}