/* Enlarges source image trying to estimate missing pixels - Quadratic Model
   (c) 2016, Eduardo Valle, blog.eduardovalle.com/
*/
#include <cassert>
#include <iostream>
#include <map>

// #define SOLVER_SPARSE
#include <Eigen/Core>
#ifdef SOLVER_SPARSE
    #include <Eigen/Sparse>
    typedef Eigen::Triplet<double> Triplet;
#else
    #include <Eigen/Dense>
#endif

#include <gd.h>

using namespace std;

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

    // Check command-line and open source image
    const int nParamsMin = 2;
    const int nParamsMax = 8;
    if ( argc <= nParamsMin || argc>(nParamsMax+1) ) {
        cerr << "Usage: " << argv[0]
             << " <in.png> <out.png> [<dom>] [<asp>] [H up] [V up] [gweight] [toler]" << endl
             << endl
             << "   in.png    Input image in PNG format (mandatory)" << endl
             << "   out.png   Output image in PNG format (mandatory)" << endl
             << "   dom       Pixel connectedness domain: 4 or 8 (def: 4)" << endl
             << "   asp       Pixel aspect ratio width/height (def: 1.0)" << endl
             << "   H up      How much to upscale the image horizontally (def: 3)" << endl
             << "   V up      How much to upscale the image vertically (def: 3)" << endl
             << "   gweight   Relative importance of respecting data (def: 1.0 = neutral)" << endl
             << "   toler     Graylevel difference for  equal pixels (def: 0, max: 255)" << endl
             << endl
             << "   Optional parameters must be entered in the right sequence" << endl
             << endl
             #ifdef SOLVER_SPARSE
                 << "   Solver: Eigen::SparseQR" << endl
             #else
                 << "   Solver: Eigen::HouseholderQR" << endl
             #endif
             << endl;
        return 1;
    }
    FILE *infile = fopen(argv[1], "r");
    if (infile == NULL) {
        cerr << "Error opening:" << argv[1] << endl;
        return 1;
    }
    gdImagePtr simage = gdImageCreateFromPng(infile);
    fclose(infile);
    if (simage == NULL) {
        cerr << "Error reading:" << argv[1] << endl;
        return 1;
    }
    const int w = simage->sx;
    const int h = simage->sy;

    // Are pixel and sample neighborhoods 4-connected or 8-connected ?
    const int connectedness = (argc <= 3) ? 4 : atoi(argv[3]);
    if (connectedness!=4 && connectedness!=8) {
        cerr << "Connectedness domain must be 4 or 8: " << argv[3] << endl;
        return 1;
    }
    const bool  four = (connectedness == 4);

    // What is the relative importance of neighbors
    const float aspect     = (argc <= 4) ? 1.0 : atof(argv[4]);
    if (aspect <= 0.0f) {
        cerr << "Aspect ratio must be >= 0.0: " << argv[4] << endl;
        return 1;
    }
    const float weightV    = 1.0;
    const float weightH    = pow(1.0/aspect, 2.0);
    const float weightD    = 1.0/(1.0 + aspect*aspect);

    // Horizontal, vertical, and surface ampliation
    const int   upX        = (argc <= 5) ? 3 : atoi(argv[5]);
    const int   upY        = (argc <= 6) ? 3 : atoi(argv[6]);
    if (upX < 1) {
        cerr << "Horizontal upscaling must be 1 or greater: " << argv[5] << endl;
        return 1;
    }
    if (upY < 1) {
        cerr << "Vertical upscaling must be 1 or greater: " << argv[6] << endl;
        return 1;
    }
    const int   up2D       = upX*upY;

    // Relative importance of attachment to graylevels
    const float grayWeight = up2D * ((argc <= 7) ? 1.0 : atof(argv[7]));
    if (grayWeight <= 0.0) {
        cerr << "Grayscale attachment weight must be > 0.0: " << argv[7] << endl;
        return 1;
    }

    // Tolerance for pixels be considered equal
    const float tolerance = (argc <= 8) ? 1.0 : atof(argv[8]);
    if (tolerance < 0.0) {
        cerr << "Pixel difference tolerance must be >= 0.0: " << argv[8] << endl;
        return 1;
    }

    // First pass : reads pixels matrix and count the number of needed variables
    const int ws = w+2;
    const int hs = h+2;
    float samples[hs][ws],
          lp = 0.0f;
    int   hotEstimate = 0;
    for (int y=0, ys=1; y<h; y++, ys++) {
        for (int x=0, xs=1; x<w; x++, xs++) {
            const int rgb = gdImageGetTrueColorPixel(simage, x, y);
            const float r = rgb >> 16 & 0xff;
            const float g = rgb >> 8  & 0xff;
            const float b = rgb       & 0xff;
            // Luminance coefficient for each color
            const float rl = 0.21f;
            const float gl = 0.72f;
            const float bl = 0.07f;
            const float l = r*rl + g*gl + b*bl;
            // Destination pixels
            samples[ys][xs] = l;
            hotEstimate += (abs(l-lp)>tolerance) ? 1 : 0;
        }
    }
    // ...correct borders (tries to create uniform border) to ease processing
    for (int ys=0; ys<hs; ys++) {
        samples[ys][0   ] = samples[   1][   1];
        samples[ys][ws-1] = samples[hs-2][ws-2];
    }
    for (int xs=0; xs<ws; xs++) {
        samples[   0][xs] = samples[   1][   1];
        samples[hs-1][xs] = samples[hs-2][ws-2];
    }

    gdImageDestroy(simage);

    cerr << "Hot pixels estimate: " << hotEstimate << endl;

    // Second pass : assembles list of the hot pixels :
    // those which are not equal to their neighbors
    map<pair<int, int>, int> hotMap;
    int hotCount = 0;
    for (int ys=1; ys<hs-1; ys++) {
        for (int xs=1; xs<ws-1; xs++) {
            if ((abs(samples[ys][xs] - samples[ys+1][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys+1][xs  ]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys+1][xs-1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys  ][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys  ][xs-1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs  ]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs-1]) > tolerance)) {
                hotMap.insert(make_pair(make_pair(xs, ys), hotCount));
                hotCount++;
            }
        }
    }

    cerr << "Hot pixels count: " << hotCount << endl;

    // Assembles optimization matrix...

    // The optimization we'll attempt tries to minimize the quadradic
    // difference between neighboring subsamples, while constraining all
    // subsamples inside a pixel to average to the given pixel. I.e.:
    // If subsamples si,1 to si,n belong to pi, we are trying to minimize
    // Sum_{for all neighboring subsample pairs s_1 s_2 } (s_1 - s_2)^2
    // constrained to : For all pixels p_i :
    // Avg_{for all subsamples s_i,j of pixel pi} si,j = pi

    // The optimization above leads to an overdetermined linear system:
    // 1) One equation per pixel for the averaging constraint
    // 2) One equation per subsample for the quadratic minimization that
    //    attains its mininimum when each subsample is the average of its
    //    neighbors

    const int vars    =  hotCount*up2D;
    const int eqns    =  hotCount*up2D                 + hotCount;
    const int nonzero = (hotCount*up2D)*(four ? 4 : 8) + (hotCount*up2D);
    cerr << "Optimizing : " << vars
             << " variables and " << eqns
             << " equations. " << nonzero
             << " non-zero entries / " << ( ((float)vars)*eqns )
             << " total (" << ( ((float) nonzero)/vars/eqns*100.0 )
             << "%)"  << endl;

    #ifdef SOLVER_SPARSE
        cerr << "Using sparse matrices" << endl;
        std::vector<Triplet> triplets(nonzero);
        Eigen::SparseMatrix<float> optimization(eqns, vars);
    #else
        cerr << "Using dense matrices" << endl;
        Eigen::MatrixXf optimization(eqns, vars);
        optimization.setZero();
    #endif
    Eigen::VectorXf targets(eqns);

    // ...variable index for subsample at position (u,v) of the pixel (x,y)
    auto varindex = [&](int i, int u, int v) -> int  {
        assert(i>=0 && i<hotCount && u>=0 && u<upX && v>=0 && v<upY);
        return i*up2D + v*upX+u;
    };
    auto findpix = [&](int x, int y) -> int {
        assert(x>=0 && x<ws && y>=0 && y<hs);
        try {
            return hotMap.at(make_pair(x, y));
        }
        catch(out_of_range e) {
            return -1;
        }
    };

    // ...assemble the graylevel constraints
    cerr << "Assembling graylevel constraints... " << endl;
    for (auto pixel : hotMap) {
        const int x = pixel.first.first;
        const int y = pixel.first.second;
        const int i = pixel.second;
        // The constraint is for the average of the subsamples to
        // be the graylevel of the pixel
        for (int v=0; v<upY; v++) {
            for (int u=0; u<upX; u++) {
                const int var   = varindex(i, u, v);
                const float val = (1.0f / up2D) * grayWeight;
                #ifdef SOLVER_SPARSE
                    triplets.push_back(Triplet(i, var, val));
                #else
                    optimization(i, var) = val;
                #endif
            }
        }
        targets(i) = samples[y][x] * grayWeight;
    }
    int eqn = hotCount;

    // ...assemble the quadratic minimizations
    cerr << "Assembling quadratic minimizers... " << endl;
    for (auto pixel : hotMap) {
        const int x = pixel.first.first;
        const int y = pixel.first.second;
        const int i = pixel.second;

        // Finds neighboring pixels
        const int iLU = four ? -1 : findpix(x-1, y-1);
        const int iL  =             findpix(x-1, y  );
        const int iLD = four ? -1 : findpix(x-1, y+1);
        const int iU  =             findpix(x  , y-1);
        const int iD  =             findpix(x  , y+1);
        const int iRU = four ? -1 : findpix(x+1, y-1);
        const int iR  =             findpix(x+1, y  );
        const int iRD = four ? -1 : findpix(x+1, y+1);

        // Finds variable corresponding to subsample at u+m,v+n
        // n, m = {-1, 0, 1}
        auto neighbor = [&](int u, int v, int m, int n) -> int {
            assert(u>=0 && u<upX && v>=0 && v<upY && m>=-1 && m<=1 && n>=-1 && n<=1);
            int nu = u+m, nv = v+n;
            // Safe cases : neighbor is in same pixel
            if (nu>=0 && nu<upX && nv>=0 && nv<upY) {
                return varindex(i, nu, nv);
            }
            // Problematic cases : neighbor is accross pixels
            int ni = -1;
            if (nu < 0) {
                if (nv <    0) { ni = iLU; nu = upX-1; nv = upY-1; } else
                if (nv >= upY) { ni = iLD; nu = upX-1; nv =     0; } else
                               { ni = iL ; nu = upX-1; }
            }
            else if (nu >= upX) {
                if (nv <    0) { ni = iRU; nu =     0; nv = upY-1; } else
                if (nv >= upY) { ni = iRD; nu =     0; nv =     0; } else
                               { ni = iR ; nu =     0; }
            }
            else {
                if (nv <    0) { ni = iU ; nv = upY-1; } else
                if (nv >= upY) { ni = iD ; nv =     0; } else
                               { assert(false); }
            }
            if (ni == -1) {
                return -2;
            }
            const int si = varindex(ni, nu, nv);
            assert(si != -1);
            return si;
        };

        // Create constraint equations for all subsamples
        for (int v=0; v<upY; v++) {
            for (int u=0; u<upX; u++) {
                // Finds neighboring subsamples
                const int sLU = four ? -3 : neighbor(u, v, -1, -1);
                const int sL  =             neighbor(u, v, -1,  0);
                const int sLD = four ? -3 : neighbor(u, v, -1,  1);
                const int sU  =             neighbor(u, v,  0, -1);
                const int sD  =             neighbor(u, v,  0,  1);
                const int sRU = four ? -3 : neighbor(u, v,  1, -1);
                const int sR  =             neighbor(u, v,  1,  0);
                const int sRD = four ? -3 : neighbor(u, v,  1,  1);
                // Counts neighbors
                const float norm =
                    ( (sLU >= 0) ? weightD : 0.0f ) +
                    ( (sL  >= 0) ? weightH : 0.0f ) +
                    ( (sLD >= 0) ? weightD : 0.0f ) +
                    ( (sU  >= 0) ? weightV : 0.0f ) +
                    ( (sD  >= 0) ? weightV : 0.0f ) +
                    ( (sRU >= 0) ? weightD : 0.0f ) +
                    ( (sR  >= 0) ? weightH : 0.0f ) +
                    ( (sRD >= 0) ? weightD : 0.0f );
                // The constraint is for each subsample to be the average
                // of the neighboring subsamples
                #ifdef SOLVER_SPARSE
                    triplets.push_back( Triplet(eqn, varindex(i, u, v), -1.0f) );
                    if (sLU >= 0) { triplets.push_back( Triplet(eqn, sLU, weightD / norm) ); }
                    if (sL  >= 0) { triplets.push_back( Triplet(eqn, sL , weightH / norm) ); }
                    if (sLD >= 0) { triplets.push_back( Triplet(eqn, sLD, weightD / norm) ); }
                    if (sU  >= 0) { triplets.push_back( Triplet(eqn, sU , weightV / norm) ); }
                    if (sD  >= 0) { triplets.push_back( Triplet(eqn, sD , weightV / norm) ); }
                    if (sRU >= 0) { triplets.push_back( Triplet(eqn, sRU, weightD / norm) ); }
                    if (sR  >= 0) { triplets.push_back( Triplet(eqn, sR , weightH / norm) ); }
                    if (sRD >= 0) { triplets.push_back( Triplet(eqn, sRD, weightD / norm) ); }
                #else
                    optimization(eqn, varindex(i, u, v)) = -1.0f;
                    if (sLU >= 0) { optimization(eqn, sLU) = weightD / norm; }
                    if (sL  >= 0) { optimization(eqn, sL ) = weightH / norm; }
                    if (sLD >= 0) { optimization(eqn, sLD) = weightD / norm; }
                    if (sU  >= 0) { optimization(eqn, sU ) = weightV / norm; }
                    if (sD  >= 0) { optimization(eqn, sD ) = weightV / norm; }
                    if (sRU >= 0) { optimization(eqn, sRU) = weightD / norm; }
                    if (sR  >= 0) { optimization(eqn, sR ) = weightH / norm; }
                    if (sRD >= 0) { optimization(eqn, sRD) = weightD / norm; }
                #endif
                targets(eqn) = 0.0f;
                eqn++;
            }
        }
    }

    // ...showtime ! --- solve the humungous system
    cerr << "Showtime !" << endl;
    Eigen::VectorXf subsamples(vars);
    #ifdef SOLVER_SPARSE
        optimization.setFromTriplets(triplets.begin(), triplets.end());
        optimization.makeCompressed(); // Requirement of SparseQR
        Eigen::SparseQR<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int> > solver;
        solver.compute(optimization);
        if (solver.info() != Eigen::Success) {
            cerr << "Matrix decomposition failed" << endl;
            return 1;
        }
        subsamples = solver.solve(targets);
        if(solver.info() != Eigen::Success) {
            cerr << "Solver failed" << endl;
            return 1;
        }
    #else
        Eigen::HouseholderQR<Eigen::MatrixXf> solver;
        solver.compute(optimization);
        subsamples = solver.solve(targets);
    #endif
    // DEBUG :
    // Eigen::JacobiSVD<Eigen::MatrixXf>
    //    solver(optimization, Eigen::ComputeThinU | Eigen::ComputeThinV); // +precise, but ++slower
    // cout << optimization << endl;
    // cout << targets << endl;

    // Prepares destination image
    cerr << "Preparing output..." << endl;
    const int wu = (int) (w * upX);
    const int hu = (int) (h * upY);
    gdImagePtr dimage = gdImageCreateTrueColor(wu, hu);
    if (dimage == NULL) {
        fprintf(stderr, "Not enough memory.\n");
        return 1;
    }
    int gray[256];
    for (int l=0; l<256; l++) {
        gray[l] = gdImageColorAllocate(dimage, l, l, l);
    }

    // DEBUG :
    // int gray2[256];
    // for (int l=0; l<256; l++) {
    //     gray2[l] = gdImageColorAllocate(dimage, l, (int) (0.8f*l), l);
    // }

    // Second pass : resample image
    for (int y=0, yu=0; y<h; y++, yu+=upY) {
        for (int x=0, xu=0; x<w; x++, xu+=upX) {
            int i = findpix(x+1, y+1);
            for (int v=0; v<upY; v++) {
                for (int u=0; u<upX; u++) {
                    int l;
                    if (i >= 0) {
                        // Hot pixels were estimated by the optimizer
                        l = subsamples( varindex(i, u, v) );
                    }
                    else {
                        // Cold pixels come from flat areas and are copied from the samples
                        l = samples[y+1][x+1];
                    }
                    l = (l < 0) ? 0 : ( (l>255) ? 255 : l );
                    gdImageSetPixel(dimage, xu+u, yu+v, gray[l]);
                    // gdImageSetPixel(dimage, xu+u, yu+v, i>=0 ? gray[l] : gray2[l]); // DEBUG
                }
            }
        }
    }

    // Save results to output file
    FILE *outfile = fopen(argv[2], "w");
    if (outfile == NULL) {
        cerr << "Error opening: " << argv[2] << endl;
        return 1;
    }
    gdImagePng(dimage, outfile);
    fclose(outfile);
    gdImageDestroy(dimage);
    return 0;
}


