/*
 * libcluster -- A collection of hierarchical Bayesian clustering algorithms.
 * Copyright (C) 2013 Daniel M. Steinberg (daniel.m.steinberg@gmail.com)
 *
 * This file is part of libcluster.
 *
 * libcluster is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * libcluster is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with libcluster. If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/math/special_functions.hpp>
#include "distributions.h"
#include "probutils.h"

//
// Namespaces
//

using namespace std;
using namespace Eigen;
using namespace probutils;
using namespace boost::math;

//
//  File scope variables
//

// Define pi
const double pi = constants::pi<double>(); // Boost high precision pi

//
// Private Helper Functions
//

/* Compare an <int,double> double pair by the double member. Useful
 *  for sorting an array in descending order while retaining a notion of
 *  the original order of the array.
 *
 *  returns: true if i.second > j.second.
 */
bool inline obscomp (
        const std::pair<int,double>& i, // the first pair to compare.
        const std::pair<int,double>& j // the second pair to compare.
        )
{
        return i.second > j.second;
}

/* Enumerate the dimensions.
 *
 *  returns: 1:D or if D = 1, return 1.
 */
ArrayXd enumdims (const int D)
{
        ArrayXd l;

        if (D > 1)
                l = ArrayXd::LinSpaced(D, 1, D);
        else
                l.setOnes(1);

        return l;
}

//
// Stick-Breaking (Dirichlet Process) weight distribution.
//

distributions::StickBreak::StickBreak ()
        : WeightDist(),
        alpha1_p(distributions::ALPHA1PRIOR),
        alpha2_p(distributions::ALPHA2PRIOR),
        alpha1(ArrayXd::Constant(1, distributions::ALPHA1PRIOR)),
        alpha2(ArrayXd::Constant(1, distributions::ALPHA2PRIOR)),
        E_logv(ArrayXd::Zero(1)),
        E_lognv(ArrayXd::Zero(1)),
        E_logpi(ArrayXd::Zero(1)),
        ordvec(1, pair<int,double>(0,0))
{
        this->priorfcalc();
}

distributions::StickBreak::StickBreak (const double concentration)
        : WeightDist(),
        alpha2_p(distributions::ALPHA2PRIOR),
        alpha2(ArrayXd::Constant(1, distributions::ALPHA2PRIOR)),
        E_logv(ArrayXd::Zero(1)),
        E_lognv(ArrayXd::Zero(1)),
        E_logpi(ArrayXd::Zero(1)),
        ordvec(1, pair<int,double>(0,0))
{
        if (concentration <=0)
                throw invalid_argument("Concentration parameter has to be > 0!");

        this->alpha1_p = concentration;
        this->alpha1 = ArrayXd::Constant(1, concentration);
        this->priorfcalc();
}

void distributions::StickBreak::priorfcalc (void)
{
        // Prior free energy contribution
        this->F_p = lgamma(this->alpha1_p) + lgamma(this->alpha2_p)
                    - lgamma(this->alpha1_p + this->alpha2_p);
}

void distributions::StickBreak::update (const ArrayXd& Nk)
{
        const int K = Nk.size();

        // Destructively resize members to be the same size as Nk, no-op if same
        this->alpha1.resize(K);
        this->alpha2.resize(K);
        this->E_logv.resize(K);
        this->E_lognv.resize(K);
        this->E_logpi.resize(K);
        this->ordvec.resize(K, pair<int,double>(-1, -1));

        // Order independent update
        this->Nk     = Nk;
        this->alpha1 = this->alpha1_p + Nk;

        // Get at sort size order of clusters
        for (int k = 0; k < K; ++k)
        {
                this->ordvec[k].first  = k;
                this->ordvec[k].second = Nk(k);
        }
        sort(this->ordvec.begin(), this->ordvec.end(), obscomp);

        // Now do order dependent updates
        const double N = Nk.sum();
        double cumNk = 0, cumE_lognv = 0;
        for (int idx = 0, k; idx < K; ++idx)
        {
                k = this->ordvec[idx].first;

                // Alpha 2
                cumNk += Nk(k); // Accumulate cluster size sum
                this->alpha2(k) = this->alpha2_p + (N - cumNk);

                // Expected stick lengths
                double psisum    = digamma(this->alpha1(k) + this->alpha2(k));
                this->E_logv(k)  = digamma(this->alpha1(k)) - psisum;
                this->E_lognv(k) = digamma(this->alpha2(k)) - psisum;

                // Expected weights
                this->E_logpi(k) = this->E_logv(k) + cumE_lognv;
                cumE_lognv += E_lognv(k); // Accumulate log stick length left
        }
}

double distributions::StickBreak::fenergy () const
{
        const int K = this->alpha1.size();

        return K * this->F_p + (mxlgamma(this->alpha1 + this->alpha2).array()
                                - mxlgamma(this->alpha1).array() - mxlgamma(this->alpha2).array()
                                + (this->alpha1 - this->alpha1_p) * this->E_logv
                                + (this->alpha2 - this->alpha2_p) * this->E_lognv).sum();
}

//
// Generalised Dirichlet weight distribution.
//

void distributions::GDirichlet::update (const ArrayXd& Nk)
{
        // Call base class (stick breaking) update
        this->StickBreak::update(Nk);
        const int smallk = (this->ordvec.end() - 1)->first; // Get smallest cluster

        // Set last stick lengths to 1 ( log(0) = 1 ) and adjust log marginal
        this->E_logpi(smallk) = this->E_logpi(smallk) - this->E_logv(smallk);
        this->E_logv(smallk)  = 0; // exp(E[log v_K]) = 1
        this->E_lognv(smallk) = 0; // Undefined, but set to zero
}

double distributions::GDirichlet::fenergy () const
{
        const int K = this->ordvec.size();

        // GDir only has K-1 parameters, so we don't calculate the last F contrib.
        double Fpi = 0;
        for (int idx = 0, k = 0; idx < K-1; ++idx)
        {
                k = this->ordvec[idx].first;
                Fpi += lgamma(this->alpha1(k) + this->alpha2(k))
                       - lgamma(this->alpha1(k)) - lgamma(this->alpha2(k))
                       + (this->alpha1(k) - this->alpha1_p) * this->E_logv(k)
                       + (this->alpha2(k) - this->alpha2_p) * this->E_lognv(k);
        }

        return (K-1) * this->F_p + Fpi;
}

//
// Dirichlet weight distribution.
//

distributions::Dirichlet::Dirichlet ()
        : WeightDist(),
        alpha_p(distributions::ALPHA1PRIOR),
        alpha(ArrayXd::Constant(1, distributions::ALPHA1PRIOR)),
        E_logpi(ArrayXd::Zero(1))
{
}

distributions::Dirichlet::Dirichlet (const double alpha)
        : WeightDist(),
        E_logpi(ArrayXd::Zero(1))
{
        if (alpha <= 0)
                throw invalid_argument("Alpha prior must be > 0!");

        alpha_p = alpha;
        this->alpha = ArrayXd::Constant(1, alpha);
}

void distributions::Dirichlet::update (const ArrayXd& Nk)
{
        const int K = Nk.size();

        // Destructively resize members to be the same size as Nk, no-op if same
        this->alpha.resize(K);
        this->E_logpi.resize(K);

        // Hyperparameter update
        this->Nk    = Nk;
        this->alpha = this->alpha_p + Nk;

        // Expectation update
        this->E_logpi = mxdigamma(this->alpha).array() - digamma(this->alpha.sum());
}

double distributions::Dirichlet::fenergy () const
{
        const int K = this->alpha.size();

        return lgamma(this->alpha.sum()) - (this->alpha_p-1) * this->E_logpi.sum()
               + ((this->alpha-1) * this->E_logpi - mxlgamma(this->alpha).array()).sum()
               - lgamma(K * this->alpha_p) + K * lgamma(this->alpha_p);
}

//
// Gaussian Wishart cluster distribution.
//

