/**
 * @file    pose_estimation_3d3d.cpp
 * @version v1.0.0
 * @date    Nov,6 2017
 * @author  jacob.lin
 */

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

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

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

/*  */
void pose_estimation_3d3d(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/*  */
void bundleAdjustment(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/* g2o edge */
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map(_point);
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 1) = -z;
        _jacobianOplusXi(0, 2) = y;
        _jacobianOplusXi(0, 3) = -1;
        _jacobianOplusXi(0, 4) = 0;
        _jacobianOplusXi(0, 5) = 0;

        _jacobianOplusXi(1, 0) = z;
        _jacobianOplusXi(1, 1) = 0;
        _jacobianOplusXi(1, 2) = -x;
        _jacobianOplusXi(1, 3) = 0;
        _jacobianOplusXi(1, 4) = -1;
        _jacobianOplusXi(1, 5) = 0;

        _jacobianOplusXi(2, 0) = -y;
        _jacobianOplusXi(2, 1) = x;
        _jacobianOplusXi(2, 2) = 0;
        _jacobianOplusXi(2, 3) = 0;
        _jacobianOplusXi(2, 4) = 0;
        _jacobianOplusXi(2, 5) = -1;
    }

    bool read(std::istream& in) {}
    bool write(std::ostream & out) const {}

protected:
    Eigen::Vector3d _point;
};

int main(int argc, char** argv)
{
    if (argc != 5) {
        cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);
    if (img_1.empty() || img_2.empty()) {
        cerr << "img1 or img2 is empty." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat depth1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat depth2 = cv::imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);
    if (depth1.empty() || depth2.empty()) {
        cerr << "depth1 or depth2 is empty." << endl;
        return 1;
    }

    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts1, pts2;

    for (cv::DMatch m : matches) {
        ushort d1 = depth1.ptr<unsigned short>((int)keypoints_1[m.queryIdx].pt.y)[(int)keypoints_1[m.queryIdx].pt.x];
        ushort d2 = depth2.ptr<unsigned short>((int)keypoints_2[m.trainIdx].pt.y)[(int)keypoints_2[m.trainIdx].pt.x];
        if (d1 == 0 || d2 == 0) {
            continue;
        }
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        cv::Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1) / 5000.0;
        float dd2 = float(d2) / 5000.0;
        pts1.push_back(cv::Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(cv::Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    cout << "3d-3d pairs: " << pts1.size() << endl;

    cv::Mat R, t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout << "ICP vid SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;
    cout << "R_inv = " << R.t() << endl;
    cout << "t_inv = " << -R.t() * t << endl;

    cout << "calling bundle adjustment. \r\n";

    bundleAdjustment(pts1, pts2, R, t);

    // verify p1 = R*p2 + t
    for (int i = 0; i < 5; i ++) {
        cout << "p1 = " << pts1[i] << endl;
        cout << "p2 = " << pts2[i] << endl;
        cout << "R*p2 + t = " << R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout << endl;
    }

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void pose_estimation_3d3d(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // center of mass
    cv::Point3f p1, p2;
    int N = pts1.size();
    for (int i = 0; i < N; i ++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = cv::Point3f(Vec3f(p1) / N);
    p2 = cv::Point3f(Vec3f(p2) / N);

    // remove the center
    std::vector<cv::Point3f> q1(N), q2(N);
    for (int i = 0; i < N; i ++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i ++) {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    cout << "W = " << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 利用SVD求解3D-3D变换，需要U和V的行列式同号。换言之，旋转矩阵的行列式只能为1，不能为-1
    if (U.determinant() * V.determinant() < 0) {
        for (int x = 0; x < 3; ++x) {
            U(x, 2) *= -1;
        }
    }
    cout << "U = " << U << endl;
    cout << "V = " << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::mat
    R = (cv::Mat_<double>(3, 3) <<
        R_(0, 0), R_(0, 1), R_(0, 2),
        R_(1, 0), R_(1, 1), R_(1, 2),
        R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

void bundleAdjustment(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // Initialize g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;   // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType* linear_solver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();   // 线性方程求解器
    Block* solver_ptr = new Block((std::unique_ptr<Block::LinearSolverType>) linear_solver);
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton((std::unique_ptr<Block>) solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();    // camera pose
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0)));
    optimizer.addVertex(pose);

    // edges
    int index = 1;
    std::vector<EdgeProjectXYZRGBDPoseOnly*> edges;
    for (size_t i = 0; i < pts1.size(); i ++) {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*>(pose));
        edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity() * 1e4);
        optimizer.addEdge(edge);
        index ++;
        edges.push_back(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << "\r\n";

    cout << endl << "after optimization: " << endl;
    cout << "T = " << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "expsum/exponential_sum.hpp"
#include "expsum/kernel_functions/sph_bessel_kernel.hpp"
// #include "expsum/fitting/fast_esprit.hpp"
#include <boost/math/special_functions/bessel.hpp>

using size_type   = arma::uword;
using expsum_type = expsum::exponential_sum<double, std::complex<double>>;

template <typename F, typename Vec>
void make_sample(F f, double xmin, double xmax, Vec& result)
{
    auto np = result.n_elem;
    auto h  = (xmax - xmin) / (np - 1);
    for (size_type n = 0; n < np; ++n)
    {
        result(n) = f(xmin + n * h);
    }

    return;
}

// expsum_type sph_bessel_kernel(int l, double xmax, size_type N, size_type L,
//                               size_type M, double eps)
// {
//     using esprit_type = expsum::fast_esprit<double>;
//     using vector_type = arma::Col<double>;

//     vector_type exact(N);
//     make_sample([=](double x) { return boost::math::sph_bessel(l, x); }, 0.0,
//                 xmax, exact);
//     auto delta = xmax / (N - 1);

//     esprit_type esprit(N, L, M);

//     esprit.fit(exact, 0.0, delta, eps);
//     expsum_type ret(esprit.exponents(), esprit.weights());

//     ret.remove_small_terms(eps / 100.0);

//     return ret;
// }

expsum_type sph_bessel_kernel(int l, double xmax, double eps)
{
    // using vector_type = arma::Col<double>;
    expsum::sph_bessel_kernel<double> body;
    body.compute(l, xmax, eps);

    expsum_type ret(body.exponents(), body.weights());

    return ret;
}

void sph_bessel_kernel_error(int l, double xmax, size_type n_samples,
                             const expsum_type& ret)
{
    using vector_type = arma::Col<double>;

    vector_type x(arma::linspace<vector_type>(0.0, xmax, n_samples));
    vector_type exact(n_samples), approx(n_samples);

    for (size_type i = 0; i < n_samples; ++i)
    {
        exact(i)    = boost::math::sph_bessel(l, x(i));
        approx(i)   = ret(x(i));
        auto abserr = std::abs(exact(i) - approx(i));
        std::cout << std::setw(24) << x(i) << std::setw(24) << exact(i)
                  << std::setw(24) << approx(i) << std::setw(24) << abserr
                  << '\n';
    }

    vector_type abserr(arma::abs(exact - approx));

    size_type imax = abserr.index_max();

    std::cout << "\n  abs. error in interval [0," << xmax << "]\n"
              << "    maximum : " << abserr(imax) << '\n'
              << "    averaged: " << arma::sum(abserr) / n_samples << '\n';
}

int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    const int lmax = 2;

    size_type N = 100001; // # of sampling points
    // size_type L = N / 2;       // window length
    // size_type M = 1000;        // max # of terms
    double xmax = 1.0e12;
    double eps  = 1.0e-12;

    expsum_type ret;

    std::cout
        << "# Approximation of spherical Bessel function by exponential sum\n";

    // for (int l = 0; l <= lmax; ++l)
    // {
    //     std::cout << "\n# --- order " << l << '\n';
    //     ret = sph_bessel_kernel(l, xmax, eps);
    //     ret.print(std::cout);

    //     sph_bessel_kernel_error(l, 10.0, N, ret);
    //     sph_bessel_kernel_error(l, 1.0e8, N, ret);
    // }

    // for (int l = 0; l <= lmax; ++l)
    // {
    //     std::cout << "\n# --- order " << l << '\n';
    //     sph_bessel_kernel(l, xmax, eps);
    // }

    std::cout << "\n# --- order " << lmax << '\n';
    ret = sph_bessel_kernel(1, xmax, eps);
    ret.print(std::cout);
    std::cout << "sum weights: " << arma::sum(ret.weight()) << std::endl;
    sph_bessel_kernel_error(1, 1.0, 100001, ret);
    // sph_bessel_kernel_error(1, xmax, N, ret);

    return 0;
}


// Copyright Nick Thompson, 2017
// Use, modification and distribution are 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)

#define BOOST_TEST_MODULE test_cubic_b_spline

#include <random>
#include <functional>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/math/interpolators/detail/cardinal_cubic_b_spline_detail.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::multiprecision::cpp_bin_float_50;
using boost::math::constants::third;
using boost::math::constants::half;

template<class Real>
void test_b3_spline()
{
    std::cout << "Testing evaluation of spline basis functions on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    // Outside the support:
    Real eps = std::numeric_limits<Real>::epsilon();
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline<Real>(2.5), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline<Real>(-2.5), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_prime<Real>(2.5), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_prime<Real>(-2.5), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_double_prime<Real>(2.5), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_double_prime<Real>(-2.5), (Real) 0);

    // On the boundary of support:
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline<Real>(2), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline<Real>(-2), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_prime<Real>(2), (Real) 0);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_prime<Real>(-2), (Real) 0);

    // Special values:
    BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline<Real>(-1), third<Real>()*half<Real>(), eps);
    BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline<Real>( 1), third<Real>()*half<Real>(), eps);
    BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline<Real>(0), 2*third<Real>(), eps);

    BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline_prime<Real>(-1), half<Real>(), eps);
    BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline_prime<Real>( 1), -half<Real>(), eps);
    BOOST_CHECK_SMALL(boost::math::interpolators::detail::b3_spline_prime<Real>(0), eps);

    // Properties: B3 is an even function, B3' is an odd function.
    for (size_t i = 1; i < 200; ++i)
    {
        Real arg = i*0.01;
        BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline<Real>(arg), boost::math::interpolators::detail::b3_spline<Real>(arg), eps);
        BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline_prime<Real>(-arg), -boost::math::interpolators::detail::b3_spline_prime<Real>(arg), eps);
        BOOST_CHECK_CLOSE(boost::math::interpolators::detail::b3_spline_double_prime<Real>(-arg), boost::math::interpolators::detail::b3_spline_double_prime<Real>(arg), eps);
    }

}
/*
 * This test ensures that the interpolant s(x_j) = f(x_j) at all grid points.
 */
