#include <iostream>
#include <vector>
#include <fstream>
#include <thread>
#include <cmath>
#include <chrono>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

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

using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include "constants.h"
#include "cuda_wrapper.h"
#include "kernels.cuh"
#include "plot.h"

using namespace cv;

/**
 * Dataset from:
 *
 *   http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip
 *
 * */

inline double getBilinearInterpolatedValue_no_eigen(const unsigned char *img, double pt[2]) {
    const unsigned char* d = &img[(int)pt[1] * width + (int)pt[0]];
    double xx = pt[0] - floor(pt[0]);
    double yy = pt[1] - floor(pt[1]);
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[width]) +
            xx * yy * double(d[width + 1])) / 255.0;
}

inline void pix2cam_no_eigen(const double in[2], double out[3]) {
    out[0] = (in[0] - cx) / fx;
    out[1] =  (in[1] - cy) / fy;
    out[2] = 1.0;
}

inline void cam2pix_no_eigen(const double in[3], double out[2]) {
    out[0] = in[0] * fx / in[2] + cx;
    out[1] = in[1] * fy / in[2] + cy;
}

inline double norm3_no_eigen(const double in[3])
{
    return sqrt(in[0]*in[0] + in[1]*in[1] + in[2]*in[2]);
}

inline double norm2_no_eigen(const double in[2])
{
    return sqrt(in[0]*in[0] + in[1]*in[1]);
}

// inplace normalization vec 3

inline void normalize3_no_eigen(double in_out[3]) {
    double d = sqrt(in_out[0]*in_out[0]
                    + in_out[1]*in_out[1]
                    + in_out[2]*in_out[2]);
    in_out[0] /= d;
    in_out[1] /= d;
    in_out[2] /= d;
}

// inplace normalization vec 2

inline void normalize2_no_eigen(double in_out[2]) {
    double d = sqrt(in_out[0]*in_out[0] + in_out[1]*in_out[1]);
    in_out[0] /= d;
    in_out[1] /= d;
}

inline void transform_no_eigen(double x[3], const double T[12], double out[3])
{
    for (int i = 0; i < 3; ++i)
    {
        out[i] = x[0] * T[i*4] + x[1] * T[i*4+1] + x[2] * T[i*4+2] +  T[i*4+3];
    }
}

double ZNCC_no_eigen(const unsigned char *im1, const double pt1[2], const unsigned char *im2, const double pt2[2])
{
    // no need to consider block partly outside because of boarder
    double v1[ncc_area], v2[ncc_area];
    double s1 = 0.0, s2 = 0.0;
    int idx = 0;
    for (int i = -ncc_window_size; i <= ncc_window_size; ++i)
    {
        for (int j = -ncc_window_size; j <= ncc_window_size; ++j)
        {
            double val_1 = ((double) im1[((int)pt1[1] + i) * width + (int)pt1[0] + j]) / 255;
            double temp_p2[2] = {pt2[0] + j, pt2[1] + i};
            double val_2 = getBilinearInterpolatedValue_no_eigen(im2, temp_p2);
            s1 += val_1;
            s2 += val_2;
            v1[idx] = val_1;
            v2[idx] = val_2;
            ++idx;
        }
    }

    double mean_1 = s1 / ncc_area;
    double mean_2 = s2 / ncc_area;

    double numerator = 0.0;
    double den1 = 0.0, den2 = 0.0;
    for (int i = 0; i < ncc_area; ++i)
    {
        double zv1 = v1[i] - mean_1;
        double zv2 = v2[i] - mean_2;
        numerator += zv1*zv2;
        den1 += zv1 * zv1;
        den2 += zv2 * zv2;
    }
    auto zncc =  numerator / (sqrt(den1 * den2 + epsilon));
    // std::cout << "zncc = " << zncc << "\n";
    return zncc;
}

bool epipolar_search_no_eigen(const unsigned char* ref, const unsigned char* cur,
                          const double Tcr[12], const double pt[2],
                          double depth_mu, double depth_sigma2,
                          double best_pc[2], double epipolar_dir[2])
{

    double depth_sigma = sqrt(depth_sigma2);
    double dmax = depth_mu + 3 * depth_sigma;
    double dmin = depth_mu - 3 * depth_sigma;
    dmin = max(0.1, dmin);

    double pn[3];
    pix2cam_no_eigen(pt, pn);
    normalize3_no_eigen(pn);
    double P_max[3] = {pn[0] * dmax, pn[1] * dmax, pn[2] * dmax};
    double P_min[3] = {pn[0] * dmin, pn[1] * dmin, pn[2] * dmin};
    double P_mu[3] = {pn[0] * depth_mu, pn[1] * depth_mu, pn[2] * depth_mu};

    double P_max_cur[3], P_min_cur[3], P_mu_cur[3];
    transform_no_eigen(P_max, Tcr, P_max_cur);
    transform_no_eigen(P_min, Tcr, P_min_cur);
    transform_no_eigen(P_mu, Tcr, P_mu_cur);

    double pc_max[2], pc_min[2], pc_mu[2];
    cam2pix_no_eigen(P_max_cur, pc_max);
    cam2pix_no_eigen(P_min_cur, pc_min);
    cam2pix_no_eigen(P_mu_cur, pc_mu);

    double epipolar_line[2] = {pc_max[0] - pc_min[0], pc_max[1] - pc_min[1]};
    epipolar_dir[0] = epipolar_line[0];
    epipolar_dir[1] = epipolar_line[1];
    normalize2_no_eigen(epipolar_dir);
    double epipolar_line_norm = norm2_no_eigen(epipolar_line);

    // double step = 0.7;
    // int nb_samples = std::ceil(epipolar_line.norm() / step);

    double half_range = 0.5 * epipolar_line_norm;
    if (half_range > 100) half_range = 100;

    double best_zncc = -1.0;
    for (double l = -half_range; l<= half_range; l+= 0.7)
    {
        double p[2] = {pc_mu[0] + l * epipolar_dir[0], pc_mu[1] + l * epipolar_dir[1]};

        if (p[0] < boarder || p[0] >= width-boarder || p[1] < boarder || p[1] >= height-boarder)
            continue; // p is outside the cur image

        double zncc = ZNCC_no_eigen(ref, pt, cur, p);
        if (zncc > best_zncc)
        {
            best_zncc = zncc;
            best_pc[0] = p[0];
            best_pc[1] = p[1];
        }
    }
    if (best_zncc < 0.85)
        return false;
    else
        return true;
}

double dot3_no_eigen(const double a[3], const double b[3])
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

double det2_no_eigen(const double A[2][2])
{
    return A[0][0] * A[1][1] - A[1][0] * A[0][1];
}

void solve_Axb2_no_eigen(const double A[2][2], const double b[2], double res[2])
{
    double det_inv = 1.0 / det2_no_eigen(A);
    double A_inv[2][2];
    A_inv[0][0] = det_inv * A[1][1];
    A_inv[0][1] = -det_inv * A[0][1];
    A_inv[1][0] = -det_inv * A[1][0];
    A_inv[1][1] = det_inv * A[0][0];

    res[0] = A_inv[0][0] * b[0] + A_inv[0][1] * b[1];
    res[1] = A_inv[1][0] * b[0] + A_inv[1][1] * b[1];
}

void update_depth_filter_no_eigen(const double pr[2], const double pc[2], const double Trc[12], const double epipolar_dir[2], double *depth, double *cov2)
{
    double fr[3];
    pix2cam_no_eigen(pr, fr);
    normalize3_no_eigen(fr);

    double fc[3];
    pix2cam_no_eigen(pc, fc);
    normalize3_no_eigen(fc);

    double f2[3] = {dot3_no_eigen(Trc, fc),
                    dot3_no_eigen(Trc+4, fc),
                    dot3_no_eigen(Trc+8, fc)};

    double trc[3] = {Trc[3], Trc[7], Trc[11]};
    double A[2][2];
    double b[2];

    A[0][0] = dot3_no_eigen(fr, fr);
    A[0][1] = dot3_no_eigen(fr, f2);
    A[1][0] = dot3_no_eigen(f2, fr);
    A[1][1] = dot3_no_eigen(f2, f2);
    A[0][1] *= -1;
    A[1][1] *= -1;

    b[0] = dot3_no_eigen(fr, trc);
    b[1] = dot3_no_eigen(f2, trc);

    if (abs(det2_no_eigen(A)) < 1e-20) // not invertible
        return;

    double res[2];
    solve_Axb2_no_eigen(A, b, res);
    double P1[3] = {fr[0] * res[0], fr[1] * res[0], fr[2] * res[0]};
    double P2[3] = {trc[0] + fc[0] * res[1], trc[1] + fc[1] * res[1], trc[2] + fc[2] * res[1]};
    double P_est[3] = {(P1[0] + P2[0]) * 0.5,
                       (P1[1] + P2[1]) * 0.5,
                       (P1[2] + P2[2]) * 0.5};
    double depth_obs = norm3_no_eigen(P_est);

    double P[3] = {fr[0] * depth_obs, fr[1] * depth_obs, fr[2] * depth_obs};
    double a[3] = {P[0] - trc[0], P[1] - trc[1], P[2] - trc[2]};

    double t[3] = {trc[0], trc[1], trc[2]};
    normalize3_no_eigen(t);

    double alpha = acos(dot3_no_eigen(fr, t));
    double beta = acos(-dot3_no_eigen(a, t) / norm3_no_eigen(a));

    double pc2[2] = {pc[0] + epipolar_dir[0], pc[1] + epipolar_dir[1]};
    double fc2[3];
    pix2cam_no_eigen(pc2, fc2);
    normalize3_no_eigen(fc2);
    double beta_2 = acos(-dot3_no_eigen(fc2, t));

    double gamma = M_PI - alpha - beta_2;
    double d_noise = norm3_no_eigen(trc) * sin(beta_2) / sin(gamma); // sinus law
    double sigma_obs = depth_obs - d_noise;
    double sigma2_obs = sigma_obs * sigma_obs;

    // Depth fusion
    double d = depth[(int)pr[1] * width + (int)pr[0]];
    double sigma2 = cov2[(int)pr[1] * width + (int)pr[0]];

    double d_fused = (sigma2_obs * d + sigma2 * depth_obs) / (sigma2 + sigma2_obs);
    double sigma2_fused = (sigma2 * sigma2_obs) / (sigma2 + sigma2_obs);

    depth[(int)pr[1] * width + (int)pr[0]] = d_fused;
    cov2[(int)pr[1] * width + (int)pr[0]] = sigma2_fused;

}