distributions::GaussWish::GaussWish (
        const double clustwidth,
        const unsigned int D
        )
        : ClusterDist(clustwidth, D),
        nu_p(D),
        beta_p(distributions::BETAPRIOR),
        m_p(RowVectorXd::Zero(D))
{
        if (clustwidth <= 0)
                throw invalid_argument("clustwidth must be > 0!");

        // Create Prior
        this->iW_p = this->nu_p * this->prior * MatrixXd::Identity(D, D);

        try
        { this->logdW_p = -logdet(this->iW_p); }
        catch (invalid_argument e)
        { throw invalid_argument(string("Creating prior: ").append(e.what())); }

        // Calculate prior free energy contribution
        this->F_p = mxlgamma((this->nu_p + 1
                              - enumdims(this->m_p.cols())).matrix() / 2).sum();

        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::GaussWish::addobs(const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        MatrixXd qZkX = qZk.asDiagonal() * X;

        this->N_s += qZk.sum();
        this->x_s += qZkX.colwise().sum();       // [1xD] row vector
        this->xx_s.noalias() += qZkX.transpose() * X; // [DxD] matrix
}

void distributions::GaussWish::update ()
{
        // Prepare the Sufficient statistics
        RowVectorXd xk = RowVectorXd::Zero(this->D);
        if (this->N_s > 0)
                xk = this->x_s/this->N_s;
        MatrixXd Sk = this->xx_s - xk.transpose() * this->x_s;
        RowVectorXd xk_m = xk - this->m_p;         // for iW, (xk - m)

        // Update posterior params
        this->N    = this->N_s;
        this->nu   = this->nu_p + this->N;
        this->beta = this->beta_p + this->N;
        this->m    = (this->beta_p * this->m_p + this->x_s) / this->beta;
        this->iW   = this->iW_p + Sk
                     + (this->beta_p * this->N/this->beta) * xk_m.transpose() * xk_m;

        try
        { this->logdW = -logdet(this->iW); }
        catch (invalid_argument e)
        { throw runtime_error(string("Calc log(det(W)): ").append(e.what())); }
}

void distributions::GaussWish::clearobs ()
{
        // Reset parameters back to prior values
        this->nu    = this->nu_p;
        this->beta  = this->beta_p;
        this->m     = this->m_p;
        this->iW    = this->iW_p;
        this->logdW = this->logdW_p;

        // Empty sufficient statistics
        this->N_s  = 0;
        this->x_s  = RowVectorXd::Zero(D);
        this->xx_s = MatrixXd::Zero(D,D);
}

VectorXd distributions::GaussWish::Eloglike (const MatrixXd& X) const
{
        // Expectations of log Gaussian likelihood
        VectorXd E_logX(X.rows());
        double sumpsi = mxdigamma((this->nu+1-enumdims(this->D)).matrix()/2).sum();
        try
        {
                E_logX = 0.5 * (sumpsi + this->logdW - this->D * (1/this->beta + log(pi))
                                - this->nu * mahaldist(X, this->m, this->iW).array()).matrix();
        }
        catch (invalid_argument e)
        { throw(string("Calculating Gaussian likelihood: ").append(e.what())); }

        return E_logX;
}

distributions::ArrayXb distributions::GaussWish::splitobs (
        const MatrixXd& X
        ) const
{

        // Find the principle eigenvector using the power method if not done so
        VectorXd eigvec;
        eigpower(this->iW, eigvec);

        // 'split' the observations perpendicular to this eigenvector.
        return (((X.rowwise() - this->m)
                 * eigvec.asDiagonal()).array().rowwise().sum()) >= 0;
}

double distributions::GaussWish::fenergy () const
{
        const ArrayXd l = enumdims(this->D);
        double sumpsi = mxdigamma((this->nu + 1 - l).matrix() / 2).sum();

        return this->F_p + (this->D * (this->beta_p/this->beta - 1 - this->nu
                                       - log(this->beta_p/this->beta))
                            + this->nu * ((this->iW.ldlt().solve(this->iW_p)).trace()
                                          + this->beta_p * mahaldist(this->m, this->m_p, this->iW).coeff(0,0))
                            + this->nu_p * (this->logdW_p - this->logdW) + this->N*sumpsi)/2
               - mxlgamma((this->nu+1-l).matrix() / 2).sum();
}

//
// Normal Gamma parameter distribution.
//

distributions::NormGamma::NormGamma (
        const double clustwidth,
        const unsigned int D
        )
        : ClusterDist(clustwidth, D),
        nu_p(distributions::NUPRIOR),
        beta_p(distributions::BETAPRIOR),
        m_p(RowVectorXd::Zero(D))
{
        if (clustwidth <= 0)
                throw invalid_argument("clustwidth must be > 0!");

        // Create Prior
        this->L_p = this->nu_p * this->prior * RowVectorXd::Ones(D);
        this->logL_p = this->L_p.array().log().sum();

        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::NormGamma::addobs (const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        MatrixXd qZkX = qZk.asDiagonal() * X;

        this->N_s  += qZk.sum();
        this->x_s  += qZkX.colwise().sum();                           // [1xD]
        this->xx_s += (qZkX.array() * X.array()).colwise().sum().matrix(); // [1xD]
}

void distributions::NormGamma::update ()
{
        // Prepare the Sufficient statistics
        RowVectorXd xk = RowVectorXd::Zero(this->D);
        RowVectorXd Sk = RowVectorXd::Zero(this->D);
        if (this->N_s > 0)
        {
                xk = this->x_s/this->N_s;
                Sk = this->xx_s.array() - this->x_s.array().square()/this->N_s;
        }

        // Update posterior params
        this->N    = this->N_s;
        this->beta = this->beta_p + this->N;
        this->nu   = this->nu_p + this->N/2;
        this->m    = (this->beta_p * this->m_p + x_s) / this->beta;
        this->L    = this->L_p + Sk/2 + (this->beta_p * this->N / (2 * this->beta))
                     * (xk - this->m_p).array().square().matrix();

        if ((this->L.array() <= 0).any())
                throw invalid_argument(string("Calc log(L): Variance is zero or less!"));

        this->logL = this->L.array().log().sum();
}

void distributions::NormGamma::clearobs ()
{
        // Reset parameters back to prior values
        this->nu   = this->nu_p;
        this->beta = this->beta_p;
        this->m    = this->m_p;
        this->L    = this->L_p;
        this->logL = this->logL_p;

        // Empty sufficient statistics
        this->N_s  = 0;
        this->x_s  = RowVectorXd::Zero(this->D);
        this->xx_s = RowVectorXd::Zero(this->D);
}

VectorXd distributions::NormGamma::Eloglike (const MatrixXd& X) const
{
        // Distance evaluation in the exponent
        VectorXd Xmdist = (X.rowwise() - this->m).array().square().matrix()
                          * this->L.array().inverse().matrix().transpose();

        // Expectations of log Gaussian likelihood
        return 0.5 * (this->D * (digamma(this->nu) - log(2 * pi) - 1/this->beta)
                      - this->logL - this->nu * Xmdist.array());
}

distributions::ArrayXb distributions::NormGamma::splitobs (
        const MatrixXd& X
        ) const
{
        // Find location of largest element in L, this is the 'eigenvector'
        int eigvec;
        this->L.maxCoeff(&eigvec);

        // 'split' the observations perpendicular to this 'eigenvector'.
        return (X.col(eigvec).array() - this->m(eigvec)) >= 0;
}

double distributions::NormGamma::fenergy () const
{
        const VectorXd iL = this->L.array().inverse().matrix().transpose();

        return D*(lgamma(this->nu_p) - lgamma(this->nu)
                  + this->N*digamma(this->nu)/2 - this->nu)
               + D/2 * (log(this->beta) - log(this->beta_p) - 1 + this->beta_p/this->beta)
               + this->beta_p*this->nu/2*(this->m - this->m_p).array().square().matrix()*iL
               + this->nu_p*(this->logL - this->logL_p) + this->nu*this->L_p*iL;
}

//
// Exponential Gamma parameter distribution.
//

distributions::ExpGamma::ExpGamma (const double obsmag, const unsigned int D)
        : ClusterDist(obsmag, D),
        a_p(distributions::APRIOR),
        b_p(obsmag)
{
        this->clearobs(); // Empty suff. stats. and set posteriors equal to priors
}

void distributions::ExpGamma::addobs (const VectorXd& qZk, const MatrixXd& X)
{
        if (X.cols() != this->D)
                throw invalid_argument("Mismatched dims. of cluster params and obs.!");
        if (qZk.rows() != X.rows())
                throw invalid_argument("qZk and X ar not the same length!");

        this->N_s += qZk.sum();
        this->x_s += (qZk.asDiagonal() * X).colwise().sum();
}

void distributions::ExpGamma::update ()
{
        // Update posterior params
        this->N    = this->N_s;
        this->a    = this->a_p + this->N;
        this->ib   = (this->b_p + this->x_s.array()).array().inverse().matrix();
        this->logb = -this->ib.array().log().sum();
}

void distributions::ExpGamma::clearobs ()
{
        // Reset parameters back to prior values
        this->a    = this->a_p;
        this->ib   = RowVectorXd::Constant(this->D, 1/this->b_p);
        this->logb = this->D * log(this->b_p);

        // Empty sufficient statistics
        this->N_s = 0;
        this->x_s = RowVectorXd::Zero(this->D);
}

VectorXd distributions::ExpGamma::Eloglike (const MatrixXd& X) const
{
        return this->D * digamma(this->a) - this->logb
               - (this->a * X * this->ib.transpose()).array();
}

distributions::ArrayXb distributions::ExpGamma::splitobs (
        const MatrixXd& X
        ) const
{
        ArrayXd XdotL = X;// * (this->a * this->ib).transpose();
        return (XdotL > (XdotL.sum()/XdotL.size()));
}

double distributions::ExpGamma::fenergy () const
{
        return this->D * ((this->a - this->a_p) * digamma(this->a) - this->a
                          - this->a_p * log(this->b_p) - lgamma(this->a) + lgamma(this->a_p))
               + this->b_p * this->a * this->ib.sum() + this->a_p * this->logb;
}


#define BOOST_TEST_MODULE TEST_MODEL_EXPR
#include <boost/test/unit_test.hpp>
#include "libs/experiments/ROC.h"
#include <cmath>

float trancate(float input, int by)
{
    input *= by;
    return std::roundf(input)/by;
}

BOOST_AUTO_TEST_CASE(test_basic_algorithm)
{
    constexpr exprs::value_type acc = 0.461538f;
    constexpr exprs::value_type rmse = 1.03775f;
    constexpr exprs::value_type roc_area = 0.380952f;
    const  exprs::data_array tpr{
        0.f, 0.142857f, 0.714286f, 1.f
    };

    const exprs::data_array fpr{
        0.f, 0.333333f, 0.833333f, 1.f
    };

    exprs::data_array actual_measures = {
        1.f, 0.f, 2.f, 1.f, 0.f, 1.f, 2.f, 0.f, 0.f, 1.f, 1.f, 0.f, 0.f
    };

    exprs::data_array calcualted_values = {
        1.f, 1.f, 1.f, 0.f, 2.f, 1.f, 2.f, 1.f, 2.f, 0.f, 1.f, 0.f, 1.f
    };

    exprs::ROC roc_calc;
    BOOST_TEST(roc_calc.results.area == 0.f);
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
    BOOST_TEST_REQUIRE((roc_calc.calculate(actual_measures, calcualted_values)));
    BOOST_TEST(trancate(roc_calc.results.area, 4) == trancate(roc_area, 4));
    BOOST_TEST(trancate(roc_calc.results.rmse, 4) == trancate(rmse, 4));
    BOOST_TEST(trancate(roc_calc.results.accuracy, 4) == trancate(acc, 4));
    BOOST_TEST_REQUIRE(roc_calc.results.sensitivity.size() == tpr.size());
    BOOST_TEST_REQUIRE(roc_calc.results.fall_out.size() == fpr.size());
    BOOST_TEST_REQUIRE(roc_calc.results.fall_out.size() == roc_calc.results.sensitivity.size());
    for (auto i = 0u; i < roc_calc.results.sensitivity.size(); i++) {
        BOOST_TEST(trancate(roc_calc.results.sensitivity[i], 4) == trancate(tpr[i], 4));
        BOOST_TEST(trancate(roc_calc.results.fall_out[i], 4) == trancate(fpr[i], 4));
    }
}

BOOST_AUTO_TEST_CASE(test_invalid_entries)
{
    exprs::ROC roc_calc;
    BOOST_TEST_REQUIRE((roc_calc.classes_calculate(std::vector<int>{}, std::vector<float>{}) == false));
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
    BOOST_TEST_REQUIRE((roc_calc.calculate(std::vector<float>{}, std::vector<float>{}) == false));
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
    BOOST_TEST_REQUIRE((roc_calc.calculate(std::vector<float>{1.f, 2.f, 3.f}, std::vector<float>{1.f, 2.f}) == false));
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
    BOOST_TEST_REQUIRE((roc_calc.classes_calculate(std::vector<int>{1, 2, 3}, std::vector<float>{1.f, 2.f}) == false));
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
}

BOOST_AUTO_TEST_CASE(test_classifier_calc)
{
    constexpr exprs::value_type acc = 0.461538f;
    constexpr exprs::value_type rmse = 1.03775f;
    constexpr exprs::value_type roc_area = 0.380952f;
    const  exprs::data_array tpr{
        0.f, 0.142857f, 0.714286f, 1.f
    };

    const exprs::data_array fpr{
        0.f, 0.333333f, 0.833333f, 1.f
    };

    std::vector<int> actual_measures = {
        1, 0, 2, 1, 0, 1, 2, 0, 0, 1, 1, 0, 0
    };

    exprs::data_array calcualted_values = {
        1.f, 1.f, 1.f, 0.f, 2.f, 1.f, 2.f, 1.f, 2.f, 0.f, 1.f, 0.f, 1.f
    };
    exprs::ROC roc_calc;
    BOOST_TEST(roc_calc.results.area == 0.f);
    BOOST_TEST(roc_calc.results.accuracy == 0.f);
    BOOST_TEST(roc_calc.results.rmse == 0.f);
    BOOST_TEST(roc_calc.results.fall_out.empty());
    BOOST_TEST(roc_calc.results.sensitivity.empty());
    BOOST_TEST_REQUIRE((roc_calc.classes_calculate(actual_measures, calcualted_values)));
    BOOST_TEST(trancate(roc_calc.results.area, 4) == trancate(roc_area, 4));
    BOOST_TEST(trancate(roc_calc.results.rmse, 4) == trancate(rmse, 4));
    BOOST_TEST(trancate(roc_calc.results.accuracy, 4) == trancate(acc, 4));
    BOOST_TEST_REQUIRE(roc_calc.results.sensitivity.size() == tpr.size());
    BOOST_TEST_REQUIRE(roc_calc.results.fall_out.size() == fpr.size());
    BOOST_TEST_REQUIRE(roc_calc.results.fall_out.size() == roc_calc.results.sensitivity.size());
    for (auto i = 0u; i < roc_calc.results.sensitivity.size(); i++) {
        BOOST_TEST(trancate(roc_calc.results.sensitivity[i], 4) == trancate(tpr[i], 4));
        BOOST_TEST(trancate(roc_calc.results.fall_out[i], 4) == trancate(fpr[i], 4));
    }
}


#include <cstdlib>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/matrix_expression.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double> matrix;
    typedef vector::size_type size_type;
    size_type n=8;
    vector v(n);
    blas::set(1.0, v);
    std::cout << v << '\n';
    matrix M(n, 2*n);
    for (size_type j=0; j<2*n; ++j)
      for (size_type i=0; i<n; ++i)
     	M(i, j)=1;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    blas::set(2.0, mc);
    blas::set(3.0, mr);
    std::cout << M << '\n';
  }

  {
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    size_type n=8;
    vector v(n);
    blas::set(1.0, v);
    std::cout << v << '\n';
    matrix M(n, 2*n);
    for (size_type j=0; j<2*n; ++j)
      for (size_type i=0; i<n; ++i)
     	M(i, j)=1;
    auto mc=M.col(2);
    auto mr=M.row(3);
    blas::set(2.0, mc);
    blas::set(3.0, mr);
    std::cout << M << '\n';
  }
  return EXIT_SUCCESS;
}