/**
 * cubature_kalman_filter.hpp
 * @author wendao
 * 20/12/6
 **/
#ifndef KKL_CUBATURE_KALMAN_FILTER_X_HPP
#define KKL_CUBATURE_KALMAN_FILTER_X_HPP

#include <random>
#include <Eigen/Dense>
#include <kalman/kalman_filter.hpp>

/**
 * @brief Cubature Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class CubatureKalmanFilterX : public KalmanFilter<T, System>
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
  using KalmanFilter<T, System>::state_dim;
  using KalmanFilter<T, System>::N;
  using KalmanFilter<T, System>::input_dim;
  using KalmanFilter<T, System>::measurement_dim;
  using KalmanFilter<T, System>::M;
  using KalmanFilter<T, System>::mean;
  using KalmanFilter<T, System>::cov;
  using KalmanFilter<T, System>::system;
  using KalmanFilter<T, System>::process_noise;
  using KalmanFilter<T, System>::measurement_noise;
  using KalmanFilter<T, System>::kalman_gain;

public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  CubatureKalmanFilterX(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                 const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                 const VectorXt& _mean, const MatrixXt& _cov):
    KalmanFilter<T, System>(_system, _state_dim, _input_dim, _measurement_dim, _process_noise, _measurement_noise, _mean, _cov),
    S(2 * _state_dim) //容积点个数
  {

    weights.resize(S, 1);
    cubature_points.resize(S, N);
    ext_weights.resize(2*(N+M), 1);
    ext_cubature_points.resize(2*(N+M), N + M);
    expected_measurements.resize(2*(N+M), M);

    // initialize weights for cubature filter
    for (int i = 0; i < S; i++)
      weights[i] = 1.0 / S;

    // weights for extended state space which includes error variances
    for (int i = 0; i < 2*(N+M); i++)
      ext_weights[i] = 1.0 / 2*(N+M);

  }

  /**
   * @brief predict  预测函数
   * @param control  input vector
   */
  virtual void predict(const VectorXt& control) override
  {
    // calculate cubature points
    ensurePositiveFinite(cov);
    computeCubaturePoints(mean, cov, cubature_points); //根据上一时刻的均值和方差计算cubature点
    for (int i = 0; i < S; i++) {
      cubature_points.row(i) = system.f(cubature_points.row(i), control); //根据系统方程传播cubature点
    }

    const auto& Q = process_noise; //系统噪声|过程噪声

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * cubature_points.row(i);   //传播后的cubature点集均值
    }
    for (int i = 0; i < S; i++)
      cov_pred += weights[i] * cubature_points.row(i).transpose() * cubature_points.row(i);
    cov_pred -= mean_pred.transpose() * mean_pred; //传播后的cubature点集方差
    cov_pred += Q;                                      //加上过程噪声

    //得到预测值和预测协方差
    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct      校正函数
   * @param measurement  观测值
   */
  virtual void correct(const VectorXt& measurement) override
  {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + M, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + M, N + M);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(M, M) = measurement_noise;

    ensurePositiveFinite(ext_cov_pred);
    computeCubaturePoints(ext_mean_pred, ext_cov_pred, ext_cubature_points); //根据预测均值和协方差以及测量噪声计算cubature点
                                                                             //此时测量误差并未添加到cubature主体,而是存放于拓展部分

    // cubature transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_cubature_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_cubature_points.row(i).transpose().topLeftCorner(N, 1));     //观测方程传播cubature点集
      expected_measurements.row(i) += VectorXt(ext_cubature_points.row(i).transpose().bottomRightCorner(M, 1));//添加测量噪声
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(M);
    for (int i = 0; i < ext_cubature_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);  //传播后的cubature点集均值
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(M, M);
    for (int i = 0; i < ext_cubature_points.rows(); i++)
      expected_measurement_cov += ext_weights[i] * expected_measurements.row(i).transpose() * expected_measurements.row(i);

    expected_measurement_cov -= expected_measurement_mean.transpose() * expected_measurement_mean; //传播后的cubature点集方差
    expected_measurement_cov += measurement_noise;  //R = measurement_noise

    // calculated transformed covariance
    MatrixXt cross_cov = MatrixXt::Zero(N, M); //互协方差
    for(int i=0; i<S; ++i)
      cross_cov += ext_weights[i] * ext_cubature_points.row(i).transpose() * expected_measurements.row(i);
    cross_cov -= ext_mean_pred * expected_measurement_mean;

    kalman_gain = cross_cov * cross_cov.inverse(); //卡尔曼增益

    VectorXt ext_mean = ext_mean_pred + kalman_gain * (measurement - expected_measurement_mean); //最优估计
    MatrixXt ext_cov = ext_cov_pred - kalman_gain * expected_measurement_cov * kalman_gain.transpose();    //最优估计的协方差

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  const MatrixXt& getSamplePoints() const { return cubature_points; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
  const int S;  //容积点个数

public:
  VectorXt weights;          //容积点权重,2N个容积点，权重相同
  MatrixXt cubature_points;  //容积点

  VectorXt ext_weights;
  MatrixXt ext_cubature_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute cubature points
   * @param mean          mean
   * @param cov           covariance
   * @param cubature_points  calculated cubature points
   */
  void computeCubaturePoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& cubature_points) {
    const int n = mean.size(); //状态维度
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt(cov);
    MatrixXt P_chol = llt.matrixL();
    MatrixXt l = P_chol * sqrt(n);

    for (int i = 0; i < n; i++) {
      cubature_points.row(  i) = mean + l.col(i);
      cubature_points.row(n+i) = mean - l.col(i);
    }
  }

};