template<class Real>
void test_interpolation_condition()
{
    using std::sqrt;
    std::cout << "Testing interpolation condition for cubic b splines on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::random_device rd;
    std::mt19937 gen(rd());
    boost::random::uniform_real_distribution<Real> dis(1, 10);
    std::vector<Real> v(5000);
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = dis(gen);
    }

    Real step = 0.01;
    Real a = 5;
    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), a, step);

    for (size_t i = 0; i < v.size(); ++i)
    {
        Real y = spline(i*step + a);
        // This seems like a very large tolerance, but I don't know of any other interpolators
        // that will be able to do much better on random data.
        BOOST_CHECK_CLOSE(y, v[i], 10000*sqrt(std::numeric_limits<Real>::epsilon()));
    }

}

template<class Real>
void test_constant_function()
{
    std::cout << "Testing that constants are interpolated correctly by cubic b splines on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::vector<Real> v(500);
    Real constant = 50.2;
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = 50.2;
    }

    Real step = 0.02;
    Real a = 5;
    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), a, step);

    for (size_t i = 0; i < v.size(); ++i)
    {
        // Do not test at interpolation point; we already know it works there:
        Real y = spline(i*step + a + 0.001);
        BOOST_CHECK_CLOSE(y, constant, 10*std::numeric_limits<Real>::epsilon());
        Real y_prime = spline.prime(i*step + a + 0.002);
        BOOST_CHECK_SMALL(y_prime, 5000*std::numeric_limits<Real>::epsilon());
        Real y_double_prime = spline.double_prime(i*step + a + 0.002);
        BOOST_CHECK_SMALL(y_double_prime, 5000*std::numeric_limits<Real>::epsilon());

    }

    // Test that correctly specified left and right-derivatives work properly:
    spline = boost::math::interpolators::cardinal_cubic_b_spline<Real>(v.data(), v.size(), a, step, 0, 0);

    for (size_t i = 0; i < v.size(); ++i)
    {
        Real y = spline(i*step + a + 0.002);
        BOOST_CHECK_CLOSE(y, constant, std::numeric_limits<Real>::epsilon());
        Real y_prime = spline.prime(i*step + a + 0.002);
        BOOST_CHECK_SMALL(y_prime, std::numeric_limits<Real>::epsilon());
    }

    //
    // Again with iterator constructor:
    //
    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline2(v.begin(), v.end(), a, step);

    for (size_t i = 0; i < v.size(); ++i)
    {
       // Do not test at interpolation point; we already know it works there:
       Real y = spline2(i*step + a + 0.001);
       BOOST_CHECK_CLOSE(y, constant, 10 * std::numeric_limits<Real>::epsilon());
       Real y_prime = spline2.prime(i*step + a + 0.002);
       BOOST_CHECK_SMALL(y_prime, 5000 * std::numeric_limits<Real>::epsilon());
    }

    // Test that correctly specified left and right-derivatives work properly:
    spline2 = boost::math::interpolators::cardinal_cubic_b_spline<Real>(v.begin(), v.end(), a, step, 0, 0);

    for (size_t i = 0; i < v.size(); ++i)
    {
       Real y = spline2(i*step + a + 0.002);
       BOOST_CHECK_CLOSE(y, constant, std::numeric_limits<Real>::epsilon());
       Real y_prime = spline2.prime(i*step + a + 0.002);
       BOOST_CHECK_SMALL(y_prime, std::numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_affine_function()
{
    using std::sqrt;
    std::cout << "Testing that affine functions are interpolated correctly by cubic b splines on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::vector<Real> v(500);
    Real a = 10;
    Real b = 8;
    Real step = 0.005;

    auto f = [a, b](Real x) { return a*x + b; };
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = f(i*step);
    }

    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), 0, step);

    for (size_t i = 0; i < v.size() - 1; ++i)
    {
        Real arg = i*step + 0.0001;
        Real y = spline(arg);
        BOOST_CHECK_CLOSE(y, f(arg), sqrt(std::numeric_limits<Real>::epsilon()));
        Real y_prime = spline.prime(arg);
        BOOST_CHECK_CLOSE(y_prime, a, 100*sqrt(std::numeric_limits<Real>::epsilon()));
    }

    // Test that correctly specified left and right-derivatives work properly:
    spline = boost::math::interpolators::cardinal_cubic_b_spline<Real>(v.data(), v.size(), 0, step, a, a);

    for (size_t i = 0; i < v.size() - 1; ++i)
    {
        Real arg = i*step + 0.0001;
        Real y = spline(arg);
        BOOST_CHECK_CLOSE(y, f(arg), sqrt(std::numeric_limits<Real>::epsilon()));
        Real y_prime = spline.prime(arg);
        BOOST_CHECK_CLOSE(y_prime, a, 100*sqrt(std::numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_quadratic_function()
{
    using std::sqrt;
    std::cout << "Testing that quadratic functions are interpolated correctly by cubic b splines on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::vector<Real> v(500);
    Real a = 1.2;
    Real b = -3.4;
    Real c = -8.6;
    Real step = 0.01;

    auto f = [a, b, c](Real x) { return a*x*x + b*x + c; };
    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = f(i*step);
    }

    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), 0, step);

    for (size_t i = 0; i < v.size() -1; ++i)
    {
        Real arg = i*step + 0.001;
        Real y = spline(arg);
        BOOST_CHECK_CLOSE(y, f(arg), 0.1);
        Real y_prime = spline.prime(arg);
        BOOST_CHECK_CLOSE(y_prime, 2*a*arg + b, 2.0);
    }
}

template<class Real>
void test_trig_function()
{
    std::cout << "Testing that sine functions are interpolated correctly by cubic b splines on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::mt19937 gen;
    std::vector<Real> v(500);
    Real x0 = 1;
    Real step = 0.125;

    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = sin(x0 + step * i);
    }

    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), x0, step);

    boost::random::uniform_real_distribution<Real> absissa(x0, x0 + 499 * step);

    for (size_t i = 0; i < v.size(); ++i)
    {
        Real x = absissa(gen);
        Real y = spline(x);
        BOOST_CHECK_CLOSE(y, sin(x), 1.0);
        auto y_prime = spline.prime(x);
        BOOST_CHECK_CLOSE(y_prime, cos(x), 2.0);
    }
}

template<class Real>
void test_copy_move()
{
    std::cout << "Testing that copy/move operation succeed on cubic b spline\n";
    std::vector<Real> v(500);
    Real x0 = 1;
    Real step = 0.125;

    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = sin(x0 + step * i);
    }

    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), x0, step);

    // Default constructor should compile so that splines can be member variables:
    boost::math::interpolators::cardinal_cubic_b_spline<Real> d;
    d = boost::math::interpolators::cardinal_cubic_b_spline<Real>(v.data(), v.size(), x0, step);
    BOOST_CHECK_CLOSE(d(x0), sin(x0), 0.01);
    // Passing to lambda should compile:
    auto f = [=](Real x) { return d(x); };
    // Make sure this variable is used.
    BOOST_CHECK_CLOSE(f(x0), sin(x0), 0.01);

    // Move operations should compile.
    auto s = std::move(spline);

    // Copy operations should compile:
    boost::math::interpolators::cardinal_cubic_b_spline<Real> c = d;
    BOOST_CHECK_CLOSE(c(x0), sin(x0), 0.01);

    // Test with std::bind:
    auto h = std::bind(&boost::math::interpolators::cardinal_cubic_b_spline<double>::operator(), &s, std::placeholders::_1);
    BOOST_CHECK_CLOSE(h(x0), sin(x0), 0.01);
}