void update_no_eigen(cv::Mat ref, cv::Mat cur, const Sophus::SE3d& Tcr, cv::Mat depth, cv::Mat cov2)
{
    Eigen::Vector2d pc;
    Eigen::Vector2d epipolar_dir;
    double pc_out[3];
    double epipolar_dir_out[2];

    Sophus::SE3d Trc = Tcr.inverse();
    double Tcr_data[12];
    double Trc_data[12];

    Eigen::Matrix<double, 3, 4, Eigen::RowMajor> Tcr_matrix = Tcr.matrix3x4();
    Eigen::Matrix<double, 3, 4, Eigen::RowMajor> Trc_matrix = Trc.matrix3x4();

    int total=0;
    for (int j = boarder; j < width-boarder; ++j)
    {
        for (int i = boarder; i < height-boarder; ++i)
        {
            double depth_mu = depth.at<double>(i, j);
            double depth_sigma2 = cov2.at<double>(i, j);
            if (depth_sigma2 < min_cov || depth_sigma2 > max_cov)
                continue;
            Eigen::Vector2d pr(j, i);
            bool found = epipolar_search_no_eigen(ref.ptr<unsigned char>(0), cur.ptr<unsigned char>(0),
                                                  Tcr_matrix.data(), pr.data(),
                                                  depth_mu, depth_sigma2,
                                                  pc.data(), epipolar_dir.data());
            total += found;
            if (!found)
                continue;
            // showEpipolarMatch(ref, cur, pr, pc);

            update_depth_filter_no_eigen(pr.data(), pc.data(), Trc_matrix.data(), epipolar_dir.data(), depth.ptr<double>(0), cov2.ptr<double>(0));
        }
    }
    std::cout << "total found " << total << " / " << width*height << "\n";
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // Read dataset
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // Initial depth image
    Mat ref = imread(color_image_files[0], 0); // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;
    double init_cov2 = 3.0;
    Mat depth(height, width, CV_64F, init_depth);
    Mat depth_cov2(height, width, CV_64F, init_cov2);

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // T_C_W * T_W_R = T_C_R
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        update_no_eigen(ref, curr, pose_T_C_R, depth, depth_cov2);
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

        auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
        std::cout << "Time used: " << time_used.count() << "s\n";

        evaludateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        plotCur(curr);
        // imshow("image", curr);
        // waitKey(1);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;
    double ave_depth_error_sq = 0;
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}


/*
 // solvers for Algebraic Riccati equation
 // - Iteration (continuous)
 // - Iteration (discrete)
 // - Arimoto-Potter
 //
 // author: Horibe Takamasa
 */

#include <Eigen/Dense>
#include <iostream>
#include <time.h>
#include <vector>
#include "riccati_solver.h"

#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl

int
main ()
{
  const uint dim_x = 4;
  const uint dim_u = 1;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(dim_x, dim_u);
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_u, dim_u);
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x, dim_x);

  A(0, 1) = 1.0;
  A(1, 1) = -15.0;
  A(1, 2) = 10.0;
  A(2, 3) = 1.0;
  A(3, 3) = -15.0;
  B(1, 0) = 10.0;
  B(3, 0) = 1.0;

  Q(0, 0) = 1.0;
  Q(2, 2) = 1.0;
  Q(3, 3) = 2.0;

  R(0, 0) = 1.0;

  PRINT_MAT(A);
  PRINT_MAT(B);
  PRINT_MAT(Q);
  PRINT_MAT(R);

  /* == iteration based Riccati solution (continuous) == */
  std::cout << "-- Iteration based method (continuous) --" << std::endl;
  clock_t start = clock();
  solveRiccatiIterationC(A, B, Q, R, P);
  clock_t end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  /* == iteration based Riccati solution (discrete) == */
  // discretization
  const double dt = 0.001;
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x, dim_x);
  Eigen::MatrixXd Ad = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Ad = (I + 0.5 * dt * A) * (I - 0.5 * dt * A).inverse();
  Eigen::MatrixXd Bd;
  Bd = B * dt;

  std::cout << "-- Iteration based method (discrete)--" << std::endl;
  start = clock();
  solveRiccatiIterationD(Ad, Bd, Q, R, P);
  end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  /* == eigen decomposition method (Arimoto-Potter algorithm) == */
  std::cout << "-- Eigen decomposition mathod --" << std::endl;
  start = clock();
  solveRiccatiArimotoPotter(A, B, Q, R, P);
  end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  return 0;
}


/*
* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
* its licensors.
*
* For complete copyright and license terms please see the LICENSE at the root of this
* distribution (the "License"). All use of this software is governed by the License,
* or, if provided, by the license below or the license accompanying this file. Do not
* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
*/

#include <NumericalMethods_precompiled.h>

#include <cmath>

#include <AzCore/std/algorithm.h>

#include <LinearAlgebra.h>
#include <Eigenanalysis/Utilities.h>

namespace NumericalMethods::Eigenanalysis
{
    VectorVariable CrossProduct(const VectorVariable& lhs, const VectorVariable& rhs)
    {
        AZ_Assert(
            lhs.GetDimension() == 3 && rhs.GetDimension() == 3, "VectorVariable dimensions invalid for cross product."
        );

        return VectorVariable::CreateFromVector({
            lhs[1] * rhs[2] - lhs[2] * rhs[1],
            lhs[2] * rhs[0] - lhs[0] * rhs[2],
            lhs[0] * rhs[1] - lhs[1] * rhs[0]
        });
    }

    void ComputeOrthogonalComplement(
        const VectorVariable& vecW, VectorVariable& vecU, VectorVariable& vecV
    )
    {
        // Robustly computes a right-handed orthogonal basis {vecU, vecV, vecW}.
        double invLength = 1.0;

        if (fabs(vecW[0]) > fabs(vecW[1]))
        {
            // The component of maximum absolute value is either vecW[0] or vecW[2].
            invLength /= sqrt(vecW[0] * vecW[0] + vecW[2] * vecW[2]);
            vecU = VectorVariable::CreateFromVector({ -vecW[2] * invLength, 0.0, vecW[0] * invLength });
        }
        else
        {
            // The component of maximum absolute value is either vecW[1] or vecW[2].
            invLength /= sqrt(vecW[1] * vecW[1] + vecW[2] * vecW[2]);
            vecU = VectorVariable::CreateFromVector({ 0.0, vecW[2] * invLength, -vecW[1] * invLength });
        }

        vecV = CrossProduct(vecW, vecU);
    }

    VectorVariable ComputeEigenvector0(
        double a00, double a01, double a02, double a11, double a12, double a22, double val
    )
    {
        // By definition, (A−e∗I)v = 0, where e is the eigenvalue and v is the corresponding eigenvector to be found.
        // This condition implies that the rows (A−e∗I) must be perpendicular to v. This matrix must have rank 2, so two
        // rows will be linearly dependent. For those two rows, the cross product will be (nearly) zero. So to find v,
        // we can simply take the cross product of the two rows that maximize its magnitude.
        VectorVariable row0 = VectorVariable::CreateFromVector({ a00 - val, a01, a02 });
        VectorVariable row1 = VectorVariable::CreateFromVector({ a01, a11 - val, a12 });
        VectorVariable row2 = VectorVariable::CreateFromVector({ a02, a12, a22 - val });

        VectorVariable r0xr1 = CrossProduct(row0, row1);
        VectorVariable r0xr2 = CrossProduct(row0, row2);
        VectorVariable r1xr2 = CrossProduct(row1, row2);

        double d0 = r0xr1.Dot(r0xr1);
        double d1 = r0xr2.Dot(r0xr2);
        double d2 = r1xr2.Dot(r1xr2);

        return d0 >= d1 && d0 >= d2 ? r0xr1 * (1.0 / sqrt(d0)) :
               d1 >= d0 && d1 >= d2 ? r0xr2 * (1.0 / sqrt(d1)) :
                                      r1xr2 * (1.0 / sqrt(d2)) ;
    }

    VectorVariable ComputeEigenvector1(
        double a00,
        double a01,
        double a02,
        double a11,
        double a12,
        double a22,
        double val,
        const VectorVariable& vec
    )
    {
        // Real symmetric matrices must have orthogonal eigenvectors. Thus, if we generate two vectors vecU and vecV
        // orthogonal to the eigenvector vec already found, the remaining eigenvectors must be a circular combination
        // of vecU and vecW. This reduces the problem to a 2D system. For details see Eberly.
        VectorVariable vecU(3);
        VectorVariable vecV(3);
        ComputeOrthogonalComplement(vec, vecU, vecV);

        MatrixVariable matA(3, 3);

        matA.Element(0, 0) = a00;
        matA.Element(0, 1) = a01;
        matA.Element(0, 2) = a02;

        matA.Element(1, 0) = a01;
        matA.Element(1, 1) = a11;
        matA.Element(1, 2) = a12;

        matA.Element(2, 0) = a02;
        matA.Element(2, 1) = a12;
        matA.Element(2, 2) = a22;

        double m00 = vecU.Dot(matA * vecU) - val;
        double absM00 = fabs(m00);

        double m01 = vecU.Dot(matA * vecV);
        double absM01 = fabs(m01);

        double m11 = vecV.Dot(matA * vecV) - val;
        double absM11 = fabs(m11);

        auto discardComponentAndNormalize = [](double& factor, double& other) {
            other /= factor;
            factor = 1.0 / sqrt(1.0 + other * other);
            other *= factor;
        };

        if (absM00 > absM11)
        {
            if (AZStd::max(absM00, absM01) > 0.0)
            {
                if (absM00 >= absM01)
                {
                    discardComponentAndNormalize(m00, m01);
                }
                else
                {
                    discardComponentAndNormalize(m01, m00);
                }
                return vecU * m01 - vecV * m00;
            }
            else
            {
                return vecU;
            }
        }
        else
        {
            if (AZStd::max(absM11, absM01) > 0.0)
            {
                if (absM11 >= absM01)
                {
                    discardComponentAndNormalize(m11, m01);
                }
                else
                {
                    discardComponentAndNormalize(m01, m11);
                }
                return vecU * m11 - vecV * m01;
            }
            else
            {
                return vecU;
            }
        }
    }

    VectorVariable ComputeEigenvector2(const VectorVariable& vec0, const VectorVariable& vec1)
    {
        return CrossProduct(vec0, vec1);
    }
} // namespace NumericalMethods::Eigenanalysis


/** @file NormalDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief NormalDist class definition
 *
 */
#include "PriorHessian/NormalDist.h"
#include "PriorHessian/PriorHessianError.h"

#include <sstream>
#include <cmath>
#include <limits>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>

namespace prior_hessian {

/* Static member variables */
const StringVecT NormalDist::_param_names = { "mu", "sigma" };
const NormalDist::NparamsVecT NormalDist::_param_lbound = {-INFINITY, 0}; //Lower bound on valid parameter values
const NormalDist::NparamsVecT NormalDist::_param_ubound = {INFINITY, INFINITY}; //Upper bound on valid parameter values

/* Constructors */
NormalDist::NormalDist(double mu, double sigma)
    : UnivariateDist()
{
    set_params(mu,sigma);
}

/* Non-static member functions */

void NormalDist::set_sigma(double val)
{
    _sigma = checked_sigma(val);
    _sigma_inv = 1./_sigma;
    llh_const_initialized = false;
}

double NormalDist::cdf(double x) const
{
    return .5*(1 + boost::math::erf((x - _mu)*_sigma_inv*constants::sqrt2_inv));
}

double NormalDist::icdf(double u) const
{
    return mu() + sigma()*constants::sqrt2*boost::math::erf_inv(2*u-1);
}

double NormalDist::checked_mu(double val)
{
    if(!std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"NormalDist: got bad mu value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double NormalDist::checked_sigma(double val)
{
    if(val<=0 || !std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"NormalDist: got bad sigma value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double NormalDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const(); //Lazy computation of llh_const.
    return rllh(x) + llh_const;
}

void NormalDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(sigma());
    llh_const_initialized = true;
}

double NormalDist::compute_llh_const(double sigma)
{
    return -log(sigma) - .5*constants::log2pi;
}

} /* namespace prior_hessian */


#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// Vertex of curve model, template parameters: optimize variable dimension and data type
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // Reset
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // update
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // Save and read: leave blank
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// Error model Template parameters: observation dimension, type, connected vertex type
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // Calculation curve model error
  virtual void computeError() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // Calculate the Jacobian matrix
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x value, y value is _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // True parameter value
  double ae = 2.0, be = -1.0, ce = 5.0;        // Estimate parameter values
  int N = 100;                                 // data point
  double w_sigma = 1.0;                        // noise Sigma value
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV random number generator

  vector<double> x_data, y_data;      // data
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  //Build graph optimization, first set g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // The optimization variable dimension of each error term is 3, and the error value dimension is 1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // Linear solver type

  // Gradient descent method, choose from GN, LM, DogLe
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // graph model
  optimizer.setAlgorithm(solver);   // Set the solver
  optimizer.setVerbose(true);       // Turn on debug output

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // Add a side to the picture
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                //  Set the vertices of the connection
    edge->setMeasurement(y_data[i]);      // Observe the value
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); //Information matrix: the inverse of the covariance matrix
    optimizer.addEdge(edge);
  }

  // Perform optimization
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(20);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // output optimized value
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}