#endif


#include <iostream>
#include <stack>
#include <vector>
#include <list>

#include <boost/range/algorithm_ext/erase.hpp>

template <class T>
class QStack : public std::stack<T>
{
  public:
    T take() {
        T v = this->top();
        this->pop();
        return v;
    }

    T takeBottom() {
        T v = this->c.front();
        this->c.pop_front();
        return v;
    }

    void print() const
    {
        for (auto && v : this->c)
            std::cout << v << "\t";
        std::cout << std::endl;
    }
};

// Stack of Plates. Composite stack behaves as usual stack
namespace SoP
{
    template <class T>
    class CompositeStack
    {
    public:
        void push(T const& t)
        {
            if (m_stacks.empty() || m_stacks.back().size() == capacity())
                m_stacks.emplace_back(QStack<T>());

            m_stacks.back().push(t);
        }

        T take()
        {
            if (empty())
                throw std::logic_error("The compoiste stack is empty.");

            auto &&back = m_stacks.back();
            T v = back.take();
            if (back.empty())
                m_stacks.erase(--std::end(m_stacks));
            return v;
        }

        T takeAt(std::size_t index)
        {
            if (index >= m_stacks.size())
                throw std::logic_error("Cannot take from this stack.");

            return leftShift(index, true /*removeTop*/);
        }

        bool empty() const { return m_stacks.empty(); }

        static std::size_t capacity() { return 3; }

    private:
        T leftShift(std::size_t index, bool removeTop)
        {
            auto &&stack = m_stacks.at(index);
            T removedItem = removeTop ? stack.take() : stack.takeBottom();
            if (stack.empty())
                boost::range::remove_erase(m_stacks, stack);
            else if (m_stacks.size() > index + 1)  {// not last element
                T v = leftShift(index + 1, false /*remove top*/);
                stack.push(v);
            }

            return removedItem;
        }

        std::vector<QStack<T>> m_stacks;
    };

    using IntCompositeStack = CompositeStack<int>;
}

// Implement Queue via two stacks
namespace tsq
{
    template <class T>
    class Queue
    {
    public:
        std::size_t size() const { return m_oldest.size() + m_newest.size(); }

        void add(T const& e)
        {
            // Newest stack always has new element on top
            m_newest.push(e);
        }

        T & peek() { return const_cast<T&>(const_cast<Queue<T> *>(this)->peek()); }
        T const& peek() const
        {
            shiftStacks();
            return m_oldest.top();
        }

        T take()
        {
            shiftStacks();
            return m_oldest.take();
        }

    private:
        /// Moves elements from newest stack to oldest
        void shiftStacks() const
        {
            if (m_oldest.empty())
                while (!m_newest.empty())
                    m_oldest.push(m_newest.take());
        }

        mutable QStack<T> m_oldest;
        mutable QStack<T> m_newest;
    };

    using IntQueue = Queue<int>;
}

// Implement sorted stack with using two stacks
namespace ss
{
    template <class T>
    class SortedStack
    {
    public:
        T const & peek() const { return m_stack.pop(); }
        T  & peek() { return m_stack.top(); }

        T take() { return m_stack.take(); }

        void push(T const& v)
        {
            m_stack.push(v);
            sort();
        }

        bool empty() const { return m_stack.empty(); }

    private:
        void sort()
        {
            QStack<T> tmpStask;
            while (!m_stack.empty()) {

                // Insert each element to the s in the sorted order
                int tmpVal = m_stack.take();
                while (!tmpStask.empty() && tmpStask.top() > tmpVal)
                    m_stack.push(tmpStask.take());

                tmpStask.push(tmpVal);

                std::cout << "stask: ";
                m_stack.print();
                std::cout << "tmp: ";
                tmpStask.print();
            }

            // Copy elements back
            while (!tmpStask.empty())
                m_stack.push(tmpStask.take());
        }

        QStack<T> m_stack;
    };

    using IntStack = SortedStack<int>;
}

// Animal shelter. Implement functions to "adopt" the oldest animal (in general) or perticulat animal
// (i.e. dog or cat).
namespace as
{
    struct Animal
    {
        std::string name;

        // Easy one, we don't have any behavioural difference between dogs and cats in this model
        enum Type { Dog, Cat };
        Type type;
    };

    class AnimalQueue
    {
    public:
        void enqueue(Animal const& animal)
        {
            if (animal.type == Animal::Dog)
                m_dogs.push_back({s_animalCounter++,animal});
            else
                m_cats.push_back({s_animalCounter++,animal});
        }

        Animal dequeueAny()
        {
            if (m_dogs.empty())
                return dequeueCat();

            if (m_cats.empty())
                return dequeueDog();

            OrderedAnimal dog = m_dogs.back();
            OrderedAnimal cat = m_cats.back();
            if (dog.first > cat.first) {
                m_dogs.pop_back();
                return dog.second;
            } else {
                m_cats.pop_back();
                return cat.second;
            }
        }

        Animal dequeueDog()
        {
            return dequeueImpl(m_dogs);
        }

        Animal dequeueCat()
        {
            return dequeueImpl(m_cats);
        }

    private:
        using OrderedAnimal = std::pair<std::size_t, Animal>;
        using AnimalsList = std::list<OrderedAnimal>;

        Animal dequeueImpl(AnimalsList & animals)
        {
            if (animals.empty())
                throw std::logic_error("No animals of this type or at all.");

            OrderedAnimal animal = animals.back();
            animals.pop_back();
            return animal.second;
        }

        static std::size_t s_animalCounter;

        AnimalsList m_dogs;
        AnimalsList m_cats;
    };

    std::size_t AnimalQueue::s_animalCounter = 0;

    void print(Animal const& animal)
    {
        std::cout << animal.name
                  << "\t"
                  << (animal.type == Animal::Dog ? "dog" : "cat") << std::endl;
    }
}

int main(int /*argc*/, char */*argv*/[])
{
    // 1
//    SoP::IntCompositeStack intStack;

//    for (std::size_t i = 0; i < 10; ++i)
//        intStack.push(i);

//    std::cout << intStack.takeAt(3) << "\n" << std::endl;

//    while (!intStack.empty())
//        std::cout << intStack.take() << std::endl;

    // 2
//    tsq::IntQueue queue;
//    for (int i = 0; i < 10; ++i)
//        queue.add(i);

//    while (queue.size() != 0)
//        std::cout << queue.take() << std::endl;

    // 3
//    ss::IntStack s;
//    s.push(10);
//    s.push(3);
//    s.push(12);
//    s.push(7);
//    s.push(5);

//    while (!s.empty())
//        std::cout << s.take() << std::endl;

    // 4
    as::AnimalQueue queue;
    queue.enqueue({"a1", as::Animal::Dog});
    queue.enqueue({"a2", as::Animal::Cat});
    queue.enqueue({"a3", as::Animal::Dog});
    queue.enqueue({"a4", as::Animal::Dog});
    queue.enqueue({"a5", as::Animal::Cat});

    try {
        as::print(queue.dequeueAny());
        as::print(queue.dequeueDog());
        as::print(queue.dequeueCat());
        as::print(queue.dequeueCat());
    } catch (std::exception const& e) {
        std::cout << e.what() << std::endl;
    }

    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    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.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_linear_interpolation )

// Test implementation of linear interpolation function for data vectors.
BOOST_AUTO_TEST_CASE( test_linearInterpolation_vector )
{
    // Set vectors of data.
    const Eigen::Vector3d sortedIndependentVariables( 0.0, 1.0, 3.0 );
    const Eigen::Vector3d associatedDependentVariables( -20.0, 20.0, 21.0 );

    // Test 1: Test vector data with expected result of 0.0.
    {
        // Set target independent value in vector data.
        const double targetIndependentVariableValue = 0.5;

        // Compute interpolation.
        const double interpolatedValue = interpolators::computeLinearInterpolation(
                    sortedIndependentVariables, associatedDependentVariables,
                    targetIndependentVariableValue );

        // Verify that interpolated value corresponds to expected value (0.0).
        BOOST_CHECK_SMALL( std::fabs( interpolatedValue - 0.0 ),
                           std::numeric_limits< double >::min( ) );
    }

    // Test 2: Test vector data with expected result of 20.5.

    // Set target independent value in vector data.
    const double targetIndependentVariableValue = 2.0;

    // Compute interpolation.
    const double interpolatedValue = interpolators::computeLinearInterpolation(
                sortedIndependentVariables, associatedDependentVariables,
                targetIndependentVariableValue );

    // Verify that interpolated value corresponds to expected value (20.5).
    BOOST_CHECK_SMALL( std::fabs( interpolatedValue - 20.5 ),
                       std::numeric_limits< double >::min( ) );
}