// Author: Henrique Mendonça <henrique@apache.org>
#include "rsba/solveRSpnp.h"
#include "rsba/mat/cam.h"
#include "rsba/VideoSfmBaRs.h"
#include "rsba/struct/VideoSfM.h"

#include <ceres/rotation.h>
#include <opencv2/core/core_c.h>
#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2)
#include <opencv2/core/internal.hpp>
#else
#define __OPENCV_BUILD
#include <opencv2/cvconfig.h>
#include <opencv2/core/private.hpp>
#endif
#include <iostream>
//#include <boost/timer/timer.hpp>

using namespace ::cv;
using namespace ::vision;
using namespace ::vision::sfm;

template <typename TF>
struct RsBA: public ReprojectionError {
  TF point[NUM_POINT_PARAMS];
  SHUTTER rs;
  const int* const scanlines;

  RsBA(const double camera[NUM_CAM_PARAMS],
       const TF* const observed, // 2D
       const TF* const point3d,
       const SHUTTER _rs,
       const int* const _scanlines) // 3D
  : scanlines(_scanlines)
  {
    observed_x = (observed[0]); observed_y = (observed[1]);
    memcpy(camera_params, camera, sizeof(camera_params));
    memcpy(point, point3d, sizeof(point));
    rs = _rs;
  };

  template <typename T>
  bool operator()(const T* const pose0,
                  T* residuals) const {
    return operator()(pose0, pose0, residuals);
  };

  template <typename T>
  bool operator()(const T* const pose0,
                  const T* const pose1,
                  T* residuals) const {
    T pose[6];
    T obs[2] = { T(observed_x), T(observed_x) };
    interpolate_rs(pose0, pose1, rs, scanlines, obs, pose);

    T camera[NUM_CAM_PARAMS];
    for (short i = 0; i < NUM_CAM_PARAMS; i++)
      camera[i] = T(camera_params[i]);

    T proj[2]; //reprojection
    T p[NUM_POINT_PARAMS] = { T(point[0]), T(point[1]), T(point[2]) };
    if ( ! w2i(camera, pose, p, proj, false)) {
      return false;
    }

    // The error is the difference between the reprojection and observed position.
    residuals[0] = (proj[0] - T(observed_x));
    residuals[1] = (proj[1] - T(observed_y));

    T threshold(5);
    if (abs(residuals[0]) < threshold and abs(residuals[1]) < threshold) {
      return true;
    } else { //TODO check matches
      return true;
    }
  }

  // Factory to hide the construction of the CostFunction object from the client code.
  static ceres::CostFunction* Create(
      const double camera[NUM_CAM_PARAMS],
      const TF* const observed,
      const TF* const point3d,
      const SHUTTER rs,
      const int* const scanlines)
  {
    return (new ceres::AutoDiffCostFunction<RsBA,
        NUM_RESIDUALS
        ,NUM_POSE_PARAMS // Initial frame pose
        ,NUM_POSE_PARAMS
        >( // Final frame pose
                new RsBA(camera, observed, point3d, rs, scanlines)));
  }
};