#include <math_lib/vec_base.h>

#include <math_lib/orthogonal.h>

#include <boost/qvm/vec_operations.hpp>

#include <gtest/gtest.h>

using namespace pagoda;

TEST(Orthogonal, when_calculating_the_orthogonal_vector_of_a_2d_vector_should_have_zero_as_dot_product)
{
	Vec2F v{1, 2};
	ASSERT_EQ(boost::qvm::dot(v, orthogonal(v)), 0);
}

TEST(Orthogonal, when_calculating_the_orthogonal_vector_of_a_3d_vector_should_have_zero_as_dot_product)
{
	Vec3F v{1, 2, 3};
	ASSERT_EQ(boost::qvm::dot(v, orthogonal(v)), 0);
}

TEST(Orthogonal, when_calculating_the_orthogonal_of_a_vector_parallel_with_an_axis_should_have_zero_as_dot_product)
{
	for (auto v : {Vec3F{1, 0, 0}, Vec3F{0, 1, 0}, Vec3F{0, 0, 1}})
	{
		EXPECT_EQ(boost::qvm::dot(v, orthogonal(v)), 0);
	}
}


// adapted from lassopack, see:
// https://github.com/statalasso/lassopack/blob/master/lassoutils.ado

#include "../eigen-3.4.0/Eigen/Eigen"
#include <iostream>
#include <math.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Eigen>

namespace py = pybind11;
using namespace Eigen;
using namespace std;
// using MatrixXdRef = Eigen::Ref<Eigen::MatrixXd>;
// using VectorXdRef = Eigen::Ref<Eigen::VectorXd>;
using MatrixXdRef = Eigen::MatrixXd;
using VectorXdRef = Eigen::VectorXd;

VectorXd coordinateDescent(MatrixXdRef X, VectorXdRef y, MatrixXdRef XX, VectorXdRef Xy,
                       double lambd, VectorXdRef psi, VectorXdRef startingValues,
                       bool sqrtLasso = false, bool fitIntercept = true,
                       double optTol = 1e-10, int maxIter = 1000,
                       double zeroTol = 1e-4) {

  int n = X.rows(), p = X.cols();

  VectorXd beta = startingValues;

  VectorXd sdVec = X.colwise().norm();
  double ySd = y.norm();
  // normal lasso shooting
  if (!sqrtLasso) {

    XX *= 2, Xy *= 2;
    // loop over max iter
    for (int iter = 0; iter < maxIter; iter++) {
      // copy beta to beta_old
      VectorXd betaOld = beta;
      // loop over p
      for (int j = 0; j < p; j++) {
        // calculate s0 and do shooting
        double S0 = (XX.row(j) * beta).sum() - XX(j, j) * beta(j) - Xy(j);

        if (S0 > lambd * psi(j)) {
          beta(j) = (lambd * psi(j) - S0) / XX(j, j);

        } else if (S0 < -lambd * psi(j)) {
          beta(j) = (-lambd * psi(j) - S0) / XX(j, j);

        } else {
          beta(j) = 0;
        }
      }
      // check for convergence
      double diff = ((beta - betaOld).cwiseAbs() * sdVec / ySd).sum();
      // double diff = (beta - betaOld).cwiseAbs().sum();
      if (diff < optTol) {
        break;
      }
    }
    // sqrt-lasso shooting algorithm
  } else {
    // rescale XX and Xy
    XX /= n;
    Xy /= n;
    double MaxErrorNorm = 1.0e-10;

    // demean X and y
    // if (fitIntercept) {
    //   MatrixXd meanX = X.colwise().mean().replicate(n, 1);
    //   VectorXd meanY = VectorXd::Ones(n) * y.mean();
    //   X = X - meanX;
    //   y = y - meanY;
    // }

    // get error
    VectorXd error = y - X * beta;
    double qhat = error.squaredNorm() / n;

    for (int iter = 0; iter < maxIter; iter++) {
      VectorXd betaOld = beta;

      for (int j = 0; j < p; j++) {

        if (fabs(beta(j)) > 0) {
          error += X.col(j) * beta(j);
          qhat = error.squaredNorm() / n;
        }

        double S0 = XX.row(j).dot(beta) - XX(j, j) * beta(j) - Xy(j);
        double qqhat = max(qhat - (pow(S0, 2) / XX(j, j)), 0.0);

        if (pow(n, 2) < pow(lambd * psi(j), 2) / XX(j, j)) {
          beta(j) = 0;
        }

        else if (S0 > lambd / n * psi(j) * sqrt(qhat)) {
          beta[j] = ((lambd * psi(j) /
                      sqrt(pow(n, 2) - pow(lambd * psi(j), 2) / XX(j, j))) *
                         sqrt(qqhat) -
                     S0) /
                    XX(j, j);
          error -= X.col(j) * beta[j];
        }

        else if (S0 < -lambd / n * psi(j) * sqrt(qhat)) { // Optimal beta(j) > 0
          beta[j] = (-(lambd * psi(j) /
                       sqrt(pow(n, 2) - pow(lambd * psi(j), 2) / XX(j, j))) *
                         sqrt(qqhat) -
                     S0) /
                    XX(j, j);
          error -= X.col(j) * beta(j);
        }

        else {
          beta(j) = 0;
        }
      } // end loop beta^(i)_j

      // Update primal and dual value
      double errorNorm = (y - X * beta).norm();
      double fobj =
          errorNorm / sqrt(n) + (lambd / n) * (psi * beta.cwiseAbs()).sum();

      double dual;
      if (errorNorm > MaxErrorNorm) {
        VectorXd aaa = sqrt(n) * (error / errorNorm);
        double bbb = ((lambd / n) * psi - (X.transpose() * aaa / n).cwiseAbs()).cwiseAbs().transpose() * beta.cwiseAbs();
        dual = aaa.transpose() * (y / n) - bbb;
      } else {
        dual = (lambd / n) * (psi * beta.cwiseAbs()).sum();
      }

      // check for convergence
      double diff = (beta - betaOld).cwiseAbs().sum();
      // double diff = ((beta - betaOld).cwiseAbs() * (sdVec / ySd)).sum();
      if (diff < optTol) {
        if ((fobj - dual)  < 1e-6) {
        // if ((fobj - dual) / ySd < optTol) {
          break;
        }
      }
    }
  }
  // set beta to zero below threshold
  for (int j = 0; j < p; j++) {
    if (abs(beta(j)) < zeroTol) {
      beta(j) = 0;
    }
  }
  return beta;
}

// docstring
string docstring = R"mydelimiter(
    "Lasso Shooting algorithm for and sqrt lasso."

    Parameters
    ----------
    X : numpy array
        design matrix
    y : numpy array
        response vector
    XX : ndarray
      cross product matrix of X
    Xy : ndarray
      cross product vector of X and y
    lambd : float
        Regularization parameter.
    psi : float
        Penalty loadings.
    starting_values : ndarray, optional, default: None
        Initial beta estimate.
    sqrt : bool, optional, default False
        If True, use sqrt lasso.
        beta = min ||(y - X @ beta)||_2^2 + lambd ||psi @ beta||_1
    fit_intercept : bool, optional, default true
        If True, fit intercept. Only relevant for sqrt lasso.
    max_iter : int, optional, default: 1000
        Maximum number of iterations.
    opt_tol : float, optional, default: 1e-10
        Optimality tolerance.
    zero_tol : float, optional, default: 1e-4
        Zero tolerance. If beta(j) is smaller than zero_tol,
        set beta(j) = 0.

    Returns
    -------
    beta : ndarray
        Estimated beta.
)mydelimiter";

PYBIND11_MODULE(_solver_fast, m) {
  py::options options;
  options.disable_function_signatures();
  m.doc() = "Coordinate descent solver for lasso and sqrt-lasso";
  m.def("_cd_solver", &coordinateDescent, docstring.c_str(),
        py::arg("X").noconvert() = NULL,
        py::arg("y").noconvert() = NULL,
        py::arg("XX").noconvert() = NULL,
        py::arg("Xy").noconvert() = NULL,
        py::arg("lambd").noconvert() = NULL,
        py::arg("psi").noconvert() = NULL,
        py::arg("starting_values").noconvert() = NULL,
        py::arg("sqrt") = false,
        py::arg("fit_intercept") = true,
        py::arg("opt_tol") = 1e-10,
        py::arg("max_iter") = 1000,
        py::arg("zero_tol") = 1e-4);
};


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <Eigen/Geometry>

#include "Tudat/Astrodynamics/Relativity/relativisticAccelerationCorrection.h"