// Test linear interpolation with map of vectors with keys as independent variable.
BOOST_AUTO_TEST_CASE( test_linearInterpolation_map )
{
    // Declare map of data and vectors for map value.
    std::map < double, Eigen::VectorXd > sortedIndepedentAndDependentVariables;
    const Eigen::Vector3d vectorOne( 10.0, -10.0, 70.0 );
    const Eigen::Vector3d vectorTwo( 20.0, -5.0, 80.0 );
    const Eigen::Vector3d vectorThree( 30.0, 60.0, 90.0 );

    // Set map values in map using vector data.
    sortedIndepedentAndDependentVariables[ 0.0 ] = vectorOne;
    sortedIndepedentAndDependentVariables[ 1.0 ] = vectorTwo;
    sortedIndepedentAndDependentVariables[ 2.0 ] = vectorThree;

    // Set target independent variable value for interpolation.
    const double targetIndependentVariableValue = 1.5;

    // Compute interpolation.
    Eigen::Vector3d interpolatedVector = interpolators::computeLinearInterpolation(
                sortedIndepedentAndDependentVariables,
                targetIndependentVariableValue );

    // Check that interpolated values correspond to expected elements [25, 27.5, 85].
    BOOST_CHECK_SMALL( std::fabs( interpolatedVector( 0 ) - 25.0 ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_SMALL( std::fabs( interpolatedVector( 1 ) - 27.5 ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_SMALL( std::fabs( interpolatedVector( 2 ) - 85.0 ),
                       std::numeric_limits< double >::min( ) );
}

// Test linear interpolation from benchmark values generetd by Matlab, interpolating the
// error function.
BOOST_AUTO_TEST_CASE( test_linearInterpolation_matlab_compare )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create linear interpolator using hunting algorithm.
    LinearInterpolatorDouble linearInterpolator(
                independentVariableValues, dependentVariableValues, huntingAlgorithm );

    // Load points at which interpolator is to be evaluated and data generated by Matlab.
    Eigen::MatrixXd benchmarkData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/linear_interpolator_test_output_data.dat",
                "," );

    // Perform interpolation for required data points.
    Eigen::VectorXd outputData = Eigen::VectorXd( benchmarkData.rows( ) );
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = linearInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0E-13 );

    // Create linear interpolator, now with nearest neighbvour search.
    linearInterpolator = LinearInterpolatorDouble(
                independentVariableValues, dependentVariableValues, binarySearch );

    // Perform interpolation for required data points.
    for ( int i = 0; i < outputData.rows( ); i++ )
    {
        outputData[ i ] = linearInterpolator.interpolate( benchmarkData( i, 0 ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( benchmarkData.block( 0, 1, benchmarkData.rows( ), 1 ),
                                       outputData, 1.0E-13 );
}

// Test linear interpolation outside of independent variable range
BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;
    std::vector< double > dependentVariableValues;

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
        dependentVariableValues.push_back( inputData( i, 1 ) );
    }

    // Create linear interpolator using hunting algorithm.
    double valueOffset = 2.0;
    double valueBelowMinimumValue = independentVariableValues[ 0 ] - valueOffset;
    double valueAboveMaximumValue = independentVariableValues[ inputData.rows( ) - 1 ] + valueOffset;
    double interpolatedValue = TUDAT_NAN, expectedValue = TUDAT_NAN;
    bool exceptionIsCaught = false;

    for( unsigned int i = 0; i < 7; i++ )
    {
        LinearInterpolatorDouble linearInterpolator(
                    independentVariableValues, dependentVariableValues, huntingAlgorithm,
                    static_cast< BoundaryInterpolationType >( i ) );

        if( static_cast< BoundaryInterpolationType >( i ) == throw_exception_at_boundary )
        {
            try
            {
                linearInterpolator.interpolate( valueBelowMinimumValue );
            }
            catch( std::runtime_error )
            {
                exceptionIsCaught = true;
            }
            BOOST_CHECK_EQUAL( exceptionIsCaught, true );

            exceptionIsCaught = false;
            try
            {
                linearInterpolator.interpolate( valueAboveMaximumValue );
            }
            catch( std::runtime_error )
            {
                exceptionIsCaught = true;
            }
            BOOST_CHECK_EQUAL( exceptionIsCaught, true );
        }
        else if( ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value ) ||
                 ( static_cast< BoundaryInterpolationType >( i ) == use_boundary_value_with_warning ) )
        {
            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, dependentVariableValues.at( 0 ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, dependentVariableValues.at( inputData.rows( ) - 1 ), 1.0E-15 );

        }
        else if( ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary ) ||
                 ( static_cast< BoundaryInterpolationType >( i ) == extrapolate_at_boundary_with_warning ) )
        {
            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            expectedValue = dependentVariableValues.at( 0 ) - valueOffset * (
                        dependentVariableValues.at( 1 ) - dependentVariableValues.at( 0 ) ) / (
                        independentVariableValues.at( 1 ) - independentVariableValues.at( 0 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, expectedValue, 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            expectedValue = dependentVariableValues.at( inputData.rows( ) - 1 ) + valueOffset * (
                        dependentVariableValues.at( inputData.rows( ) - 1 ) - dependentVariableValues.at( inputData.rows( ) - 2 ) ) / (
                        independentVariableValues.at( inputData.rows( ) - 1 ) - independentVariableValues.at( inputData.rows( ) - 2 ) );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, expectedValue, 1.0E-15 );
        }
        else if( ( static_cast< BoundaryInterpolationType >( i ) == use_default_value ) ||
                 ( static_cast< BoundaryInterpolationType >( i ) == use_default_value_with_warning ) )
        {
            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0, 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0, 1.0E-15 );
        }
    }
}

// Test linear interpolation outside of independent variable range with default extrapolation value
BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case_extrapolation_default_value )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
    }

    // Create linear interpolator using hunting algorithm.
    double valueOffset = 2.0;
    double valueBelowMinimumValue = independentVariableValues[ 0 ] - valueOffset;
    double valueAboveMaximumValue = independentVariableValues[ inputData.rows( ) - 1 ] + valueOffset;

    // Test with long double
    {
        // Put data in STL vectors.
        std::vector< long double > dependentVariableValues;
        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            dependentVariableValues.push_back( inputData( i, 1 ) );
        }
        long double interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, long double > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0L, 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, 0.0L, 1.0E-15 );
        }
    }

    // Test with Eigen::Vector3d
    {
        // Put data in STL vectors.
        std::vector< Eigen::Vector3d > dependentVariableValues;
        Eigen::Vector3d tempData = Eigen::Vector3d::Zero( );
        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            tempData[ 0 ] = inputData( i, 1 );
            dependentVariableValues.push_back( tempData );
        }
        Eigen::Vector3d interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, Eigen::Vector3d > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Vector3d::Zero( ) ).norm( ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Vector3d::Zero( ) ).norm( ), 1.0E-15 );
        }
    }

    // Test with Eigen::Matrix3d
    {
        // Put data in STL vectors.
        std::vector< Eigen::Matrix3d > dependentVariableValues;
        Eigen::Matrix3d tempData = Eigen::Matrix3d::Zero( );
        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            tempData( 0, 2 ) = inputData( i, 1 );
            dependentVariableValues.push_back( tempData );
        }
        Eigen::Matrix3d interpolatedValue;

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, Eigen::Matrix3d > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ) );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Matrix3d::Zero( ) ).norm( ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - Eigen::Matrix3d::Zero( ) ).norm( ), 1.0E-15 );
        }
    }
}

// Test linear interpolation outside of independent variable range with extrapolation value given by user
BOOST_AUTO_TEST_CASE( test_linearInterpolation_boundary_case_extrapolation_user_value )
{
    using namespace interpolators;

    // Load input data used for generating matlab interpolation.
    Eigen::MatrixXd inputData = input_output::readMatrixFromFile(
                input_output::getTudatRootPath( ) +
                "Mathematics/Interpolators/UnitTests/interpolator_test_input_data.dat","," );

    // Put data in STL vectors.
    std::vector< double > independentVariableValues;

    for ( int i = 0; i < inputData.rows( ); i++ )
    {
        independentVariableValues.push_back( inputData( i, 0 ) );
    }

    // Create linear interpolator using hunting algorithm.
    double valueOffset = 2.0;
    double valueBelowMinimumValue = independentVariableValues[ 0 ] - valueOffset;
    double valueAboveMaximumValue = independentVariableValues[ inputData.rows( ) - 1 ] + valueOffset;

    // Test with long double
    {
        // Put data in STL vectors.
        std::vector< long double > dependentVariableValues;

        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            dependentVariableValues.push_back( inputData( i, 1 ) );
        }
        long double interpolatedValue;
        long double extrapolationValue = 1.0L;

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, long double > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, extrapolationValue, 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_CLOSE_FRACTION( interpolatedValue, extrapolationValue, 1.0E-15 );
        }
    }

    // Test with Eigen::Vector3d
    {
        // Put data in STL vectors.
        std::vector< Eigen::Vector3d > dependentVariableValues;
        Eigen::Vector3d tempData = Eigen::Vector3d::Zero( );
        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            tempData[ 0 ] = inputData( i, 1 );
            dependentVariableValues.push_back( tempData );
        }
        Eigen::Vector3d interpolatedValue;
        Eigen::Vector3d extrapolationValue;
        extrapolationValue[ 0 ] = 1.5;
        extrapolationValue[ 1 ] = -3;
        extrapolationValue[ 2 ] = 0;

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, Eigen::Vector3d > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );
        }
    }

    // Test with Eigen::Matrix3d
    {
        // Put data in STL vectors.
        std::vector< Eigen::Matrix3d > dependentVariableValues;
        Eigen::Matrix3d tempData = Eigen::Matrix3d::Zero( );
        for ( int i = 0; i < inputData.rows( ); i++ )
        {
            tempData( 0, 2 ) = inputData( i, 1 );
            dependentVariableValues.push_back( tempData );
        }
        Eigen::Matrix3d interpolatedValue;
        Eigen::Matrix3d extrapolationValue = Eigen::Matrix3d::Random( );

        for( unsigned int i = 5; i < 7; i++ )
        {
            LinearInterpolator< double, Eigen::Matrix3d > linearInterpolator(
                        independentVariableValues, dependentVariableValues, huntingAlgorithm,
                        static_cast< BoundaryInterpolationType >( i ), extrapolationValue );

            interpolatedValue = linearInterpolator.interpolate( valueBelowMinimumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );

            interpolatedValue = linearInterpolator.interpolate( valueAboveMaximumValue );
            BOOST_CHECK_SMALL( ( interpolatedValue - extrapolationValue ).norm( ), 1.0E-15 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef __SIMPLEX_HPP__
#define __SIMPLEX_HPP__
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <vector>
#include <unordered_set>
#define INF 1e10
#define EPSILON 1e-5

class Simplex{
public:
    /// 输入目标函数向量（按照单纯形表首行格式）以及约束增广矩阵
    Simplex(Eigen::RowVectorXd _tar, Eigen::MatrixXd _cstrn):
        target(_tar), constrain(_cstrn), _m(_cstrn.rows()), _n(_cstrn.cols() - 1), rhs(constrain.col(_n))
    {
        loop_cnt = 0;
        artifacts = 0;
        counter = new int[_m];
        memset(counter, 0, _m * sizeof(int));
    }

    Simplex(Eigen::MatrixXd _cstrn):
        constrain(_cstrn), _m(_cstrn.rows()), _n(_cstrn.cols() - 1), rhs(constrain.col(_n))
    {
        loop_cnt = 0;
        artifacts = 0;
        counter = new int[_m];
        memset(counter, 0, _m * sizeof(int));
    }

    ~Simplex(){
        delete[] counter;
    }
public:
    // 双阶段解法
    bool doubleStageSolve(const Eigen::RowVectorXd& tar);
    bool solve();
    void showResults() const;
private:
    // 起始情况：首先从增广矩阵中找到m个线性无关向量（使用阶梯化的方式）
    // 此后取出这些列，拼合矩阵B，得到(B^-1)，继续使用对角化？
    // 化为典式，函数退出
    void getCanonical();
    bool stageOneOptimize(std::unordered_set<int>& slct);        // 阶段一优化

    // 阶梯化，Binv为阶梯化结果，得到矩阵B
    void ladderize(Eigen::MatrixXd& B);
    bool findBiggestInspect(int& index) const;
    bool isLooping() const;
private:
    Eigen::RowVectorXd target;
    Eigen::MatrixXd constrain;
    int artifacts;                  // 人工变量个数
    int _m;
    int _n;                         // 此处_n的定义是 除了RHS列外的行数（输入的constrain包含RHS）
    int* counter;
    int loop_cnt;
    std::vector<int> base_index;
    const Eigen::Block<Eigen::MatrixXd, -1, 1, true>& rhs;
};

#endif  //__SIMPLEX_HPP__


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/ratio.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/group.hpp>
#include <laws/monoid.hpp>

#include <ratio>
using namespace boost::hana;

int main() {
    auto ratios = make<Tuple>(
          std::ratio<0>{}
        , std::ratio<1, 3>{}
        , std::ratio<1, 2>{}
        , std::ratio<2, 6>{}
        , std::ratio<3, 1>{}
        , std::ratio<7, 8>{}
        , std::ratio<3, 5>{}
        , std::ratio<2, 1>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    {
        // plus
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                plus(std::ratio<3, 4>{}, std::ratio<5, 10>{}),
                std::ratio<3*10 + 5*4, 4*10>{}
            ));
        }

        // zero
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                zero<ext::std::Ratio>(),
                std::ratio<0, 1>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                zero<ext::std::Ratio>(),
                std::ratio<0, 2>{}
            ));
        }

        // laws
        test::TestMonoid<ext::std::Ratio>{ratios};
    }

    //////////////////////////////////////////////////////////////////////////
    // Group
    //////////////////////////////////////////////////////////////////////////
    {
        // minus
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                minus(std::ratio<3, 4>{}, std::ratio<5, 10>{}),
                std::ratio<3*10 - 5*4, 4*10>{}
            ));
        }

        // laws
        test::TestGroup<ext::std::Ratio>{ratios};
    }
}