bool vision::solveRsPnP(InputArray _opoints, InputArray _ipoints,
                  InputArray _cameraMatrix, InputArray _distCoeffs,
                  OutputArray _rvec, OutputArray _tvec,
                  OutputArray _rvec2, OutputArray _tvec2,
                  const SHUTTER shutter, const int scanlines[2],
                  bool useExtrinsicGuess, int flags)
{
  //boost::timer::auto_cpu_timer btimer;

  _rvec.create(3, 1, CV_64F);
  _tvec.create(3, 1, CV_64F);
  _rvec2.create(3, 1, CV_64F);
  _tvec2.create(3, 1, CV_64F);
  cv::Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
  cv::Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat();

  if (cv::norm(rvec, NORM_L1) + cv::norm(tvec, NORM_L1) +
      cv::norm(rvec2, NORM_L1) + cv::norm(tvec2, NORM_L1) == 0) {
    if (solvePnP(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvec, tvec, useExtrinsicGuess, flags))
    { // GS Init
      rvec.copyTo(rvec2);
      tvec.copyTo(tvec2);
      std::cout << "GS PnP Init: " << rvec << tvec << endl;
    }
  }

  {
    ceres::Problem problem;
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();

    vector<double> pose(NUM_POSE_PARAMS), pose2(NUM_POSE_PARAMS);
    { // Init poses
      double rInv[3];
      assign3(rvec.at<Vec3d>(0, 0).val, pose.data());
      invert3(pose.data(), rInv);
      ceres::AngleAxisRotatePoint(rInv, (-tvec.at<Vec3d>(0, 0)).val, pose.data()+3);

      assign3(rvec2.at<Vec3d>(0, 0).val, pose2.data());
      invert3(pose2.data(), rInv);
      ceres::AngleAxisRotatePoint(rInv, (-tvec2.at<Vec3d>(0, 0)).val, pose2.data()+3);
    }

    std::vector<double> cam = sfmCam(_cameraMatrix.getMat(), _distCoeffs.getMat());
    for (int i = 0; i < _opoints.size().width; i++) {
      ceres::CostFunction* costFunction = RsBA<float>::Create(cam.data(),
                                                              ipoints.at<Vec2f>(0, i).val,
                                                              opoints.at<Vec3f>(0, i).val,
                                                              shutter, scanlines);
      problem.AddResidualBlock(costFunction, NULL, pose.data(), pose2.data());
    }

    // Make Ceres automatically detect the bundle structure. Note that the
    // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
    // for standard bundle adjustment problems.
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::SPARSE_SCHUR;
    //options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 10;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    if (summary.IsSolutionUsable())
    {
//      std::cout << summary.BriefReport() << endl;

      assign3(pose.data(), rvec.at<Vec3d>(0, 0).val);
      assign3(pose.data()+3, tvec.at<Vec3d>(0, 0).val);
      // invert translation
      ceres::AngleAxisRotatePoint(pose.data(), tvec.at<Vec3d>(0, 0).val, tvec.at<Vec3d>(0, 0).val);
      tvec *= -1;
//      std::cout << rvec << endl;
//      std::cout << tvec << endl;

      assign3(pose2.data(), rvec2.at<Vec3d>(0, 0).val);
      assign3(pose2.data()+3, tvec2.at<Vec3d>(0, 0).val);
      // invert translation
      ceres::AngleAxisRotatePoint(pose2.data(), tvec2.at<Vec3d>(0, 0).val, tvec2.at<Vec3d>(0, 0).val);
      tvec2 *= -1;
//      std::cout << rvec2 << endl;
//      std::cout << tvec2 << endl;

//      solvePnP(_opoints, _ipoints, _cameraMatrix, _distCoeffs, orvec, otvec, useExtrinsicGuess, flags);
//      std::cout << orvec << endl;
//      std::cout << otvec << endl;

      return true;
    }
  }

  return false;
}

namespace vision
{
    namespace pnpransac
    {
        struct CameraParameters
        {
            void init(Mat _intrinsics, Mat _distCoeffs, const SHUTTER _shutter, const int* _scanlines)
            {
                _intrinsics.copyTo(intrinsics);
                _distCoeffs.copyTo(distortion);
                shutter = _shutter;
                scanlines = _scanlines;
            }

            Mat intrinsics;
            Mat distortion;
            SHUTTER shutter;
            const int* scanlines;
        };

        struct Parameters
        {
            int iterationsCount;
            float reprojectionError;
            int minInliersCount;
            bool useExtrinsicGuess;
            int flags;
            CameraParameters camera;
            int min_points_count;
        };

        static vector<Point2f> project3dPoints(const Mat& opoints, const Mat& ipoints, const Parameters& params,
                                           const Mat& rvec,  const Mat& tvec,
                                           const Mat& rvec2, const Mat& tvec2)
        {
          vector<Point2f> projected_points(opoints.cols);

          for (int i = 0; i < opoints.cols; i++) {
            Point3f op(opoints.at<Vec3f>(0, i));
            Point2f ip(ipoints.at<Vec2f>(0, i));

            vector<double> pose(6, 0);
            assign3(rvec.at<Vec3d>(0, 0).val, pose.data());
            double rInv[3];
            invert3(pose.data(), rInv);
            ceres::AngleAxisRotatePoint(rInv, (-tvec.at<Vec3d>(0, 0)).val, pose.data()+3);

            vector<double> pose2(6, 0);
            assign3(rvec2.at<Vec3d>(0, 0).val, pose2.data());
            invert3(pose2.data(), rInv);
            ceres::AngleAxisRotatePoint(rInv, (-tvec2.at<Vec3d>(0, 0)).val, pose2.data()+3);

            std::vector<double> cam = sfmCam(params.camera.intrinsics, params.camera.distortion);
            double point[] = { op.x, op.y, op.z };
            double obs[] = { ip.x, ip.y };
            double poseInter[NUM_POSE_PARAMS];
            interpolate_rs(pose.data(), pose2.data(), params.camera.shutter, params.camera.scanlines, obs, poseInter);

            double proj[2]; //reprojection
            if ( ! w2i(cam.data(), poseInter, point, proj, false)) {
              std::abort();
            }
            projected_points[i].x = proj[0];
            projected_points[i].y = proj[1];
          }

          return projected_points;
        }

        static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
                     const Parameters& params, vector<int>& inliers,
                     Mat& rvec, Mat& tvec, Mat& rvec2, Mat& tvec2,
                     const Mat& rvecInit, const Mat& tvecInit, const Mat& rvecInit2, const Mat& tvecInit2,
                     Mutex& resultsMutex)
        {
            Mat modelObjectPoints(1, params.min_points_count, CV_32FC3), modelImagePoints(1, params.min_points_count, CV_32FC2);
            for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
            {
                if (pointsMask[i])
                {
                    Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1));
                    imagePoints.col(i).copyTo(colModelImagePoints);
                    Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1));
                    objectPoints.col(i).copyTo(colModelObjectPoints);
                    colIndex = colIndex+1;
                }
            }

            //filter same 3d points, hang in solveRsPnP
            double eps = 1e-10;
            int num_same_points = 0;
            for (int i = 0; i < params.min_points_count; i++)
                for (int j = i + 1; j < params.min_points_count; j++)
                {
                    if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps)
                        num_same_points++;
                }
            if (num_same_points > 0)
                return;

            Mat localRvec, localTvec;
            Mat localRvec2, localTvec2;
            rvecInit.copyTo(localRvec);
            tvecInit.copyTo(localTvec);
            rvecInit2.copyTo(localRvec2);
            tvecInit2.copyTo(localTvec2);

            vector<int> localInliers;
            vision::solveRsPnP(modelObjectPoints, modelImagePoints,
                params.camera.intrinsics, params.camera.distortion,
                localRvec, localTvec,
                localRvec2, localTvec2,
                params.camera.shutter, params.camera.scanlines,
                params.useExtrinsicGuess, params.flags);

            vector<Point2f> projected_points = project3dPoints(objectPoints, imagePoints, params, localRvec, localTvec, localRvec2, localTvec2);
            for (int i = 0; i < objectPoints.cols; i++) {
                Point2f p(imagePoints.at<Vec2f>(0, i));
                if ((norm(p - projected_points[i]) < params.reprojectionError)) {
                    localInliers.push_back(i);
                }
            }

            if (localInliers.size() > inliers.size())
            {
              cout << localInliers.size() << "/" << objectPoints.cols << endl;
              resultsMutex.lock();

              inliers.clear();
              inliers.resize(localInliers.size());
              memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
              localRvec.copyTo(rvec);
              localTvec.copyTo(tvec);
              localRvec2.copyTo(rvec2);
              localTvec2.copyTo(tvec2);

              resultsMutex.unlock();
            }
        }

        class PnPSolver
        {
        public:
            void operator()( const BlockedRange& r ) const
            {
                vector<char> pointsMask(objectPoints.cols, 0);
                memset(&pointsMask[0], 1, parameters.min_points_count );
                for( int i=r.begin(); i!=r.end(); ++i )
                {
                    generateVar(pointsMask);
                    pnpTask(pointsMask, objectPoints, imagePoints, parameters,
                            inliers, rvec, tvec, rvec2, tvec2,
                            initRvec, initTvec, initRvec2, initTvec2, syncMutex);

                    if ((int)inliers.size() >= parameters.minInliersCount)
                    {
#ifdef HAVE_TBB
                        tbb::task::self().cancel_group_execution();
#else
                        break;
#endif
                    }
                }
            }

            PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
                Mat& _rvec, Mat& _tvec, Mat& _rvec2,
                Mat& _tvec2, vector<int>& _inliers):
            objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
            rvec(_rvec), tvec(_tvec), rvec2(_rvec2), tvec2(_tvec2), inliers(_inliers)
            {
              rvec.copyTo(initRvec);
              tvec.copyTo(initTvec);
              rvec2.copyTo(initRvec2);
              tvec2.copyTo(initTvec2);

              generator.state = theRNG().state; //to control it somehow...
            }

        private:
            PnPSolver& operator=(const PnPSolver&);

            const Mat& objectPoints;
            const Mat& imagePoints;
            const Parameters& parameters;
            Mat &rvec, &tvec;
            Mat &rvec2, &tvec2;
            vector<int>& inliers;
            Mat initRvec, initTvec;
            Mat initRvec2, initTvec2;

            static RNG generator;
            static Mutex syncMutex;

            void generateVar(vector<char>& mask) const
            {
                int size = (int)mask.size();
                for (int i = 0; i < size; i++)
                {
                    int i1 = generator.uniform(0, size);
                    int i2 = generator.uniform(0, size);
                    char curr = mask[i1];
                    mask[i1] = mask[i2];
                    mask[i2] = curr;
                }
            }
        };

        Mutex PnPSolver::syncMutex;
        RNG PnPSolver::generator;

    }
}