namespace tudat
{

namespace relativity
{

//! Function to compute a term common to several relativistic acceleration terms
double calculateRelativisticAccelerationCorrectionsCommonterm(
        const double centralBodyGravitationalParameter,
        const double relativeDistance )
{
    return centralBodyGravitationalParameter /
            ( physical_constants::SPEED_OF_LIGHT * physical_constants::SPEED_OF_LIGHT *
              relativeDistance * relativeDistance * relativeDistance );
}

//! Function to compute the Schwarzschild term of the relativistic acceleration correction.
Eigen::Vector3d calculateScharzschildGravitationalAccelerationCorrection(
        const double centralBodyGravitationalParameter,
        const Eigen::Vector3d& relativePosition,
        const Eigen::Vector3d& relativeVelocity,
        const double relativeDistance,
        const double commonCorrectionTerm,
        const double ppnParameterGamma,
        const double ppnParameterBeta )
{
    Eigen::Vector3d acceleration = ( 2.0 * ( ppnParameterGamma + ppnParameterBeta ) *
                      centralBodyGravitationalParameter / relativeDistance - ppnParameterGamma *
                      relativeVelocity.dot( relativeVelocity ) ) * relativePosition +
            2.0 * ( 1.0 + ppnParameterGamma ) * ( relativePosition.dot( relativeVelocity ) ) * relativeVelocity;
    return commonCorrectionTerm * acceleration;
}

//! Function to compute the Schwarzschild term of the relativistic acceleration correction.
Eigen::Vector3d calculateScharzschildGravitationalAccelerationCorrection(
        double centralBodyGravitationalParameter,
        const Eigen::Vector6d& relativeState,
        double ppnParameterGamma,
        double ppnParameterBeta )
{
    return calculateScharzschildGravitationalAccelerationCorrection(
                centralBodyGravitationalParameter, relativeState.segment( 0, 3 ),
                relativeState.segment( 3, 3 ), relativeState.segment( 0, 3 ).norm( ),
                calculateRelativisticAccelerationCorrectionsCommonterm(
                    centralBodyGravitationalParameter, relativeState.segment( 0, 3 ).norm( ) ),
                ppnParameterGamma, ppnParameterBeta );
}

//! Function to compute the Lense-Thirring term of the relativistic acceleration correction.
Eigen::Vector3d calculateLenseThirringCorrectionAcceleration(
        const Eigen::Vector3d& relativePosition,
        const Eigen::Vector3d& relativeVelocity,
        const double relativeDistance,
        const double commonCorrectionTerm,
        const Eigen::Vector3d& centralBodyAngularMomentum,
        const double ppnParameterGamma )
{
    Eigen::Vector3d acceleration = 3.0 / (
                relativeDistance * relativeDistance ) *
            relativePosition.cross( relativeVelocity ) *
            ( relativePosition.dot( centralBodyAngularMomentum ) ) +
            relativeVelocity.cross( centralBodyAngularMomentum );
    return acceleration * ( 1.0 + ppnParameterGamma ) * commonCorrectionTerm;
}

//! Function to compute the Lense-Thirring term of the relativistic acceleration correction.
Eigen::Vector3d calculateLenseThirringCorrectionAcceleration(
        const double centralBodyGravitationalParameter,
        const Eigen::Vector6d& relativeState,
        const Eigen::Vector3d& centralBodyAngularMomentum,
        const double ppnParameterGamma )
{
    return calculateLenseThirringCorrectionAcceleration(
                relativeState.segment( 0, 3 ), relativeState.segment( 3, 3 ), relativeState.segment( 0, 3 ).norm( ),
                calculateRelativisticAccelerationCorrectionsCommonterm(
                    centralBodyGravitationalParameter, relativeState.segment( 0, 3 ).norm( ) ),
                centralBodyAngularMomentum, ppnParameterGamma );

}

//! Function to compute the de Sitter term of the relativistic acceleration correction.
Eigen::Vector3d calculateDeSitterCorrectionAcceleration(
        const Eigen::Vector3d& orbiterRelativeVelocity,
        const Eigen::Vector3d& orbitedBodyPositionWrtLargerBody,
        const Eigen::Vector3d& orbitedBodyVelocityWrtLargerBody,
        const double commonCorrectionTermOfLargerBody,
        const double ppnParameterGamma )
{
    return - commonCorrectionTermOfLargerBody * ( 1.0 + 2.0 * ppnParameterGamma ) *
            ( orbitedBodyVelocityWrtLargerBody.cross( orbitedBodyPositionWrtLargerBody ) ).cross( orbiterRelativeVelocity );
}

//! Function to compute the de Sitter term of the relativistic acceleration correction.
Eigen::Vector3d calculateDeSitterCorrectionAcceleration(
        const double largerBodyGravitationalParameter,
        const Eigen::Vector6d& orbiterRelativeState,
        const Eigen::Vector6d&orbitedBodyStateWrtLargerBody,
        const double ppnParameterGamma )
{
    return calculateDeSitterCorrectionAcceleration(
                orbiterRelativeState.segment( 3, 3 ),
                orbitedBodyStateWrtLargerBody.segment( 0, 3 ),
                orbitedBodyStateWrtLargerBody.segment( 3, 3 ),
                calculateRelativisticAccelerationCorrectionsCommonterm(
                    largerBodyGravitationalParameter,
                    orbitedBodyStateWrtLargerBody.segment( 0, 3 ).norm( ) ),
                ppnParameterGamma );
}

}

}


// The MIT License (MIT)
//
// Copyright (c) 2014 Julian Gehring
//
// 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 <boost/math/distributions/hypergeometric.hpp>
#include <algorithm>

double fisher_test(int a, int b, int c, int d) {
  unsigned N = a + b + c + d;
  unsigned r = a + c;
  unsigned n = c + d;
  unsigned max_for_k = std::min(r, n);
  unsigned min_for_k = (unsigned)std::max(0, int(r + n - N));
  boost::math::hypergeometric_distribution<> hgd(r, n, N);
  double cutoff = pdf(hgd, c);
  double tmp_p = 0.0;
  for(int k = min_for_k;k < max_for_k + 1;k++) {
    double p = pdf(hgd, k);
    if(p <= cutoff) tmp_p += p;
  }
  return tmp_p;
}


/**
 * @file stableevaluationatapoint_main.cc
 * @brief NPDE homework StableEvaluationAtAPoint
 * @author Amélie Loher, Erick Schulz & Philippe Peter
 * @date 29.11.2021
 * @copyright Developed at SAM, ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <utility>

#include "stableevaluationatapoint.h"

int main(int /*argc*/, const char ** /*argv*/) {
  // exact solution
  auto uExact = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };
  // Fixed evaluation point
  Eigen::Vector2d x(0.3, 0.4);
  std::cout << "Exact evaluation at (0.3,0.4) : " << uExact(x) << std::endl;

  // Number of meshes used in the error analysis:
  int N_meshes = 6;

  // Error analysis vectors:
  Eigen::VectorXd mesh_sizes(N_meshes);
  mesh_sizes.setZero();
  Eigen::VectorXd dofs(N_meshes);
  dofs.setZero();

  // Error vector used for the error analysis in exercise b)
  Eigen::VectorXd errors_potential(N_meshes);
  errors_potential.setZero();

  // Error vectors used for the error analysis in exercise h
  Eigen::VectorXd errors_direct(N_meshes);
  errors_direct.setZero();
  Eigen::VectorXd errors_stable(N_meshes);
  errors_stable.setZero();

  // iterate over meshes:
  for (int k = 0; k < N_meshes; k++) {
    // read mesh::
    std::string idx = std::to_string(k + 1);
    auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    lf::io::GmshReader reader(std::move(mesh_factory), CURRENT_SOURCE_DIR
                                                           "/../meshes/square" +
                                                           idx + ".msh");
    auto mesh_p = reader.mesh();

    // Initialize fe-space and dofh
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
    const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
    dofs(k) = dofh.NumDofs();

    // Printing mesh statistics
    mesh_sizes(k) = StableEvaluationAtAPoint::MeshSize(mesh_p);
    std::cout << "square" + idx + ".msh: "
              << "N_dofs = " << dofs(k) << ", h=" << mesh_sizes(k) << std::endl;

    // Error anlysis part b) (Potentials)
    errors_potential(k) = StableEvaluationAtAPoint::PointEval(mesh_p);

    // error analysis part g/h: Compare direct vs stable point evaluation:
    auto [direct_eval, stable_eval] =
        StableEvaluationAtAPoint::ComparePointEval(fe_space, uExact, x);
    errors_direct(k) = std::abs(uExact(x) - direct_eval);
    errors_stable(k) = std::abs(uExact(x) - stable_eval);
  }

  // Compute rates of convergence:
  Eigen::VectorXd rates_potential(N_meshes - 1);
  Eigen::VectorXd rates_direct(N_meshes - 1);
  Eigen::VectorXd rates_stable(N_meshes - 1);

#if SOLUTION
  for (int k = 0; k < N_meshes - 1; ++k) {
    double log_denum = std::log(mesh_sizes(k) / mesh_sizes(k + 1));
    rates_potential(k) =
        std::log(errors_potential(k) / errors_potential(k + 1)) / log_denum;
    rates_direct(k) =
        std::log(errors_direct(k) / errors_direct(k + 1)) / log_denum;
    rates_stable(k) =
        std::log(errors_stable(k) / errors_stable(k + 1)) / log_denum;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  // Report computed errors and rates:
  std::cout << "Subtask b) Evaluation based on Potentials \n";
  std::cout << "Errors: \n" << errors_potential << "\n";
  std::cout << "Rates: \n" << rates_potential << "\n";
  std::cout << "Subtask h) Comparison of direct and stable evaluation: \n";
  std::cout << "Errors direct: \n" << errors_direct << "\n";
  std::cout << "Rates direct: \n" << rates_direct << "\n";
  std::cout << "Errors stable: \n" << errors_stable << "\n";
  std::cout << "Rates stable: \n" << rates_stable << "\n";

  // Output
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");

  Eigen::MatrixXd convergence_potential(N_meshes, 2);
  convergence_potential << mesh_sizes, errors_potential;

  Eigen::MatrixXd convergence_stable(N_meshes, 3);
  convergence_stable << mesh_sizes, errors_direct, errors_stable;

  std::ofstream file;
  file.open("convergence_potential.csv");
  file << "h, Error u(x) (Potential) \n";
  file << convergence_potential.format(CSVFormat);
  file.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/convergence_potential.csv"
            << std::endl;

  file.open("convergence_stable.csv");
  file << "h, Error u(x) (Direct), Error u(x) (Stable) \n";
  file << convergence_stable.format(CSVFormat);
  file.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/convergence_stable.csv"
            << std::endl;

  // Plot
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_convergence_potential.py " CURRENT_BINARY_DIR);
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_convergence_stable.py " CURRENT_BINARY_DIR);

  return 0;
}


#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <numeric>
#include <vector>
#include <memory>
#include <map>
#include <tuple>
#include <exception>
#include <iostream>
#include <functional>
#include <mutex>          // std::mutex
#include "ThreadPool.h"
#include "cubature.h"
#if !defined(NO_CUBA)
#include "cuba.h"
#endif
#include "MultiComplex/MultiComplex.hpp"

#include "nlohmann/json.hpp"

double factorial(double n) {
    return std::tgamma(n + 1);
}

std::mutex mtx;  // mutex for cout

#if !defined(M_PI)
constexpr auto M_PI = 3.14159265358979323846;
#endif

auto geomspace(double xmin, double xmax, int N) {
    std::vector<double> vec;
    double dT = (log(xmax) - log(xmin)) / (N - 1);
    for (auto i = 0; i < N; ++i) {
        vec.push_back(exp(log(xmin) + dT * i));
    }
    return vec;
}

double gr = (sqrt(5) + 1) / 2;

auto gss(std::function<double(double)> f, double a, double b, const double tol = 1e-5) {
    /*
    Golden section search
    C++ translation of https://en.wikipedia.org/wiki/Golden-section_search#Algorithm
    Text is available under the Creative Commons Attribution-ShareAlike License
    */
    auto c = b - (b - a) / gr;
    auto d = a + (b - a) / gr;
    while (abs(c - d) > tol) {
        if (f(c) < f(d)) {
            b = d;
        }
        else {
            a = c;
        }
        // We recompute both c and d here to avoid loss of precision which may lead to incorrect results or infinite loop
        c = b - (b - a) / gr;
        d = a + (b - a) / gr;
    }
    return std::make_tuple((b + a) / 2, f((b + a) / 2));
}

/*
Trapezoidal integration -- the workhorse numerical integration routine
*/
template<typename TYPEX, typename TYPEY>
TYPEY trapz(const Eigen::Array<TYPEX, Eigen::Dynamic, 1>& x,
           const Eigen::Array<TYPEY, Eigen::Dynamic, 1>& y) {
    TYPEY out = 0;
    for (auto i = 0; i < x.size()-1; ++i) {
        auto ymean = (y[i+1]+y[i])/2.0;
        out = out + ymean*(x[i+1] - x[i]);
    }
    return out;
}
/*
Simpson's integration rule
*/
template<typename TYPEX, typename TYPEY>
TYPEY simps(const Eigen::Array<TYPEX, Eigen::Dynamic, 1>& x,
           const Eigen::Array<TYPEY, Eigen::Dynamic, 1>& f) {
    // C++ translation of https://en.wikipedia.org/wiki/Simpson%27s_rule#Composite_Simpson's_rule_for_irregularly_spaced_data
    auto N = x.size() - 1;
    auto h = x.tail(N) - x.head(N);
    auto cube = [](TYPEX x) { return x * x * x; };
    auto sq = [](TYPEX x) { return x * x; };
    TYPEY result = 0.0;
    for (auto i = 1; i < N; i += 2) {
        auto hph = h[i] + h[i - 1];
        result += f[i] * (cube(h[i]) + cube(h[i - 1]) + 3.0 * h[i] * h[i - 1] * hph) / (6 * h[i] * h[i - 1]);
        result += f[i - 1] * (2.0 * cube(h[i - 1.0]) - cube(h[i]) + 3.0 * h[i] * sq(h[i - 1])) / (6 * h[i - 1] * hph);
        result += f[i + 1] * (2.0 * cube(h[i]) - cube(h[i - 1]) + 3.0 * h[i - 1] * sq(h[i])) / (6 * h[i] * hph);
    }
    if ((N + 1) % 2 == 0) {
        result += f[N] * (2 * sq(h[N - 1]) + 3.0 * h[N - 2] * h[N - 1]) / (6 * (h[N - 2] + h[N - 1]));
        result += f[N - 1] * (sq(h[N - 1]) + 3.0 * h[N - 1] * h[N - 2]) / (6 * h[N - 2]);
        result -= f[N - 2] * cube(h[N - 1]) / (6 * h[N - 2] * (h[N - 2] + h[N - 1]));
    }
    return result;
}