#ifndef MIN_JERK_CURVE_VEC_H
#define MIN_JERK_CURVE_VEC_H

#include <Eigen/Dense>
#include <Utils/Math/minjerk_one_dim.hpp>

// vector version of min jerk interpolation

class MinJerkCurveVec{
public:
	MinJerkCurveVec();
	MinJerkCurveVec(const Eigen::VectorXd & start_pos, const Eigen::VectorXd & start_vel, const Eigen::VectorXd & start_acc,
			  	    const Eigen::VectorXd & end_pos, const Eigen::VectorXd & end_vel, const Eigen::VectorXd & end_acc,
			  	    double duration);
	~MinJerkCurveVec();
	Eigen::VectorXd evaluate(const double & t_in);
	Eigen::VectorXd evaluateFirstDerivative(const double & t_in);
	Eigen::VectorXd evaluateSecondDerivative(const double & t_in);

private:
	double Ts;

	Eigen::VectorXd p1;
	Eigen::VectorXd v1;
	Eigen::VectorXd a1;

	Eigen::VectorXd p2;
	Eigen::VectorXd v2;
	Eigen::VectorXd a2;

	std::vector<MinJerk_OneDimension> curves;
 	Eigen::VectorXd output;
};

#endif


#include "graphing.h"

#include <boost/graph/connected_components.hpp>

namespace gaus::underground_modelling {

Graph
make_graph(const arma::umat &adj_mat){

  assert(adj_mat.is_symmetric());

  Graph g;

  const arma::uvec connections = arma::find(adj_mat == 1);

  const auto [n_rows, n_cols] = arma::size(adj_mat);

  assert(n_rows == n_cols);

  for(const auto connection : connections){

    const auto vertex_a = connection / n_rows;
    const auto vertex_b = connection % n_rows;

    add_edge(vertex_a, vertex_b, g);

  }

  return g;

}

double
check_connectivity(const arma::umat &adjacency){

  const auto graph = make_graph(adjacency);

  std::vector<int> component(num_vertices(graph));

  return connected_components(graph, &component[0]);

}

arma::mat
outer_difference(const arma::vec &vec){

  const auto n_elem = vec.n_elem;

  arma::mat mat = arma::zeros(arma::size(n_elem, n_elem));

  for(int i = 0; i < n_elem; i++){
    for(int j = 0; j < i; j++){

      mat(i, j) = vec(i) - vec(j);
      mat(j, i) = vec(j) - vec(i);

    }
  }

  return mat;
}

arma::mat
find_distances(const StationCoordinates &station_coord){

  const auto x = outer_difference(station_coord.x);
  const auto y = outer_difference(station_coord.y);

  const auto distances = arma::sqrt(arma::square(x) + arma::square(y));

  return distances;

}

double
calculate_cost(const arma::umat &adjacency,
               const StationCoordinates &station_coord,
               const CostParameters &cp){

  const auto all_distances = find_distances(station_coord);

  const auto built_distances = all_distances % adjacency;

  //0.5 to stop double counting
  return 0.5 * (arma::accu(built_distances * cp.cost_per_unit)
                + arma::accu(adjacency) * cp.base_cost);
}

double
find_terminal_stations(const arma::umat &adjacency){

  const auto n_connections = arma::sum(adjacency, 1);

  return 1. + arma::find(n_connections <= 1).eval().n_elem;

}

}


#include <vector>
#include "Solvers.h"
#include "math.h"
#include "MatrixMath.h"
#include <iostream>
#include "CoolPropTools.h"
#include <Eigen/Dense>