void vision::solveRsPnPRansac(InputArray _opoints, InputArray _ipoints,
                        InputArray _cameraMatrix, InputArray _distCoeffs,
                        OutputArray _rvec, OutputArray _tvec,
                        OutputArray _rvec2, OutputArray _tvec2,
                        const SHUTTER shutter, const int scanlines[2],
                        bool useExtrinsicGuess, int iterationsCount,
                        float reprojectionError, int minInliersCount,
                        OutputArray _inliers, int flags, int min_points_count)
{
  //boost::timer::auto_cpu_timer btimer;

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();

    CV_Assert(opoints.isContinuous());
    CV_Assert(opoints.depth() == CV_32F);
    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
    CV_Assert(ipoints.isContinuous());
    CV_Assert(ipoints.depth() == CV_32F);
    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);

    _rvec.create(3, 1, CV_64FC1);
    _tvec.create(3, 1, CV_64FC1);
    _rvec2.create(3, 1, CV_64FC1);
    _tvec2.create(3, 1, CV_64FC1);
    Mat rvec = _rvec.getMat();
    Mat tvec = _tvec.getMat();
    Mat rvec2 = _rvec2.getMat();
    Mat tvec2 = _tvec2.getMat();

    if (cv::norm(rvec, NORM_L1) + cv::norm(tvec, NORM_L1) +
        cv::norm(rvec2, NORM_L1) + cv::norm(tvec2, NORM_L1) == 0) {
      cv::Mat gs_inliers;
      solvePnPRansac(opoints, ipoints, cameraMatrix, distCoeffs, rvec, tvec,
                     useExtrinsicGuess, iterationsCount, reprojectionError*2,
                     minInliersCount, gs_inliers, flags);

      if (gs_inliers.rows > 4)
      { // GS Init
        rvec.copyTo(rvec2);
        tvec.copyTo(tvec2);
        std::cout << "GS PnP Init: " << rvec << tvec << endl;
      }
    }

    Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);

    if (minInliersCount <= 0)
        minInliersCount = objectPoints.cols;
    pnpransac::Parameters params;
    params.iterationsCount = iterationsCount;
    params.minInliersCount = minInliersCount;
    params.reprojectionError = reprojectionError;
    params.useExtrinsicGuess = useExtrinsicGuess;
    params.camera.init(cameraMatrix, distCoeffs, shutter, scanlines);
    params.flags = flags;
    params.min_points_count = min_points_count;

    vector<int> localInliers;
    Mat localRvec, localTvec;
    Mat localRvec2, localTvec2;
    rvec.copyTo(localRvec);
    tvec.copyTo(localTvec);
    rvec2.copyTo(localRvec2);
    tvec2.copyTo(localTvec2);

    if (objectPoints.cols >= params.min_points_count)
    {
        cv::parallel_for(BlockedRange(0,iterationsCount),
            pnpransac::PnPSolver(objectPoints, imagePoints, params,
                localRvec, localTvec, localRvec2, localTvec2, localInliers));
    }

    if (localInliers.size() >= (size_t)params.min_points_count)
    {
        if (flags != CV_P3P)
        {
            int i, pointsCount = (int)localInliers.size();
            Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
            for (i = 0; i < pointsCount; i++)
            {
                int index = localInliers[i];
                Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
                imagePoints.col(index).copyTo(colInlierImagePoints);
                Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
                objectPoints.col(index).copyTo(colInlierObjectPoints);
            }
            vision::solveRsPnP(inlierObjectPoints, inlierImagePoints,
                params.camera.intrinsics, params.camera.distortion,
                localRvec, localTvec,
                localRvec2, localTvec2,
                shutter, scanlines,
                true, flags);
        }
        localRvec.copyTo(rvec);
        localTvec.copyTo(tvec);
        localRvec2.copyTo(rvec2);
        localTvec2.copyTo(tvec2);
        if (_inliers.needed())
            Mat(localInliers).copyTo(_inliers);
    }
    else
    {
      tvec.setTo(Scalar(0));
      tvec2.setTo(Scalar(0));
      Mat R = Mat::eye(3, 3, CV_64F);
      Rodrigues(R, rvec);
      Rodrigues(R, rvec2);
      if ( _inliers.needed() ) _inliers.release();
    }
    return;
}


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <iostream>
#include <random> // Requires C++ 11

#include <SymEigsSolver.h>
#include <MatOp/DenseSymMatProd.h>
#include <MatOp/SparseSymMatProd.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;
typedef Eigen::SparseMatrix<double> SpMatrix;

// Traits to obtain operation type from matrix type
template <typename MatType>
struct OpTypeTrait
{
    typedef DenseSymMatProd<double> OpType;
};

template <>
struct OpTypeTrait<SpMatrix>
{
    typedef SparseSymMatProd<double> OpType;
};

// Generate random sparse matrix
SpMatrix sprand(int size, double prob = 0.5)
{
    SpMatrix mat(size, size);
    std::default_random_engine gen;
    gen.seed(0);
    std::uniform_real_distribution<double> distr(-1.0, 1.0);
    for(int i = 0; i < size; i++)
    {
        for(int j = 0; j < size; j++)
        {
            if(distr(gen) < prob)
                mat.insert(i, j) = distr(gen);
        }
    }
    return mat;
}

template <typename MatType, int SelectionRule>
void run_test(const MatType& mat, int k, int m)
{
    typename OpTypeTrait<MatType>::OpType op(mat);
    SymEigsSolver<double, SelectionRule, typename OpTypeTrait<MatType>::OpType>
        eigs(&op, k, m);
    eigs.init();
    int nconv = eigs.compute();
    int niter = eigs.num_iterations();
    int nops  = eigs.num_operations();

    INFO( "nconv = " << nconv );
    INFO( "niter = " << niter );
    INFO( "nops  = " << nops  );
    REQUIRE( eigs.info() == SUCCESSFUL );

    Vector evals = eigs.eigenvalues();
    Matrix evecs = eigs.eigenvectors();

    Matrix resid = mat.template selfadjointView<Eigen::Lower>() * evecs - evecs * evals.asDiagonal();
    const double err = resid.array().abs().maxCoeff();

    INFO( "||AU - UD||_inf = " << err );
    REQUIRE( err == Approx(0.0) );
}

template <typename MatType>
void run_test_sets(const MatType& mat, int k, int m)
{
    SECTION( "Largest Magnitude" )
    {
        run_test<MatType, LARGEST_MAGN>(mat, k, m);
    }
    SECTION( "Largest Value" )
    {
        run_test<MatType, LARGEST_ALGE>(mat, k, m);
    }
    SECTION( "Smallest Magnitude" )
    {
        run_test<MatType, SMALLEST_MAGN>(mat, k, m);
    }
    SECTION( "Smallest Value" )
    {
        run_test<MatType, SMALLEST_ALGE>(mat, k, m);
    }
    SECTION( "Both Ends" )
    {
        run_test<MatType, BOTH_ENDS>(mat, k, m);
    }
}