template <typename TYPE>
class Molecule {

public:
    using EColArray = Eigen::Array<TYPE, Eigen::Dynamic, 1>;
    using CoordMatrix = Eigen::Array<TYPE, 3, Eigen::Dynamic>;
    CoordMatrix coords, coords_initial;

    Molecule(const std::vector<std::vector<TYPE>>& pts) {
        coords.resize(3, pts.size());
        for (auto i = 0; i < pts.size(); ++i) {
            auto &pt = pts[i];
            for (auto j = 0; j < pt.size(); ++j) {
                coords(j, i) = pt[j];
            }
        }
        coords_initial = coords;
    };
    void reset() {
        coords = coords_initial;
    };
    CoordMatrix rotZ3(TYPE theta) const {
        // Rotation matrix for rotation around the z axis in 3D
        // See https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
        return (Eigen::ArrayXXd(3,3) <<
            cos(theta), -sin(theta), 0,
            sin(theta), cos(theta),  0,
                0,           0,      1 ).finished();
    };
    CoordMatrix rotY3(const TYPE theta) const {
        // Rotation matrix for rotation around the y axis in 3D
        // See https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
        const TYPE c = cos(theta), s = sin(theta);
        return (Eigen::ArrayXXd(3,3) <<
            c,  0, s,
            0,  1, 0,
            -s, 0, c).finished();
    }
    CoordMatrix rotX3(TYPE theta) const {
        // Rotation matrix for rotation around the x axis in 3D
        // See https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
        return (Eigen::ArrayXXd(3,3) <<
            1,      0,          0,
            0, cos(theta), -sin(theta),
            0, sin(theta), cos(theta)).finished();
    }
    void rotate_plusx(TYPE angle) {
        Eigen::Transform<double, 3, Eigen::Affine> rot(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()));
        coords = rot.linear() * coords.matrix();
        //coords = rotX3(angle).matrix() * coords.matrix(); // Old method
    }
    void rotate_negativex(TYPE angle) {
        rotate_plusx(-angle);
    }
    void rotate_plusy(TYPE angle) {
        Eigen::Transform<double, 3, Eigen::Affine> rot(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitY()));
        coords = rot.linear() * coords.matrix();
        //coords = rotY3(angle).matrix() * coords.matrix(); // Old method
    }
    void rotate_negativey(TYPE angle) {
        rotate_plusy(-angle);
    }
    void rotate_plusz(TYPE angle) {
        Eigen::Transform<double, 3, Eigen::Affine> rot(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));
        coords = rot.linear()*coords.matrix();
    }
    void rotate_negativez(TYPE angle) {
        rotate_plusz(-angle);
    }
    void translatex(TYPE dx) {
        coords.row(0) += dx;
    }
    void translatey(TYPE dy) {
        coords.row(1) += dy;
    }
    template<typename ArrayType>
    TYPE get_dist(const ArrayType&rA, const ArrayType&rB) const{
        return sqrt((rA - rB).square().sum());
    }
    Eigen::Index get_Natoms() const {
        return coords.cols();
    }
    auto get_xyz_atom(const Eigen::Index i) const {
        if (i > coords.cols()) {
            throw std::invalid_argument("Bad atom index");
        }
        return coords.col(i);
    }
};

template<typename TYPE>
class PotentialEvaluator {
public:
    std::map<std::tuple<std::size_t, std::size_t>, std::function<double(double)> > potential_map;

    ///< Additional contributions, handled in a very generic way via a callback
    std::vector<std::function<double(const Molecule<TYPE> &, const Molecule<TYPE>&)>> generic_contributions;

    TYPE eval_pot(const Molecule<TYPE>& molA, const Molecule<TYPE>& molB) const {
        TYPE u = 0.0;
        // Sum up site-site contributions to total potential energy
        for (auto iatom = 0; iatom < molA.get_Natoms(); ++iatom) {
            auto xyzA = molA.get_xyz_atom(iatom);
            for (auto jatom = 0; jatom < molB.get_Natoms(); ++jatom) {
                auto xyzB = molB.get_xyz_atom(jatom);
                TYPE distij = molA.get_dist(xyzA, xyzB);
                auto f_potential = get_potential(iatom, jatom);
                u += f_potential(distij);
            }
        }
        // Add also contributions for other kinds of interactions (e.g., dipole, quadrupole, etc.)
        for (auto contrib : generic_contributions) {
            u += contrib(molA, molB);
        }
        return u;
    }
    /*
    Connect up all the site-site potentials, all molecules have the same number of sites
    */
    void connect_potentials(std::function<double(double)>& f,std::size_t Natoms) {
        for (auto iatom = 0; iatom < Natoms; ++iatom) {
            for (auto jatom = 0; jatom < Natoms; ++jatom) {
                potential_map[std::make_tuple(iatom, jatom)] = f;
            }
        }
    }
    /*
    Add a potential for a particular site-site interaction

    @param iatom The index of site on molecule A
    @param jatom The index of site on molecule B
    */
    void add_potential(std::size_t iatom, std::size_t jatom, std::function<double(double)>& f) {
        potential_map[std::make_tuple(iatom, jatom)] = f;
    }
    /*
    Add a generalized contribution to the overall potential energy.  Callback function takes two molecules
    */
    void add_generic_contribution(const std::function<double(const Molecule<TYPE>&, const Molecule<TYPE>&)>& f) {
        generic_contributions.push_back(f);
    }
    /*
    Get a reference to the potential function for a particular site-site interaction

    @param i The index of site on molecule A
    @param j The index of site on molecule B

    */
    auto& get_potential(std::size_t i, std::size_t j) const {
        auto itf = potential_map.find(std::make_tuple(i, j));
        if (itf != potential_map.end()) {
            return itf->second;
        }
        else {
            throw std::invalid_argument("Bad potential");
        }
    }
};
/// A helper class
template<typename TYPE, typename TEMPTYPE>
class SharedDataBase {
public:
    TEMPTYPE Tstar;
    TYPE rstar;
    Molecule<TYPE> molA, molB;
    std::valarray<TYPE> xmin, xmax;
    const PotentialEvaluator<TYPE>& evaltr;
    SharedDataBase(TEMPTYPE Tstar, TYPE rstar,
        Molecule<TYPE> molA, Molecule<TYPE> molB,
        const PotentialEvaluator<TYPE>& evaltr,
        const std::valarray<TYPE>& xmin = {},
        const std::valarray<TYPE>& xmax = {}
    )
        : Tstar(Tstar), rstar(rstar), molA(molA), molB(molB), evaltr(evaltr), xmin(xmin), xmax(xmax) {};

    TYPE eval_pot(const Molecule<TYPE>& molA, const Molecule<TYPE>& molB) {
        return evaltr.eval_pot(molA, molB);
    };