namespace CoolProp{

/** \brief Calculate the Jacobian using numerical differentiation by column
 */
std::vector<std::vector<double> > FuncWrapperND::Jacobian(const std::vector<double> &x)
{
    double epsilon;
    std::size_t N = x.size();
    std::vector<double> r, xp;
    std::vector<std::vector<double> > J(N, std::vector<double>(N, 0));
    std::vector<double> r0 = call(x);
    // Build the Jacobian by column
    for (std::size_t i = 0; i < N; ++i)
    {
        xp = x;
        epsilon = 0.001*x[i];
        xp[i] += epsilon;
        r = call(xp);

        for(std::size_t j = 0; j < N; ++j)
        {
            J[j][i] = (r[j]-r0[j])/epsilon;
        }
    }
    return J;
}

/**
In this formulation of the Multi-Dimensional Newton-Raphson solver the Jacobian matrix is known.
Therefore, the dx vector can be obtained from

J(x)dx=-f(x)

for a given value of x.  The pointer to the class FuncWrapperND that is passed in must implement the call() and Jacobian()
functions, each of which take the vector x. The data is managed using std::vector<double> vectors

@param f A pointer to an subclass of the FuncWrapperND class that implements the call() and Jacobian() functions
@param x0 The initial guess value for the solution
@param tol The root-sum-square of the errors from each of the components
@param maxiter The maximum number of iterations
@param errstring  A string with the returned error.  If the length of errstring is zero, no errors were found
@returns If no errors are found, the solution.  Otherwise, _HUGE, the value for infinity
*/
std::vector<double> NDNewtonRaphson_Jacobian(FuncWrapperND *f, const std::vector<double> &x, double tol, int maxiter)
{
    int iter=0;
    f->errstring.clear();
    std::vector<double> f0,v;
    std::vector<std::vector<double> > JJ;
    std::vector<double> x0 = x;
    Eigen::VectorXd r(x0.size());
    Eigen::MatrixXd J(x0.size(), x0.size());
    double error = 999;
    while (iter==0 || std::abs(error)>tol){
        f0 = f->call(x0);
        JJ = f->Jacobian(x0);

        for (std::size_t i = 0; i < x0.size(); ++i)
        {
            r(i) = f0[i];
            for (std::size_t j = 0; j < x0.size(); ++j)
            {
                J(i,j) = JJ[i][j];
            }
        }

        Eigen::VectorXd v = J.colPivHouseholderQr().solve(-r);

        // Update the guess
        double max_relchange = -1;
        for (std::size_t i = 0; i<x0.size(); i++){
            x0[i] += v(i);
            double relchange = std::abs(v(i)/x0[i]);
            if (std::abs(x0[i]) > 1e-16 && relchange > max_relchange ){
                max_relchange = relchange;
            }
        }

        // Stop if the solution is not changing by more than numerical precision
        double max_abschange = v.cwiseAbs().maxCoeff();
        if (max_abschange < DBL_EPSILON*100){
            return x0;
        }
        if (max_relchange < 1e-12){
            return x0;
        }
        error = root_sum_square(f0);
        if (iter>maxiter){
            f->errstring = "reached maximum number of iterations";
            x0[0] = _HUGE;
        }
        iter++;
    }
    return x0;
}

/**
In the newton function, a 1-D Newton-Raphson solver is implemented using exact solutions.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solution
@param ftol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Newton(FuncWrapper1DWithDeriv* f, double x0, double ftol, int maxiter)
{
    double x, dx, fval=999;
    int iter=1;
    f->errstring.clear();
    x = x0;
    while (iter < 2 || std::abs(fval) > ftol)
    {
        fval = f->call(x);
        dx = -fval/f->deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in newton returned invalid number");
        };

        x += dx;

        if (std::abs(dx/x) < 1e-11){
            return x;
        }

        if (iter>maxiter)
        {
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Newton reached maximum number of iterations"));
        }
        iter=iter+1;
    }
    return x;
}
/**
In the Halley's method solver, two derivatives of the input variable are needed, it yields the following method:

\f[
x_{n+1} = x_n - \frac {2 f(x_n) f'(x_n)} {2 {[f'(x_n)]}^2 - f(x_n) f''(x_n)}
\f]

http://en.wikipedia.org/wiki/Halley%27s_method

@param f A pointer to an instance of the FuncWrapper1DWithTwoDerivs class that implements the call() and two derivatives
@param x0 The initial guess for the solution
@param ftol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@param xtol_rel The minimum allowable (relative) step size
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Halley(FuncWrapper1DWithTwoDerivs* f, double x0, double ftol, int maxiter, double xtol_rel)
{
    double x, dx, fval=999, dfdx, d2fdx2;

    // Initialize
    f->iter=0;
    f->errstring.clear();
    x = x0;

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    while (f->iter < 2 || std::abs(fval) > ftol)
    {
        if (f->input_not_in_range(x)){
            throw ValueError(format("Input [%g] is out of range",x));
        }

        fval = f->call(x);
        dfdx = f->deriv(x);
        d2fdx2 = f->second_deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in Halley returned invalid number");
        };
        if (!ValidNumber(dfdx)){
            throw ValueError("Derivative function in Halley returned invalid number");
        };

        dx = -omega*(2*fval*dfdx)/(2*POW2(dfdx)-fval*d2fdx2);

        x += dx;

        if (std::abs(dx/x) < xtol_rel){
            return x;
        }

        if (f->iter>maxiter){
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Halley reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x;
}

/**
 In the 4-th order Householder method, three derivatives of the input variable are needed, it yields the following method:

 \f[
 x_{n+1} = x_n - f(x_n)\left( \frac {[f'(x_n)]^2 - f(x_n)f''(x_n)/2  } {[f'(x_n)]^3-f(x_n)f'(x_n)f''(x_n)+f'''(x_n)*[f(x_n)]^2/6 } \right)
 \f]

http://numbers.computation.free.fr/Constants/Algorithms/newton.ps

 @param f A pointer to an instance of the FuncWrapper1DWithThreeDerivs class that implements the call() and three derivatives
 @param x0 The initial guess for the solution
 @param ftol The absolute value of the tolerance accepted for the objective function
 @param maxiter Maximum number of iterations
 @param xtol_rel The minimum allowable (relative) step size
 @returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
 */
double Householder4(FuncWrapper1DWithThreeDerivs* f, double x0, double ftol, int maxiter, double xtol_rel)
{
    double x, dx, fval=999, dfdx, d2fdx2, d3fdx3;

    // Initialization
    f->iter=1;
    f->errstring.clear();
    x = x0;

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    while (f->iter < 2 || std::abs(fval) > ftol)
    {
        if (f->input_not_in_range(x)){
            throw ValueError(format("Input [%g] is out of range",x));
        }

        fval = f->call(x);
        dfdx = f->deriv(x);
        d2fdx2 = f->second_deriv(x);
        d3fdx3 = f->third_deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in Householder4 returned invalid number");
        };
        if (!ValidNumber(dfdx)){
            throw ValueError("Derivative function in Householder4 returned invalid number");
        };
        if (!ValidNumber(d2fdx2)){
            throw ValueError("Second derivative function in Householder4 returned invalid number");
        };
        if (!ValidNumber(d3fdx3)){
            throw ValueError("Third derivative function in Householder4 returned invalid number");
        };

        dx = -omega*fval*(POW2(dfdx)-fval*d2fdx2/2.0)/(POW3(dfdx)-fval*dfdx*d2fdx2+d3fdx3*POW2(fval)/6.0);

        x += dx;

        if (std::abs(dx/x) < xtol_rel){
            return x;
        }

        if (f->iter>maxiter){
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Householder4 reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x;
}

/**
In the secant function, a 1-D Newton-Raphson solver is implemented.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solutionh
@param dx The initial amount that is added to x in order to build the numerical derivative
@param tol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Secant(FuncWrapper1D* f, double x0, double dx, double tol, int maxiter)
{
    #if defined(COOLPROP_DEEP_DEBUG)
    static std::vector<double> xlog, flog;
    xlog.clear(); flog.clear();
    #endif

    // Initialization
    double x1=0,x2=0,x3=0,y1=0,y2=0,x,fval=999;
    f->iter=1;
    f->errstring.clear();

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    if (std::abs(dx)==0){ f->errstring="dx cannot be zero"; return _HUGE;}
    while (f->iter<=2 || std::abs(fval)>tol)
    {
        if (f->iter==1){x1=x0; x=x1;}
        if (f->iter==2){x2=x0+dx; x=x2;}
        if (f->iter>2) {x=x2;}

            if (f->input_not_in_range(x)){
                throw ValueError(format("Input [%g] is out of range",x));
            }

            fval = f->call(x);

            #if defined(COOLPROP_DEEP_DEBUG)
                xlog.push_back(x);
                flog.push_back(fval);
            #endif

            if (!ValidNumber(fval)){
                throw ValueError("Residual function in secant returned invalid number");
            };
        if (f->iter==1){y1=fval;}
        if (f->iter>1)
        {
            double deltax = x2-x1;
            if (std::abs(deltax)<1e-14){
                return x;
            }
            y2=fval;
            double deltay = y2-y1;
            if (f->iter > 2 && std::abs(deltay)<1e-14){
                return x;
            }
            x3=x2-omega*y2/(y2-y1)*(x2-x1);
            y1=y2; x1=x2; x2=x3;

        }
        if (f->iter>maxiter)
        {
            f->errstring=std::string("reached maximum number of iterations");
            throw SolutionError(format("Secant reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x3;
}

/**
In the secant function, a 1-D Newton-Raphson solver is implemented.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solution
@param xmax The upper bound for the solution
@param xmin The lower bound for the solution
@param dx The initial amount that is added to x in order to build the numerical derivative
@param tol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double BoundedSecant(FuncWrapper1D* f, double x0, double xmin, double xmax, double dx, double tol, int maxiter)
{
    double x1=0,x2=0,x3=0,y1=0,y2=0,x,fval=999;
    int iter=1;
    f->errstring.clear();
    if (std::abs(dx)==0){ f->errstring = "dx cannot be zero"; return _HUGE;}
    while (iter<=3 || std::abs(fval)>tol)
    {
        if (iter==1){x1=x0; x=x1;}
        else if (iter==2){x2=x0+dx; x=x2;}
        else {x=x2;}
            fval=f->call(x);
        if (iter==1){y1=fval;}
        else
        {
            y2=fval;
            x3=x2-y2/(y2-y1)*(x2-x1);
            // Check bounds, go half the way to the limit if limit is exceeded
            if (x3 < xmin)
            {
                x3 = (xmin + x2)/2;
            }
            if (x3 > xmax)
            {
                x3 = (xmax + x2)/2;
            }
            y1=y2; x1=x2; x2=x3;

        }
        if (iter>maxiter){
            f->errstring = "reached maximum number of iterations";
            throw SolutionError(format("BoundedSecant reached maximum number of iterations"));
        }
        iter=iter+1;
    }
    f->errcode = 0;
    return x3;
}

/**

This function implements a 1-D bounded solver using the algorithm from Brent, R. P., Algorithms for Minimization Without Derivatives.
Englewood Cliffs, NJ: Prentice-Hall, 1973. Ch. 3-4.

a and b must bound the solution of interest and f(a) and f(b) must have opposite signs.  If the function is continuous, there must be
at least one solution in the interval [a,b].

@param f A pointer to an instance of the FuncWrapper1D class that must implement the class() function
@param a The minimum bound for the solution of f=0
@param b The maximum bound for the solution of f=0
@param macheps The machine precision
@param t Tolerance (absolute)
@param maxiter Maximum number of steps allowed.  Will throw a SolutionError if the solution cannot be found
*/
double Brent(FuncWrapper1D* f, double a, double b, double macheps, double t, int maxiter)
{
    int iter;
    f->errstring.clear();
    double fa,fb,c,fc,m,tol,d,e,p,q,s,r;
    fa = f->call(a);
    fb = f->call(b);

    // If one of the boundaries is to within tolerance, just stop
    if (std::abs(fb) < t) { return b;}
    if (!ValidNumber(fb)){
        throw ValueError(format("Brent's method f(b) is NAN for b = %g, other input was a = %g",b,a).c_str());
    }
    if (std::abs(fa) < t) { return a;}
    if (!ValidNumber(fa)){
        throw ValueError(format("Brent's method f(a) is NAN for a = %g, other input was b = %g",a,b).c_str());
    }
    if (fa*fb>0){
        throw ValueError(format("Inputs in Brent [%f,%f] do not bracket the root.  Function values are [%f,%f]",a,b,fa,fb));
    }

    c=a;
    fc=fa;
    iter=1;
    if (std::abs(fc)<std::abs(fb)){
        // Goto ext: from Brent ALGOL code
        a=b;
        b=c;
        c=a;
        fa=fb;
        fb=fc;
        fc=fa;
    }
    d=b-a;
    e=b-a;
    m=0.5*(c-b);
    tol=2*macheps*std::abs(b)+t;
    while (std::abs(m)>tol && fb!=0){
        // See if a bisection is forced
        if (std::abs(e)<tol || std::abs(fa) <= std::abs(fb)){
            m=0.5*(c-b);
            d=e=m;
        }
        else{
            s=fb/fa;
            if (a==c){
                //Linear interpolation
                p=2*m*s;
                q=1-s;
            }
            else{
                //Inverse quadratic interpolation
                q=fa/fc;
                r=fb/fc;
                m=0.5*(c-b);
                p=s*(2*m*q*(q-r)-(b-a)*(r-1));
                q=(q-1)*(r-1)*(s-1);
            }
            if (p>0){
                q=-q;
            }
            else{
                p=-p;
            }
            s=e;
            e=d;
            m=0.5*(c-b);
            if (2*p<3*m*q-std::abs(tol*q) || p<std::abs(0.5*s*q)){
                d=p/q;
            }
            else{
                m=0.5*(c-b);
                d=e=m;
            }
        }
        a=b;
        fa=fb;
        if (std::abs(d)>tol){
            b+=d;
        }
        else if (m>0){
            b+=tol;
        }
        else{
            b+=-tol;
        }
        fb=f->call(b);
        if (!ValidNumber(fb)){
            throw ValueError(format("Brent's method f(t) is NAN for t = %g",b).c_str());
        }
        if (std::abs(fb) < macheps){
            return b;
        }
        if (fb*fc>0){
            // Goto int: from Brent ALGOL code
            c=a;
            fc=fa;
            d=e=b-a;
        }
        if (std::abs(fc)<std::abs(fb)){
            // Goto ext: from Brent ALGOL code
            a=b;
            b=c;
            c=a;
            fa=fb;
            fb=fc;
            fc=fa;
        }
        m=0.5*(c-b);
        tol=2*macheps*std::abs(b)+t;
        iter+=1;
        if (!ValidNumber(a)){
            throw ValueError(format("Brent's method a is NAN").c_str());}
        if (!ValidNumber(b)){
            throw ValueError(format("Brent's method b is NAN").c_str());}
        if (!ValidNumber(c)){
            throw ValueError(format("Brent's method c is NAN").c_str());}
        if (iter>maxiter){
            throw SolutionError(format("Brent's method reached maximum number of steps of %d ", maxiter));}
        if (std::abs(fb)< 2*macheps*std::abs(b)){
            return b;
        }
    }
    return b;
}

}; /* namespace CoolProp */


#include "../simplex.hpp"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <iostream>
#include <vector>
#define PROBLEM "https://yukicoder.me/problems/no/1122"
using namespace std;

int main() {
    using Float = boost::multiprecision::cpp_dec_float_50;
    vector<Float> B(5);
    for (auto &x : B) cin >> x;
    vector<vector<Float>> A{{1, 1, 1, 0, 0}, {0, 1, 1, 1, 0}, {0, 0, 1, 1, 1}, {1, 0, 0, 1, 1}, {1, 1, 0, 0, 1}};
    vector<Float> C(5, 1);
    Simplex<Float, 30> simplex(A, B, C);
    cout << llround(simplex.ans - 0.17) << '\n'; // I haven't proved yet this always returns correct answer.
}


/*
    File Name:      main.cpp
    ROS module:     simpl_ekf
    Description:    Implements an extended Kalman Filter for attitude estimation.
                    Only uses the accelerometer measurements with a simplified
                    model to compute pitch and roll of the device.
    Author:         Thomas Lew
    Last update:    2017.03.13
    Comments:       -Assumptions: - No movement (linear & angular) of the device
                                  - The model is therefore overly simplified.
                    -Known bug: The pitch value is always negative regardless of
                      (solved)  the rotation around y-axis. The cause is a wrong
                                z-value of vector "y" in update function.
                                This problem is solved in publishOdometryResult()
                                by using accel-x (ax) to determine the direction
                                of the pitch angle.
                                                                                */
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>

//  Library for linear algebra
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <math.h>

using namespace std;
using namespace Eigen;

//  Debugging boolean for printing EKF variables during execution
#define EKF_DEBUG 1

//  Number of IMU measurements used for calibration
#define IMU_COUNT_CALIBRATION 30

//  Euler angles values limits for debugging
#define PITCH_MAX M_PI/2
#define ROLL_MAX  M_PI/2
#define YAW_MAX   M_PI/2

/*  ------------------------------------------------------------
    Parameters of the Extended Kalman Filter. Depends on the IMU
    ------------------------------------------------------------                        */
/*//  Python Script simulated IMU measurements:
#define FREQ_HZ         50      // 50Hz for python Script
#define PERIOD_SEC      0.02    // 1/50 for python Script
#define SIGMA_SENSORS   0.1     // Variance of python script IMU data (imu_publisher)   */

//  Nvdia Shield Tablet IMU (only accelerometer) measurements:
#define FREQ_HZ       50        // 50Hz for Nvdia Shield Tablet
#define PERIOD_SEC    0.02      // 1/50 for Nvdia Shield Tablet
//  Computed with imu_variance.py : Variance_Accelerometer = 0.0004
#define SIGMA_SENSORS 0.02      // Variance of Nvdia Shield Tablet IMU                  */

/*  -----------------------------------------------------------------------------
    ROS Topics used for publishing the results and receiving the IMU measurements
    -----------------------------------------------------------------------------       */
ros::Publisher pub_odometry;
ros::Subscriber sub_imu;

const char imu_topic_name[] = "/imu_3dm_gx4/imu";
//const char imu_topic_name[] = "/python_IMU";

/*  ----------------------------------
    State vector

    Representation using Euler angles:

                [ Yaw ]
            x = [Pitch]
                [Roll ]

    ----------------------------------   */
VectorXd x(VectorXd::Zero(3));

//  Bias of sensors
const MatrixXd Q(PERIOD_SEC * PERIOD_SEC * MatrixXd::Identity(3,3));

//  Noise of sensors. We assume same value for accelerometer and gyroscope
const MatrixXd R(SIGMA_SENSORS * MatrixXd::Identity(3,3));

/*  --------------------
    Prediction variables
    --------------------    */

/*  Transition matrix A
    We assume a linear model with no motion, no rotation.   */
MatrixXd A(MatrixXd::Identity(3,3));

//  Covariance estimate matrix P
MatrixXd P(MatrixXd::Zero(3,3));

/*  --------------------
    Update variables
    --------------------    */

/*  Form of the measurements z:

        [accel-x]
    z = [accel-y]   with (-1 <= z <= 1)
        [accel-z]
    Note. These accelerometer measurements values are normalised.   */

/*  Measurement prediction vector H(x). It depends on the current
    Yaw, Pitch and Roll values of the state.                        */
VectorXd H(VectorXd::Zero(3));
/*  Jacobian matrix of H in regard of x                             */
MatrixXd J(MatrixXd::Zero(3,3));

/*  Accelerometer and gyroscope measurement for IMU calibration.
    These values are not used in the EKF but can be useful for
    further implementations.                                        */
double gx = 0;
double gy = 0;
double gz = 0;  //9.81

double bias_gyro_x = 0;
double bias_gyro_y = 0;
double bias_gyro_z = 0;

/*  IMU variables set at the start of the execution of the program.
    Used for calibration.                                           */
double current_t = 0;
bool is_time_init = false;
bool is_imu_init = false;
int imu_count = 0;

/*  Initialisation of the State initialisation.                     */
void state_init(){
    x.segment<3>(0) = Vector3d(0,0,0);
}

/*  Jacobian matrix Initialisation                                  */
void jacobian_init(){
    // block use: Selects the .block(1st value location, size matrix)
    /*J.block(0,3,3,3) = MatrixXd::Identity(3,3);
    J.block(0,6,3,3) = MatrixXd::Identity(3,3);*/
}

/* Covariance matrix Q initialisation                               */
void my_Q_covariance_matrix_init(){
    /*//  Values for the Nvidia shield tablet
    Q.block(0,0,3,3) = 0.032 * 0.032 * MatrixXd::Identity(3,3);
    Q.block(3,3,3,3) = 0.1 * 0.032 * MatrixXd::Identity(3,3);
    Q.block(6,6,3,3) = 0.03 * 0.032 * MatrixXd::Identity(3,3);  //  */
    /*//  Values for the test python publisher
    //  0.02 ) 1/50 with 50hz
    /*double delta_t = 1 / double(FREQ_HZ);
    Q = delta_t * delta_t * MatrixXd::Identity(3,3);*/
    /*Q.block(3,3,3,3) = 0.1 * 0.02 * MatrixXd::Identity(3,3);
    Q.block(6,6,3,3) = 0.03 * 0.02 * MatrixXd::Identity(3,3);   //  */
}

/*  Computes the original orientation and gyroscope bias.
    These values are not used in the EKF but can be useful for
    further implementations.                                        */
void imu_init(double ax, double ay, double az,
              double rx, double ry, double rz){
    gx += ax;
    gy += ay;
    gz += az;

    bias_gyro_x += rx;
    bias_gyro_y += ry;
    bias_gyro_z += rz;

    if(imu_count == IMU_COUNT_CALIBRATION){
        gx /= imu_count;
        gy /= imu_count;
        gz /= imu_count;

        bias_gyro_x /= imu_count;
        bias_gyro_y /= imu_count;
        bias_gyro_z /= imu_count;

        cout << "calibrated gx: " << gx << endl;
        cout << "calibrated gy: " << gy << endl;
        cout << "calibrated gz: " << gz << endl;

        cout << "bias_gyro_x: " << bias_gyro_x << endl;
        cout << "bias_gyro_y: " << bias_gyro_y << endl;
        cout << "bias_gyro_z: " << bias_gyro_z << endl;
    }
}

Vector3d get_euler_angles()
{
    return x.segment<3>(0);
}

void publishOdometryResult(double time_imu, double ax)
{
    //  Get current state variables
    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    //  Odometry results are published
    nav_msgs::Odometry odometry;
    odometry.header.stamp = ros::Time::now();
    odometry.header.frame_id = "world";
    odometry.pose.pose.position.x = 0;
    odometry.pose.pose.position.y = 0;
    odometry.pose.pose.position.z = 0;
    odometry.pose.pose.orientation.x = 0;
    odometry.pose.pose.orientation.y = 0;
    odometry.pose.pose.orientation.z = 0;
    odometry.pose.pose.orientation.w = 1;
    odometry.twist.twist.linear.x = 0;
    odometry.twist.twist.linear.y = 0;
    odometry.twist.twist.linear.y = 0;
    odometry.twist.twist.angular.z = angles(0);
    /*  Fix for the pitch angular being always negative */
    if(ax >= 0)
        odometry.twist.twist.angular.y = abs(angles(1));
    else
        odometry.twist.twist.angular.y = -abs(angles(1));
    odometry.twist.twist.angular.x = angles(2);

    //  Publish EKF Results
    pub_odometry.publish(odometry);

    /*  For Debugging purposes:
        The pitch, roll and yaw values are displayed on the terminal                */
    if(EKF_DEBUG){
        cout << "--------------------------------------------" << endl;
        cout << "End of kalman filtering for this imu dataset" << endl;
        cout << "--------------------------------------------" << endl;
        ROS_INFO("Odometry results published after IMU sample time: %lf", time_imu);
        cout << "Yaw: " <<      yaw     << endl;
        cout << "Pitch: " <<    pitch   << endl;
        cout << "Roll: " <<     roll    << endl;
        cout << "                     " << endl;

        //  We limit the pitch and roll values to stop a non converging Kalman Filter
        if(pitch > PITCH_MAX || pitch < -PITCH_MAX || roll > ROLL_MAX || roll < -ROLL_MAX){
            cout << "pitch or roll value too high " << endl;
            abort();
        }
    }
}

/*  Extended Kalman Filter prediction function. */
void predict(double dt){
    if(EKF_DEBUG){
        cout << "                       " << endl;
        cout << " --------------------- " << endl;
        cout << "  NEW STATE PREDICTION " << endl;
        cout << " --------------------- " << endl;
        cout << "                       " << endl;
    }

    /*  -----------------------------
        Predict the prediction matrix
        -----------------------------
            Prediction matrix A
                    {1 0 0}
                A = {0 1 0}
                    {0 0 1}         */

    MatrixXd A(MatrixXd::Identity(3,3));

    x = A * x;

    /*  The Yaw cannot be computed with this model.
        To avoid a drift of this angle, it is set to zero.              */
    x[0] = 0;

    //  Covariance matrix prediction
    P = A * P * A.transpose() + Q;

    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    if(EKF_DEBUG){
        cout << "predicted pitch   yaw   roll "         << endl;
        cout << pitch << ";  "  << yaw << ";  " << roll << endl;
        cout << "                 "                     << endl;

        cout << "Value of P after prediction"   << endl;
        cout << P                               << endl;
        cout << "                 "             << endl;

        cout << "Value of Q : "                 << endl;
        cout << Q                               << endl;
        cout << "                 "             << endl;
    }
}

/*  For Debugging purposes:
    the state vector, measurements and EKF matrices
    are displayed on the terminal.                              */
void debug_ekf_cout(double pitch, double yaw, double roll,
                    VectorXd z, Vector3d H, Matrix<double, 3, 3> J,
                    VectorXd y, Matrix<double, 3, 3> S,
                    Matrix<double, 3, 3> K, MatrixXd P){
    cout << "Old value of Pitch, yaw and Roll " << endl;
    cout << pitch << "  " << yaw << "  " << roll << endl;
    cout << "                 " << endl;
    cout << "Value of z (ax, ay, az) :" << endl;
    cout << z << endl;
    cout << "                 " << endl;
    cout << "H vector updated: " << endl;
    cout << H             << endl;
    cout << "           " << endl;
    cout << "J matix updated: " << endl;
    cout << J << endl;
    cout << "                 " << endl;
    cout << "Value of y " << endl;
    cout << y << endl;
    cout << "                 " << endl;
    /*cout << "S  updated: "<< endl;
    cout << S << endl;
    cout << "                 " << endl;
    cout << "S inverse updated: "<< endl;
    cout << S.inverse() << endl;
    cout << "                 " << endl;*/
    cout << "K updated: " << endl;
    cout << K << endl;
    cout << "                 " << endl;
    cout << "K * y updated: " << endl;
    cout << K * y << endl;
    cout << "                 " << endl;
    /*cout << "P updated: "<< endl;
    cout << P << endl;
    cout << "                 " << endl;*/
}

/*  Extended Kalman Filter update function. */
void update(double ax, double ay, double az, double rx, double ry, double rz, double dt)
{
    if(EKF_DEBUG){
        cout << "                 " << endl;
        cout << "---------------- " << endl;
        cout << " NEW UPDATE n. "   << imu_count << endl;
        cout << "---------------- " << endl;
        cout << "                 " << endl;
    }

    Vector3d angles = get_euler_angles();
    double yaw   = angles(0);
    double pitch = angles(1);
    double roll  = angles(2);

    //  Measurement Vector
    VectorXd z(3);
    z << ax, ay, az;

    /*  ---------------------------------------------------
        -                Measurement Model                -
        ---------------------------------------------------   */

    /*          --------------------------
                Measurement vector z model
                --------------------------
                               T  (0)            -1       T
                z = H(x) = Rzyx * (0),  with Rzyx   = Rzyx
                                  (1)   and Rzyx = Rz*Ry*Rx = R = ...

    [cos(yaw) -sin(yaw) 0][cos(pitch) 0 sin(pitch)][1     0          0    ]
    [sin(yaw) cos(yaw)  0][    0      1      0    ][0 cos(roll) -sin(roll)]
    [   0         0     1][-sin(pitch)0 cos(pitch)][0 sin(roll)  cos(roll)]

            --------------------------------------
            Jacobian matrix from measurement model
            --------------------------------------
                    J = d/dx (H(x))
                                                                        */

    //  Theoretical Measurement from state estimate:
    Vector3d H(3);
    H <<    -sin(pitch),
            cos(pitch)*sin(roll),
            cos(pitch)*cos(roll);

            /*cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll),
            sin(yaw)*sin(pitch)*sin(roll) - cos(yaw)*sin(roll),
            cos(pitch)*cos(roll);*/

    //  Jacobian matrix J computation
    Matrix<double, 3, 3> J;
    J <<    0,  -cos(pitch),            0,
            0,  -sin(pitch)*sin(roll),  cos(pitch)*cos(roll),
            0,  -sin(pitch)*cos(roll),  -cos(pitch)*sin(roll);

    /*
    -sin(yaw)*sin(pitch)*cos(roll)+cos(yaw)*sin(roll), cos(yaw)*cos(pitch)*cos(roll), -cos(yaw)*sin(pitch)*sin(roll)+sin(yaw)*cos(roll),
    cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll),  sin(yaw)*cos(pitch)*cos(roll), -sin(yaw)*sin(pitch)*sin(roll)-cos(yaw)*cos(roll),
    0,                                                 -sin(pitch)*cos(roll),         -cos(pitch)*sin(roll);*/

    /*  -------------------------------------
        Update step of Extended Kalman Filter
        -------------------------------------  */

    //  Innovation vector
    VectorXd y(3);
    y = z - H;

    //  Innovation covariance
    MatrixXd S(3,3);
    S = J * P * J.transpose() + R;

    //  Kalman gain
    MatrixXd K(3,3);
    K = P * J * S.inverse();

    //  State estimate update
    x = x + K * y;

    //  Covariance estimate update
    P = (MatrixXd::Identity(3, 3) - K * J) * P;

    //  If Debug Mode: Print all variables computed by this update
    if(EKF_DEBUG){
        debug_ekf_cout(pitch, yaw, roll, z, H, J, y, S, K, P);
    }
}

/*  Extended Kalman Filter global function.             */
void kalman_filtering(double ax, double ay, double az,
                      double rx, double ry, double rz,
                      double dt){
    predict(dt);
    update(ax, ay, az, rx, ry, rz, dt);
}

/*  IMU callback function.
    Uses the IMU measurements to compute the new state variables using an EKF. */
void imu_callback(const sensor_msgs::Imu& imu_msg)
{
    ROS_INFO("Time of IMU sample: %lf", imu_msg.header.stamp.toSec());

    /*  Accelerometer and Gyroscope measurements.
        This version doesn't use the angular velocity values */
    double ax = imu_msg.linear_acceleration.x;
    double ay = imu_msg.linear_acceleration.y;
    double az = imu_msg.linear_acceleration.z;
    double rx = imu_msg.angular_velocity.x;
    double ry = imu_msg.angular_velocity.y;
    double rz = imu_msg.angular_velocity.z;

    /*  ----------------------------------------------------------------------
        Calibrate Inertial Measurement Unit (IMU) at the start of the program.
        Can be useful in case of an initial spatial frame which is moving.
        Uses the IMU_COUNT_CALIBRATION first IMU values.
        ----------------------------------------------------------------------  */
    imu_count++;
    if(!is_imu_init){
        imu_init(ax,ay,az,rx,ry,rz);
        if(imu_count == IMU_COUNT_CALIBRATION){
            is_imu_init = true;
            imu_count = 0;
        }
        return;
    }

    //  After imu calibration, set the IMU time
    if(!is_time_init){
        current_t = imu_msg.header.stamp.toSec();
        is_time_init = true;
        return;
    }

    /*  --------------------------------
        Accelerometer data normalisation
        --------------------------------  */
    double norm_accel = sqrt(ax * ax + ay * ay + az * az);
    ax = ax / norm_accel;
    ay = ay / norm_accel;
    az = az / norm_accel;

    /*  ---------------
        IMU Time update
        ---------------  */
    double new_time = imu_msg.header.stamp.toSec();
    double dt = new_time - current_t;
    current_t = new_time;

    /*  --------------------------------------------------------------
        Extended Kalman Filtering.

        In this version, only the accelerometer measurements are used.
        --------------------------------------------------------------  */
    kalman_filtering(ax, ay, az, rx, ry, rz, dt);

    /*  -------------------------------------------------
        Results of the EKF are pusblished in a ROS topic.
        -------------------------------------------------  */
    publishOdometryResult(imu_msg.header.stamp.toSec(), ax);
}

void setupROS()
{
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);