TEST_CASE("Eigensolver of symmetric real matrix [10x10]", "[eigs_sym]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(10, 10);
    const Matrix M = A + A.transpose();
    int k = 3;
    int m = 6;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of symmetric real matrix [100x100]", "[eigs_sym]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(100, 100);
    const Matrix M = A + A.transpose();
    int k = 10;
    int m = 20;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of symmetric real matrix [1000x1000]", "[eigs_sym]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(1000, 1000);
    const Matrix M = A + A.transpose();
    int k = 20;
    int m = 50;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of sparse symmetric real matrix [10x10]", "[eigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    const SpMatrix M = sprand(10, 0.5);
    int k = 3;
    int m = 6;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of sparse symmetric real matrix [100x100]", "[eigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    const SpMatrix M = sprand(100, 0.5);
    int k = 10;
    int m = 20;

    run_test_sets(M, k, m);
}

TEST_CASE("Eigensolver of sparse symmetric real matrix [1000x1000]", "[eigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    const SpMatrix M = sprand(1000, 0.5);
    int k = 20;
    int m = 50;

    run_test_sets(M, k, m);
}


#include <engine/Neighbours.hpp>

#include <Eigen/Dense>

namespace Engine
{
    namespace Neighbours
    {
        std::vector<scalar> Get_Shell_Radius(const Data::Geometry & geometry, const int n_shells)
        {
            const scalar shell_width = 1e-3;
            auto shell_radius = std::vector<scalar>(n_shells);

            Vector3 a = geometry.bravais_vectors[0];
            Vector3 b = geometry.bravais_vectors[1];
            Vector3 c = geometry.bravais_vectors[2];

            // The n_shells + 2 is a value that is big enough by experience to
            // produce enough needed shells, but is small enough to run sufficiently fast
            int tMax = n_shells + 2;
            int imax = std::min(tMax, geometry.n_cells[0]-1),
                jmax = std::min(tMax, geometry.n_cells[1]-1),
                kmax = std::min(tMax, geometry.n_cells[2]-1);

            // Abort condidions for all 3 vectors
            if (a.norm() == 0.0) imax = 0;
            if (b.norm() == 0.0) jmax = 0;
            if (c.norm() == 0.0) kmax = 0;

            int i, j, k, iatom, jatom, ishell;
            scalar current_radius=0, dx, min_distance=0;
            Vector3 x0={0,0,0}, x1={0,0,0};
            for (ishell = 0; ishell < n_shells; ++ishell)
            {
                min_distance = current_radius;
                current_radius = 1e10;
                for (iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
                {
                    x0 =  geometry.cell_atoms[iatom][0] * a
                        + geometry.cell_atoms[iatom][1] * b
                        + geometry.cell_atoms[iatom][2] * c;
                    // Note: due to symmetry we only need to check half the space
                    for (i = imax; i >= 0; --i)
                    {
                        for (j = jmax; j >= -jmax; --j)
                        {
                            for (k = kmax; k >= -kmax; --k)
                            {
                                for (jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    if ( !( iatom==jatom && i==0 && j==0 && k==0 ) )
                                    {
                                        x1 =  geometry.cell_atoms[jatom][0] * a
                                            + geometry.cell_atoms[jatom][1] * b
                                            + geometry.cell_atoms[jatom][2] * c
                                            + i*a + j*b + k*c;
                                        dx = (x0-x1).norm();
                                        if (dx - min_distance > shell_width && dx < current_radius)
                                        {
                                            current_radius = dx;
                                            shell_radius[ishell] = dx;
                                        }
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor iatom
            }

            return shell_radius;
        }

        void Get_Neighbours_in_Shells(const Data::Geometry & geometry, int n_shells, pairfield & neighbours, intfield & shells, bool use_redundant_neighbours)
        {
            const scalar shell_width = 1e-3;
            auto shell_radius = Get_Shell_Radius(geometry, n_shells);

            Vector3 a = geometry.bravais_vectors[0];
            Vector3 b = geometry.bravais_vectors[1];
            Vector3 c = geometry.bravais_vectors[2];

            // The n_shells + 2 is a value that is big enough by experience to
            // produce enough needed shells, but is small enough to run sufficiently fast
            int tMax = n_shells + 2;
            int imax = std::min(tMax, geometry.n_cells[0]-1),
                jmax = std::min(tMax, geometry.n_cells[1]-1),
                kmax = std::min(tMax, geometry.n_cells[2]-1);
            int imin, jmin, kmin, jatommin;
            // If redundant neighbours should not be used, we restrict the search to half of the space
            if( use_redundant_neighbours )
            {
                imin=-imax; jmin=-jmax; kmin=-kmax;
            }
            else
            {
                imin=0; jmin=-jmax; kmin=-kmax;
            }

            // Abort condidions for all 3 vectors
            if (a.norm() == 0.0) imax = 0;
            if (b.norm() == 0.0) jmax = 0;
            if (c.norm() == 0.0) kmax = 0;

            int i, j, k, iatom, jatom, ishell;
            scalar dx, radius;
            Vector3 x0={0,0,0}, x1={0,0,0};
            for (iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
            {
                if( use_redundant_neighbours )
                    jatommin=0;
                else
                    jatommin=iatom;

                x0 =  geometry.cell_atoms[iatom][0] * a
                    + geometry.cell_atoms[iatom][1] * b
                    + geometry.cell_atoms[iatom][2] * c;
                for (ishell = 0; ishell < n_shells; ++ishell)
                {
                    radius = shell_radius[ishell];
                    for (i = imax; i >= imin; --i)
                    {
                        for (j = jmax; j >= jmin; --j)
                        {
                            for (k = kmax; k >= kmin; --k)
                            {
                                for (jatom = jatommin; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    if ((jatom > iatom) || (i>0 || (i==0 && j>0) || (i==0 && j==0 && k>0)) || use_redundant_neighbours)
                                    {
                                        x1 =  geometry.cell_atoms[jatom][0] * a
                                            + geometry.cell_atoms[jatom][1] * b
                                            + geometry.cell_atoms[jatom][2] * c
                                            + i*a + j*b + k*c;
                                        dx = (x0-x1).norm();
                                        if (std::abs(dx - radius) < shell_width)
                                        {
                                            Pair neigh;
                                            neigh.i = iatom;
                                            neigh.j = jatom;
                                            neigh.translations[0] = i;
                                            neigh.translations[1] = j;
                                            neigh.translations[2] = k;
                                            neighbours.push_back( neigh );
                                            shells.push_back(ishell);
                                        }
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor ishell
            }//endfor iatom
        }

        pairfield Get_Pairs_in_Radius(const Data::Geometry & geometry, scalar radius)
        {
            auto pairs = pairfield(0);

            if (radius > 1e-6)
            {
                Vector3 a = geometry.bravais_vectors[0];
                Vector3 b = geometry.bravais_vectors[1];
                Vector3 c = geometry.bravais_vectors[2];

                Vector3 bounds_diff = geometry.bounds_max - geometry.bounds_min;
                Vector3 ratio = {
                    bounds_diff[0]/std::max(1, geometry.n_cells[0]),
                    bounds_diff[1]/std::max(1, geometry.n_cells[1]),
                    bounds_diff[2]/std::max(1, geometry.n_cells[2]) };

                // This should give enough translations to contain all DDI pairs
                int imax = 0, jmax = 0, kmax = 0;
                if ( bounds_diff[0] > 0 )
                    imax = std::min(geometry.n_cells[0], (int)(1.1 * radius * geometry.n_cells[0] / bounds_diff[0]));
                if ( bounds_diff[1] > 0 )
                    jmax = std::min(geometry.n_cells[1], (int)(1.1 * radius * geometry.n_cells[1] / bounds_diff[1]));
                if ( bounds_diff[2] > 0 )
                    kmax = std::min(geometry.n_cells[2], (int)(1.1 * radius * geometry.n_cells[2] / bounds_diff[2]));

                int i,j,k;
                scalar dx;
                Vector3 x0={0,0,0}, x1={0,0,0};

                // Abort condidions for all 3 vectors
                if (a.norm() == 0.0) imax = 0;
                if (b.norm() == 0.0) jmax = 0;
                if (c.norm() == 0.0) kmax = 0;

                for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
                {
                    x0 = geometry.cell_atoms[iatom];
                    for (i = imax; i >= -imax; --i)
                    {
                        for (j = jmax; j >= -jmax; --j)
                        {
                            for (k = kmax; k >= -kmax; --k)
                            {
                                for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    x1 = geometry.cell_atoms[jatom] + i*a + j*b + k*c;
                                    dx = (x0-x1).norm();
                                    if (dx < radius)
                                    {
                                        pairs.push_back( {iatom, jatom, {i, j, k} } );
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor iatom
            }

            return pairs;
        }

        Vector3 DMI_Normal_from_Pair(const Data::Geometry & geometry, const Pair & pair, int chirality)
        {
            Vector3 ta = geometry.bravais_vectors[0];
            Vector3 tb = geometry.bravais_vectors[1];
            Vector3 tc = geometry.bravais_vectors[2];

            int da = pair.translations[0];
            int db = pair.translations[1];
            int dc = pair.translations[2];

            Vector3 ipos = geometry.cell_atoms[pair.i];
            Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

            if (chirality == 1)
            {
                // Bloch chirality
                return (jpos - ipos).normalized();
            }
            else if (chirality == -1)
            {
                // Inverse Bloch chirality
                return (ipos - jpos).normalized();
            }
            else if (chirality == 2)
            {
                // Neel chirality (surface)
                return (jpos - ipos).normalized().cross(Vector3{0,0,1});
            }
            else if (chirality == -2)
            {
                // Inverse Neel chirality (surface)
                return Vector3{0,0,1}.cross((jpos - ipos).normalized());
            }
            else
            {
                return Vector3{ 0,0,0 };
            }
        }

        void DDI_from_Pair(const Data::Geometry & geometry, const Pair & pair, scalar & magnitude, Vector3 & normal)
        {
            Vector3 ta = geometry.bravais_vectors[0];
            Vector3 tb = geometry.bravais_vectors[1];
            Vector3 tc = geometry.bravais_vectors[2];

            int da = pair.translations[0];
            int db = pair.translations[1];
            int dc = pair.translations[2];

            Vector3 ipos = geometry.cell_atoms[pair.i];
            Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

            // Calculate positions and difference vector
            Vector3 vector_ij = jpos - ipos;

            // Length of difference vector
            magnitude = vector_ij.norm();
            normal = vector_ij.normalized();
        }

    }// end Namespace Neighbours
}// end Namespace Engine


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file math.hpp
 * @author Ondrej Prochazka <ondrej.prochazka@citationtech.net>
 *
 * Low level math helper functions
 */

#ifndef MATH_MATH_HPP
#define MATH_MATH_HPP

#include <algorithm>
#include <stdexcept>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "dbglog/dbglog.hpp"

namespace math {

namespace ublas = boost::numeric::ublas;

/** square function */
template <typename T>
T sqr( T val ) { return val * val; }

namespace detail {

// enabled for N == 1
template<int N, typename T>
typename std::enable_if<N == 1, T>::type pow( T value ) {
    return value;
}
// enabled for even N
template<int N, typename T>
typename std::enable_if<(N > 1) && (N & 1) == 0, T>::type pow( T value ) {
    return pow<N/2>(value * value);
}
// enabled for odd N
template<int N, typename T>
typename std::enable_if<(N > 1) && (N & 1) == 1, T>::type pow( T value ) {
    return pow<N/2>(value * value) * value;
}

} // namespace detail

/** Power function with integer compile-time exponent */
template<int N, typename T>
T pow( T value ) {
    return detail::pow<N>(value);
}

/** even and odd */

template <typename T>
bool even( T val ) { return ( val >> 1 << 1 == val ); }

template <typename T>
bool odd( T val ) { return ( val >> 1 << 1 != val ); }

/** interval check */

template<typename T>
bool ccinterval( const T & lb, const T  & ub, const T & value ) {
    return ( lb <= value && value <= ub );
}

/**
  * Signum function
  */

template <typename Value_t>
int sgn( const Value_t & value ) {
   if ( value > 0 ) return 1;
   if ( value < 0 ) return -1;
   return 0;
}

/** Clamp value to given range.
 */
template <typename T>
inline T clamp(T value, T min, T max)
{
    return std::max(min, std::min(value, max));
}

template <typename T>
inline bool isInteger(T value, T tolerance = T(0))
{
    T integer;
    return (std::abs(std::modf(value, &integer)) <= tolerance);
}

/**
  * Matrix inversion
  */

template <typename T, typename L, typename C>
ublas::matrix<T,L,C> matrixInvert( const ublas::matrix<T,L,C> & input ) {

    typedef ublas::permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    ublas::matrix<T,L,C> A(input);

    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    auto res = lu_factorize(A,pm);
    if( res != 0 ) {
        LOGTHROW(warn1, std::runtime_error)
            << "Singular matrix in math::matrixInvert. Aborting.";
    }

    // create identity matrix of "inverse"
    ublas::matrix<T,L,C> inverse = ublas::identity_matrix<T>(A.size1());

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);
    return inverse;
}

template <typename T, typename L, typename C>
bool matrixInvertInplace( ublas::matrix<T,L,C> &input)
{
    typedef ublas::permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    ublas::matrix<T,L,C> A(input);

    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    auto res = lu_factorize(A,pm);
    if (res != 0) { return false; }

    // backsubstitute to get the inverse
    lu_substitute(A, pm, input);
    return true;
}

/**
 * Returns the trace of a square matrix.
 */
template <class T, class L, class C>
double trace(ublas::matrix<T, L, C> m) {
    const int n = m.size1();
    ublas::matrix_vector_range<ublas::matrix<T, L, C>> diag
            (m, ublas::range(0, n), ublas::range(0, n));
    return sum(diag);
}

/**
 * Matrix determinant
 */

template <class matrix_T>
double determinant(ublas::matrix_expression<matrix_T> const& mat_r)
{
  double det = 1.0;

  matrix_T mLu(mat_r() );
  ublas::permutation_matrix<std::size_t> pivots(mat_r().size1() );

  int is_singular = lu_factorize(mLu, pivots);

  if (!is_singular)
  {
    for (std::size_t i=0; i < pivots.size(); ++i)
    {
      if (pivots(i) != i)
        det *= -1.0;

      det *= mLu(i,i);
    }
  }
  else
    det = 0.0;

  return det;
}

/**
 * Solve a 2x2 linear system
 */
template<typename MatrixType, typename VectorType>
void solve2x2(const MatrixType &mat, const VectorType &rhs, VectorType &result)
{
    double det = mat(0,0)*mat(1,1) - mat(0,1)*mat(1,0);

    // check the determinant
    if (std::abs(det) < 1e-14) {
        LOGTHROW(err1, std::runtime_error) << "Singular matrix in solve2x2.";
    }

    det = 1.0 / det;
    result(0) = (rhs(0)*mat(1,1) - mat(0,1)*rhs(1)) * det;
    result(1) = (mat(0,0)*rhs(1) - rhs(0)*mat(1,0)) * det;
}

} // namespace math

#endif // MATH_MATH_HPP


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Domain/CoordinateMaps/CubicScale.hpp"

#include <array>
#include <boost/none.hpp>
#include <ostream>
#include <pup.h>
#include <pup_stl.h>
#include <utility>

#include "ControlSystem/FunctionOfTime.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "ErrorHandling/Error.hpp"
#include "NumericalAlgorithms/RootFinding/NewtonRaphson.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/DereferenceWrapper.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/MakeWithValue.hpp"

namespace CoordMapsTimeDependent {

CubicScale::CubicScale(const double outer_boundary) noexcept
    : outer_boundary_(outer_boundary) {
  if (outer_boundary_ <= 0.0) {
    ERROR("For invertability, we require outer_boundary to be positive.\n");
  }
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 1> CubicScale::operator()(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  return {{source_coords[0] *
           (a_of_t +
            (b_of_t - a_of_t) * square(source_coords[0] / outer_boundary_))}};
}

template <typename T>
boost::optional<std::array<tt::remove_cvref_wrap_t<T>, 1>> CubicScale::inverse(
    const std::array<T, 1>& target_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  // the original coordinates are found by solving for the roots
  // of (b-a)/X^2*\xi^3 + a*\xi - x = 0, where a and b are the FunctionsOfTime,
  // X is the outer_boundary, and x represents the mapped coordinates
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];

  // these checks ensure that the function is monotonically increasing
  // and that there is one real root in the domain of \xi, [0,X]
  if (a_of_t <= 0.0) {
    ERROR("We require expansion_a > 0 for invertibility, however expansion_a = "
          << a_of_t << ".");
  }
  if (b_of_t < 2.0 / 3.0 * a_of_t or b_of_t <= 0.0) {
    ERROR("The map is invertible only if 0 < expansion_b < expansion_a*2/3, "
          << " but expansion_b = " << b_of_t << " and expansion_a = " << a_of_t
          << ".");
  }

  // Make the coordinates dimensionless
  const tt::remove_cvref_wrap_t<T> x_bar = target_coords[0] / outer_boundary_;

  // Check if x_bar is outside of the range of the map
  if (x_bar < 0.0 or x_bar > b_of_t) {
    return boost::none;
  }

  // with the assumptions above:
  // x_bar lies within the range [0,b]
  // and \xi_bar = \xi/X is restricted to the domain [0,1]
  // For an initial guess, we provide a linearly approximated solution for
  // \xi_bar, which is just x_bar/b.
  const tt::remove_cvref_wrap_t<T> initial_guess = x_bar / b_of_t;
  const double cubic_coef_a = (b_of_t - a_of_t);

  const auto cubic_and_deriv =
      [&cubic_coef_a, &a_of_t, &x_bar ](double x) noexcept {
    return std::make_pair(x * (cubic_coef_a * square(x) + a_of_t) - x_bar,
                          3.0 * cubic_coef_a * square(x) + a_of_t);
  };

  // The original implementation of this inverse function used a cubic
  // equation solver. However, given that the problem of finding the inverse
  // in this case is well constrained -- using a Newton-Raphson root find with
  // a linearly approximated guess is almost twice as fast.
  // Using google benchmark,
  // the CubicEquation solver: ~ 480 ns
  // boost implemented Newton-Raphson: ~ 280 ns
  // minimal Newton-Raphson from Numerical Recipes: ~ 255 ns
  // Despite the minimal Newton-Raphson being more efficient than the boost
  // version, here we utilize the boost implementation, as it includes
  // additional checks for zero derivative, checks on bounds, and can implement
  // bisection if necessary.
  return {
      {{outer_boundary_ * RootFinder::newton_raphson(
                              cubic_and_deriv, initial_guess, 0.0, 1.0, 14)}}};
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 1> CubicScale::frame_velocity(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto dt_a_of_t = map_list.at(f_of_t_a_).func_and_deriv(time)[1][0];
  const auto dt_b_of_t = map_list.at(f_of_t_b_).func_and_deriv(time)[1][0];
  const auto frame_vel =
      source_coords[0] *
      (dt_a_of_t +
       (dt_b_of_t - dt_a_of_t) * square(source_coords[0] / outer_boundary_));

  return {{frame_vel}};
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame> CubicScale::jacobian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  auto jac{
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};

  get<0, 0>(jac) =
      a_of_t +
      3.0 * (b_of_t - a_of_t) * square(source_coords[0] / outer_boundary_);

  return jac;
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>
CubicScale::inv_jacobian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  auto inv_jac{
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};

  get<0, 0>(inv_jac) = 1.0 / (a_of_t +
                              3.0 * (b_of_t - a_of_t) *
                                  square(source_coords[0] / outer_boundary_));

  return inv_jac;
}

template <typename T>
tnsr::Iaa<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame> CubicScale::hessian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t_and_derivs = map_list.at(f_of_t_a_).func_and_2_derivs(time);
  const auto b_of_t_and_derivs = map_list.at(f_of_t_b_).func_and_2_derivs(time);

  auto result{
      make_with_value<tnsr::Iaa<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};
  // time-time
  get<0, 0, 0>(result) = a_of_t_and_derivs[2][0] * source_coords[0] +
                         (b_of_t_and_derivs[2][0] - a_of_t_and_derivs[2][0]) *
                             cube(source_coords[0]) / square(outer_boundary_);
  // time-space
  get<0, 0, 1>(result) =
      a_of_t_and_derivs[1][0] +
      3.0 * (b_of_t_and_derivs[1][0] - a_of_t_and_derivs[1][0]) *
          square(source_coords[0] / outer_boundary_);
  // space-space
  get<0, 1, 1>(result) = 6.0 *
                         (b_of_t_and_derivs[0][0] - a_of_t_and_derivs[0][0]) *
                         source_coords[0] / square(outer_boundary_);

  return result;
}

void CubicScale::pup(PUP::er& p) noexcept {
  p | f_of_t_a_;
  p | f_of_t_b_;
  p | outer_boundary_;
}

bool operator==(const CoordMapsTimeDependent::CubicScale& lhs,
                const CoordMapsTimeDependent::CubicScale& rhs) noexcept {
  return lhs.f_of_t_a_ == rhs.f_of_t_a_ and lhs.f_of_t_b_ == rhs.f_of_t_b_ and
         lhs.outer_boundary_ == rhs.outer_boundary_;
}

// Explicit instantiations
/// \cond
template boost::optional<std::array<tt::remove_cvref_wrap_t<double>, 1>>
CubicScale::inverse(
    const std::array<double, 1>& target_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept;
template boost::optional<std::array<
    tt::remove_cvref_wrap_t<std::reference_wrapper<const double>>, 1>>
CubicScale::inverse(
    const std::array<std::reference_wrapper<const double>, 1>& target_coords,
    const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept;

#define DTYPE(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                                   \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 1> CubicScale::    \
  operator()(const std::array<DTYPE(data), 1>& source_coords,                  \
             const double time,                                                \
             const std::unordered_map<std::string, FunctionOfTime&>& map_list) \
      const noexcept;                                                          \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 1>                 \
  CubicScale::frame_velocity(                                                  \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>   \
  CubicScale::jacobian(                                                        \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>   \
  CubicScale::inv_jacobian(                                                    \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Iaa<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>  \
  CubicScale::hessian(                                                         \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;

GENERATE_INSTANTIATIONS(INSTANTIATE, (double, DataVector,
                                      std::reference_wrapper<const double>,
                                      std::reference_wrapper<const DataVector>))
#undef DTYPE
#undef INSTANTIATE
/// \endcond
}  // namespace CoordMapsTimeDependent


#pragma once

#include <route_solver/structs/data_structs.hpp>
#include <route_solver/solution/truck_solution.hpp>

#include <Eigen/Core>

namespace rs {
rs::TruckSolution truckSolve(
	const std::vector<Vehicle>& venchiles,
	const std::vector<rs::Zone>& zones,
	const std::vector<Task>& tasks,
	const Eigen::MatrixXd& destTimeMatrix,
	const Eigen::MatrixXd& getDestMatrix,
	const BeeColonyParams& beeParams,
	bool partialSolutionEnabled,
	rs::Statistics* stat = 0
);

}


// Test program for ConvexHull2d class
//
// Command line compile options used:
// Visual Studio 2013:
//    cl /W4 /O2 /EHsc /Fehull.exe main.cpp
// Linux (Debian) g++ 4.9.2:
//    g++ -std=c++11 -Wall -Wextra -pedantic -O2 -o hull main.cpp
//
// Usage:
//    hull [COUNT] [TRUE]
//
// Options:
//    COUNT = number of random points to generate for each test. Defaults to 10
//    TRUE =  Flag to write data to file and create gnuplot script. You must specify a COUNT to use this flag.
//            This will create 4 files: dbl_test.dat, dbl_test.gp, int_test.dat, int_test.gp.
// Examples:
//    hull  - Generates 10 random integers and doubles with no write to disk.
//    hull 20  - Generates 20 random integers and doubles with no write to disk.
//    hull 20 true  - Generates 20 random integers and doubles and creates data file and gnuplot script.
//

#include "convexhull2d.h"
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <iostream>
#include <iomanip>
#include <vector>
#include <random>
#include <fstream>
#include <chrono>
#include <string>
#include <limits>

using namespace hpm;
using namespace Eigen;
using Timer = std::chrono::steady_clock;
using Duration = std::chrono::duration<double, std::milli>;
using DoubleLimit = std::numeric_limits<double>;
using IntLimit = std::numeric_limits<int>;

template <typename T>
struct Point2d
{
  T x;
  T y;
};

template <typename T>
void writeData(const std::string& data_file, const std::vector<T>& points,
               ConvexHull2d<T>& hull, std::vector<std::pair<double, double>> rotated)
{
  std::ofstream data(data_file);
  if (!data.is_open()) return;
  data << std::setprecision(15);
  data << "# Point set\n";
  for (auto i : points) {
    data << i.x() << " " << i.y() << '\n';
  }

  data << "\n# Centroid\n";
  data << hull.getCentroid().first << ' ' << hull.getCentroid().second << "\n";

  data << "\n# Convex hull\n";
  if (!hull.isEmpty()) {
    for (auto i : hull.getHull()) {
      data << i.x() << " " << i.y() << '\n';
    }
    data << hull.getHull().front().x() << " " << hull.getHull().front().y() << '\n';
  }

  data << "\n# Rotated vertices\n";
  if (!rotated.empty()) {
    for (auto i : rotated) {
      data << i.first << " " << i.second << '\n';
    }
    data << rotated.front().first << " " << rotated.front().second << '\n';
  }

}

void writeGPScript(const std::string& script, const std::string& data)
{
  std::ofstream gp(script);
  if (!gp.is_open()) return;
  gp << "set title \"Convex Hull Test\"\n";
  gp << "set key outside right box\n";
  gp << "set size ratio -1\n";
  gp << "plot \"" << data << "\" every :::0::0 title \"All Points\" with points, \\\n";
  gp << "\"\" every :::1::1 title \"Centroid\" with points, \\\n";
  gp << "\"\" every :::2::2 title \"Convex Hull\" with lp, \\\n";
  gp << "\"\" every :::3::3 title \"Rotated\" with lp\n";
  gp << "\npause -1\n";
}

int main(int argc, char* argv[])
{
  size_t num_points = 10; // Default number of points
  bool do_write = false;

  if (argc > 1) {
    int arg1 = std::strtol(argv[1], nullptr, 10);
    if (arg1 >= 0 && arg1 < IntLimit::max() / 2) {
      num_points = arg1;
    }
    else
      std::cout << "\nInvalid number of points! Defaulting to " << num_points << " points per test.\n";
    if (argc > 2) {
      std::string arg2(argv[2]);
      std::transform(arg2.begin(), arg2.end(), arg2.begin(), ::toupper);
      if (arg2 == "TRUE") do_write = true;
    }
  }

  const int min_int = 0;
  //const int min_int = IntLimit::min();
  const int max_int = IntLimit::max();
  //const double min_double = 0.0;
  const double min_double = -std::sqrt(std::sqrt(DoubleLimit::max())) * 10e24;
  const double max_double = std::sqrt(std::sqrt(DoubleLimit::max())) * 10e24;

  std::mt19937 engine;
  engine.seed(std::random_device{}());

  // Start timer and generate random integers to test
  auto timer_start = Timer::now();
  std::vector<Vector2i> int_points(num_points);
  std::uniform_int_distribution<> int_distribution(min_int, max_int);
  for (auto& i : int_points) {
    i.x() = int_distribution(engine);
    i.y() = int_distribution(engine);
  }

  // Generate random doubles to test
  std::vector<Vector2d> double_points(num_points);
  std::uniform_real_distribution<> double_distribution(min_double, max_double);
  for (auto& i : double_points) {
    i.x() = double_distribution(engine);
    i.y() = double_distribution(engine);
  }
  auto random_time = Timer::now() - timer_start;

  // Create hulls
  auto hull_timer_start = Timer::now();
  ConvexHull2d<Vector2i> int_hull(int_points);
  auto int_hull_time = Timer::now() - hull_timer_start;

  ConvexHull2d<Vector2d> double_hull(double_points);
  auto double_hull_time = Timer::now() - hull_timer_start - int_hull_time;
  auto hull_time = Timer::now() - hull_timer_start;

  // Test rotate
  const double to_radians = acos(-1) / 180.0;
  std::pair<double, double> centroid = double_hull.getCentroid();
  std::vector<std::pair<double, double>> double_rotated = double_hull.rotateVertices(-45.0 * to_radians, centroid);
  std::vector<std::pair<double, double>> int_rotated = int_hull.rotateVertices(45.0 * to_radians);

  if (do_write) {
    writeData("int_test.dat", int_points, int_hull, int_rotated);
    writeGPScript("int_test.gp", "int_test.dat");
    writeData("dbl_test.dat", double_points, double_hull, double_rotated);
    writeGPScript("dbl_test.gp", "dbl_test.dat");
  }
  auto total_time = Timer::now() - timer_start;
  auto write_time = total_time - hull_time - random_time;

  std::cout << "\nMin int: " << min_int << '\n';
  std::cout << "Max int: " << max_int << '\n';
  std::cout << "Min double: " << min_double << '\n';
  std::cout << "Max double: " << max_double << '\n';

  std::cout << "\nRandom creation time: " << Duration(random_time).count() << " ms\n";
  std::cout << "Hull creation time: " << Duration(hull_time).count() << " ms\n";
  std::cout << "Write time: " << Duration(write_time).count() << " ms\n";

  std::cout << "\nTotal time: " << Duration(total_time).count() << " ms\n";
  std::cout << "Total points: " << int_points.size() + double_points.size() << '\n';

  std::cout << "\nInteger hull point count: " << int_hull.getHull().size() << '\n';
  std::cout << "Integer hull creation time: " << Duration(int_hull_time).count() << " ms\n";
  std::cout << "Integer hull area: " << int_hull.getArea() << '\n';
  std::cout << "Integer hull is " << (int_hull.isValid() ? "valid\n" : "not valid\n");

  std::cout << "\nDouble hull point count: " << double_hull.getHull().size() << '\n';
  std::cout << "Double hull creation time: " << Duration(double_hull_time).count() << " ms\n";
  std::cout << "Double hull area: " << double_hull.getArea() << '\n';
  std::cout << "Double hull is " << (double_hull.isValid() ? "valid\n" : "not valid\n");

  std::getchar();
}