template<class Real>
void test_outside_interval()
{
    std::cout << "Testing that the spline can be evaluated outside the interpolation interval\n";
    std::vector<Real> v(400);
    Real x0 = 1;
    Real step = 0.125;

    for (size_t i = 0; i < v.size(); ++i)
    {
        v[i] = sin(x0 + step * i);
    }

    boost::math::interpolators::cardinal_cubic_b_spline<Real> spline(v.data(), v.size(), x0, step);

    // There's no test here; it simply does it's best to be an extrapolator.
    //
    std::ostream cnull(0);
    cnull << spline(0);
    cnull << spline(2000);
}

BOOST_AUTO_TEST_CASE(test_cubic_b_spline)
{
    test_b3_spline<float>();
    test_b3_spline<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_b3_spline<long double>();
#endif
    test_b3_spline<cpp_bin_float_50>();

    test_interpolation_condition<float>();
    test_interpolation_condition<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_interpolation_condition<long double>();
#endif
    test_interpolation_condition<cpp_bin_float_50>();

    test_constant_function<float>();
    test_constant_function<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_constant_function<long double>();
#endif
    test_constant_function<cpp_bin_float_50>();

    test_affine_function<float>();
    test_affine_function<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_affine_function<long double>();
#endif
    test_affine_function<cpp_bin_float_50>();

    test_quadratic_function<float>();
    test_quadratic_function<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_quadratic_function<long double>();
#endif
    test_affine_function<cpp_bin_float_50>();

    test_trig_function<float>();
    test_trig_function<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_trig_function<long double>();
#endif
    test_trig_function<cpp_bin_float_50>();

    test_copy_move<double>();
    test_outside_interval<double>();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/compute/function.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/buffer_iterator.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace compute = boost::compute;

int main()
{
    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);

    std::cout << "device: " << gpu.name() << std::endl;

    using compute::uint_;
    using compute::uint2_;

    // ten million random points
    size_t n = 10000000;

    // generate random numbers
    compute::default_random_engine rng(queue);
    compute::vector<uint_> vector(n * 2, context);
    rng.generate(vector.begin(), vector.end(), queue);

    // function returing true if the point is within the unit circle
    BOOST_COMPUTE_FUNCTION(bool, is_in_unit_circle, (const uint2_ point),
    {
        const float x = point.x / (float) UINT_MAX - 1;
        const float y = point.y / (float) UINT_MAX - 1;

        return (x*x + y*y) < 1.0f;
    });

    // iterate over vector<uint> as vector<uint2>
    compute::buffer_iterator<uint2_> start =
        compute::make_buffer_iterator<uint2_>(vector.get_buffer(), 0);
    compute::buffer_iterator<uint2_> end =
        compute::make_buffer_iterator<uint2_>(vector.get_buffer(), vector.size() / 2);

    // count number of random points within the unit circle
    size_t count = compute::count_if(start, end, is_in_unit_circle, queue);

    // print out values
    float count_f = static_cast<float>(count);
    std::cout << "count: " << count << " / " << n << std::endl;
    std::cout << "ratio: " << count_f / float(n) << std::endl;
    std::cout << "pi = " << (count_f / float(n)) * 4.0f << std::endl;

    return 0;
}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 Soumyajit De
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <iostream>
#include <tesseract/regression/LeastSquares.hpp>
#include <Eigen/Eigen>

using namespace tesseract;
using namespace Eigen;
using namespace std;

template <class Matrix, class Vector>
void test_svd(const Matrix& A, const Vector& b, Vector& x)
{
	LeastSquares<double, LS_SVD> ls;
	ls.solve(A, b, x);
//	cout << "beta = " << endl << x << endl;
//	cout << "residual = " << b-A*x << endl;
}

template <class Matrix, class Vector>
void test_qr(const Matrix& A, const Vector& b, Vector& x)
{
	LeastSquares<double, LS_QR> ls;
	ls.solve(A, b, x);
//	cout << "beta = " << endl << x << endl;
//	cout << "residual = " << b-A*x << endl;
}

template <class Matrix, class Vector>
void test_normal(Matrix& A, Vector& b, Vector& x)
{
	LeastSquares<double, LS_NORMAL> ls;
	ls.solve(A, b, x);
//	cout << "beta = " << endl << x << endl;
//	cout << "residual = " << b-A*x << endl;
}

int main(int argc, char** argv)
{
	MatrixXd A = MatrixXd::Random(3, 2);
	VectorXd b = VectorXd::Random(3);
	VectorXd res = VectorXd::Zero(2);

//	cout << "A = " << endl << A << endl;
//	cout << "b = " << endl << b << endl;

	test_svd(A, b, res);
	test_qr(A, b, res);
	test_normal(A, b, res);

	Map<MatrixXd> map_A(A.data(), A.rows(), A.cols());
	Map<VectorXd> map_b(b.data(), b.rows());
	Map<VectorXd> map_res(res.data(), res.rows());

	test_svd(map_A, map_b, map_res);
	test_qr(map_A, map_b, map_res);
	test_normal(map_A, map_b, map_res);

	return 0;
}


#include <iostream>

#include <Eigen/Dense>

int main (int argc, char *argv[]){
    Eigen::MatrixXf m(2, 2);
    m << 1, 2, 3, 4;
    Eigen::VectorXf v(2);
    v << -1, 1;
    auto r = m * v;
    std::cout << r(0) << " " << r(1) << std::endl;
    return 0;
}