    /*
    Given the orientational angles, calculate the integrand
    */
    void orient_integrand(double theta1, double theta2, double phi, double *fval)
    {
        // Rotate molecule #1
        molA.reset(); // Back to COM at origin
        molA.rotate_negativey(theta1); // First rotate around -y axis

        // Rotate and move molecule #2
        molB.reset(); // Back to COM at origin
        molB.rotate_negativey(theta2); // First rotate around -y axis
        molB.rotate_negativex(phi); // Then rotate around +x
        molB.translatex(rstar); // Then translate

        auto V = eval_pot(molA, molB); // And finally evaluate the potential
        auto a = (exp(-V/Tstar)-1.0)*sin(theta1)*sin(theta2)*pow(rstar, 2);

        if constexpr (std::is_same<decltype(Tstar), double>::value) {
            // If T is double (real)
            fval[0] = a;
        }
        else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
            // If T is a complex number (perhaps for complex step derivatives)
            fval[0] = a.real();
            fval[1] = a.imag();
        }
        else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
            // If T is a multicomplex number
            auto &c = a.get_coef();
            for (auto i = 0; i < c.size(); ++i) {
                fval[i] = c[i];
            }
        }
    }
    /*
    Given the separations and angle, calculate the integrand for B_3 for a spherically-symmetric potential
    for an atomic fluid
    */
    void atomic_B3_integrand(const double r12, const double r13, const double eta_angle, double* fval)
    {
        // Get the potential function V(r) that we should use
        auto &pot = this->evaltr.get_potential(0, 0);
        TEMPTYPE Tstar = this->Tstar; // Local reference just for sharing with the lambda function f
        auto f = [pot, Tstar](double r) -> TEMPTYPE { return 1.0 - exp(-pot(r)/Tstar); };
        auto SQUARE = [](double x) { return x*x; };
        auto rangle = sqrt(SQUARE(r12) + SQUARE(r13) - 2*r12*r13*eta_angle);
        auto a = SQUARE(r12)*f(r12)*SQUARE(r13)*f(r13)*f(rangle);

        if constexpr (std::is_same<decltype(Tstar), double>::value) {
            // If T is double (real)
            fval[0] = a;
        }
        else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
            // If T is a complex number (perhaps for complex step derivatives)
            fval[0] = a.real();
            fval[1] = a.imag();
        }
        else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
            // If T is a multicomplex number
            auto& c = a.get_coef();
            for (auto i = 0; i < c.size(); ++i) {
                fval[i] = c[i];
            }
        }
    }

	/*
	Given the separations and angles, calculate the integrand for B4_1 for a spherically-symmetric potential
	for an atomic fluid
	*/
	void atomic_B4_1_integrand(const double r14, const double r13, const double gamma_angle, const double r12, const double eta_angle, double* fval)
	{
		// Get the potential function V(r) that we should use
		auto &pot = this->evaltr.get_potential(0, 0);
		TEMPTYPE Tstar = this->Tstar; // Local reference just for sharing with the lambda function f
		auto f = [pot, Tstar](double r) -> TEMPTYPE { return 1.0 - exp(-pot(r) / Tstar); };
		auto SQUARE = [](double x) { return x * x; };
		auto sq_r12 = SQUARE(r12);
		auto sq_r13 = SQUARE(r13);
		auto sq_r14 = SQUARE(r14);
		auto rangle_12_13 = sqrt(sq_r12 + sq_r13 - 2 * r12*r13*eta_angle);
		auto rangle_13_14 = sqrt(sq_r14 + sq_r13 - 2 * r14*r13*gamma_angle);

		auto a = sq_r12 * f(r12)*sq_r13*sq_r14*f(r14)*f(rangle_12_13)*f(rangle_13_14);

		if constexpr (std::is_same<decltype(Tstar), double>::value) {
			// If T is double (real)
			fval[0] = a;
		}
		else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
			// If T is a complex number (perhaps for complex step derivatives)
			fval[0] = a.real();
			fval[1] = a.imag();
		}
		else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
			// If T is a multicomplex number
			auto& c = a.get_coef();
			for (auto i = 0; i < c.size(); ++i) {
				fval[i] = c[i];
			}
		}
	}

	/*
Given the separations and angles, calculate the integrand for B4_2 for a spherically-symmetric potential
for an atomic fluid
*/
	void atomic_B4_2_integrand(const double eta_angle, const double r12, const double r13, const double gamma_angle, const double r14, double* fval)
	{
		// Get the potential function V(r) that we should use
		auto &pot = this->evaltr.get_potential(0, 0);
		TEMPTYPE Tstar = this->Tstar; // Local reference just for sharing with the lambda function f
		auto f = [pot, Tstar](double r) -> TEMPTYPE { return 1.0 - exp(-pot(r) / Tstar); };
		auto SQUARE = [](double x) { return x * x; };
		auto sq_r12 = SQUARE(r12);
		auto sq_r13 = SQUARE(r13);
		auto sq_r14 = SQUARE(r14);
		auto rangle_12_13 = sqrt(sq_r12 + sq_r13 - 2 * r12*r13*eta_angle);
		auto rangle_13_14 = sqrt(sq_r14 + sq_r13 - 2 * r14*r13*gamma_angle);

		auto a = sq_r12 * f(r12)*sq_r13*f(r13)*sq_r14*f(r14)*f(rangle_12_13)*f(rangle_13_14);

		if constexpr (std::is_same<decltype(Tstar), double>::value) {
			// If T is double (real)
			fval[0] = a;
		}
		else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
			// If T is a complex number (perhaps for complex step derivatives)
			fval[0] = a.real();
			fval[1] = a.imag();
		}
		else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
			// If T is a multicomplex number
			auto& c = a.get_coef();
			for (auto i = 0; i < c.size(); ++i) {
				fval[i] = c[i];
			}
		}
	}

	/*
Given the separations and angles, calculate the integrand for B4_3 for a spherically-symmetric potential
for an atomic fluid
*/
	void atomic_B4_3_integrand(const double eta_angle, const double zeta_angle, const double gamma_angle, const double r12, const double r13, const double r14, double* fval)
	{
		// Get the potential function V(r) that we should use
		auto &pot = this->evaltr.get_potential(0, 0);
		TEMPTYPE Tstar = this->Tstar; // Local reference just for sharing with the lambda function f
		auto f = [pot, Tstar](double r) -> TEMPTYPE { return 1.0 - exp(-pot(r) / Tstar); };
		auto SQUARE = [](double x) { return x * x; };
		auto sq_r12 = SQUARE(r12);
		auto sq_r13 = SQUARE(r13);
		auto sq_r14 = SQUARE(r14);
		auto rangle_12_13 = sqrt(sq_r12 + sq_r14 - 2 * r12*r14*eta_angle);
		auto rangle_13_14 = sqrt(sq_r13 + sq_r14 - 2 * r13*r14*gamma_angle);
		auto rangle_12_14 = sqrt(sq_r12 + sq_r13 - 2 * r12*r13*(eta_angle * gamma_angle + sqrt(1.0 - SQUARE(eta_angle))*sqrt(1.0 - SQUARE(gamma_angle))*cos(zeta_angle)));
		auto a = sq_r12 * f(r12)*sq_r13*f(r13)*sq_r14*f(r14)*f(rangle_12_13)*f(rangle_13_14)*f(rangle_12_14);

		if constexpr (std::is_same<decltype(Tstar), double>::value) {
			// If T is double (real)
			fval[0] = a;
		}
		else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
			// If T is a complex number (perhaps for complex step derivatives)
			fval[0] = a.real();
			fval[1] = a.imag();
		}
		else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
			// If T is a multicomplex number
			auto& c = a.get_coef();
			for (auto i = 0; i < c.size(); ++i) {
				fval[i] = c[i];
			}
		}
	}
};

template<typename TYPE>
class Integrator {
private:
    std::unique_ptr<ThreadPool> m_pool;
    nlohmann::json m_conf;

public:
    using EColArray = Eigen::Array<TYPE, Eigen::Dynamic, 1>;
    const Molecule<TYPE> mol1, mol2;
    PotentialEvaluator<TYPE> potcls;

    Integrator(const Molecule<TYPE>& mol1, const Molecule<TYPE>& mol2) : mol1(mol1), mol2(mol2) {};

    auto& get_conf_view() {
        return m_conf;
    }

    /* For a one-dimensional integration for B_2, use trapezoidal integration to calculate B_2 */
    template <typename TEMPTYPE>
    TEMPTYPE radial_integrate_B2(TEMPTYPE Tstar, TYPE rstart, TYPE rend, int N) {
        using arr = Eigen::Array<TYPE, Eigen::Dynamic, 1>;
        using arrT = Eigen::Array<TEMPTYPE, Eigen::Dynamic, 1>;
        arr rv = exp(arr::LinSpaced(N, log(rstart), log(rend)));
        arrT integrand;
        integrand.resize(rv.size());
        Molecule<TYPE> mol1 = this->mol1, mol2 = this->mol2;
        for (auto ir = 0; ir < rv.size(); ++ir) {
            auto r = rv[ir];
            mol2.reset();
            mol2.translatex(r);
            auto V = potcls.eval_pot(mol1, mol2);
            integrand[ir] = (exp(-V/Tstar)-1.0)*r*r;
        }
        return -2*M_PI*trapz(rv, integrand);
    };
    /*
    Get a reference to the evaluator class, giving access to matrix of site-site potential functions, for instance
    */
    auto &get_evaluator() {
        return potcls;
    }

    void init_thread_pool(short Nthreads) {
        if (!m_pool || m_pool->GetThreads().size() != Nthreads) {
            // Make a thread pool for the workers
            m_pool = std::unique_ptr<ThreadPool>(new ThreadPool(Nthreads));
        }
    }
    /*
    A helper function to evaluate the potential given COM separation r and the orientation angles
    */
    double potential(double r, double theta1, double theta2, double phi){
        Molecule<TYPE> molA = mol1, molB = mol2;
        // Rotate molecule #1
        molA.reset(); // Back to COM at origin
        molA.rotate_negativey(theta1); // First rotate around -y axis

        // Rotate and move molecule #2
        molB.reset(); // Back to COM at origin
        molB.rotate_negativey(theta2); // First rotate around -y axis
        molB.rotate_negativex(phi); // Then rotate around +x
        molB.translatex(r); // Then translate

        auto V = potcls.eval_pot(molA, molB); // And finally evaluate the potential in the form V/epsilon
        return V;
    }
    /*
    Calculate the orientationally-averaged potential
    */
    TYPE orient_averaged_potential(TYPE rstar, Molecule<TYPE> mol1, Molecule<TYPE> mol2) const {
        using SharedData = SharedDataBase<TYPE, double>;
        SharedData shared(0.0, 0.0, mol1, mol2, potcls, {0,0,0}, { M_PI, M_PI, 2 * M_PI });
        //typedef int (*integrand) (unsigned ndim, const double *x, void *, unsigned fdim, double* fval);
        auto f_integrand = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
            auto& shared = *((class SharedDataBase<double, double>*)(p_shared_data));
            auto& molA = shared.molA;
            auto& molB = shared.molB;
            double theta1 = x[0], theta2 = x[1], phi = x[2];
            shared.orient_integrand(theta1, theta2, phi, fval);
            return 0; // success
        };
        shared.rstar = rstar;
        int ndim = 1;
        std::valarray<double> val(0.0, 4), err(0.0, 4);
        hcubature(ndim, f_integrand, &shared, 3, &(shared.xmin[0]), &(shared.xmax[0]), 100000, 0, 1e-13, ERROR_INDIVIDUAL, &(val[0]), &(err[0]));
        return val[0]/(8*M_PI);
    }

    /**
    Do the calculations for one temperature

    @param order The order of the virial coefficient (2=B_2, 3=B_3, etc.)
    @param Tstar The temperature
    @param rstart The initial value of r to be considered in integration
    @param rend The final value of r to be considered in integration
    @param mol1 The first molecule
    @param mol2 The second molecule
    @returns Tuple of (value, estimated error in value)
    @note The return numerical type maybe be one of double, std::complex<double> or MultiComplex<double>
    */
    template <typename TEMPTYPE>
    std::tuple<TEMPTYPE,TEMPTYPE> one_temperature(int order, TEMPTYPE Tstar, TYPE rstart, TYPE rend, Molecule<TYPE> mol1, Molecule<TYPE> mol2) const
    {

        // Some local typedefs to avoid typing
        using SharedData = SharedDataBase<TYPE, TEMPTYPE>;

        std::vector<std::valarray<double>> xmins, xmaxs;
        switch (order)
        {
        case 2:
            xmins = {{ 0, 0, 0, rstart }}, xmaxs = {{ M_PI, M_PI, 2 * M_PI, rend }}; // Limits on theta1, theta2, phi, r
            break;
        case 3:{
            double rbreak = 1.3;
            xmins = {{ rstart, rstart, -1 },{ rbreak, rstart, -1 } }, xmaxs = {{ rbreak, rend, 1 },{ rend, rend, 1 } }; // Limits on r12, r13, eta
            break;
            }
        case 4: {
            // Limits on r14, r13, gamma, r12, eta
            // Limits on eta, r12, r13, eta, r14
            // Limits on eta,zeta,gamma, r12, r13, r14
            xmins = { {rstart, rstart, -1 , rstart, -1 }, { -1, rstart, rstart, -1 , rstart }, { -1,  rstart, -1 , rstart, rstart , rstart } };
            xmaxs = { { rend, rend, 1 , rend, 1 }, { 1, rend, rend, 1 , rend }, { 1, 2 * M_PI, 1, rend, rend , rend } };
            break;
        }
        default:
            break;
        }
        SharedData shared(Tstar, 0.0, mol1, mol2, potcls);

        int ndim = 1; // If T is a floating point number (default)
        std::valarray<double> outval, outerr;
        std::vector<std::valarray<double >> vals(10), errs(10);
        if constexpr (std::is_same<decltype(shared.Tstar), std::complex<double>>::value) {
            ndim = 2;
        }
        else if constexpr (std::is_same<decltype(shared.Tstar), MultiComplex<double>>::value) {
            ndim = static_cast<int>(shared.Tstar.get_coef().size());
        }
        // Fill with zero
        for (auto& val : vals) {
            val = std::valarray<double>(0.0, ndim);
        }
        for (auto& err : errs) {
            err = std::valarray<double>(0.0, ndim);
        }
        outval = std::valarray<double>(0.0, ndim);
        outerr = std::valarray<double>(0.0, ndim);

        int feval_max = 0;
        if (m_conf.contains("feval_max")) {
            feval_max = static_cast<int>(m_conf["feval_max"]);
        }
        else {
            throw std::invalid_argument("Key \"feval_max\" must be specified in the configuration JSON");
        }

#if !defined(NO_CUBA)
        auto Cuba_integrand = [](const int *pndim, const cubareal x[], const int *pncomp, cubareal fval[], void *p_shared_data) {
            auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));

            double theta1, theta2, phi, r;
            double jacobian = 1.0;
            for (auto i  = 0; i < *pndim; ++i){
                auto range = shared.xmax[i] - shared.xmin[i];
                jacobian *= range;
            }
            theta1 = shared.xmin[0] + x[0]*(shared.xmax[0]-shared.xmin[0]);
            theta2 = shared.xmin[1] + x[1]*(shared.xmax[1]-shared.xmin[1]);
            phi =    shared.xmin[2] + x[2]*(shared.xmax[2]-shared.xmin[2]);
            r   =    shared.xmin[3] + x[3]*(shared.xmax[3]-shared.xmin[3]);
            shared.rstar = r;
            shared.orient_integrand(theta1, theta2, phi, fval);
            for (auto i = 0; i < *pncomp; ++i){
                fval[i] *= jacobian;
            }
            return 0; // success
        };

        int NVEC = 1;
        int EPSREL = 1e-8;
        int EPSABS = 1e-12;
        int VERBOSE = 0;
        int LAST = 4;
        int MINEVAL = 0;
        int MAXEVAL = feval_max;
        int NSTART = 1000;
        int NINCREASE = 500;
        int NBATCH = 1000;
        int GRIDNO = 0;
        const char *STATEFILE = nullptr;
        void *SPIN = nullptr;
        int neval, fail;
        cubareal integral[ndim], error[ndim], prob[ndim];

        int nregions;
        int KEY = 0;
        auto startTimeC = std::chrono::high_resolution_clock::now();
        Cuhre(4, ndim, Cuba_integrand, &shared, NVEC,
        EPSREL, EPSABS, VERBOSE | LAST,
        MINEVAL, MAXEVAL, KEY,
        STATEFILE, SPIN,
        &nregions, &neval, &fail, integral, error, prob);
        auto endTimeC = std::chrono::high_resolution_clock::now();
        auto timeC = std::chrono::duration<double>(endTimeC - startTimeC).count();

        // The quadruple integral needs to be divided by 8*pi, but the leading term in the
        // expression for B_2 is -2\pi, so factor becomes -1/4, or -0.25
        for (auto i = 0; i < outval.size(); ++i) {
            outval[i] = -0.25 * integral[i];
            outerr[i] = -0.25 * error[i];
        }