    //  Define ROS topics to publish the results
    pub_odometry     = n.advertise<nav_msgs::Odometry>("odometry", 1000);

    //  Define IMU Measurements ROS topic.
    sub_imu = n.subscribe(imu_topic_name, 1000, imu_callback);
}

int main(int argc, char **argv)
{
    state_init();
    jacobian_init();
    my_Q_covariance_matrix_init();

    ros::init(argc, argv, "simpl_ekf");

    setupROS();
    ros::spin();

    return 0;
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/biconnected_components.hpp>

using namespace std;

struct topological {
  bool operator() (const pair<int, int> t0, const pair<int, int> t1) {
    return (t0.first < t1.first) || (t0.first == t1.first && t0.second < t1.second);
  }
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      graph;
typedef boost::property_map<graph, boost::edge_weight_t>::type edge_map;
typedef boost::graph_traits<graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<graph>::vertex_descriptor          vertex_desc;
typedef boost::graph_traits<graph>::edge_iterator                         edge_it;

void important_bridges(graph &G) {
  edge_map component = boost::get(boost::edge_weight, G);
  int ncc = boost::biconnected_components(G, component);

  vector<pair<int, int>> imp_bridges(0);
  edge_it ebeg, eend;
  vector<int> size_ncc(ncc, 0);
  for (boost::tie(ebeg, eend) = boost::edges(G); ebeg != eend; ++ebeg) {
    size_ncc[component[*ebeg]]++;
  }

  for (boost::tie(ebeg, eend) = boost::edges(G); ebeg != eend; ++ebeg) {
    int u = boost::source(*ebeg, G), v = boost::target(*ebeg, G);
    if (size_ncc[component[*ebeg]] == 1) imp_bridges.push_back({min(u,v),max(u,v)});
  }
  std::sort(imp_bridges.begin(), imp_bridges.end(), topological());
  cout << imp_bridges.size() << endl;
  for(auto e : imp_bridges)
    cout << e.first <<  " " << e.second << endl;

}

void testcase()
{
  int n; cin >> n;
  int m; cin >> m;
  graph G(n);
  for(int i = 0; i < m; i++) {
    int e1, e2; cin >> e1; cin >> e2;
    boost::add_edge(e1, e2, G);
  }

  important_bridges(G);
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) testcase();
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "MathTools.h"

#include <Eigen/Dense>
#include <cmath>

#include "Point3d.h"

namespace MathLib
{

double calcProjPntToLineAndDists(Point3d const& pp, Point3d const& pa,
                                 Point3d const& pb, double& lambda, double& d0)
{
    auto const a = Eigen::Map<Eigen::Vector3d const>(pa.getCoords());
    auto const b = Eigen::Map<Eigen::Vector3d const>(pb.getCoords());
    auto const p = Eigen::Map<Eigen::Vector3d const>(pp.getCoords());

    // g(lambda) = a + lambda v, v = b-a
    Eigen::Vector3d const v = b - a;

    // orthogonal projection: (p - g(lambda))^T * v = 0
    // <=> (a-p - lambda (b-a))^T * (b-a) = 0
    // <=> (a-p)^T * (b-a) = lambda (b-a)^T ) (b-a)
    lambda = (((p - a).transpose() * v) / v.squaredNorm())(0, 0);

    // compute projected point
    Eigen::Vector3d const proj_pnt = a + lambda * v;

    d0 = (proj_pnt - a).norm();

    return (p - proj_pnt).norm();
}

double getAngle(Point3d const& p0, Point3d const& p1, Point3d const& p2)
{
    auto const a = Eigen::Map<Eigen::Vector3d const>(p0.getCoords());
    auto const b = Eigen::Map<Eigen::Vector3d const>(p1.getCoords());
    auto const c = Eigen::Map<Eigen::Vector3d const>(p2.getCoords());
    Eigen::Vector3d const v0 = a - b;
    Eigen::Vector3d const v1 = c - b;

    // apply Cauchy Schwarz inequality
    return std::acos(
        (v0.transpose() * v1 / (v0.norm() * v1.norm()))(0, 0));
}

double scalarTriple(Eigen::Vector3d const& u, Eigen::Vector3d const& v,
                    Eigen::Vector3d const& w)
{
    return u.cross(v).dot(w);
}

}  // namespace MathLib