#include <ctime>
#include <fftw3.h>
#include <string>
#include <algorithm>
#include <iostream>
#include <random>
#include <math.h>
#include <vtkNew.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkCellArray.h>
#include <vtkIdList.h>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include "shtns.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){

    // Read the polydata
    vtkNew<vtkPolyDataReader> reader;
    reader->SetFileName("T7.vtk");
    reader->Update();
    auto pointCloud = reader->GetOutput();
    int N = pointCloud->GetNumberOfPoints();

    //**********************************************************************//
    //SHTns: Generate quadrature points
    int lmax = std::floor( sqrt(N) - 1 );
    int mmax = lmax, nlat = 0, nphi = 0, mres = 1;
    shtns_cfg shtns;
    shtns_verbose(1);
    shtns_norm norm = static_cast<shtns_norm>(SHT_REAL_NORM | SHT_NO_CS_PHASE);
    shtns = shtns_create(lmax, mmax, mres, norm );
    shtns_type flags = static_cast<shtns_type>( sht_gauss | SHT_NATIVE_LAYOUT);
    shtns_set_grid_auto(shtns, flags, 1e-14, 1, &nlat, &nphi );
    int NLM = shtns->nlm;
    int numQ = nphi*nlat;

    // Memory allocation
    double_t *f;
    f = static_cast<double_t*>(
	    fftw_malloc( NSPAT_ALLOC(shtns)*sizeof(double_t) ));
    std::complex<double_t> *Slm;
    Slm = static_cast<std::complex<double_t>*>(
	    fftw_malloc( NLM*sizeof(std::complex<double_t>) ));
    // Memory initialization
    for( auto lm = 0; lm < NLM; ++lm){
	Slm[lm] = 0.0;
    }

    // Calculate the quad point Q0
    Eigen::Matrix3Xd Q0(3,numQ);
    for( auto ip = 0; ip < nphi; ++ip){
	for( auto it = 0; it < nlat; ++it){
	    auto phi = ip*M_PI*2/nphi;
	    Q0.col(it + ip*nlat) << shtns->st[it]*std::cos(phi),
		shtns->st[it]*std::sin(phi), shtns->ct[it];
	}
    }

    //************************* Some handy functions ***********************//

    // A lambda function to write a Eigen::Matrix3Xd to VTK file
    auto writeMatToVTK =
	[](Eigen::Matrix3Xd M, std::string file, bool Proj = false){
	    vtkNew<vtkPoints> pts;
	    vtkNew<vtkCellArray> verts;
	    for( auto b = 0; b < M.cols(); ++b ){
		double x, y, z;
		x = M(0,b);
		y = M(1,b);
		z = Proj? 0.0 : M(2,b);
		pts->InsertNextPoint(x, y, z);
		verts->InsertNextCell( 1 );
		verts->InsertCellPoint( b );
	    }
	    vtkNew<vtkPolyData> poly;
	    poly->SetPoints( pts );
	    poly->SetVerts( verts );
	    vtkNew<vtkPolyDataWriter> wr;
	    wr->SetInputData( poly );
	    wr->SetFileName( file.c_str() );
	    wr->Write();
	};

    //**********************************************************************//
    clock_t t = clock();

    for( auto z = 0; z < 1000; ++z ){
	// Copy the point coordinates into a matrix
	Eigen::Map<Eigen::Matrix3Xd> points((double_t*)pointCloud->GetPoints()
		->GetData()->GetVoidPointer(0),3,N);

	// Project points to unit sphere
	points.colwise().normalize();

	// Reset the center of the sphere to origin by translating
	Vector3d center = points.rowwise().mean();
	points = points.colwise() - center;

	// Write original points to VTK
	//writeMatToVTK( points, "BasePoints.vtk" );

	// Rotate all points so that the point in 0th column is along z-axis
	Vector3d c = points.col(0);
	double_t cos_t = c(2);
	double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
	Vector3d axis;
	axis << c(1), -c(0), 0.;
	Matrix3d rotMat, axis_cross, outer;
	axis_cross << 0. , -axis(2), axis(1),
		   axis(2), 0., -axis(0),
		   -axis(1), axis(0), 0.;

	outer.noalias() = axis*axis.transpose();

	rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
	Matrix3Xd rPts(3,N);
	rPts = rotMat*points; // The points on a sphere rotated

	// Write the rotated points to VTK
	//writeMatToVTK( rPts, "RotBasePoints.vtk" );

	// Calculate the stereographic projections
	Vector3d p0;
	Map3Xd l0( &(rPts(0,1)), 3, N-1 );
	Matrix3Xd l(3,N-1), proj(3,N-1);
	p0 << 0,0,-1; // Point on the plane of projection
	c = rPts.col(0); // The point from which we are projecting
	l = (l0.colwise() - c).colwise().normalized(); // directions of projections
	for( auto j=0; j < N-1; ++j ){
	    proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
	}

	// Write the rotated points to VTK
	//writeMatToVTK( proj, "ProjBasePoints.vtk", true );

	// Insert the projected points in a CGAL vertex_with_info vector
	std::vector< std::pair< Point, unsigned> > verts;
	for( auto j=0; j < N-1; ++j ){
	    verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
	}

	Delaunay dt( verts.begin(), verts.end() );

	/*
	// Write the triangulation to file
	vtkNew<vtkCellArray> sphereTri;
	for( auto fc = dt.all_faces_begin(); fc != dt.all_faces_end(); ++fc ){
	sphereTri->InsertNextCell(3);
	for( auto zz = 2; zz >= 0; --zz ){
	auto vid = dt.is_infinite( fc->vertex( zz ) )? 0 :
	fc->vertex(zz)->info();
	sphereTri->InsertCellPoint(vid);
	}
	}
	pointCloud->SetPolys( sphereTri );
	vtkNew<vtkPolyDataWriter> wr;
	wr->SetInputData( pointCloud );
	wr->SetFileName( "Sphere.vtk" );
	wr->Write();
	*/

	// Create the input scalar field f(theta,phi)
	std::complex<double_t> Qlm[NLM];
	for(auto pp = 0; pp < NLM; ++pp)
	    Qlm[pp] = 0.0;
	Qlm[ LM(shtns, 0, 0) ] = std::complex<double_t>(0,0);
	Qlm[ LM(shtns, 1, 0) ] = std::complex<double_t>(2,0);
	Qlm[ LM(shtns, 1, 1) ] = std::complex<double_t>(0,3);
	Qlm[ LM(shtns, 7, 7) ] = std::complex<double_t>(7,0);

	Eigen::VectorXd finput(N);
	for(auto i = 0; i < N; ++i){
	    Eigen::Vector3d q;
	    q = points.col(i);
	    double_t x = q(0);
	    double_t y = q(1);
	    double_t z = q(2);
	    auto phi = std::atan2(y,x);
	    finput(i) = SH_to_point(shtns, Qlm, z, phi);
	}

	//Now we need to first identify which triangles each of the quadrature
	//points belongs to. For this, we need the coordinates of the points
	Eigen::Matrix3Xd Qr(3,numQ);// Rotated quadrature points
	Eigen::Matrix3Xd Qsp(3,numQ);// StereoProjection of quadrature points

	// Write Quadrature points to VTK file
	//writeMatToVTK( Q0, "QuadPointsOrig.vtk" );

	// Now we need to rotate and project the quadrature points
	// p0 is already defined as the z = -1 plane
	// c is the point from which we are projecting (calculated earlier)
	Qr = rotMat*Q0; // Rotate the quadrature points

	// Write the rotated points to VTK
	//writeMatToVTK( Qr, "RotQuadPoints.vtk" );

	Eigen::Matrix3Xd lQ(3,numQ);
	lQ = (Qr.colwise() - c).colwise().normalized();//projn dirn unit vectors
	for( auto j=0; j < numQ; ++j ){
	    Qsp.col(j) = ((p0(2) - Qr(2,j))/lQ(2,j))*lQ.col(j) + Qr.col(j);
	}

	// Write Stereographic projections of quadrature points to VTK file
	//writeMatToVTK( Qsp, "ProjQuadPoints.vtk", true );

	//Finally let's try to locate these points in the Triangulation and
	//interpolate it as per the weights
	Eigen::Map<Eigen::VectorXd> fquad(f,numQ);
	auto interpolate = [&points, &Q0, &finput, &fquad](const size_t i,
		const size_t j, const size_t k, const size_t q){
	    Eigen::Vector3d v0 = points.col(i);
	    Eigen::Vector3d v1 = points.col(j);
	    Eigen::Vector3d v2 = points.col(k);
	    Eigen::Vector3d qp = Q0.col(q);
	    auto A0 = ((v1-qp).cross((v2-qp))).norm();
	    auto A1 = ((v2-qp).cross((v0-qp))).norm();
	    auto A2 = ((v0-qp).cross((v1-qp))).norm();
	    auto A = A0 + A1 + A2;
	    fquad(q) = (A0/A)*finput(i) + (A1/A)*finput(j) + (A2/A)*finput(k);
	};
	for( auto j=0; j < numQ; ++j ){
	    auto query = Point( Qsp(0,j), Qsp(1,j) );
	    Delaunay::Locate_type lt;
	    int li;
	    Face_handle face = dt.locate( query, lt, li );
	    switch(lt){
		case Delaunay::FACE:
		    {
			auto id0 = face->vertex(0)->info();
			auto id1 = face->vertex(1)->info();
			auto id2 = face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		case Delaunay::EDGE:
		    {
			auto id1 = face->vertex( (li + 1)%3 )->info();
			auto id2 = face->vertex( (li + 2)%3 )->info();
			Eigen::Vector3d v1, v2, qp;
			v1 = points.col(id1);
			v2 = points.col(id2);
			qp = Q0.col(j);
			double_t ratio = (qp - v1).norm()/(qp - v2).norm();
			fquad(j) = (finput(id1) + ratio*finput(id2))/(1 + ratio);
			break;
		    }
		case Delaunay::VERTEX:
		    fquad(j) = finput( face->vertex( li )->info() );
		    break;
		case Delaunay::OUTSIDE_CONVEX_HULL:
		    {
			Eigen::Vector3d v0, v1, v2;
			auto id0 = dt.is_infinite(face->vertex(0))?
			    0 : face->vertex(0)->info();
			auto id1 = dt.is_infinite(face->vertex(1))?
			    0 : face->vertex(1)->info();
			auto id2 = dt.is_infinite(face->vertex(2))?
			    0 : face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		default:
		    std::cout<< "Point " << j << " not found!" << std::endl;
	    }
	}

	/*
	   std::cout<< "Print interpolated values and actual values..."<<std::endl;
	   Eigen::VectorXd direct(numQ);
	   for(auto i = 0; i < numQ; ++i){
	   Eigen::Vector3d q;
	   q = Q0.col(i);
	   double_t x = q(0);
	   double_t y = q(1);
	   double_t z = q(2);
	   auto phi = std::atan2(y,x);
	   direct(i) = SH_to_point(shtns, Qlm, z, phi);
	   std::cout<< fquad(i) << " " << direct(i) << std::endl;
	   }
	   */

	// Now time for spherical harmonic analysis -- find the coefficients flm
	spat_to_SH(shtns, f, Slm);

	/*
	   std::cout<< "Printing the coefficients..." << std::endl;
	// Print the spherical harmonic coefficients
	for( auto l = 0; l <= lmax; ++l ){
	for( auto m = -l; m <= l; ++m ){
	std::complex<double_t> flm;
	if( m < 0){
	auto factor = (-m%2 == 0)? 1.0 : -1.0;
	flm = factor*std::conj(Slm[ LM(shtns,l,-m) ]);
	}
	else{
	flm = Slm[ LM(shtns,l,m) ];
	}
	std::cout<< l << " " << m << " " << flm << std::endl;
	}
	}
	*/
    }
    std::cout<< "Time for 1000 steps = "
	<< ((float)(clock() - t))/CLOCKS_PER_SEC << std::endl;
    for( auto lm = 0; lm < NLM; ++lm){
	std::cout<< Slm[ lm ] << std::endl;
    }
    return 0;
}


// -*- coding: utf-8 -*-

#define _USE_MATH_DEFINES
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <chrono>
#include <assert.h>

double slp_laplace(const Eigen::Vector2d &xm, const Eigen::Vector2d &ym, const Eigen::Vector2d &y1,
		   const Eigen::Vector2d &y2, const double &hy, const Eigen::Vector2d &yn){
//  Eigen::Vector2d ym;
  Eigen::Vector2d rr1;
  Eigen::Vector2d rr2;
  double r1;
  double r2;
  Eigen::Vector2d yt;
  double uk1, uk2, uk3, uk4, uk5;
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0));