#else
        // Use cubature to do the integration...

        switch (order) {
        case 2:
        {
            // The integrand function
            //typedef int (*integrand) (unsigned ndim, const double *x, void *, unsigned fdim, double* fval);
            auto cubature_integrand = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
                auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));
                double theta1 = x[0], theta2 = x[1], phi = x[2], r = x[3];
                shared.rstar = r;
                shared.orient_integrand(theta1, theta2, phi, fval);
                return 0; // success
            };

            int naxes = 4; // How many dimensions the integral is taken over (theta, phi1, phi2, r)
            hcubature(ndim, cubature_integrand, &shared, naxes, &(xmins[0][0]), &(xmaxs[0][0]), feval_max, 0, 1e-13, ERROR_INDIVIDUAL, &(vals[0][0]), &(errs[0][0]));

            // Copy into output
            // ....
            // The quadruple integral needs to be divided by 8*pi, but the leading term in the
            // expression for B_2 is -2\pi, so factor becomes -1/4, or -0.25
            outval = -0.25*vals[0]; outerr = -0.25*errs[0];
            break;
        }
        case 3:
        {
            // The integrand function
            //typedef int (*integrand) (unsigned ndim, const double *x, void *, unsigned fdim, double* fval);
            auto cubature_integrand = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
                auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));
                shared.atomic_B3_integrand(x[0], x[1], x[2], fval);
                return 0; // success
            };

            int naxes = 3; // How many dimensions the integral is taken over (r12, r13, eta)
            for (auto i = 0; i < xmins.size(); ++i){

                hcubature(ndim, cubature_integrand, &shared, naxes, &(xmins[i][0]), &(xmaxs[i][0]), feval_max, 0, 1e-13, ERROR_INDIVIDUAL, &(vals[i][0]), &(errs[i][0]));

                // Copy into output
                outval += vals[i]; outerr += std::abs(errs[i]);
            }
            // Rescale with the leading factor
            outval *= 8*M_PI*M_PI/3; outerr *= 8*M_PI*M_PI/3;

            break;
        }
		case 4:
		{
			// Fourth virial coefficient: three parts B_4_1,B_4_2,B_4_3
			// The integrand functions
			//typedef int (*integrand) (unsigned ndim, const double *x, void *, unsigned fdim, double* fval);
			auto cubature_integrand_1 = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
				auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));
				shared.atomic_B4_1_integrand(x[0], x[1], x[2], x[3], x[4], fval);
				return 0; // success
			};

			auto cubature_integrand_2 = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
				auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));
				shared.atomic_B4_2_integrand(x[0], x[1], x[2], x[3], x[4], fval);
				return 0; // success
			};

			auto cubature_integrand_3 = [](unsigned ndim, const double* x, void* p_shared_data, unsigned fdim, double* fval) {
				auto& shared = *((class SharedDataBase<double, TEMPTYPE>*)(p_shared_data));
				shared.atomic_B4_3_integrand(x[0], x[1], x[2], x[3], x[4], x[5], fval);
				return 0; // success
			};

			// prefactors for each contribution
			std::valarray<double> pre_factors = {-3.0*(27.0/4.0), 3.0*(27.0/2.0), -27.0/(8.0*M_PI)};

			int naxes = 5; // How many dimensions the integral is taken over (r14, r13, gamma, r12, eta)
			hcubature(ndim, cubature_integrand_1, &shared, naxes, &(xmins[0][0]), &(xmaxs[0][0]), feval_max, 0, 1e-4, ERROR_INDIVIDUAL, &(vals[0][0]), &(errs[0][0]));
			hcubature(ndim, cubature_integrand_2, &shared, naxes, &(xmins[1][0]), &(xmaxs[1][0]), feval_max, 0, 1e-4, ERROR_INDIVIDUAL, &(vals[1][0]), &(errs[1][0]));

			naxes = 6;
			hcubature(ndim, cubature_integrand_3, &shared, naxes, &(xmins[2][0]), &(xmaxs[2][0]), feval_max, 0, 1e-4, ERROR_INDIVIDUAL, &(vals[2][0]), &(errs[2][0]));

			for (auto i = 0; i < pre_factors.size(); ++i) {
                vals[i] *= pre_factors[i];
                errs[i] *= pre_factors[i];
			}

            // Copy into output
			outval = vals[0] + vals[1] + vals[2];
			outerr = std::abs(errs[0]) + std::abs(errs[1]) + std::abs(errs[2]);

			break;
		}
        default:
            throw -1;
        }
#endif
        if constexpr (std::is_same<decltype(Tstar), double>::value) {
            // If T is double (real)
            return std::make_tuple(outval[0],outerr[0]);
        }
        else if constexpr (std::is_same<decltype(Tstar), std::complex<double>>::value) {
            // If T is a complex number (perhaps for complex step derivatives)
            return std::make_tuple(decltype(Tstar)(outval[0], outval[1]), decltype(Tstar)(outerr[0], outerr[1]));
        }
        else if constexpr (std::is_same<decltype(Tstar), MultiComplex<double>>::value) {
            // If T is a multicomplex number
            return std::make_tuple(decltype(Tstar)(outval), decltype(Tstar)(outerr));
        }
    }

    std::map<std::string, double> B_and_derivs(int order, int Nderivs, double T, double rstart, double rend, Molecule<TYPE> mol1, Molecule<TYPE> mol2){

        if (Nderivs == 0) {
            auto [val,esterr] = this->one_temperature(order, T, rstart, rend, mol1, mol2);
            return {
                {"T", T},
                {"B", val},
                {"error(B)", esterr}
            };
        }
        if (Nderivs == 1) {
            double h = 1e-100;
            auto [val,esterr] = this->one_temperature(order, std::complex<double>(T,h), rstart, rend, mol1, mol2);
            return {
                {"T", T},
                {"B", val.real()},
                {"error(B)", esterr.real()},
                {"dBdT", val.imag()/h},
                {"error(dBdT)", esterr.imag()/h},
            };
        }
        else {
            std::function<std::tuple<MultiComplex<double>,MultiComplex<double>>(const MultiComplex<double>&)> f(
                [this, order, rstart, rend, mol1, mol2](const MultiComplex<double>& T) {
                    return this->one_temperature(order, T, rstart, rend, mol1, mol2);
                });
            bool and_val = true;
            auto [val,esterr] = diff_mcx1(f, T, Nderivs, and_val);
            std::map<std::string, double> o = { {"T", T} };
            for (auto i = 0; i <= Nderivs; ++i) {
                switch (i) {
                case 0:
                    o["B"] = val[0];
                    o["error(B)"] = esterr[0];
                    break;
                case 1:
                    o["dBdT"] = val[1];
                    o["error(dBdT)"] = esterr[1];
                    break;
                default:
                    auto n = std::to_string(i);
                    o["d" + n + "BdT" + n] = val[i];
                    o["error(d" + n + "BdT" + n + ")"] = esterr[i];
                }
            }
            return o;
        }
    }
    auto parallel_B_and_derivs(int order, int Nthreads, int Nderivs, std::vector<double> Tvec, double rstart, double rend, Molecule<TYPE> mol1, Molecule<TYPE> mol2)
    {
        init_thread_pool(Nthreads);
        std::vector<double> times(Tvec.size());
        std::vector<std::map<std::string, double>> outputs(Tvec.size());
        std::size_t i = 0;
        for (auto T : Tvec) {
            auto& result = outputs[i];
            auto& time = times[i];
            std::function<void(void)> one_Temp = [this, order, Nderivs, T, rstart, rend, mol1, mol2, &result, &time]() {
                auto startTime = std::chrono::high_resolution_clock::now();
                result = this->B_and_derivs(order, Nderivs, T, rstart, rend, mol1, mol2);
                auto endTime = std::chrono::high_resolution_clock::now();
                time = std::chrono::duration<double>(endTime - startTime).count();
                {
                    mtx.lock();
                    std::cout << "Done " << T << " in " << time << " seconds\n" ;
                    mtx.unlock();
                }
                result["elapsed / s"] = time;
            };
            m_pool->AddJob(one_Temp);
            i++;
        }
        // Wait until all the threads finish...
        m_pool->WaitAll();
        return outputs;
    }
    TYPE orientation_averaged_integrate(const TYPE Tstar, const TYPE rstart, const TYPE rend) {
        using arr = Eigen::Array<TYPE, Eigen::Dynamic, 1>;
        bool parallel = false;
        if (parallel){
            // Parallel
            TYPE result, time;
            Molecule<TYPE> mol1 = this->mol1, mol2 = this->mol2;
            auto one_temperature_job = [this, Tstar, rstart, rend, mol1, mol2, &result, &time]() {
                auto startTime = std::chrono::high_resolution_clock::now();
                result = one_temperature<double>(Tstar, rstart, rend, mol1, mol2);
                auto endTime = std::chrono::high_resolution_clock::now();
                time = std::chrono::duration<double>(endTime - startTime).count();
            };
            m_pool->AddJob(one_temperature_job);
            // Wait until all the threads finish...
            m_pool->WaitAll();
            return result;
        }
        else {
            // Serial
            return one_temperature(Tstar, rstart, rend, mol1, mol2);
        }
    }
};


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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 <Testing/Utils/SCIRunUnitTests.h>
#include <fstream>
#include <boost/filesystem.hpp>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Math/SolveLinearSystemWithEigen.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixIO.h>
#define register
#include <Eigen/Sparse>
#undef register
#include <Testing/Utils/MatrixTestUtilities.h>
#include <Core/Algorithms/DataIO/EigenMatrixFromScirunAsciiFormatConverter.h>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::DataIO::internal;
using namespace SCIRun::Core;
using namespace SCIRun::TestUtils;
using namespace SCIRun;
using namespace ::testing;

/// @todo: remove these Eigen tests if they are duplicates of existing SLS tests. Other tests below need to live in separate files.

#if 0
TEST(SolveLinearSystemWithEigenAlgorithmTests, CanSolveBasicSmallDenseSystemWithEigenClasses)
{
  int n = 3;
  DenseMatrix m1(n,n);
  m1 << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  using namespace Eigen;
  VectorXd v(n);
  v << 1,2,3;
  std::cout << "expected solution = \n" << v << std::endl;

  auto rhs = m1*v;
  std::cout << "rhs = \n" << rhs << std::endl;

  VectorXd x(n);
  x.setZero();

  std::cout << "matrix to solve: \n" << m1 << std::endl;

  ConjugateGradient<DenseMatrix::EigenBase> cg;
  cg.compute(m1);

  ASSERT_TRUE(cg.info() == Success);

  cg.setTolerance(1e-15);
  x = cg.solve(rhs);

#if 0
  cg.setMaxIterations(1);
  int i = 0;
  do {
    x = cg.solveWithGuess(rhs, x);

    std::cout << i << " : " << cg.error() << std::endl;
    ++i;
  } while (cg.info() != Success && i < 2000);
#endif

  std::cout << "#iterations:     " << cg.iterations() << std::endl;
  std::cout << "estimated error: " << cg.error()      << std::endl;

  std::cout << x << std::endl;

  EXPECT_EQ(v, x);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, CanSolveBasicSmallDenseSystem)
{
  int n = 3;
  DenseMatrixHandle A(new DenseMatrix(n,n));
  *A << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  DenseColumnMatrix v(n);
  v << 1,2,3;
  std::cout << "expected solution = \n" << v << std::endl;

  DenseColumnMatrixHandle rhs(new DenseColumnMatrix(*A * v));
  std::cout << "rhs = \n" << *rhs << std::endl;

  SolveLinearSystemAlgorithm algo;

  auto x = algo.run(boost::make_tuple(A, rhs), boost::make_tuple(1e-15, 10));
  DenseColumnMatrixHandle solution = x.get<0>();

  ASSERT_TRUE(solution.get() != nullptr);
  EXPECT_EQ(v, *solution);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, CanSolveBasicSmallSparseSystem)
{
  int n = 3;
  DenseMatrix Adense(n,n);
  Adense << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  SparseRowMatrixHandle A(new SparseRowMatrix(n,n));
  copyDenseToSparse(Adense, *A);

  DenseColumnMatrix v(n);
  v << 1,2,3;
  std::cout << "expected solution = \n" << v << std::endl;

  DenseColumnMatrixHandle rhs(new DenseColumnMatrix(*A * v));
  std::cout << "rhs = \n" << *rhs << std::endl;

  SolveLinearSystemAlgorithm algo;

  auto x = algo.run(boost::make_tuple(A, rhs), boost::make_tuple(1e-15, 10));
  DenseColumnMatrixHandle solution = x.get<0>();

  ASSERT_TRUE(solution.get() != nullptr);
  EXPECT_EQ(v, *solution);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, ThrowsOnNullMatrix)
{
  DenseMatrixHandle A;

  DenseColumnMatrixHandle rhs(new DenseColumnMatrix(3));

  SolveLinearSystemAlgorithm algo;

  EXPECT_THROW(algo.run(boost::make_tuple(A, rhs), boost::make_tuple(1e-15, 10)), AlgorithmInputException);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, ThrowsOnNullRHS)
{
  int n = 3;
  DenseMatrixHandle A(new DenseMatrix(n,n));
  *A << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  DenseColumnMatrixHandle rhs;

  SolveLinearSystemAlgorithm algo;

  EXPECT_THROW(algo.run(boost::make_tuple(A, rhs), boost::make_tuple(1e-15, 10)), AlgorithmInputException);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, ThrowsOnNegativeTolerance)
{
  int n = 3;
  DenseMatrixHandle A(new DenseMatrix(n,n));
  *A << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  DenseColumnMatrix v(n);
  v << 1,2,3;

  DenseColumnMatrixHandle rhs(new DenseColumnMatrix(*A * v));

  SolveLinearSystemAlgorithm algo;

  EXPECT_THROW(algo.run(boost::make_tuple(A, rhs), boost::make_tuple(-4, 10)), AlgorithmInputException);
}

TEST(SolveLinearSystemWithEigenAlgorithmTests, ThrowsOnNegativeMaxIterations)
{
  int n = 3;
  DenseMatrixHandle A(new DenseMatrix(n,n));
  *A << 2,-1,0,
    -1,2,-1,
    0,-1,2;

  DenseColumnMatrix v(n);
  v << 1,2,3;

  DenseColumnMatrixHandle rhs(new DenseColumnMatrix(*A * v));

  SolveLinearSystemAlgorithm algo;

  EXPECT_THROW(algo.run(boost::make_tuple(A, rhs), boost::make_tuple(1e-15, -1)), AlgorithmInputException);
}
#endif
//todo: remove unused code
TEST(SparseMatrixReadTest, DISABLED_RegexOfScirun4Format)
{
  EigenMatrixFromScirunAsciiFormatConverter converter;

  auto file = TestResources::rootDir() / "sp2.mat";

  if (!boost::filesystem::exists(file))
  {
    FAIL() << "TODO: Issue #142 will standardize these file locations other than being on Dan's hard drive." << std::endl
        << "Once that issue is done however, this will be a user setup error." << std::endl;
    return;
  }

  std::string matStr = converter.readFile(file.string());

  //2 3 4 {8 0 2 4 }{8 0 2 0 1 }{1 3.5 -1 2 }}

  std::string contents = converter.getMatrixContentsLine(matStr).get_value_or("");

  std::string newline;
#ifndef WIN32
  newline += "\r";
#endif

  EXPECT_EQ("2 3 4 {8 0 2 4 }{8 0 2 0 1 }{1 3.5 -1 2 }}" + newline, contents);

  auto rawOpt = converter.parseSparseMatrixString(contents);
  ASSERT_TRUE(static_cast<bool>(rawOpt));
  auto raw = rawOpt.get();

  EXPECT_EQ("2", raw.get<0>());
  EXPECT_EQ("3", raw.get<1>());
  EXPECT_EQ("4", raw.get<2>());
  EXPECT_EQ("0 2 4 ", raw.get<3>());
  EXPECT_EQ("0 2 0 1 ", raw.get<4>());
  EXPECT_EQ("1 3.5 -1 2 ", raw.get<5>());

  auto data = converter.convertRaw(raw);
  EXPECT_EQ(2, data.get<0>());
  EXPECT_EQ(3, data.get<1>());
  EXPECT_EQ(4, data.get<2>());
  EXPECT_THAT(data.get<3>(), ElementsAre(0, 2, 4));
  EXPECT_THAT(data.get<4>(), ElementsAre(0, 2, 0, 1));
  EXPECT_THAT(data.get<5>(), ElementsAre(1.0, 3.5, -1.0, 2.0));

  auto mat = converter.makeSparse(file.string());
  ASSERT_TRUE(mat.get() != nullptr);
  EXPECT_EQ(2, mat->rows());
  EXPECT_EQ(3, mat->cols());

  DenseMatrix a(2, 3);
  a << 1, 0, 3.5,
    -1, 2, 0;

  EXPECT_EQ(a, *convertMatrix::toDense(mat));
#if !DEBUG
  EXPECT_EQ(to_string(a), to_string(mat->castForPrinting()));
#endif
}

TEST(EigenSparseSolverTest, CanSolveTinySystem)
{
  typedef Eigen::Triplet<double> T;
  std::vector<T> tripletList;
  int estimation_of_entries = 10;
  tripletList.reserve(estimation_of_entries);
  for(int i = 0; i < estimation_of_entries; ++i)
  {
    tripletList.push_back(T(i,i, i *3 + 1));
  }
  int n = 10;
  Eigen::SparseMatrix<double> mat(n,n);
  mat.setFromTriplets(tripletList.begin(), tripletList.end());

  Eigen::VectorXd x(n), b(n);
  // fill A and b
  b.setZero();
  b(1) = 1;

  Eigen::ConjugateGradient<Eigen::SparseMatrix<double> > cg;
  cg.compute(mat);
  x = cg.solve(b);
  std::cout << "#iterations:     " << cg.iterations() << std::endl;
  std::cout << "estimated error: " << cg.error()      << std::endl;
}

TEST(SparseMatrixReadTest, DISABLED_CanReadInBigMatrix)
{
  auto file = TestResources::rootDir() / "CGDarrell" / "A_txt.mat ";
  EigenMatrixFromScirunAsciiFormatConverter converter;
  auto mat = converter.makeSparse(file.string());
  ASSERT_TRUE(mat.get() != nullptr);
  EXPECT_EQ(428931, mat->rows());
  EXPECT_EQ(428931, mat->cols());
}

TEST(SparseMatrixReadTest, DISABLED_CanReadInBigVector)
{
  //428931 1 {0 0.005436646179877679 -0.002975964005526226
  auto file = TestResources::rootDir() / "CGDarrell" / "RHS_text.txt";
  EigenMatrixFromScirunAsciiFormatConverter converter;
  auto mat = converter.makeDense(file.string());
  ASSERT_TRUE(mat.get() != nullptr);
  EXPECT_EQ(428931, mat->rows());
  EXPECT_EQ(1, mat->cols());
}

TEST(EigenSparseSolverTest, DISABLED_CanSolveBigSystem)
{
  auto AFile = TestResources::rootDir() / "CGDarrell" / "A_txt.mat";
  auto rhsFile = TestResources::rootDir() / "CGDarrell" / "RHS_text.txt";
  EigenMatrixFromScirunAsciiFormatConverter converter;
  auto A = converter.make(AFile.string());
  ASSERT_TRUE(A.get() != nullptr);

  std::cout << A->nrows() << " x " << A->ncols() << std::endl;

  auto b = converter.make(rhsFile.string());
  ASSERT_TRUE(b.get() != nullptr);
  std::cout << b->nrows() << " x " << b->ncols() << std::endl;
  auto bCol = convertMatrix::toColumn(b);

  SolveLinearSystemAlgorithm::Outputs x;
  {
    ScopedTimer t("using algorithm object");
    SolveLinearSystemAlgorithm algo;

    x = algo.run(std::make_tuple(A, bCol), std::make_tuple(1e-20, 4000, "cg"));
    MatrixHandle solution = std::get<0>(x);

    ASSERT_TRUE(solution.get() != nullptr);
    std::cout << "error: " << std::get<1>(x) << std::endl;
    std::cout << "iterations: " << std::get<2>(x) << std::endl;
  }

  {
    ScopedTimer t("comparing solutions.");
    auto xFileEigen = TestResources::rootDir() / "CGDarrell" / "xEigenNEW.txt";
    std::ofstream output(xFileEigen.string());
    auto solution = *std::get<0>(x);
    output << std::setprecision(15) << solution << std::endl;

    auto xFileScirun = TestResources::rootDir() / "CGDarrell" / "xScirunColumn.mat";
    auto xExpected = converter.makeColumn(xFileScirun.string());
    ASSERT_TRUE(xExpected.get() != nullptr);
    EXPECT_EQ(428931, xExpected->nrows());
    EXPECT_EQ(1, xExpected->ncols());

    EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*xExpected, solution , .1);
    EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*xExpected, solution , .01);
    EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*xExpected, solution , .001);
    EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*xExpected, solution , .0001);
  }
}