//  ym = 0.5*(y1 + y2);

  if((xm - ym).squaredNorm() < 1.0e-8){
    return hy*(1.0 - std::log(hy*0.5))*arctwopi;
  }
  else{
    rr1 = xm - y1;
    rr2 = xm - y2;
    r1 = rr1.norm();
    r2 = rr2.norm();
    yt(0) = -yn(1);
    yt(1) = yn(0);

    uk1 = rr1.dot(yn);
    uk2 = rr1.dot(yt);
    uk3 = rr2.dot(yn);
    uk4 = rr2.dot(yt);
    uk5 = std::atan2(uk3, uk4) - std::atan2(uk1, uk2);
    return (uk4*std::log(r2) - uk2*std::log(r1) + hy - uk1*uk5)*arctwopi;
//    std::cout << "tes3 " << std::endl; //dbg
  }
}

double dlp_laplace(const Eigen::Vector2d &xm, const Eigen::Vector2d &ym, const Eigen::Vector2d &y1,
		   const Eigen::Vector2d &y2, const double &hy, const Eigen::Vector2d &yn,
		   const int &exterior){

//  Eigen::Vector2d ym;
  Eigen::Vector2d rr1;
  Eigen::Vector2d rr2;
  double r1;
  double r2;
  Eigen::Vector2d yt;
  double uk1, uk2, uk3, uk4;
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0));

  //  ym = 0.5*(y1 + y2);

  if((xm - ym).squaredNorm() < 1.0e-8){
    if(exterior == 0){
      return 0.5;
    }else if(exterior == 1){
      return -0.5;
    }else{
      assert(false);
    }
  }
  else{
    rr1 = xm - y1;
    rr2 = xm - y2;
    r1 = rr1.norm();
    r2 = rr2.norm();
    yt(0) = -yn(1);
    yt(1) = yn(0);

    uk1 = rr1.dot(yn);
    uk2 = rr1.dot(yt);
    uk3 = rr2.dot(yn);
    uk4 = rr2.dot(yt);
    return arctwopi*(std::atan2(uk3, uk4) - std::atan2(uk1, uk2));
  }
}

int main(){
  int n;
  double rad;
  std::ifstream fr("input");
  fr >> n >> rad;
  std::cout << "n " << n << std::endl;
  std::cout << "rad " << rad << std::endl;

  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(2, n);
  Eigen::MatrixXd xn = Eigen::MatrixXd::Zero(2, n);
  Eigen::MatrixXi edge = Eigen::MatrixXi::Zero(2, n);
  Eigen::VectorXd hs = Eigen::VectorXd::Zero(n);
  Eigen::MatrixXd slp = Eigen::MatrixXd::Zero(n, n);
  Eigen::MatrixXd dlp = Eigen::MatrixXd::Zero(n, n);
  Eigen::VectorXd u = Eigen::VectorXd::Zero(n);
  Eigen::VectorXd kai = Eigen::VectorXd::Zero(n);
  Eigen::VectorXd xm = Eigen::VectorXd::Zero(2);
  Eigen::VectorXd ym = Eigen::VectorXd::Zero(2);

  constexpr double pi = std::acos(-1.0);

  int i;
  int j;
  double th;
  int i0;
  int i1;
  int j0;
  int j1;
  int exterior = 0;

  for(i = 0; i < n; i++){
    th = 2.0*pi*(i + 1.5)/n;
    x(0, i) = rad*cos(th);
    x(1, i) = rad*sin(th);
    edge(0, i) = i;
    edge(1, i) = (i + 1) % n;
  }

  Eigen::Vector2d rr1; //dbg
  Eigen::Vector2d rr2; //dbg
  double r1; //dbg
  double r2; //dbg
  Eigen::Vector2d yt; //dbg
  double uk1, uk2, uk3, uk4, uk5; //dbg
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0)); //dbg

  for(i = 0; i < n; i++){
    i0 = edge(0, i);
    i1 = edge(1, i);
    xm = 0.5*(x.col(i0) + x.col(i1));

    if((i0 >= 0 && i0 < n) && (i1 >= 0 && i1 < n)){
      xn(0, i) = x(1, i1) - x(1, i0);
      xn(1, i) = x(0, i0) - x(0, i1);

      hs(i) = std::sqrt(std::pow(xn(0, i), 2.0) + std::pow(xn(1, i), 2.0));

      xn(0, i) = xn(0, i)/hs(i);
      xn(1, i) = xn(1, i)/hs(i);
    }
    else{
      assert(false);
    }
    // x**3*y - x*y**3
    u(i) = std::pow(xm(0), 3.0)*xm(1) - xm(0)*std::pow(xm(1), 3.0);

    // (3x**2*y - y**3)nx + (x**3 - 3xy**2)ny
    kai(i) = (3.0*std::pow(xm(0), 2.0)*xm(1) - std::pow(xm(1), 3.0))*xn(0, i)
      + (std::pow(xm(0), 3.0) - 3.0*xm(0)*std::pow(xm(1), 2.0))*xn(1, i);
  }

  for(j = 0; j < n; j++){
     j0 = edge(0, j);
     j1 = edge(1, j);
     ym = 0.5*(x.col(j0) + x.col(j1));
     for(i = 0; i < n; i++){
       i0 = edge(0, i);
       i1 = edge(1, i);
       xm = 0.5*(x.col(i0) + x.col(i1));

       slp(i, j) = slp_laplace(xm, ym, x.col(j0), x.col(j1), hs(j), xn.col(j));
       dlp(i, j) = dlp_laplace(xm, ym, x.col(j0), x.col(j1), hs(j), xn.col(j), exterior);
     }
  }

  u = slp.partialPivLu().solve(dlp*u);

  std::cout << "relative error " << std::sqrt(((u - kai).dot(u - kai))/kai.dot(kai)) << std::endl;
}


#include <UnitTest++/UnitTest++.h>
#include <stdexcept>
#include "../misc_math.h"
#include <iostream>
#include <boost/math/distributions/poisson.hpp>
#include <limits>
#include <boost/math/distributions/hypergeometric.hpp>

using namespace coela;
using namespace std;

SUITE(misc_math)
{
    using namespace misc_math;

    TEST(Notify_Suite_Has_Been_Run) {
        cout << "*** \"misc_math\" unit tests running ***" <<endl;
    }

    TEST(integer_factorial) {
        CHECK_EQUAL(1ul, integer_factorial(0));
        CHECK_EQUAL(1ul, integer_factorial(1));
        CHECK_EQUAL(2ul, integer_factorial(2));
        CHECK_EQUAL(6ul, integer_factorial(3));
        CHECK_EQUAL(24ul, integer_factorial(4));

//        cout<<"Unsigned long max: " << numeric_limits<unsigned long>::max() <<endl;
//
//        double max_int_fac =1;
//        while (approx_factorial_double(max_int_fac) < numeric_limits<unsigned long>::max())
//            max_int_fac++;
//
//        cout<<"Implies max input for integer_factorial:"<<max_int_fac<<endl;

    }

    TEST(boost_poisson_dist_function) {
        boost::math::poisson_distribution<> p(0.2);
        CHECK_CLOSE(0.81873 , boost::math::pdf(p, 0), 0.001);
        CHECK_CLOSE(0.16375 , boost::math::pdf(p, 1), 0.001);
        CHECK_CLOSE(0.01637 , boost::math::pdf(p, 2), 0.001);
    }
}


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

#include <Eigen/Dense>
#include <array>
#include <scicpp/core.hpp>
#include <scicpp/linalg.hpp>

int main() {
    Eigen::Matrix<double, 4, 2> A;
    A << 0., 1., //
        1., 1.,  //
        2., 1.,  //
        3., 1.;

    const std::array b{-1., 0.2, 0.9, 2.1};
    const auto x = scicpp::linalg::lstsq(A, b);
    scicpp::print(x);
}


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
#include <type_traits>
using namespace std;

#include <boost/math/constants/constants.hpp>
using namespace boost::math;

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

void case1()
{
    cout << setprecision(64);

    auto a = float_constants::pi * 2 * 2;
    cout << "area \t\t= " << a << endl;

    using namespace double_constants;

    auto x = root_two * root_three;
    cout << "root 2 * 3 \t= " << x << endl;

    cout << "root pi \t= " << root_pi << endl;
    cout << "pi pow e \t= " << pi_pow_e << endl;
}

//////////////////////////////////////////
#include <boost/multiprecision/cpp_dec_float.hpp>

void case2()
{
    using namespace constants;

    typedef decltype(pi<float>) pi_t;
    assert(is_function<pi_t>::value);

    assert(pi<float>() == float_constants::pi);
    assert(pi<double>() == double_constants::pi);

    typedef boost::multiprecision::cpp_dec_float_100 float_100;
    cout << setprecision(100)
         << pi<float_100>() << endl;
}

int main()
{
    case1();
    case2();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_EULER_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_EULER_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Euler Euler (function template)

  Generates an approximation of the Euler-Mascheroni constant.

  @headerref{<boost/simd/constant/euler.hpp>}

  @par Description

  1.  @code
      template<typename T> T Euler();
      @endcode

  2.  @code
      template<typename T> T Euler( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T evaluating to the Euler-Mascheroni constant defined as:
  \f$\gamma = \lim_{n \rightarrow \infty } \left( 1+ \frac{1}{2} + ... + \frac{1}{n} - \ln(n) \right)\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(0.57721566490153286060651209008240243104215933593992359)`

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/euler.hpp>
#include <boost/simd/constant/simd/euler.hpp>

#endif


/*
 * Copyright Nick Thompson, 2019
 * Use, modification and distribution are 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)
 */

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <random>
#include <cmath>
#include <boost/core/demangle.hpp>
#include <boost/math/special_functions/gegenbauer.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using std::abs;
using boost::math::gegenbauer;
using boost::math::gegenbauer_derivative;

template<class Real>
void test_parity()
{
    std::mt19937 gen(323723);
    std::uniform_real_distribution<Real> xdis(-1, +1);
    std::uniform_real_distribution<Real> lambdadis(-0.5, 1);

    for(unsigned n = 0; n < 50; ++n) {
        unsigned calls = 50;
        unsigned i = 0;
        while(i++ < calls) {
            Real x = xdis(gen);
            Real lambda = lambdadis(gen);
            if (n & 1) {
                CHECK_ULP_CLOSE(gegenbauer(n, lambda, -x), -gegenbauer(n, lambda, x), 0);
            }
            else {
                CHECK_ULP_CLOSE(gegenbauer(n, lambda, -x), gegenbauer(n, lambda, x), 0);
            }
        }
    }
}

template<class Real>
void test_quadratic()
{
    Real lambda = 1/Real(4);
    auto c2 = [&](Real x) { return -lambda + 2*lambda*(1+lambda)*x*x; };

    Real x = -1;
    Real h = 1/Real(256);
    while (x < 1) {
        Real expected = c2(x);
        Real computed = gegenbauer(2, lambda, x);
        CHECK_ULP_CLOSE(expected, computed, 0);
        x += h;
    }
}

template<class Real>
void test_cubic()
{
    Real lambda = 1/Real(4);
    auto c3 = [&](Real x) { return lambda*(1+lambda)*x*(-2 + 4*(2+lambda)*x*x/3); };

    Real x = -1;
    Real h = 1/Real(256);
    while (x < 1) {
        Real expected = c3(x);
        Real computed = gegenbauer(3, lambda, x);
        CHECK_ULP_CLOSE(expected, computed, 4);
        x += h;
    }
}

template<class Real>
void test_derivative()
{
    Real lambda = 0.5;
    auto c3_prime = [&](Real x) { return 2*lambda*(lambda+1)*(-1 + 2*(lambda+2)*x*x); };
    auto c3_double_prime = [&](Real x) { return 8*lambda*(lambda+1)*(lambda+2)*x; };
    Real x = -1;
    Real h = 1/Real(256);
    while (x < 1) {
        Real expected = c3_prime(x);
        Real computed = gegenbauer_derivative(3, lambda, x, 1);
        CHECK_ULP_CLOSE(expected, computed, 1);

        expected = c3_double_prime(x);
        computed = gegenbauer_derivative(3, lambda, x, 2);
        CHECK_ULP_CLOSE(expected, computed, 1);

        x += h;
    }

}

int main()
{
    test_parity<float>();
    test_parity<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_parity<long double>();
#endif

    test_quadratic<float>();
    test_quadratic<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_quadratic<long double>();
#endif

    test_cubic<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_cubic<long double>();
#endif

    test_derivative<float>();
    test_derivative<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_derivative<long double>();
#endif

#ifdef BOOST_HAS_FLOAT128
    test_quadratic<boost::multiprecision::float128>();
    test_cubic<boost::multiprecision::float128>();
#endif

    return boost::math::test::report_errors();
}


#include "physics.hpp"

#include <Eigen/Geometry>

physics::physics()
{
	this->generator = std::mt19937_64(std::random_device{}());

}

physics::~physics()
{

}

void physics::step(double delta_t)
{
	total_time += delta_t;

	// TODO: collision detection
	#pragma omp parallel for
	for(int i = 0; i < obj_count; i++)
	{
		// Euler
		//a[current][i] = accel(x[current][i], i);
		//v[next][i] = a[current][i] * delta_t + v[current][i];
		//x[next][i] = v[next][i] * delta_t + x[current][i];

		// RK4 integration, see doc/grav_sim.tex
		// this is about 4x worse than Euler
		Eigen::Vector3d xk1 = x[current][i];
		Eigen::Vector3d vk1 = v[current][i];
		Eigen::Vector3d ak1 = accel(xk1, i);

		Eigen::Vector3d xk2 = x[current][i] + 0.5 * vk1 * delta_t;
		Eigen::Vector3d vk2 = v[current][i] + 0.5 * ak1 * delta_t;
		Eigen::Vector3d ak2 = accel(xk2, i);

		Eigen::Vector3d xk3 = x[current][i] + 0.5 * vk2 * delta_t;
		Eigen::Vector3d vk3 = v[current][i] + 0.5 * ak2 * delta_t;
		Eigen::Vector3d ak3 = accel(xk3, i);

		Eigen::Vector3d xk4 = x[current][i] + vk3 * delta_t;
		Eigen::Vector3d vk4 = v[current][i] + ak3 * delta_t;
		Eigen::Vector3d ak4 = accel(xk4, i);

		v[next][i] = v[current][i] + (delta_t / 6.0) *
				(ak1 + 2 * ak2 + 2 * ak3 + ak4);
		x[next][i] = x[current][i] + (delta_t / 6.0) *
				(vk1 + 2 * vk2 + 2 * vk3 + vk4);
	}

	current = current ? 0 : 1;
	next = next ? 0 : 1;
}

Eigen::Vector3d physics::accel(Eigen::Vector3d x_i, uint16_t skip_index)
{
	Eigen::Vector3d a(0.0, 0.0, 0.0);
	for(uint16_t j = 0; j < obj_count; j++)
	{
		if(skip_index == j)
			continue;

		Eigen::Vector3d r = x[current][j] - x_i;
		a += G * m[j] * r.normalized() / r.squaredNorm();
	}
	return a;
}

void physics::init(uint16_t obj_count)
{
	this->obj_count = obj_count;

	current = 0;
	next = 1;
	total_time = 0.0;

	x[0].resize(obj_count);
	x[1].resize(obj_count);
	v[0].resize(obj_count);
	v[1].resize(obj_count);
	a[0].resize(obj_count);
	a[1].resize(obj_count);
	r.resize(obj_count);
	m.resize(obj_count);

	// for now hard code some values
	mass_range[0] = 5e7;
	mass_range[1] = 1e8;
	radius_range[0] = 0.05;
	radius_range[1] = 0.25;
	//distance_range[0] = -radius_range[0] * 15.0;
	//distance_range[1] = radius_range[0] * 15.0;
	distance_range[0] = -4.0;
	distance_range[1] = 4.0;

	// random init stuff
	std::uniform_real_distribution<double> dist_m(mass_range[0],
		mass_range[1]);
	std::uniform_real_distribution<double> dist_r(radius_range[0],
		radius_range[1]);
	std::uniform_real_distribution<double> dist_d(distance_range[0],
		distance_range[1]);

	for(uint16_t i = 0; i < obj_count; i++)
	{
		r[i] = dist_r(generator);
		m[i] = dist_m(generator);

		// TODO: detect and avoid collisions here
		bool collision = true;
		while(collision)
		{
			// generate a new point
			x[0][i] = Eigen::Vector3d(dist_d(generator),
									dist_d(generator),
									dist_d(generator));

			// look for a collision
			collision = false;
			for(uint16_t j = 0; j < i; j++)
			{
				if(i == j)
					continue;

				// direction unit vector
				Eigen::Vector3d ji_uv = (x[0][i] - x[0][j]).normalized();

				// position plus radius in the direction of the other object
				Eigen::Vector3d ji_r = x[0][j] + ji_uv * r[j];

				double distance_j = (x[0][i] - ji_r).norm();
				// if radius > distance to j object then collision
				if(r[i] > distance_j)
				{
					collision = true;
					break;
				}
			}
		}

		v[0][i] = Eigen::Vector3d(0.0, 0.0, 0.0);
		a[0][i] = Eigen::Vector3d(0.0, 0.0, 0.0);
	}
}

void physics::deinit()
{
	// TODO: should we really use swap to force a deallocation and is this the
	// right way to do it?

	std::vector<Eigen::Vector3d> n0, n1;
	x[0].clear();
	x[1].clear();
	x[0].swap(n0);
	x[1].swap(n1);

	std::vector<Eigen::Vector3d> n2, n3;
	v[0].clear();
	v[1].clear();
	v[0].swap(n2);
	v[1].swap(n3);

	std::vector<Eigen::Vector3d> n4, n5;
	a[0].clear();
	a[1].clear();
	a[0].swap(n4);
	a[1].swap(n5);

	std::vector<double> n6;
	r.clear();
	r.swap(n6);

	std::vector<double> n7;
	m.clear();
	m.swap(n7);
}


#include <iostream>
#include <armadillo>
#include <gnuplot-iostream.h>
#include "../config.hpp"

using namespace std;
using namespace arma;

pair<vec, double> entrenarPerceptron(const mat& patrones,
                                     const vec& salidaDeseada,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double tolerancia,
                                     string tituloGrafica);
double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada);

int main()
{
    arma_rng::set_seed_random();

    mat datos;
    datos.load(config::sourceDir + "/guia1/icgtp1datos/OR_trn.csv");
    mat patronesEntOR = datos.head_cols(2);
    vec salidaDeseadaEntOR = datos.tail_cols(1);
    datos.load(config::sourceDir + "/guia1/icgtp1datos/OR_tst.csv");
    mat patronesPruebaOR = datos.head_cols(2);
    vec salidaDeseadaPruebaOR = datos.tail_cols(1);

    // OR
    // Entrenar la red graficando resultados intermedios (la recta)
    vec pesos;
    tie(pesos, ignore) = entrenarPerceptron(patronesEntOR,
                                            salidaDeseadaEntOR,
                                            100,
                                            0.1,
                                            5,
                                            "OR");
    double tasaError = errorPerceptron(pesos, patronesPruebaOR, salidaDeseadaPruebaOR);
    cout << "tasa de error del OR : " << tasaError << endl;

    // XOR
    // Entrenar la red graficando resultados intermedios (la recta)
    // Prueba
    datos.load(config::sourceDir + "/guia1/icgtp1datos/XOR_trn.csv");
    mat patronesEntXOR = datos.head_cols(2);
    vec salidaDeseadaEntXOR = datos.tail_cols(1);
    datos.load(config::sourceDir + "/guia1/icgtp1datos/XOR_tst.csv");
    mat patronesPruebaXOR = datos.head_cols(2);
    vec salidaDeseadaPruebaXOR = datos.tail_cols(1);

    tie(pesos, ignore) = entrenarPerceptron(patronesEntXOR,
                                            salidaDeseadaEntXOR,
                                            100,
                                            0.4,
                                            30,
                                            "XOR");
    tasaError = errorPerceptron(pesos, patronesPruebaXOR, salidaDeseadaPruebaXOR);
    cout << "tasa de error del XOR : " << tasaError << endl;

    return 0;
}

namespace ic {
int sign(double numero)
{
    if (numero >= 0)
        return 1;
    else
        return -1;
}
}

pair<vec, double> entrenarPerceptron(const mat& patrones,
                                     const vec& salidaDeseada,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double tolerancia,
                                     string tituloGrafica)
{
    // Inicializar pesos y tasa de error
    vec pesos = randu<vec>(patrones.n_cols + 1) - 0.5;
    double tasaError = 0;

    // Extender las matrices de patrones con la entrada correspondiente al umbral
    const mat patronesExt = join_horiz(ones(patrones.n_rows) * (-1), patrones);

    // Separar patrones de entrenamiento en casos verdaderos y falsos
    // para graficarlos en colores distintos
    const mat verdaderos = patrones.rows(find(salidaDeseada == 1));
    const mat falsos = patrones.rows(find(salidaDeseada == -1));

    // Objeto para graficar
    Gnuplot gp;
    // Caracter usado para dejar de graficar
    const char caracterSalteo = 's';
    char caracter = ' ';

    // Ciclo de las epocas
    for (int epoca = 1; epoca <= nEpocas; ++epoca) {
        // Ciclo para una época
        for (unsigned int i = 0; i < patrones.n_rows; ++i) {
            double z = dot(patronesExt.row(i), pesos);
            int y = ic::sign(z);

            if (caracter != caracterSalteo) {
                cout << "Patrón que entró: " << patrones.row(i)
                     << "Salida para ese patrón: " << y << '\n'
                     << "Salida deseada: " << salidaDeseada(i) << endl;

                caracter = getchar();
            }

            // Actualizar pesos
            // Tener en cuenta el orden en que colocamos la salida por la direccion del gradiente.
            pesos += tasaAprendizaje * 0.5 * (salidaDeseada(i) - y) * patronesExt.row(i).t();

            // Graficar recta
            double pendiente = -pesos(1) / pesos(2);
            double ordenadaOrigen = pesos(0) / pesos(2);
            mat puntosRecta = {{-2, -2 * pendiente + ordenadaOrigen},
                               {2, 2 * pendiente + ordenadaOrigen}};

            if (caracter != caracterSalteo) {
                gp << "set title '" << tituloGrafica << "' font ',13'\n"
                   << "set xlabel 'x_1'\n"
                   << "set ylabel 'x_2'\n"
                   << "set grid\n"
                   << "unset key\n"
                   << "plot " << gp.file1d(verdaderos) << "with points, "
                   << gp.file1d(falsos) << "with points, "
                   << gp.file1d(puntosRecta) << "with lines" << endl;
            }
        }
        // Fin ciclo (época)

        // Parte de validación - Cálculo de la tasa de error para ver si dejamos de entrenar
        int errores = 0;

        for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
            double z = dot(patronesExt.row(i), pesos);
            int y = ic::sign(z);

            if (y != salidaDeseada(i))
                ++errores;
        }

        tasaError = static_cast<double>(errores) / patrones.n_rows * 100;
        if (tasaError < tolerancia)
            break;
    }
    // Fin ciclo (epocas)

    return {pesos, tasaError};
}

double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada)
{
    // Se extiende la matriz de patrones con la entrada correspondiente al umbral
    const mat patronesExt = join_horiz(ones(patrones.n_rows) * (-1), patrones);

    int errores = 0;

    for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
        double z = dot(patronesExt.row(i), pesos);
        int y = ic::sign(z);

        if (y != salidaDeseada(i))
            ++errores;
    }

    double tasaError = static_cast<double>(errores) / patronesExt.n_rows * 100;

    return tasaError;
}


#include "DarkART/Special_Functions.hpp"

#include <algorithm>
#include <iostream>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_coulomb.h>
#include <gsl/gsl_sf_coupling.h>

#include <boost/math/special_functions/spherical_harmonic.hpp>

#include "arb_hypgeom.h"

namespace DarkART
{
// using namespace std::complex_literals;

// 1. Gaunt coefficients
double Gaunt_Coefficient(int j1, int j2, int j3, int m1, int m2, int m3)
{
	return sqrt((2.0 * j1 + 1.0) * (2.0 * j2 + 1.0) * (2.0 * j3 + 1.0)) / sqrt(4.0 * M_PI) * gsl_sf_coupling_3j(2 * j1, 2 * j2, 2 * j3, 0, 0, 0) * gsl_sf_coupling_3j(2 * j1, 2 * j2, 2 * j3, 2 * m1, 2 * m2, 2 * m3);
}

// 2. Spherical Bessel function j_L
double Spherical_Bessel_jL_arb(int L, double x)
{
	double prefactor = sqrt(M_PI / 2.0 / x);
	double result;
	slong prec;
	arb_t J_arb, x_arb, n_arb;
	arb_init(J_arb);
	arb_init(x_arb);
	arb_init(n_arb);
	arb_set_d(x_arb, x);
	arb_set_d(n_arb, 1.0 * L + 0.5);
	for(prec = 80;; prec *= 2)
	{
		arb_hypgeom_bessel_j(J_arb, n_arb, x_arb, prec);
		if(arb_rel_accuracy_bits(J_arb) >= 53)
		{
			result = arf_get_d(arb_midref(J_arb), ARF_RND_NEAR);
			break;
		}
		else if(prec > 10000)
		{
			result = NAN;
			break;
		}
	}
	arb_clear(J_arb);
	arb_clear(n_arb);
	arb_clear(x_arb);
	return prefactor * result;
}

double Spherical_Bessel_jL(int L, double x)
{
	if(L < 10)
		return gsl_sf_bessel_jl(L, x);
	else
		return Spherical_Bessel_jL_arb(L, x);
}

// 3. Coulomb wave
double Coulomb_Wave_ARB(int L, double eta, double rho)
{
	double result;
	slong prec;
	arb_t F, l, eta_2, rho_2;
	arb_init(F);
	arb_init(l);
	arb_init(eta_2);
	arb_init(rho_2);
	arb_set_d(l, L);
	arb_set_d(eta_2, eta);
	arb_set_d(rho_2, rho);
	for(prec = 80;; prec *= 2)
	{
		arb_hypgeom_coulomb(F, NULL, l, eta_2, rho_2, prec);
		if(arb_rel_accuracy_bits(F) >= 53)
		{
			result = arf_get_d(arb_midref(F), ARF_RND_NEAR);
			break;
		}
		else if(prec > 10000)
		{
			result = NAN;
			break;
		}
	}
	arb_clear(F);
	arb_clear(eta_2);
	arb_clear(rho_2);
	arb_clear(l);
	return result;
}

double Coulomb_Wave_GSL(int L, double eta, double rho, int& status)
{
	double fc_array[1];
	double F_exponent[1];
	status = gsl_sf_coulomb_wave_F_array(L, 0, eta, rho, fc_array, F_exponent);
	return fc_array[0];
}

double Coulomb_Wave(int L, double eta, double rho)
{
	int status;
	double cw = Coulomb_Wave_GSL(L, eta, rho, status);
	if(status != 0 || std::isnan(cw))
		cw = Coulomb_Wave_ARB(L, eta, rho);
	return cw;
}

}	// namespace DarkART


#include <iostream>
// #include <Eigen/Eigenvalues>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include "include/parser.h"
// using namespace Spectra;

int main()
{
    std::string S_file, M_file, L_file;
    S_file = "../assets/S_soc-karate.mtx";
    M_file = "../assets/M_web-edu.csv";
    L_file = "../assets/L_tech-internet-as.mtx";

    // Eigen::MatrixXd S_mat = Eigen::MatrixXd::Zero(10, 10);
    // for(int i=0; i<10; i++){
    //     for(int j=0; j<10; j++){
    //         S_mat(i, j) = 1;
    //     }
    // }

    // Eigen::MatrixXd S_mat = parser::readMTXFiletoDense(L_file);
    Eigen::MatrixXd S_mat = parser::readCSVFiletoDense(M_file);
    // std::cout << S_mat << "\n";
    Eigen::EigenSolver<Eigen::MatrixXd> es(S_mat, false);
    std::cout << "The eigenvalues of the 10x10 matrix of ones are:"
     << std::endl << es.eigenvalues() << std::endl;

    // // Eigen::SparseMatrix<double> * d = new Eigen::SparseMatrix<double>(data);
    // Eigen::SparseMatrix<double> M_mat = parser::readCSVFile(M_file);
    // Eigen::SparseMatrix<double> L_mat = parser::readMTXFile(L_file);

    // Spectra::SparseGenMatProd<double> XS_op(XS_mat);
    // Spectra::SparseSymMatProd<double> S_op(S_mat);
    // Spectra::SparseSymMatProd<double> M_op(M_mat);
    // Spectra::SparseSymMatProd<double> L_op(L_mat);

    // Construct eigen solver object, requesting the largest three eigenvalues
    // Spectra::GenEigsSolver<Spectra::SparseGenMatProd<double>> XS_eigs(XS_op, 8, 10);
    // Spectra::SymEigsSolver<Spectra::SparseSymMatProd<double>> S_eigs(S_op, 33, 34);
    // Spectra::SymEigsSolver<Spectra::SparseSymMatProd<double>> M_eigs(M_op, 3, 6);
    // Spectra::SymEigsSolver<Spectra::SparseSymMatProd<double>> L_eigs(L_op, 3, 6);

    // Initialize and compute
    // XS_eigs.init();
    // S_eigs.init();
    // M_eigs.init();
    // L_eigs.init();

    // int XS_nconv = XS_eigs.compute(Spectra::SortRule::LargestReal);
    // int S_nconv = S_eigs.compute(Spectra::SortRule::LargestMagn);
    // int M_nconv = M_eigs.compute(Spectra::SortRule::LargestMagn);
    // int L_nconv = L_eigs.compute(Spectra::SortRule::LargestMagn);

    // Retrieve results
    // Eigen::VectorXcd evalues;

    // std::cout << XS_mat;
    // std::cout << "\nX Small : " << XS_mat.nonZeros()<< "\n\n";
    // std::cout << XS_eigs.eigenvectors() << "\n";
    // if (XS_eigs.info() == Spectra::CompInfo::Successful)
    //     evalues = XS_eigs.eigenvalues();
    // std::cout << "Eigenvalues found for XS :\n"
    //           << evalues << std::endl;

    // std::cout << "\nSmall : " << S_mat.nonZeros() / 2 << "\n";
    // std::cout << S_eigs.eigenvectors() << "\n";
    // if (S_eigs.info() == Spectra::CompInfo::Successful)
    //     evalues = S_eigs.eigenvalues();
    // std::cout << "Eigenvalues found for S :\n"
    //           << evalues << std::endl;

    // std::cout << "\nMedium : " << M_mat.nonZeros() / 2 << "\n";
    // if (M_eigs.info() == Spectra::CompInfo::Successful)
    //     evalues = M_eigs.eigenvalues();
    // std::cout << "Eigenvalues found for M :\n"
    //           << evalues << std::endl;

    // std::cout << "\nLarge : " << L_mat.nonZeros() / 2 << "\n";
    // if (L_eigs.info() == Spectra::CompInfo::Successful)
    //     evalues = L_eigs.eigenvalues();
    // std::cout << "Eigenvalues found for L :\n"
    //           << evalues << std::endl;

    return 0;
}


/*
 * prime_utils.cpp: Function definitions for working with primes. The following
 * methods are defined here:
 *     - Miller-Rabin probabilistic primality test, with an iterative tester
 *       method
 *     - Modular addition, which is used by modular multiplication
 *     - Modular multiplication and exponentiation, which are used by the
 *       Miller-Rabin test
 *     - Sieve of Atkin
 *     - Integer square root, which is used by the Sieve
 *
 * Version:     1.0.0
 * License:     MIT License (see LICENSE.txt for more details)
 * Author:      Joshua Morrison (MrM21632)
 * Last Edited: 2/6/2018, 1:20am
 */

#include <cstdlib>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include "prime_utils.h"

uint64_t isqrt(uint64_t n) {
    // Base Case: if n < 2, then sqrt(n) = n.
    if (n < 2) return n;

    // We calculate a "small" and "large" candidate for our return value:
    //     1. s, which is equal to 2 * isqrt(n / 4), is the small candidate.
    //     2. l, which is equal to one larger than s, is the large candidate.
    uint64_t s = isqrt(n >> 2) << 1;
    uint64_t l = s + 1;

    if (l*l > n)
        return s;
    else
        return l;
}

uint64_t mod_add(uint64_t a, uint64_t b, uint64_t n) {
    return ((a % n) + (b % n)) % n;
}

uint64_t mod_mult(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 0;  // Remainder

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // Perform modular addition on r until the multiplier is 0.
    while (b > 0) {
        if (b & 1)
            r = mod_add(r, a, n);

        a = (a * 2) % n;
        b >>= 1;
    }

    // (a * b) mod n == ((a mod n) * (b mod n)) mod n. At this point, we have
    // calculated r = (a mod n) * (b mod n), so we now need to reduce r by mod
    // n, then return that result.
    return r % n;
}

uint64_t mod_pow(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 1;  // Remainder

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // Perform modular multiplication on both the remainder and the base, until
    // our exponent is 0.
    while (b > 0) {
        if (b & 1)
            r = mod_mult(r, a, n);

        a = mod_mult(a, a, n);
        b >>= 1;
    }

    return r;
}

bool miller_rabin(uint64_t n, uint64_t d) {
    boost::random::random_device rd;
    boost::random::mt19937 mt(rd());
    boost::random::uniform_int_distribution<uint64_t> dist(2, n-2);

    uint64_t a = dist(mt);
    uint64_t x = mod_pow(a, d, n);

    // Base Case: if a^d mod n is 1 or n-1, then we already know n is prime, so
    // we can return true.
    if (x == 1 || x == n-1)
        return true;

    // Perform modular multiplication on x until d is equal to n-1.
    while (d != (n - 1)) {
        x = mod_mult(x, x, n);
        d <<= 1;

        if (x == 1) return false;
        if (x == (n - 1)) return true;
    }

    // If we reach this point, we can safely say that n is not prime, so we
    // return false.
    return false;
}

bool is_prime(uint64_t n, uint64_t k) {
    // Base cases
    if (n <= 1) return false;
    if (n <= 3) return true;
    if (!(n & 1)) return false;

    // At this point, we know n is odd and greater than 1. Now, our goal is to
    // find an integer d such that n-1 = d * 2^r, where r >= 1. The loop divides
    // d by 2 until it is no longer even - at that point, we will have the value
    // we desire.
    uint64_t d = n - 1;
    while (!(d & 1))
        d >>= 1;

    // Call miller_rabin(n, d) k times. If any of the tests fail, we know that n
    // is not prime, so we return false.
    for (uint64_t i = 0; i < k; ++i) {
        if (!miller_rabin(n, d))
            return false;
    }

    // If we reach this point, we can safely assume (NOT guarantee!) n is prime,
    // so we return true.
    return true;
}

bool *sieve_of_atkin(uint64_t n) {
    bool *data = new bool[n + 1]();
    data[2] = true;
    data[3] = true;
    uint64_t lim = isqrt(n);

    // This loop runs for all x in [1, lim].
    for (uint64_t x = 1; x <= lim; ++x) {
        // This loop runs for all y in [1, lim].
        //
        // k is prime if any of the following are true:
        //     1. For k = 4*x^2 + y^2, k is prime iff k mod 12 = 1 or 5.
        //     2. For k = 3*x^2 + y^2, k is prime iff k mod 12 = 7.
        //     3. For k = 3*x^2 - y^2, k is prime iff k mod 12 = 11.
        // All of these also assume that k is not greater than n, and for the
        // third case, that k is not negative (i.e., that x > y).
        for (uint64_t y = 1; y <= lim; ++y) {
            uint64_t k = (4*x*x) + (y*y);
            if ((k <= n) && ((k % 12 == 1) || (k % 12 == 5)))
                data[k] = !data[k];

            k = (3*x*x) + (y*y);
            if ((k <= n) && (k % 12 == 7))
                data[k] = !data[k];

            k = (3*x*x) - (y*y);
            if ((x > y) && (k <= n) && (k % 12 == 11))
                data[k] = !data[k];
        }
    }

    // This loop runs for all j in [5, lim].
    //
    // We perform one last loop through the array. If j is prime, then we mark
    // all multiples of its square as composite, a la the standard Sieve of
    // Eratosthenes.
    for (uint64_t j = 5; j <= lim; ++j) {
        if (data[j]) {
            for (uint64_t k = j*j; k <= n; k += j*j)
                data[k] = false;
        }
    }

    return data;
}