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

#include <iomanip>
#include <iostream>

#include "constants.hpp"
#include "legendre.hpp"
#include "tanh_sinh_rule.hpp"

#include <mxpfit/aak_reduction.hpp>
#include <mxpfit/balanced_truncation.hpp>
#include <mxpfit/exponential_sum.hpp>

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

template <typename T>
constexpr T pi()
{
    return T(
        3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679);
}
///
/// ### BesselKernel
///
/// Compute the multi-exponential function that approximates the spherical
/// Bessel function of the first kind such that
///
/// \f[
///   \|j_{n}(x)-sum_{m=1}^{M} w_{m} e^{-a_{m} x} \| < \epsilon
/// \f]
///
/// for integer `n` on real axis `(x)`, where \f$ \epsilon \f$ is an arbitrary
/// small positive number.
///
/// The multi-exponential function is obtained by discretize the integral
/// representation of spherical Bessel function
///
/// \f[
///   j_{n}(x)=\int_{-1}^{1} e^{ixt} P_n(t) dt
/// \f]
///
/// The integrand in above expression is highly oscillating when `x` becomes
/// large, thus the direct application of some quadrature rule is inefficient.
/// To avoid this problem, we extend the integral on the complex plane and
/// select the contour as the steepest descent paths, as follows.
///
/// \f{eqnarray*}{
///   j_{n}(x) &=& I_1 + I_2 + (-1)^{n}(I_1^{\ast} + I_{2}^{\ast}), \\
///   I_{1} &=& \frac{(-i)^{n+1}}{2} \int_{0}^{R} e^{-(y-i)x} P_{n}(1+iy) dy, \\
///   I_{2} &=& \frac{(-i)^{n}}{2} \int_{-1}^{0} e^{-(R-ix)y} P_{n}(y+iR) dy, \\
/// \f}
///
/// The integrals \f$ I_1 \f$ and \f$ I_2 \f$ are discretized using appropriate
/// quadrature rule, then the balanced truncation method is applied to reduce
/// the number of terms.
///
template <typename T>
class BesselKernel
{
public:
    typename VecY, using Index = Eigen::Index;
    using Real    = T;
    using Complex = std::complex<Real>;

    using ExponentialSumType = mxpfit::ExponentialSum<Complex, Complex>;
    using ExponentsArray     = typename ExponentialSumType::ExponentsArray;
    using WeightsArray       = typename ExponentialSumType::WeightsArray;

    static ExponentialSumType compute(Index order, Real threshold);
};

template <typename T>
typename BesselKernel<T>::ExponentialSumType
BesselKernel<T>::compute(Index n, Real threshold)
{
    using Eigen::numext::conj;
    using Eigen::numext::exp;
    using Eigen::numext::log;
    using Eigen::numext::cos;
    using Eigen::numext::acos;
    using Eigen::numext::sin;
    using Eigen::numext::sqrt;
    // ----- Constants
    constexpr const auto zero = Real();
    constexpr const auto one  = Real(1);
    constexpr const auto half = Real(0.5);
    constexpr const auto eps  = std::numeric_limits<Real>::epsilon();
    // (-i)^n
    constexpr const Complex pow_i[4] = {Complex(one, zero), Complex(zero, -one),
                                        Complex(-one, zero),
                                        Complex(zero, one)};

    //----- adjustable parameters
    const auto R = Real(3) / std::max(Index(1), n);

    const auto n_quad1 = Index(500);
    const auto tiny1   = sqrt(eps) * threshold / n_quad1;

    const auto n_quad2 = Index(150 + 2 * n);
    const auto tiny2   = eps / n_quad2;
    //---------------------------

    //
    // Find R_shift such that j_n(R_shift) < eps
    //
    Real R_shift = Real();
    if (n > Index())
    {
        Real log_dfact = Real(); // log[2^n * n!]
        for (Index k = 1; k <= n; ++k)
        {
            log_dfact += log(Real(2 * k));
        }
        const auto thresh = std::min(threshold, sqrt(eps));
        R_shift           = exp((log_dfact + log(thresh)) / Real(n));
        if (R_shift < Real(0.1))
        {
            R_shift = Real();
        }
    }

    ExponentialSumType es_merged(n_quad1 + n_quad2);

    //
    // Discretization of integral I_2 on the path `z = x + iR` for `0<=x<=1`
    //
    quad::DESinhTanhRule<T> rule2(n_quad2, tiny2);

    // std::cout << "*** quadrature rule\n";
    // Real xsum = Real();
    // for (Index k = 0; k < n_quad2; ++k)
    // {
    //     xsum += rule2.adjacentDifference(k);
    //     std::cout << rule2.x(k) << '\t' << rule2.adjacentDifference(k) <<
    //     '\t'
    //               << xsum << '\t' << std::abs(rule2.x(k) - xsum) << '\n';
    // }

    for (Index k = 0; k < n_quad2; ++k)
    {
        const auto xk = half * rule2.distanceFromLower(k); // node
        const auto uk = half * rule2.distanceFromUpper(k); // 1 - x
        const auto wk = half * rule2.w(k);                 // weight

        const auto ak = Complex(R, -xk);
        const auto pk = wk * exp(ak * R_shift) *
                        cos(Real(n) * acos(Complex(xk, R))) /
                        sqrt(Complex(one + xk, R) * Complex(uk, -R));

        es_merged.exponent(k) = ak;
        es_merged.weight(k)   = pk;
    }

    //
    // Discretization of integral I_1 on the path `1 + i y`  for
    // `0 <= y <= R`
    //
    quad::DESinhTanhRule<T> rule1(n_quad1, tiny1);

    const auto R_half = R / 2;
    Index ipos        = n_quad2;
    for (Index k = n_quad1; k > 0; --k)
    {
        const auto yk = R_half * rule1.distanceFromLower(k - 1); // node
        const auto wk = R_half * rule1.w(k - 1);                 // weight
        const auto ak = Complex(yk, -one);
        const auto pk = Complex(zero, -wk) * exp(ak * R_shift) *
                        cos(Real(n) * acos(Complex(one, yk))) /
                        (sqrt(yk * Complex(yk, Real(-2))));

        es_merged.exponent(ipos) = ak;
        es_merged.weight(ipos)   = pk;
        ++ipos;
    }

    // Remove terms with same exponents
    es_merged.uniqueExponents(Real());

    //
    // Truncation for I_1 and I_2 separately
    //
    // mxpfit::BalancedTruncation<Complex> trunc1;
    // es1 = trunc1.compute(es1, threshold * eps);

    // mxpfit::BalancedTruncation<Complex> trunc2;
    // es2 = trunc2.compute(es2, threshold);

    //
    // Truncation for I_1 and I_2 simultaneously
    //
    mxpfit::BalancedTruncation<Complex> trunc1;
    es_merged = trunc1.compute(es_merged, threshold * eps * eps);

    // mxpfit::AAKReduction<Complex> reduction;
    // reduction.setThreshold(threshold);
    // reduction.compute(es_merged);

    ExponentialSumType es_result(2 * es_merged.size());
    const auto pre1 = pow_i[n % 4] / pi<Real>(); // (-i)^n / pi
    const auto pre2 = (n & 1) ? -pre1 : pre1;

    for (Index i = 0; i < es_merged.size(); ++i)
    {
        const auto ai                 = es_merged.exponent(i);
        es_result.exponent(2 * i + 0) = ai;
        es_result.exponent(2 * i + 1) = conj(ai);

        const auto wi               = es_merged.weight(i) * exp(-ai * R_shift);
        es_result.weight(2 * i + 0) = pre1 * wi;
        es_result.weight(2 * i + 1) = pre2 * conj(wi);
    }

    return es_result;
}

// template <typename T>
// typename BesselKernel<T>::ExponentialSumType
// BesselKernel<T>::compute(Index n, Real threshold)
// {
//     using Eigen::numext::conj;
//     using Eigen::numext::exp;
//     using Eigen::numext::log;
//     using Eigen::numext::cos;
//     using Eigen::numext::acos;
//     using Eigen::numext::sin;
//     using Eigen::numext::sqrt;
//     // ----- Constants
//     constexpr const auto zero = Real();
//     constexpr const auto one  = Real(1);
//     constexpr const auto half = Real(0.5);
//     constexpr const auto eps  = std::numeric_limits<Real>::epsilon();
//     // (-i)^n
//     constexpr const Complex pow_i[4] = {Complex(one, zero), Complex(zero,
//     -one),
//                                         Complex(-one, zero),
//                                         Complex(zero, one)};

//     //----- adjustable parameters
//     const auto R       = Real(3) / std::max(Index(1), n);
//     const auto n_quad1 = Index(500);
//     // const auto tiny1   = eps * eps;
//     const auto tiny1 = eps * sqrt(eps) / n_quad1;

//     const auto n_quad2 = Index(150 + 2 * n);
//     const auto tiny2   = eps / n_quad2;
//     //---------------------------

//     //
//     // Find R_shift such that j_n(R_shift) < eps
//     //
//     Real R_shift = Real();
//     if (n > Index())
//     {
//         Real log_dfact = Real(); // log[2^n * n!]
//         for (Index k = 1; k <= n; ++k)
//         {
//             log_dfact += log(Real(2 * k));
//         }
//         const auto thresh = std::min(threshold, sqrt(eps));
//         R_shift           = exp((log_dfact + log(thresh)) / Real(n));
//         if (R_shift < Real(0.1))
//         {
//             R_shift = Real();
//         }
//     }

//     //
//     // Discretization of integral I_1 on the path `-1 + i y`  for
//     // `0 <= y <= R`
//     //
//     quad::DESinhTanhRule<T> rule1(n_quad1, tiny1);
//     ExponentialSumType es1(n_quad1);

//     const auto R_half = R / 2;
//     for (Index k = 0; k < n_quad1; ++k)
//     {
//         const auto yk = R_half * rule1.distanceFromLower(k); // node
//         const auto wk = R_half * rule1.w(k);                 // weight
//         const auto ak = Complex(yk, one);
//         const auto pk = Complex(zero, wk) * exp(ak * R_shift) *
//                         cos(Real(n) * acos(Complex(-one, yk))) /
//                         (sqrt(yk * Complex(yk, Real(2))));

//         es1.exponent(k) = ak;
//         es1.weight(k)   = pk;
//     }

//     //
//     // Discretization of integral I_2 on the path `z = x + iR` for `-1<=x<=0`
//     //
//     quad::DESinhTanhRule<T> rule2(n_quad2, tiny2);
//     ExponentialSumType es2(n_quad2);

//     for (Index k = 0; k < n_quad2; ++k)
//     {
//         const auto xk = -half * rule2.distanceFromUpper(k); // node
//         const auto lk = half * rule2.distanceFromLower(k);  // x + 1
//         const auto wk = half * rule2.w(k);                  // weight

//         const auto ak = Complex(R, -xk);
//         const auto pk = wk * exp(ak * R_shift) *
//                         cos(Real(n) * acos(Complex(xk, R))) /
//                         sqrt(Complex(lk, R) * Complex(one - xk, -R));

//         es2.exponent(k) = ak;
//         es2.weight(k)   = pk;
//     }
//     //
//     // Truncation for I_1 and I_2 separately
//     //
//     // mxpfit::BalancedTruncation<Complex> trunc1;
//     // es1 = trunc1.compute(es1, threshold * eps);

//     // mxpfit::BalancedTruncation<Complex> trunc2;
//     // es2 = trunc2.compute(es2, threshold);

//     //
//     // Merge two sums
//     //
//     ExponentialSumType es_merged(es1.size() + es2.size());
//     es_merged.exponents().head(es1.size()) = es1.exponents();
//     es_merged.exponents().tail(es2.size()) = es2.exponents();
//     es_merged.weights().head(es1.size())   = es1.weights();
//     es_merged.weights().tail(es2.size())   = es2.weights();

//     //
//     // Truncation for I_1 and I_2 simultaneously
//     //
//     // mxpfit::BalancedTruncation<Complex> trunc1;
//     // es_merged = trunc1.compute(es_merged, threshold * eps * eps);

//     ExponentialSumType es_result(2 * es_merged.size());
//     const auto pre1 = pow_i[n % 4] / pi<Real>(); // (-i)^n / pi
//     const auto pre2 = (n & 1) ? -pre1 : pre1;

//     for (Index i = 0; i < es_merged.size(); ++i)
//     {
//         const auto ai                 = es_merged.exponent(i);
//         es_result.exponent(2 * i + 0) = ai;
//         es_result.exponent(2 * i + 1) = conj(ai);

//         const auto wi               = es_merged.weight(i) * exp(-ai *
//         R_shift); es_result.weight(2 * i + 0) = pre1 * wi; es_result.weight(2
//         * i + 1) = pre2 * conj(wi);
//     }

//     return es_result;
// }

//==============================================================================
// Main
//==============================================================================

using Index              = Eigen::Index;
using Real               = double;
using Complex            = std::complex<Real>;
using RealArray          = Eigen::Array<Real, Eigen::Dynamic, 1>;
using ComplexArray       = Eigen::Array<Complex, Eigen::Dynamic, 1>;
using ExponentialSumType = BesselKernel<Real>::ExponentialSumType;

void bessel_j_kernel_error(int l, const RealArray& x,
                           const ExponentialSumType& ret)
{
    RealArray exact(x.size());
    RealArray approx(x.size());

    for (Index i = 0; i < x.size(); ++i)
    {
        exact(i)  = boost::math::cyl_bessel_j(l, x(i));
        approx(i) = std::real(ret(x(i)));
    }

    RealArray abserr(Eigen::abs(exact - approx));

    for (Index i = 0; i < x.size(); ++i)
    {
        std::cout << std::setw(24) << x(i) << ' '      // point
                  << std::setw(24) << exact(i) << ' '  // exact value
                  << std::setw(24) << approx(i) << ' ' // approximation
                  << std::setw(24) << abserr(i) << '\n';
    }

    Index imax;
    abserr.maxCoeff(&imax);

    std::cout << "\n  abs. error in interval [" << x(0) << ","
              << x(x.size() - 1) << "]\n"
              << "    maximum : " << abserr(imax) << '\n'
              << "    averaged: " << abserr.sum() / x.size() << std::endl;
}

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

    const Real threshold = 1.0e-14;
    const Real eps       = Eigen::NumTraits<Real>::epsilon();
    const Index lmax     = 4;
    const Index N        = 2000; // # of sampling points

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

    RealArray x = Eigen::exp(RealArray::LinSpaced(N, -20.0, 20.0));
    ExponentialSumType ret;
    for (Index l = 0; l <= lmax; ++l)
    {
        std::cout << "\n# --- order " << l;
        ret                      = BesselKernel<Real>::compute(l, threshold);
        const auto thresh_weight = std::max(eps, threshold) / Real(ret.size());
        ret                      = mxpfit::removeIf(
            ret, [=](const Complex& /*exponent*/, const Complex& wi) {
                return std::abs(std::real(wi)) < thresh_weight &&
                       std::abs(std::imag(wi)) < thresh_weight;
            });
        std::cout << " (" << ret.size() << " terms approximation)\n"
                  << ret << '\n';

        // std::cout << " (" << ret.size() << " terms approximation)\n";
        // std::cout
        //     << "# real(exponent), imag(exponent), real(weight),
        //     imag(weight)\n";
        // for (Index i = 0; i < ret.size(); ++i)
        // {
        //     std::cout << std::setw(24) << std::real(ret.exponent(i)) << '\t'
        //               << std::setw(24) << std::imag(ret.exponent(i)) << '\t'
        //               << std::setw(24) << std::real(ret.weight(i)) << '\t'
        //               << std::setw(24) << std::imag(ret.weight(i)) << '\n';
        // }
        // std::cout << '\n' << std::endl;

        std::cout << "\n# errors in small x\n";
        bessel_j_kernel_error(l, x, ret);
    }

    return 0;
}


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

   The MIT License

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

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

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

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

	 Author: 							Jaume Coll-Font, Yesim Serinagaoglu & Alireza Ghodrati
	 Last Modification:		September 6 2017
*/

#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Logging/LoggerInterface.h>

// Tikhonov inverse libraries
#include <Core/Algorithms/Legacy/Inverse/TikhonovAlgoAbstractBase.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTSVD_impl.h>

// EIGEN LIBRARY
#include <Eigen/Eigen>
#include <Eigen/SVD>

using namespace SCIRun;
using namespace SCIRun::Core::Datatypes;
// using namespace SCIRun::Modules::Inverse;
// using namespace SCIRun::Dataflow::Networks;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::Inverse;

///////////////////////////////////////////////////////////////////
/////// prealocate Matrices for inverse compuation
///     This function precalcualtes the SVD of the forward matrix and prepares singular vectors and values for posterior computations
///////////////////////////////////////////////////////////////////
void SolveInverseProblemWithTSVD_impl::preAlocateInverseMatrices(const SCIRun::Core::Datatypes::DenseMatrix& forwardMatrix_, const SCIRun::Core::Datatypes::DenseMatrix& measuredData_ , const SCIRun::Core::Datatypes::DenseMatrix& sourceWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& sensorWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& matrixU_, const SCIRun::Core::Datatypes::DenseMatrix& singularValues_, const SCIRun::Core::Datatypes::DenseMatrix& matrixV_)
{

		// alocate U and V matrices
			svd_MatrixU = matrixU_;
			svd_MatrixV = matrixV_;

		// alocate singular values
			if (singularValues_.ncols() == 1 ){
				svd_SingularValues = singularValues_;
			}
			else{
				svd_SingularValues = singularValues_.diagonal();
			}

		// Compute the projection of data y on the left singular vectors
			Uy = svd_MatrixU.transpose() * (measuredData_);

		// determine rank
	        rank = svd_SingularValues.nrows();

}

void SolveInverseProblemWithTSVD_impl::preAlocateInverseMatrices(const SCIRun::Core::Datatypes::DenseMatrix& forwardMatrix_, const SCIRun::Core::Datatypes::DenseMatrix& measuredData_ , const SCIRun::Core::Datatypes::DenseMatrix& sourceWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& sensorWeighting_)
{

	    // Compute the SVD of the forward matrix
	        Eigen::JacobiSVD<SCIRun::Core::Datatypes::DenseMatrix::EigenBase> SVDdecomposition( forwardMatrix_, Eigen::ComputeFullU | Eigen::ComputeFullV);

		// alocate the left and right singular vectors and the singular values
			svd_MatrixU = SVDdecomposition.matrixU();
			svd_MatrixV = SVDdecomposition.matrixV();
			svd_SingularValues = SVDdecomposition.singularValues();

	    // determine rank
	        rank = SVDdecomposition.nonzeroSingularValues();

	    // Compute the projection of data y on the left singular vectors
	        Uy = svd_MatrixU.transpose() * (measuredData_);
}

//////////////////////////////////////////////////////////////////////
// THIS FUNCTION returns regularized solution by tikhonov method
//////////////////////////////////////////////////////////////////////
SCIRun::Core::Datatypes::DenseMatrix SolveInverseProblemWithTSVD_impl::computeInverseSolution( double lambda, bool inverseCalculation ) const
{

    // prealocate matrices
        const int N = svd_MatrixV.cols();
        const int M = svd_MatrixU.rows();
        const int numTimeSamples = Uy.ncols();
        DenseMatrix solution(DenseMatrix::Zero(N,numTimeSamples));
        DenseMatrix tempInverse(DenseMatrix::Zero(N,M));

		const int truncationPoint = Min( int(lambda), rank, int(9999999999999) );

    // Compute inverse SolveInverseProblemWithTSVD
        for (int rr=0; rr < truncationPoint ; rr++)
        {
            // evaluate filter factor
                double singVal = svd_SingularValues[rr];
                auto filterFactor_i =  1 / ( singVal );

            // update solution
                solution += filterFactor_i * svd_MatrixV.col(rr) * Uy.row(rr);

            // update inverse operator
                if (inverseCalculation)
                    tempInverse += filterFactor_i * ( svd_MatrixV.col(rr) *  svd_MatrixU.col(rr).transpose() );
        }

    // output solutions
    //   if (inverseCalculation)
    //       inverseMatrix_.reset( new SCIRun::Core::Datatypes::DenseMatrix(tempInverse) );

        return solution;
}

//////////////////////////////////////////////////////////////////////
// THIS FUNCTION returns a string of lambdas from which the L-curve is computed
//////////////////////////////////////////////////////////////////////
std::vector<double> SolveInverseProblemWithTSVD_impl::computeLambdaArray( double lambdaMin, double lambdaMax, int nLambda ) const
{
	std::vector<double> lambdaArray(nLambda,0.0);
	const double lam_step = 1;

	lambdaArray[0] = lambdaMin;
	for (int j = 1; j < nLambda; j++)
	{
		lambdaArray[j] = lambdaArray[j-1]  + lam_step;
	}
	return lambdaArray;
}


//Copyright (c) 2018, Changjie
//All rights reserved.
//
//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.
//

#ifndef __BFGS_HPP__
#define __BFGS_HPP__

#include <functional>
#include <memory>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Dense>

using namespace Eigen;

using Eigen::MatrixXd;
using Eigen::VectorXd;

// ref: http://en.cppreference.com/w/cpp/utility/functional/function

// input is the position(parameter)
// Add const to avoid error of invalid initialization of non-const reference
typedef std::function<double(VectorXd const &)> Cost_Fun;
// param1: returned gradient
// param2: input position
typedef std::function<void(VectorXd &, VectorXd &)> Diff_Fun;
// param1: returned hessian matrix
// param2: input position
typedef std::function<void(MatrixXd &, VectorXd &)> Hess_Fun;

struct NonConstraintObj
{
	VectorXd m_x;
	size_t m_n;

	NonConstraintObj()
	{
		m_n = 1;
		m_x.resize(m_n);
		m_x.fill(0.);
	}
	virtual ~NonConstraintObj() = default;
	virtual double cost(VectorXd const &x) = 0;
	virtual void grad(VectorXd &grad, VectorXd &x) = 0;
};

struct BFGS
{
	size_t MAX_STEP;
	size_t stop_step;
	size_t step;
	double rho;
	double sigma;
	double epsilon;

	bool improved;
	bool initialized;

	Cost_Fun cost_fun;
	Diff_Fun grad_fun;

	std::shared_ptr<NonConstraintObj> funPtr;
	BFGS()
	{
		MAX_STEP = 80;
		stop_step = 5;
		step = 0;
		rho = 0.55;
		sigma = 0.4;
		epsilon = 1e-5;
		improved = false;
		initialized = false;
		funPtr = nullptr;
	}
	template <class T>
	void init(T &obj)
	{
		// store a call to a member function and object
		using std::placeholders::_1;
		using std::placeholders::_2;
		cost_fun = std::bind(&T::cost, obj, _1);
		grad_fun = std::bind(&T::grad, obj, _1, _2);
		initialized = true;
	}
	double solve_(Cost_Fun fun, Diff_Fun dfun, VectorXd &input);
	double solve_(Cost_Fun fun, Diff_Fun dfun, Hess_Fun hfun, VectorXd &input);
	double solve(VectorXd &input)
	{
		if (initialized)
		{
			return solve_(cost_fun, grad_fun, input);
		}
		else
		{
			printf("BFGS is not initialized! Return zeros.\n");
			return 1e9;
		}
	}
};

struct BFGS_V2
{
	//double *Bk;

	BFGS_V2()
	{
		//Bk = nullptr;
		//cudaMalloc((void **)&Bk, sizeof(double)*3);
	}
	template <class T>
	static double bfgs(std::shared_ptr<T> ptr, VectorXd &x0)
	{
		size_t maxk = 500, stop_step = 5;
		double rho = 0.55, sigma = 0.4, epsilon = 1e-5;

		size_t k = 0, n = x0.size();
		MatrixXd Bk = MatrixXd::Identity(n, n);

		VectorXd gk, dk, sk, yk, x;
		size_t m = 0, mk = 0, bad = 0;
		double fit = 0., tmp = 0.;
		double best = 1e10;
		while (k < maxk && bad < stop_step)
		{
			fit = ptr->cost(x0);
			if (fit < best - epsilon)
			{
				best = fit;
				bad = 0;
			}
			else
			{
				bad++;
			}
			ptr->grad(gk, x0);
			//std::cout<<"gk = "<<gk<<std::endl;
			if (gk.norm() < epsilon)
			{
				break;
			}
			dk = Bk.householderQr().solve(-gk); // llt ?
			m = 0, mk = 0;

			tmp = gk.transpose() * dk;
			while (m < 100)
			{
				if (ptr->cost(x0 + pow(rho, m) * dk) < (fit + sigma * pow(rho, m) * tmp))
				{
					mk = m;
					break;
				}
				m++;
			}
			x = x0 + pow(rho, mk) * dk;
			sk = pow(rho, mk) * dk;
			ptr->grad(yk, x);
			yk = yk - gk;
			if (yk.transpose() * sk > 0)
			{
				Bk = Bk - (Bk * sk * sk.transpose() * Bk) / (sk.transpose() * Bk * sk) + (yk * yk.transpose()) / (yk.transpose() * sk);
			}
			x0 = x;
			k++;
		}
		x0 = x;
		#ifdef DEBUG
		std::cout << "BFGS stops at step " << k << std::endl;
		#endif
		return fit;
	}
};

#endif


#ifndef fast_gauss_noise_h
#define fast_gauss_noise_h

#include <cstdint>
#include <cstddef>
#include <list>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <climits>
#include <cstring>
#include <tuple>
#include <typeinfo>
#include <gmp.h>
#include <mpfr.h>
#include "fastrandombytes.h"

#ifdef BOOST_RAPHSON
#include <boost/math/tools/roots.hpp>
#endif

//#define OUTPUT_BARRIERS
//#define OUTPUT_LUT_FLAGS
// Use it to test rare values do not happen too often
//#define UNITTEST_ONEMILLION

namespace nfl {

template<typename T0, typename T1>
constexpr inline auto tstbit(T0 x, T1 n) -> decltype((x << (63 - n)) >> 63 ) { return ((x << (63 - n)) >> 63 ); }

template<class in_class, class out_class>
struct output {
  out_class val;
  bool flag;
  std::list<in_class*> l_b_ptr;
};

template<class in_class, class out_class, unsigned _lu_depth>
class FastGaussianNoise {
    typedef output<in_class, out_class> output_t;
  private:
    unsigned int _bit_precision;
    unsigned int _word_precision;
    unsigned int _number_of_barriers;
    unsigned int _lu_size;
    unsigned int _flag_ctr1;
    unsigned int _flag_ctr2;
    double _sigma;
    mpfr_t _const_sigma;
    mpfr_t _center;
    int rounded_center;
    unsigned int _security;
    unsigned int _samples;
    double _tail_bound;
    bool _verbose;

    in_class **barriers;
    output_t *lu_table;
    output_t **lu_table2;

    void check_template_params();
    void init();
    void precomputeBarrierValues();
    void buildLookupTables();
    void nn_gaussian_law(mpfr_t rop, const mpfr_t x_fr);
    int cmp(in_class *op1, in_class *op2);

  public:
    static const unsigned int default_k;
    FastGaussianNoise(double sigma, unsigned int security, unsigned int samples, double center_d = 0, bool verbose = false);
    FastGaussianNoise(double sigma, unsigned int security, unsigned int samples, mpfr_t center, bool verbose = false);
    ~FastGaussianNoise();
    void getNoise(out_class * const rand_data2out, uint64_t rlen);
};

/* NOTATIONS (from paper Sampling from discrete Gaussians
 * for lattice-based cryptography on a constrainded device
 * Nagarjun C. Dwarakanath, Steven Galbraith
 * Journal : AAECC 2014 (25)
 *
 * m : lattice dimension, we set it to 1 in here (better results for same security) !!!!
 *
 * security : output distribution should be within 2^{-security}
 * statistical distance from the Gaussian distribution
 *
 * sigma : parameter of the Gaussian distribution (close
 * to the standard deviation but not equal)
 * SUM = \sum_{k=-\inf}^{\inf} exp(-(k-c)^2/(2*sigma^2))
 *
 * D_{sigma} : distribution such that for x\in \mathcal{Z}
 * Pr(x) = ro_{sigma}(x) = 1/SUM exp(-(x-c)^2/(2*sigma^2))
 *
 * Warning : can be replaced in some works by s = sqrt(2*pi)*sigma
 * if Pr(x) = 1/SUM exp(-pi*(x-c)^2/s^2)
 *
 * tail_bound : tail bound we only output points with
 * norm(point-c) < tail_bound*sigma
 *
 * bit_precision : bit precision when computing the probabilities
 *
 * delta_tailbound : statistical distance introduced
 * by the tail bound
 *
 * delta_epsi : statistical distance introduced by
 * probability approximation
 */

// HELPER FUNCTIONS

/* /!\ Warning only for x64 */
extern __inline__ uint64_t rdtsc(void)
{
  uint64_t a, d;
  __asm__ volatile ("rdtsc" : "=a" (a), "=d" (d));
  return (d<<32) | a;
}

// Function to be used in a Newton-Raphson solver
struct funct
{
	funct(double const& target) : k(target){}
	std::tuple<double, double> operator()(double const& x)
	{
		return std::make_tuple(x*x - 2*log(x) - 1 - 2*k*log(2), 2*x-2/x);
	}
	private:
	double k;
};

inline
double newton_raphson(double k, double max_guess, int digits)
{
  unsigned max_counter = 1U<<15;
  std::tuple<double, double> values;
  double delta;
  double guess = max_guess;
  for (unsigned counter = 0 ; counter < max_counter; counter++)
  {
    values = funct(k)(guess);
    delta = std::get<0>(values)/std::get<1>(values);
    guess -= delta;
    if ( fabs(delta)/fabs(guess) < pow(10.0,-digits) ) break;
  }
  // In case there is a flat zone in the function
  while(0.95*guess*0.95*guess - 2*log(0.95*guess) - 1 - 2*k*log(2)>=0) guess*=0.95;
  // Test result
  if(guess*guess - 2*log(guess) - 1 - 2*k*log(2)<0)
  {
    std::cout << "FastGaussianNoise: WARNING Newton-Raphson failed the generator is NOT secure" << std::endl;
  }
  return guess;
}

// Constructors

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::FastGaussianNoise( double sigma,
		unsigned int security, unsigned int samples, double center_d /*=0*/, bool verbose /*=true*/):
	_sigma(sigma),
	_security(security),
  _samples(samples),
  _verbose(verbose)
{
  // Check template parameters
  check_template_params();

  //Center cannot be initialized before the constructor
  mpfr_init_set_d(_center, center_d, MPFR_RNDN);
  rounded_center = round(center_d);

  //Initialization functions
	init();
	precomputeBarrierValues();
	buildLookupTables();
}

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::FastGaussianNoise( double sigma,
		unsigned int security, unsigned int samples, mpfr_t center /*=0*/, bool verbose /*=true*/):
	_sigma(sigma),
	_security(security),
  _samples(samples),
  _verbose(verbose)
{
  // Check template parameters
  check_template_params();

  //Center cannot be initialized before the constructor
  mpfr_init_set(_center, center, MPFR_RNDN);
  rounded_center = mpfr_get_d(_center, MPFR_RNDN);

  //Initialization functions
	init();
	precomputeBarrierValues();
	buildLookupTables();
}

// Check template parameters
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::check_template_params()
{
  // Lookup tables can only have depth 1 or 2
  if (_lu_depth != 1 && _lu_depth != 2)
  {
    std::cout << "FastGaussianNoise: CRITICAL _lu_depth must be 1 or 2" << std::endl;
    exit(1);
  }

  // Lookup tables can only have uint8_t or uint16_t indexes
  if (typeid(in_class) == typeid(uint8_t))
    _lu_size = 1<<8;
  else if(typeid(in_class) == typeid(uint16_t))
    _lu_size = 1<<16;
  else
  {
    std::cout << "FastGaussianNoise: CRITICAL in_class must be uint8_t or uint16_t" << std::endl;
    exit(2);
  }
}

// Compute some values (precision, number of barriers and outputs)
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::init()
{
	double epsi, k;

	/* Lemma 1:
	 * tail_bound >= sqrt(1 + 2*log(tail_bound) + 2*k*log(2))
	 * IMPLIES delta_tailbound < 2**(-k)
   *
   * -epsi <= -k - log2(2 * tail_bound * sigma))
	 * IMPLIES 2 * tail_bound * sigma * 2**(-epsi) <= 2**(-k)
	 *
   * Lemma 2 (and Lemma 1):
	 * 2 * tail_bound * sigma * 2**(-epsi) <= 2**(-k)
	 * IMPLIES  delta_epsilon < 2**(-k)
   */

	// THUS setting
	k = _security + 1 + ceil(log(_samples)/log(2));
	// IMPLIES (delta_tailbound + delta_epsilon)*_samples < 2**(-_security)
	// We can thus generate vectors of _samples samples securely

  // To compute the tail bound we use Newton-Raphson with three digits of precision
	// WE HAVE tail_bound >= sqrt(1 + 2*log(tail_bound) + 2*k*log(2))
  // IS EQUIV TO tail_bound**2 -2*log(tail_bound) - 1- 2*k*log(2) >= 0
	int digits = 3;
#ifdef BOOST_RAPHSON
  double max_guess = 1+2*k*log(2);
  double min_guess = sqrt(1 + 2*k*log(2)), guess = min_guess;
	_tail_bound = boost::math::tools::newton_raphson_iterate(funct(k),guess, min_guess, max_guess, digits);
#else
  double min_guess = sqrt(1 + 2*k*log(2));
  _tail_bound = newton_raphson(k, min_guess, digits);
#endif
	// We now can compute the precision needed
	epsi = k + log2(2 * _tail_bound * _sigma);
	_bit_precision = ceil(epsi);
	_word_precision = ceil(_bit_precision/(8.0*sizeof(in_class)));
  // For the same cost we get a simpler situation and more precision
	_bit_precision = _word_precision*8*sizeof(in_class);
  // And the number of probabilities that must be computed
  // Only half of them are computed (others are symmetric)
  // but all values are stored to speedup noise generation
  _number_of_barriers = 1+2*ceil(_tail_bound*_sigma);
  if ( ((uint64_t)_number_of_barriers >> (sizeof(out_class) * 8 - 1)) != 0)
    std::cout << "FastGaussianNoise: WARNING out_class too small to contain some of the (signed) results" << std::endl;
  if ( ((uint64_t)_number_of_barriers >> (sizeof(int) * 8 - 1)) != 0)
    std::cout << "FastGaussianNoise: WARNING outputs are above 2**31, unexpected results" << std::endl;

  // Finally we precompute 1/(2*sigma**2) to accelerate things
	mpfr_inits2(_bit_precision, _const_sigma, nullptr);
  mpfr_set_d(_const_sigma, _sigma, MPFR_RNDN);
	mpfr_sqr(_const_sigma, _const_sigma, MPFR_RNDN);
	mpfr_mul_ui(_const_sigma, _const_sigma, 2, MPFR_RNDN);
	mpfr_ui_div(_const_sigma, 1, _const_sigma, MPFR_RNDN);

  // Give some feedback
  if (_verbose) std::cout << "FastGaussianNoise: " << _number_of_barriers <<
    " barriers with " << _bit_precision <<
    " bits of precision will be computed" << std::endl;
}

template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::precomputeBarrierValues()
{
  // Declare and init mpfr vars
  mpfr_t sum, tmp, tmp2;
  mpfr_t *mp_barriers;
  mpfr_inits2(_bit_precision, sum, tmp, tmp2, nullptr);

  // This var is used to export mpfr values
  mpz_t int_value;
  mpz_init2(int_value, _bit_precision);

  // Init SUM = \sum_{k=-ceil(tail_bound*sigma)}^{ceil(tail_bound*sigma)} exp(-(k+round(c)-c)^2/(2*sigma^2))
  // and compute on the loop with the barriers
  mpfr_set_ui(sum, 0, MPFR_RNDN);

  // Allocate memory for the barrier pointers
  barriers = (in_class **) malloc(_number_of_barriers*sizeof(in_class *));
  mp_barriers = (mpfr_t *) malloc(_number_of_barriers*sizeof(mpfr_t));

  // Now loop over the barriers
  for (int i = 0; i < (int)_number_of_barriers; i++)
  {
    // Init mpfr var
    mpfr_init2(mp_barriers[i], _bit_precision);

    // Compute the barrier value (without normalization)
    mpfr_set_si(tmp2, rounded_center+i-((int)_number_of_barriers-1)/2, MPFR_RNDN);
    nn_gaussian_law(tmp, tmp2);
    //mpfr_out_str(stdout, 10, 0, tmp2, MPFR_RNDN);
    //std::cout << std::endl;
    if (i==0) mpfr_set(mp_barriers[0], tmp, MPFR_RNDN);
    else
    {
      mpfr_add(mp_barriers[i], mp_barriers[i-1], tmp, MPFR_RNDN);
    }

    // Add the probability to the sum
    mpfr_add(sum, sum, tmp, MPFR_RNDN);
  }

  // Invert the sum and scale it
  mpfr_ui_div(sum, 1, sum, MPFR_RNDN);
  mpfr_set_ui(tmp, 2, MPFR_RNDN);
  mpfr_pow_ui(tmp, tmp, _bit_precision, MPFR_RNDN);
  mpfr_sub_ui(tmp, tmp, 1, MPFR_RNDN);
  mpfr_mul(sum, sum, tmp, MPFR_RNDN);

  // Now that we got the inverted sum normalize and export
  for (unsigned i = 0; i < _number_of_barriers; i++)
  {
    // Allocate space
    barriers[i] = (in_class *) calloc(_word_precision,sizeof(in_class));

    mpfr_mul(mp_barriers[i], mp_barriers[i], sum, MPFR_RNDN);
    mpfr_get_z(int_value, mp_barriers[i], MPFR_RNDN);
    mpz_export((void *) (barriers[i] + ((int)_word_precision -
           (int)ceil( (float)mpz_sizeinbase(int_value, 256)/sizeof(in_class) ))), nullptr, 1, sizeof(in_class), 0, 0, int_value);
#ifdef OUTPUT_BARRIERS
    mpz_out_str(stdout, 10, int_value);
    std::cout << " = Barriers[" << i << "] = " << std::endl;
    if (sizeof(in_class) == 1) for (unsigned j = 0 ; j < _word_precision; j++)
      printf("%.2x", barriers[i][j]);
    if (sizeof(in_class) == 2) for (unsigned j = 0 ; j < _word_precision; j++)
      printf("%.4x", barriers[i][j]);
    std::cout <<  std::endl;
#endif
    mpfr_clear(mp_barriers[i]);
  }
	mpfr_clears(sum, tmp, tmp2, nullptr);
  mpz_clear(int_value);
	mpfr_free_cache();
  free(mp_barriers);
}

//Build lookup tables used during noise generation
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::buildLookupTables()
{
	unsigned lu_index1 = 0, lu_index2 = 0;
  _flag_ctr1 = _flag_ctr2 = 0;

  // Allocate space for the lookup tables
  lu_table = new output_t[_lu_size]();
  if(_lu_depth == 2)
    lu_table2 = (output_t **) calloc(_lu_size,sizeof(output_t *));

	// We start building the first dimension of the lookup table
  // corresponding to the first in_class word of the barriers
	for (int64_t val = -((int)_number_of_barriers-1)/2 + rounded_center, b_index = 0; val <= ((int)_number_of_barriers-1)/2 + rounded_center && lu_index1 < _lu_size;)
  {

		while (lu_index1 < barriers[b_index][0] && lu_index1 < _lu_size)
    {
      lu_table[lu_index1].val = val;
			lu_index1++;
		}

		// Flag the entry
		lu_table[lu_index1].val = val;
		lu_table[lu_index1].flag = true;
    _flag_ctr1++;
		// If _lu_depth == 1 we have to list the barriers here
    if (_lu_depth == 1)
    {
#ifdef OUTPUT_LUT_FLAGS
      std::cout << "FastGaussianNoise: flagged lu_table["
       << lu_index1 << "] for barriers " << val;
#endif

      // Prepare the first element of the chained list of barrier
			lu_table[lu_index1].l_b_ptr.push_back(barriers[b_index++]);
      val++;
			// If more that one barrier is present, we add them to the chained list
			while ( (b_index<_number_of_barriers) && (lu_index1 == barriers[b_index][0]))
      {
			  lu_table[lu_index1].l_b_ptr.push_back(barriers[b_index]);
#ifdef OUTPUT_LUT_FLAGS
      std::cout << "FastGaussianNoise: flagged lu_table[" << lu_index1 << "] for barriers " << val;
#endif
				b_index++;
				val++;
			} // while
    } // if

    if (_lu_depth == 2)
    {
      // When we meet a barrier in an entry of the lu_table,
      // we build another lu_table inside that entry
		  // corresponding to the next in_class word of the barriers
		  lu_index2 = 0;
      lu_table2[lu_index1] = new output_t[_lu_size]();
		  while (lu_index2 < _lu_size)
      {
			  if(lu_index1 < barriers[b_index][0] || lu_index2 < barriers[b_index][1])
        {
          lu_table2[lu_index1][lu_index2].val = val;
        }
        else
        {
		      // If we are on a barrier
		      if (lu_index1 == barriers[b_index][0] && lu_index2 == barriers[b_index][1])
          {
			      // Flag the entry
			      lu_table2[lu_index1][lu_index2].val = val;
			      lu_table2[lu_index1][lu_index2].flag = true;
#ifdef OUTPUT_LUT_FLAGS
            std::cout << "FastGaussianNoise: flagged lu_table2[" << lu_index1 << "][" << lu_index2 << "] for barriers " << val;
#endif
            _flag_ctr2++;
			      // And prepare the first element of the chained list of barrier
			      lu_table2[lu_index1][lu_index2].l_b_ptr.push_back(barriers[b_index++]);
            val++;
			      // If more that one barrier is present, we add them to the chained list
			      while ( (b_index<_number_of_barriers) &&
                (lu_index1 == barriers[b_index][0]) &&
                (lu_index2 == barriers[b_index][1]) )
            {
				      lu_table2[lu_index1][lu_index2].l_b_ptr.push_back(barriers[b_index]);
#ifdef OUTPUT_LUT_FLAGS
            std::cout << " " << val;
#endif
				      b_index++;
				      val++;
			      } // while
#ifdef OUTPUT_LUT_FLAGS
            std::cout << std::endl;
#endif
          } // if
        } // else
        lu_index2++;
      } // while
		} // if

		lu_index1++;
	}
  // Give some feedback
  if (_verbose) std::cout << "FastGaussianNoise: Lookup tables built" << std::endl;
}

template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::getNoise(out_class* const rand_outdata, uint64_t rlen)
{
	uint64_t computed_outputs, innoise_bytesize, innoise_words, used_words;
	int64_t output;
  bool flagged;
	in_class *noise, *noise_init_ptr, input1, input2;
  float innoise_multiplier;

  // Expected number of input bytes per output byte. Lowering this
  // (e.g. by a factor .5) works but could lead to segfaults.
  if (_lu_depth == 1)
  {
    innoise_multiplier = 1.05 * ((float)(_lu_size - _flag_ctr1)/(float)_lu_size) + _word_precision * ((float)_flag_ctr1/_lu_size);
  }
  else // _lu_depth == 2
  {
    innoise_multiplier = 1.05 * ((float)(_lu_size - _flag_ctr1)/(float)_lu_size) + 2.0 * ((float)_flag_ctr1/(float)_lu_size) + _word_precision * ((float)_flag_ctr2/((float)_lu_size*_lu_size));
  }
  innoise_words = rlen * innoise_multiplier;
  innoise_bytesize = sizeof(in_class) * innoise_words;
	noise = noise_init_ptr = new in_class[innoise_words];
  used_words = 0;
  if (_verbose) std::cout << "FastGaussianNoise: Using " << " " <<innoise_multiplier*sizeof(in_class)*8/std::min(log2(_number_of_barriers),(double)sizeof(out_class)*8) << " input bits per output bit" << std::endl;

  // Count time for uniform noise generation
	uint64_t start = rdtsc();
	fastrandombytes((uint8_t*)noise, innoise_bytesize);
	uint64_t stop = rdtsc();

  // Give some feedback
	if (_verbose) printf("FastGaussianNoise: Uniform noise  cycles = %.2e bits = %.2e cycles/bit = %.2e\n", (float) stop - start, (float) innoise_bytesize * 8, (float)(stop-start)/(innoise_bytesize*8));

  // Loop until all the outputs have been generated
  computed_outputs = 0;
	while (computed_outputs < rlen )
  {
		input1 = *noise;
		flagged = lu_table[input1].flag;

    // If flagged we have to look at the next in_class word
    if (flagged)
    {
		  if (_lu_depth == 1)
      {
				output = lu_table[input1].val;
				for(in_class* b_ptr : lu_table[input1].l_b_ptr)
        {
					// If the barrier value is greater than the noise
					if(cmp(b_ptr, noise)==1) break;
					output++;
				}
        // We shift the noise pointer of word_precision minus 1
        // As there another byte shift later
				noise += _word_precision - 1;
				used_words += _word_precision - 1;

      }
      else // _lu_depth == 2
      {
			  input2 = *(noise+1);
			  flagged = lu_table2[input1][input2].flag;
        // If flagged again we compare using full precision the random value
        // with all barriers in the linked list of the lookup table
			  if (flagged)
        {
			    output = lu_table2[input1][input2].val;
				  for(in_class* b_ptr : lu_table2[input1][input2].l_b_ptr)
          {
					  // If the barrier value is greater than the noise
					  if(cmp(b_ptr, noise)==1) break;
					  output++;
				  }
          // We shift the noise pointer of word_precision minus 2
          // As there are two other one byte shifts later
				  noise += _word_precision - 2;
				  used_words += _word_precision - 2;
			  } // if
        else
        {
			    output = lu_table2[input1][input2].val;
        }
        noise++;
        used_words++;
      } // else
    }
    else
    {
		  output = lu_table[input1].val;
    }
		noise++;
    used_words++;
		// Add the obtained result to the list of outputs
		rand_outdata[computed_outputs++] = (out_class) output;

#ifdef UNITTEST_ONEMILLION
    if ( (output > _sigma * 6) || (output < -_sigma*6) )
    {
      std::cout << output << "FastGaussianNoise: Unit test failed, this should happen once in a million. Uniform input leading to this is  ";
      for (unsigned i = 0; i < _word_precision ; i++)
        printf("%.2x", *(noise-_word_precision+i));
      std::cout << std::endl;
    }
#endif

#if 1
    // If too much noise has been used regenerate it
    if ((used_words+_word_precision) >= innoise_words)
    {
      noise = noise_init_ptr;
      used_words = 0;
      if (_verbose) std::cout << "FastGaussianNoise: All the input bits have been used, regenerating them ..." << std::endl;

	    fastrandombytes((uint8_t*)noise, innoise_bytesize);
    }
#endif
	}
	delete[] noise_init_ptr;
}

/* Compare two arrays word by word.
 * return 1 if op1 > op2, 0 if equals and -1 if op1 < op2 */
template<class in_class, class out_class, unsigned _lu_depth>
inline int FastGaussianNoise<in_class, out_class, _lu_depth>::cmp(in_class *op1, in_class *op2)
{

	for (int i = 0; i < (int)_word_precision; i++)
  {

		if (op1[i] > op2[i]) return 1;

		else if (op1[i] < op2[i]) return -1;
	}
	return 0;
}

// Compute exp(-(x-center)^2/(2*sigma^2)) this is not normalized ! (hence the nn)
template<class in_class, class out_class, unsigned _lu_depth>
void  inline FastGaussianNoise<in_class, out_class, _lu_depth>::nn_gaussian_law(mpfr_t rop, const mpfr_t x)
{
	mpfr_sub(rop, x, _center, MPFR_RNDN);
	mpfr_sqr(rop, rop, MPFR_RNDN);
	mpfr_neg(rop, rop, MPFR_RNDN);
	mpfr_mul(rop, rop, _const_sigma, MPFR_RNDN);
	mpfr_exp(rop, rop, MPFR_RNDN);
}

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::~FastGaussianNoise()
{
  // Freed allocated memory for the barriers
  for (unsigned ctr = 0; ctr < _number_of_barriers; ctr++)
  {
    if (barriers[ctr] != nullptr) free(barriers[ctr]);
    barriers[ctr] = nullptr;
  }
  if (barriers != nullptr) free(barriers);
  barriers=nullptr;

  // Free other variables
  mpfr_clear(_const_sigma);
  mpfr_clear(_center);
  delete[](lu_table);
  if(_lu_depth == 2)
  {
    for (unsigned ctr = 0 ; ctr < _lu_size; ctr++)
    {
      if (lu_table2[ctr]!=nullptr) delete[](lu_table2[ctr]);
    }
    free(lu_table2);
  }
}

}  // namespace nfl

#endif


#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>

#include "../include/TriMesh.h"
#include "../include/utils.h"
#include "../include/lagrange.h"
#include "../include/geometry.h"
#include "../include/ConstructCurveMesh.h"
#include "../include/GetQuadraturePointsWeight2D.h"
#include "../include/solver.h"
#include "../include/euler.h"
#include "../include/Param.h"
#include "../include/Collective.h"
#include "../include/InvertMatrix.h"

using namespace std;
using namespace utils;
using namespace lagrange;

int main(int argc, char *argv[])
{
    namespace ublas = boost::numeric::ublas;

    Param param;
    // Set up the param struct
    param = ReadParamIn(string(argv[1]));
    TriMesh mesh(param.mesh_file);
    // Testing Calculate Residaul
    int p = param.order;
    int q = param.order_geo;
    int Np = int((p + 1) * (p + 2) / 2);
    TriMesh curved_mesh = mesh;
    string boundary_name="bottom";
    ConstructCurveMesh(mesh, curved_mesh, geometry::BumpFunction, boundary_name, q);

    ublas::vector<double> States (curved_mesh.num_element * Np * 4, 0.0);
    // Construct free stream state
    ublas::vector<double> u_free = euler::CalcFreeStreamState_2DEuler(param);
    for (int ielem = 0; ielem < curved_mesh.num_element; ielem++)
    {
        for (int ip = 0; ip < Np; ip++)
        {
            States(ielem * Np * 4 + ip * 4 + 0) = u_free(0);
            States(ielem * Np * 4 + ip * 4 + 1) = u_free(1);
            States(ielem * Np * 4 + ip * 4 + 2) = u_free(2);
            States(ielem * Np * 4 + ip * 4 + 3) = u_free(3);
        }
    }

    ResData resdata, resdata_postproc;
    solver::CalcResData(curved_mesh, p, resdata);
    if (p == 0)
    {
        solver::CalcResData(curved_mesh, 1, resdata_postproc);
    }else
    {
        solver::CalcResData(curved_mesh, p, resdata_postproc);
    }

    ublas::vector<double> dt(curved_mesh.E.size());
    // ublas::vector<double> Residual = solver::CalcResidual(curved_mesh, param, resdata, States, dt, p);
    // cout << setprecision(20) << ublas::norm_2(Residual) << endl;
    ublas::vector<ublas::matrix<double> > M = lagrange::ConstructMassMatrix(p, curved_mesh, resdata);
    ublas::vector<ublas::matrix<double> > invM = lagrange::CalcInvMassMatrix(M);
    int MAXITER = param.MAXITER;
    int converged = 0;
    ublas::vector<double> States_new (curved_mesh.num_element * Np * 4, 0.0);
    ofstream file_residual;
    file_residual.open("residual.log");
    for (int niter = 0; niter < MAXITER; niter++)
    {
        // cout << niter << endl;
        double norm_residual = 0.0;
        States_new = solver::TimeMarching_TVDRK3(curved_mesh, param, resdata, States, invM, p, converged, norm_residual);
	    if (niter % param.dnOutput == 0)
	    {
            std::cout << "NITER: " << niter << "\t" << "Residual Norm_Inf: ";
            cout.setf(ios::scientific, ios::floatfield);
            std::cout << setprecision(10) << norm_residual << std::endl;
        }
        file_residual << niter << "\t" << setprecision(20) << norm_residual << std::endl;
        States = States_new;
        if (converged)
           break;
    }
    file_residual.close();

    // PostProcessing the States, the output will be the global coordinate of the lagrange nodes in each element and the states on the nodes
    int Np_solution;
    if (p == 0)
        Np_solution = 3;
    else
        Np_solution = Np;
    ublas::vector<ublas::matrix<double> > Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 2, 0.0));
    ublas::vector<ublas::matrix<double> > State_on_Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 4, 0.0));
    solver::PostProc(curved_mesh, States, p, Nodes, State_on_Nodes);
    // Write the Nodes Coordinates and States into the file
    ofstream file_nodes, file_states, file_info;
    file_nodes.open("nodes.dat");
    file_states.open("states.dat");
    file_info.open("info.dat");
    for (int ielem = 0; ielem < curved_mesh.E.size(); ielem++)
    {
        for (int ip = 0; ip < Np_solution; ip++)
        {
            file_nodes  << Nodes(ielem)(ip, 0) << ' ' << Nodes(ielem)(ip, 1) << std::endl;
            file_states << State_on_Nodes(ielem)(ip, 0) << ' ' << State_on_Nodes(ielem)(ip, 1) << ' ' <<
                            State_on_Nodes(ielem)(ip, 2) << ' ' << State_on_Nodes(ielem)(ip, 3) << std::endl;
        }
    }
    file_info << curved_mesh.E.size() << ' ' << p << std::endl;
    file_nodes.close();
    file_states.close();
    file_info.close();

    return 0;
}


#include <ctime>
#include <string>
#include <algorithm>
#include <iostream>
#include <random>
#include <math.h>
#include <vtkNew.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkCellLocator.h>
#include <vtkDelaunay3D.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkIdFilter.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
#include <vtkIdList.h>
#include <vtkLinearSubdivisionFilter.h>
#include <vtkGenericCell.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkDataObject.h>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include "SHTools.h"

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

int main(){

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

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

    // Test function: 2*Y_1,-1 + Y_10 + 3*Y_11, so l=1, m=-1,0,1
    int lmax = std::floor( sqrt(N) - 1 );
    Eigen::VectorXd plms( (lmax + 1)*(lmax + 2)/2 );
    auto Plm = [&plms](const size_t l, const size_t m){
	return plms( l*(l+1)/2 + m );
    };

    // Map the point coordinates into a matrix
    Map3Xd pointsOrig((double_t*)pointCloud->GetPoints()
	    ->GetData()->GetVoidPointer(0),3,N);
    Matrix3Xd points(3,N);
    points = pointsOrig;

    //**********************************************************************//
    // Create the Gauss-Lengendre grid
    int nlat, nlong;
    Eigen::VectorXd latglq(lmax + 1);
    Eigen::VectorXd longlq(2*lmax + 1);
    glqgridcoord_wrapper_(latglq.data(), longlq.data(), &lmax, &nlat, &nlong);
    auto numQ = nlat*nlong;

    Eigen::VectorXd gridglq((lmax + 1)*(2*lmax + 1));
    Eigen::VectorXd plx( (lmax + 1)*(lmax + 1)*(lmax + 2)/2 );
    Eigen::VectorXd w( lmax + 1 ), zero( lmax + 1 );
    Eigen::VectorXd cilm( 2*(lmax + 1)*(lmax + 1) );
    Eigen::VectorXd pspectrum( lmax + 1 );

    // Pre-compute all the matrices needed for expansion
    shglq_wrapper_(&lmax, zero.data(), w.data(), plx.data() );

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

    size_t index = 0;
    for( auto ip = 0; ip < nlong; ++ip){
	auto phi = longlq(ip)*M_PI/180.0;
	auto sin_p = std::sin(phi);
	auto cos_p = std::cos(phi);
	for( auto it = 0; it < nlat; ++it){
	    auto theta = (90.0 - latglq(it))*M_PI/180.0;
	    auto sin_t = std::sin(theta);
	    Q0.col(index++) << sin_t*cos_p, sin_t*sin_p, std::cos(theta);
	}
    }

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

    //**********************************************************************//

    VectorXd finput(N);
    // A function to interpolate data to the quadrature points
    auto interpolate = [&points, &Q0, &gridglq, &finput](const size_t i,
	    const size_t j, const size_t k, const size_t q){
	Eigen::Vector3d v0 = points.col(i);
	Eigen::Vector3d v1 = points.col(j);
	Eigen::Vector3d v2 = points.col(k);
	Eigen::Vector3d qp = Q0.col(q);
	auto A0 = ((v1-qp).cross((v2-qp))).norm();
	auto A1 = ((v2-qp).cross((v0-qp))).norm();
	auto A2 = ((v0-qp).cross((v1-qp))).norm();
	auto A = A0 + A1 + A2;
	gridglq(q) = (A0/A)*finput(i) + (A1/A)*finput(j) + (A2/A)*finput(k);
    };

    clock_t t = clock();
    for( auto z = 0; z < 1000; ++z){

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

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

	// Calculate the function to be expanded using Spherical Harmonics
	for(auto i = 0; i < N; ++i){
	    Eigen::Vector3d q;
	    q =  points.col(i);
	    double_t x = q(0);
	    double_t y = q(1);
	    double_t z = q(2);
	    plmon_wrapper_(plms.data(), &lmax, &z);
	    auto phi = std::atan2(y,x);
	    finput(i) = 2*Plm(1,1)*std::sin(phi) + Plm(1,0) +
		3*Plm(1,1)*std::cos(phi);
	}

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

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

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

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

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

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

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

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

	/*
	// Write the triangulation to file
	vtkNew<vtkPolyData> sphere;
	vtkNew<vtkCellArray> sphereTri;
	vtkNew<vtkPoints> spherePts;
	for(auto zz = 0; zz < N; ++zz){
	Vector3d pp = points.col(zz);
	spherePts->InsertNextPoint( &pp(0) );
	}
	for( auto fc = dt.all_faces_begin(); fc != dt.all_faces_end(); ++fc ){
	sphereTri->InsertNextCell(3);
	for( auto zz = 2; zz >= 0; --zz ){
	auto vid = dt.is_infinite( fc->vertex( zz ) )? 0 :
	fc->vertex(zz)->info();
	sphereTri->InsertCellPoint(vid);
	}
	}
	sphere->SetPoints( spherePts );
	sphere->SetPolys( sphereTri );
	vtkNew<vtkPolyDataWriter> wr;
	wr->SetInputData( sphere );
	wr->SetFileName( "UnitSphere.vtk" );
	wr->Write();
	*/

	// Rotate and project the quadrature points
	Qr = rotMat*Q0; // Rotate the quadrature points

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

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

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

	// Locate the quadrature points using stereographic triangulation
	for( auto j=0; j < numQ; ++j ){
	    auto query = Point( Qsp(0,j), Qsp(1,j) );
	    Delaunay::Locate_type lt;
	    int li;
	    Face_handle face = dt.locate( query, lt, li );
	    switch(lt){
		case Delaunay::FACE:
		    {
			auto id0 = face->vertex(0)->info();
			auto id1 = face->vertex(1)->info();
			auto id2 = face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		case Delaunay::EDGE:
		    {
			auto id1 = face->vertex( (li + 1)%3 )->info();
			auto id2 = face->vertex( (li + 2)%3 )->info();
			Eigen::Vector3d v1, v2, qp;
			v1 = points.col(id1);
			v2 = points.col(id2);
			qp = Q0.col(j);
			double_t ratio = (qp - v1).norm()/(qp - v2).norm();
			gridglq(j) = (finput(id1) +
				ratio*finput(id2))/(1 + ratio);
			break;
		    }
		case Delaunay::VERTEX:
		    gridglq(j) = finput( face->vertex( li )->info() );
		    break;
		case Delaunay::OUTSIDE_CONVEX_HULL:
		    {
			Eigen::Vector3d v0, v1, v2;
			auto id0 = dt.is_infinite(face->vertex(0))?
			    0 : face->vertex(0)->info();
			auto id1 = dt.is_infinite(face->vertex(1))?
			    0 : face->vertex(1)->info();
			auto id2 = dt.is_infinite(face->vertex(2))?
			    0 : face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		default:
		    std::cout<< "Quadrature point " << j << " not found!"
			<< std::endl;
	    }
	}

	// Expand using Gauss-Legendre Quadrature and get the power spectrum
	shexpandglq_wrapper_( cilm.data(), &lmax, gridglq.data(), w.data(),
		plx.data());
	shpowerspectrum_wrapper_( cilm.data(), &lmax, pspectrum.data() );
    }
    t = clock() - t;
    std::cout<< "Time for 1000 steps GLQ = " << ((float)t)/CLOCKS_PER_SEC
	<< std::endl;

    // Cross-check
    Eigen::VectorXd gridglq_out( (lmax + 1)*(2*lmax + 1) );
    makegridglq_wrapper_(gridglq_out.data(), cilm.data(), &lmax, plx.data());
    std::cout<< "Error norm of GLQ = " << (gridglq - gridglq_out).norm()
	<< std::endl;

    // Print the coefficients
    std::cout<< "l\tm\tAlm_GLQ" << std::endl;
    for( int l = 0; l <= lmax; ++l ){
	for( int m = -l; m <=l; ++m ){
	    int i = m < 0? 1 : 0;
	    int n = m < 0? -m: m;
	    std::cout<< l << "\t" << m << "\t"
		<< cilm( i + 2*(l + n*(lmax + 1)) )<< "\t"
		<< std::endl;
	}
    }
    return 0;
}


// Copyright 2002 - 2008, 2010, 2011 National Technology Engineering
// Solutions of Sandia, LLC (NTESS). Under the terms of Contract
// DE-NA0003525 with NTESS, the U.S. Government retains certain rights
// in this software.
//
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#ifndef percept_math_TransformationMatrix_hpp
#define percept_math_TransformationMatrix_hpp

#include <stdexcept>
#include <sstream>
#include <vector>
#include <cmath>
#include <iostream>
#include <string>
#include <typeinfo>

#include <math.h>

#include <Teuchos_ScalarTraits.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <percept/Stacktrace.hpp>
#include <percept/Util.hpp>

namespace percept {

  namespace ublas =  boost::numeric::ublas;

  class Math
  {
  public:

    typedef ublas::c_matrix<double,3,3> Matrix;

    typedef ublas::c_vector<double,3> Vector;

    typedef ublas::c_vector<double,3> ubvec;

    class MyVector : public ubvec
    {
    public:

      MyVector(double x=0.0) : ubvec()
      {
        (*this)(0) = x;
        (*this)(1) = x;
        (*this)(2) = x;
      }

      MyVector(double *x) : ubvec()
      {
        (*this)(0) = x[0];
        (*this)(1) = x[1];
        (*this)(2) = x[2];
      }
      //Vector(const ubvec& v) : ubvec(v) {}

      MyVector& operator=(const ubvec& v)
      {
        //ubvec& v0 = ubvec::operator=(v);
        (*this)(0) = v(0);
        (*this)(1) = v(1);
        (*this)(2) = v(2);
        return *this;
      }

      //v = ublas::prod(m_rotMat, v);

    };

    static double my_abs_hi(double x, double eps=1.e-6) { return std::sqrt(x*x + eps*eps); }
    static double my_min_hi(double x, double y, double eps=1.e-6) { return 0.5*(x+y - my_abs_hi(x-y,eps)); }
    static double my_max_hi(double x, double y, double eps=1.e-6) { return 0.5*(x+y + my_abs_hi(x-y,eps)); }
    // heavyside
    static double heavy_smooth(double x, double x0, double eps=1.e-6) { return 1./(1.+std::exp(-2.0*(x-x0)/eps)); }

    static double random01()
    {
      double rnd = Teuchos::ScalarTraits<double>::random();
      return (rnd+1.0)/2.0;
    }

    static Matrix rotationMatrix(int axis, double angle_degrees)
    {
      Matrix rm;
      rm.clear();
      double theta = M_PI * angle_degrees / 180.0;
      double cost = std::cos(theta);
      double sint = std::sin(theta);
      if (axis == 2)
        {
          rm(0,0) = cost; rm(0,1) = -sint;
          rm(1,0) = sint; rm(1,1) = cost;
          rm(2,2) = 1.0;
        }
      else if (axis == 1)
        {
          rm(0,0) = cost; rm(0,2) = -sint;
          rm(2,0) = sint; rm(2,2) = cost;
          rm(1,1) = 1.0;
        }
      else if (axis == 0)
        {
          rm(1,1) = cost; rm(1,2) = -sint;
          rm(2,1) = sint; rm(2,2) = cost;
          rm(0,0) = 1.0;
        }
      return rm;
    }

    static Matrix scalingMatrix(int axis, double scale)
    {
      Matrix sm;
      sm.clear();
      sm(0,0)=1.0;
      sm(1,1)=1.0;
      sm(2,2)=1.0;
      sm(axis,axis)=scale;
      return sm;
    }

    static Matrix scalingMatrix( double scale)
    {
      Matrix sm;
      sm.clear();
      sm(0,0)=scale;
      sm(1,1)=scale;
      sm(2,2)=scale;
      return sm;
    }

    static double norm_3d(const double * vec)
    {
      double norm = std::sqrt(vec[0]*vec[0]+
                              vec[1]*vec[1]+
                              vec[2]*vec[2]);
      return norm;
    }

    static void normalize_3d(double * vec)
    {
      double norm = norm_3d(vec);
      if (norm > 0.0)
        {
          vec[0] /= norm;
          vec[1] /= norm;
          vec[2] /= norm;
        }
      else
        {
          std::cout << "norm= " << vec[0] << ", " << vec[1] << ", " << vec[2] << "\n"
                    << Stacktrace::demangled_stacktrace(30) << std::endl;
          VERIFY_MSG("bad norm");
        }
    }
    static void cross_3d(const double * a, const double * b, double * axb)
    {
      axb[0] = (a[1]*b[2]-a[2]*b[1]);
      axb[1] = -(a[0]*b[2]-a[2]*b[0]);
      axb[2] = (a[0]*b[1]-a[1]*b[0]);
    }
    static double dot_3d(const double *a, const double *b)
    {
      return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
    }
    static double distance_squared_3d(const double *a, const double *b)
    {
      return (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]);
    }
    static double distance_3d(const double *a, const double *b)
    {
      return std::sqrt(distance_squared_3d(a,b));
    }
    static void copy_3d(double *a, const double *b)
    {
      a[0] = b[0];
      a[1] = b[1];
      a[2] = b[2];
    }
    static void copy_2d(double *a, const double *b)
    {
      a[0] = b[0];
      a[1] = b[1];
    }
    static std::string print_3d(const double *a, int prec=6)
    {
      std::ostringstream ostr;
      ostr << std::setprecision(prec);
      ostr << " " << a[0] << ", " << a[1] << ", " << a[2];
      return ostr.str();
    }
    static std::string print_2d(const double *a, int prec=6)
    {
      std::ostringstream ostr;
      ostr << std::setprecision(prec);
      ostr << " " << a[0] << ", " << a[1];
      return ostr.str();
    }
    static void subtract_3d(double *a, const double *b)
    {
      a[0] -= b[0];
      a[1] -= b[1];
      a[2] -= b[2];
    }

    static double project_to_line_3d(const double *a, const double *b, double *xyz, double& u)
    {
      double xma[3] = {0,0,0}, bma[3] = {0,0,0}, bman[3] = {0,0,0}, xyz_in[3] = {0,0,0};
      copy_3d(xyz_in, xyz);
      copy_3d(bma, b);
      subtract_3d(bma, a);
      copy_3d(xma, xyz);
      subtract_3d(xma, a);
      copy_3d(bman, bma);
      normalize_3d(bman);
      double dd = dot_3d(xma, bman);
      double ba_len = distance_3d(a, b);
      if (dd < 0.0)
        {
          u = 0.0;
          copy_3d(xyz, a);
        }
      else if (dd > ba_len)
        {
          u = 1.0;
          copy_3d(xyz, b);
        }
      else
        {
          u = dd/ba_len;
          xyz[0] = a[0] + u*(b[0] - a[0]);
          xyz[1] = a[1] + u*(b[1] - a[1]);
          xyz[2] = a[2] + u*(b[2] - a[2]);
        }
      return distance_3d(xyz, xyz_in);
    }
  };

  inline Math::Vector operator*(Math::Matrix& mat, Math::Vector& vec) { return ublas::prod(mat, vec); }
  inline Math::Matrix operator*(Math::Matrix& mat, Math::Matrix& mat2) { return ublas::prod(mat, mat2); }

}

#endif


#ifndef NBT_DYNAMICS_ENGINE_HPP
#define NBT_DYNAMICS_ENGINE_HPP

#include <Eigen>
#include "octree.hpp"
#include "units.hpp"

/**
 * Abstract class for Dynamics engines.
 * Used to update the accelerations of a set of
 * particles based on their positions and masses.
 */
class DynamicsEngine {
    public:
        /**
         * @brief Recalculates acceleration matrix using object positions and masses
         *
         * @param a Acceleration matrix
         * @param x Position matrix
         * @param m Mass vector
         */
        virtual void updateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                         const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                         const Eigen::Ref<const Eigen::RowVectorXd>& m) = 0;

        /**
         * @brief Updates the individual acceleration between particles i and j
         *
         * @param a_i Acceleration of i
         * @param x_i Position of i
         * @param x_j Position of j
         * @param m_i Mass of i
         * @param m_j Mass of j
         */
        virtual void pairAcceleration(Eigen::Ref<Eigen::Vector3d> a_i,
                                      const Eigen::Vector3d& x_i,
                                      const Eigen::Vector3d& x_j,
                                      double m_i,
                                      double m_j) = 0;

        /**
         * @brief Returns the potential energy of a system based on particle positions and masses.
         *
         * @param x Position matrix
         * @param m Mass vector
         *
         * @return double
         */
        double totalPotentialEnergy(const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                    const Eigen::Ref<const Eigen::RowVectorXd>& m);

        /**
         * @brief Returns the potential energy between a pair of particles i and j.
         *        Default abstract class method returns zero for everything, can be overriden
         *        in custom subclasses.
         *
         * @param x_i Position of i
         * @param x_j Position of j
         * @param m_i Mass of i
         * @param m_j Mass of j
         * @return double
         */
        virtual double pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                           const Eigen::Vector3d& x_j,
                                           double m_i,
                                           double m_j);
};

/* ABSTRACT CLASSES FOR N-BODY UPDATE METHODS */
/* Implements DynamicsEngine::updateAccelerations() */

/**
 * Computes force directly between every pair of objects. O(n^2)
 */
class Abstract_Direct: public DynamicsEngine {
    public:
        /**
         * @brief Computes force between each pair of particles individually.
         *
         * @param a
         * @param x
         * @param m
         */
        void updateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                 const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                 const Eigen::Ref<const Eigen::RowVectorXd>& m) override;
};

/**
 * A multithreaded Barnes-Hut force computer. O(nlogn)
 */
class Abstract_BarnesHut: public DynamicsEngine {
    public:
        OctreeNode* root;       //!< Root node of the Barnes-Hut tree.
        const double theta;     //!< Theta parameter for Barnes-Hut algorithm

        /**
         * @brief Construct a Abstract_Direct object.
         *
         * @param theta Theta parameter for the Barnes-Hut algorithm
         */
        Abstract_BarnesHut(double theta);

        /**
         * @brief Destroy the Abstract_BarnesHut object
         */
        ~Abstract_BarnesHut();

        /**
         * @brief Function for threads. Computes the acceleration of bodies from indices startIdx to endIdx (endIdx not included)
         *
         * @param a
         * @param x
         * @param m
         * @param startIdx
         * @param endIdx
         */
        void threadUpdateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                      const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                      const Eigen::Ref<const Eigen::RowVectorXd>& m,
                                      int startIdx,
                                      int endIdx);

        /**
         * @brief Computes forces acting on each particle using the Barnes-Hut algorithm
         *
         * @param a
         * @param x
         * @param m
         */
        void updateAccelerations(Eigen::Ref<Eigen::Matrix3Xd> a,
                                const Eigen::Ref<const Eigen::Matrix3Xd>& x,
                                const Eigen::Ref<const Eigen::RowVectorXd>& m) override;
};

/* SPECIFIC DYNAMICS ENGINES */
/* Implement DynamicsEngine::pairAccleration() */

class Gravitational_Direct: public Abstract_Direct {
    public:
        const double G;
        const double softening;

        /**
         * @brief Construct a new Gravitational_Direct object
         *
         * @param softening Softening parameter
         * @param l Unit of length
         * @param m Unit of mass
         * @param t Unit of time
         */
        Gravitational_Direct(double softening, unit_t l = Unit::Meter, unit_t m = Unit::Kilogram, unit_t t = Unit::Second);

        /**
         * @brief Computes acceleration from Newtonian gravitation between two particles i and j
         *
         * @param a_i
         * @param x_i
         * @param x_j
         * @param m_i
         * @param m_j
         */
        void pairAcceleration(Eigen::Ref<Eigen::Vector3d> a_i,
                              const Eigen::Vector3d& x_i,
                              const Eigen::Vector3d& x_j,
                              double m_i,
                              double m_j) override;

         /**
         * @brief Returns the gravitational potential energy between a pair of particles i and j.
         *
         * @param x_i Position of i
         * @param x_j Position of j
         * @param m_i Mass of i
         * @param m_j Mass of j
         * @return double
         */
        double pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                   const Eigen::Vector3d& x_j,
                                   double m_i,
                                   double m_j) override;
};

class Gravitational_BarnesHut: public Abstract_BarnesHut {
    public:
        const double G;
        const double softening;

        /**
         * @brief Construct a new Gravitational_Direct object
         *
         * @param theta Theta parameter for the Barnes-Hut algorithm
         * @param softening Softening parameter
         * @param l Unit of length
         * @param m Unit of mass
         * @param t Unit of time
         */
        Gravitational_BarnesHut(double theta, double softening, unit_t l = Unit::Meter, unit_t m = Unit::Kilogram, unit_t t = Unit::Second);

        /**
         * @brief Computes acceleration from Newtonian gravitation between two particles i and j
         *
         * @param a_i
         * @param x_i
         * @param x_j
         * @param m_i
         * @param m_j
         */
        void pairAcceleration(Eigen::Ref<Eigen::Vector3d> a_i,
                              const Eigen::Vector3d& x_i,
                              const Eigen::Vector3d& x_j,
                              double m_i,
                              double m_j) override;

         /**
         * @brief Returns the gravitational potential energy between a pair of particles i and j.
         *
         * @param x_i Position of i
         * @param x_j Position of j
         * @param m_i Mass of i
         * @param m_j Mass of j
         * @return double
         */
        double pairPotentialEnergy(const Eigen::Vector3d& x_i,
                                   const Eigen::Vector3d& x_j,
                                   double m_i,
                                   double m_j) override;
};

#endif


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

/** \file
 *
 * A simple example that demonstrates the usage of mpi2. Spawns a couple of tasks that each run
 * Monte Carlo integration to estimate the value of pi; then averages and outputs the result.
 */
#include <iostream>
#include <map>
#include <numeric>
#include <queue>
#include <set>

#include <boost/foreach.hpp>

#include <util/io.h>
#include <util/random.h>

#include <mpi2/mpi2.h>

using namespace std;
using namespace mpi2;
using namespace rg;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

/** Task that computes pi via Monte Carlo integration */
struct PiTask {
	static inline string id() { return "PiTask"; }
	static inline void run(Channel ch, TaskInfo info) {
		// receive the seed for the random number generator
		Random32 random = getSeed(ch);

		// receive the number of iterations to perform
		unsigned n;
		ch.recv(n);

		// run Monte Carlo integration
		LOG4CXX_INFO(logger, "Task " << info.groupId() << ": Generating " << n
				<< " MC samples");
		int count = 0;
		for (unsigned i=0; i<n; i++) {
			double x = random.nextDouble(); // actually need [-1,1]; but [0,1] is OK since we square
			double y = random.nextDouble();
			if (x*x+ y*y < 1) count++;
		}
		double est = (double)count/n * 4.; // area unit circle = pi; area square = 4

		// send back the results
		LOG4CXX_INFO(logger, "Task " << info.groupId() << ": Sending " << est);
		ch.send(est);
	}
};

int main(int argc, char* argv[]) {
	// initialize mpi2
	boost::mpi::communicator& world = mpi2init(argc, argv);

	// register PiTask (this is required to be able to actually run it!)
	TaskManager& tm = TaskManager::getInstance();
	registerTask<PiTask>();

	// fire up task managers (this blocks on all but the root node)
	mpi2start();

	// main driver; only executed at root rank
	if (world.rank() == 0) {
		// spawn 3 copies of PiTask at each rank
		std::vector<Channel> channels;
		int threads = 3;
		tm.spawnAll<PiTask>(threads, channels);

		// send them each a different random number seed
		Random32 random; // note: this takes a default seed (not randomized!)
		seed(channels, random);

		// send them the number of samples to take
		sendAll(channels, 100000); // each thread computes 100000 samples

		// receive the results
		std::vector<double> results(channels.size());
		recvAll(channels, results);

		// aggregate and output the result
		double pi = std::accumulate(results.begin(), results.end(), 0.) / channels.size();
		LOG4CXX_INFO(logger, "Estimate of pi: " << pi);
	}

	// shut down task managers
	mpi2stop();

	// shut down mpi2 and mpi
	mpi2finalize();

	return 0;
}


/**
*  @file    mdm_ParamSummaryStats.cxx
*  @brief   Implementation of mdm_ParamSummaryStats class
*
*  Original author MA Berks 13 Nov 2020
*  (c) Copyright QBI, University of Manchester 2020
*/

#ifndef MDM_API_EXPORTS
#define MDM_API_EXPORTS
#endif // !MDM_API_EXPORTS

#include "mdm_ParamSummaryStats.h"

#include <cassert>
#include <cmath>
#include <fstream>
#include <algorithm>
#include <boost/format.hpp>
#include <madym/mdm_exception.h>

const std::vector<std::string> mdm_ParamSummaryStats::headers_ = {
	"param",
	"n_valid",
	"n_invalid",
	"mean",
	"stddev",
	"median",
	"lowerQ",
	"upperQ",
	"iqr"
};

//!Helper function to compute percentile of vector of values
double percentile(const std::vector<double> &A, const double &prct)
{
	//We only ever call this function for 25,50 and 100, but for completeness
	assert(prct >= 0 && prct <= 100.0);
	if (prct == 0)
		return A[0];
	if (prct == 100)
		return A.back();

	//See https://en.wikipedia.org/wiki/Quartile#Method_4
	double n1 = (double)A.size() + 1;
	double pn1 = n1 * prct / 100.00;
	size_t k = size_t(std::floor(pn1));

	//If size A < 3, then k = 0 for prctile(25) and k = 3 for prctile(75)
	if (!k)
		return A[0];
	else if (k == A.size())
		return A.back();

	double alpha = pn1 - k;
	return A[k - 1] + alpha * (A[k] - A[k - 1]);//Remember indexing starts at 0 in cxx

	/*double x = (double)A.size() * prct / 100.00;
	int i2 = std::floor(x);
	int i1 = i2 - 1;
	double m1 = 0.5 - (x - (double)i2);
	double m2 = 1.0 - m1;
	return A[i1] * m1 + A[i2] * m2;*/
}

//
MDM_API mdm_ParamSummaryStats::mdm_ParamSummaryStats()
	:
	roiIdx_(0)
{

}

//
MDM_API mdm_ParamSummaryStats::~mdm_ParamSummaryStats()
{
	closeNewStatsFile();
}

//!Set ROI
MDM_API void mdm_ParamSummaryStats::setROI(const mdm_Image3D& roi)
{
	//Don't store the image, just save the non-zero IDX
	roiIdx_.clear();
	for (int i = 0; i < roi.numVoxels(); i++)
	{
		if (roi.voxel(i))
			roiIdx_.push_back(i);
	}

	xmm_ = roi.info().Xmm.value();
	ymm_ = roi.info().Ymm.value();
	zmm_ = roi.info().Zmm.value();
}

//!Make output stats for an image given an ROI
MDM_API void mdm_ParamSummaryStats::makeStats(const mdm_Image3D& img, const std::string &paramName,
	const double scale, bool invert)
{
	//Set param name
	stats_.paramName_ = paramName;

	//Check ROI idx are set
	checkIdx(img);

	//Reset the stats
	stats_.reset();

	// loop image extracting parameter values and taking running sum
	double ROI_sum = 0.0;
	double ROI_sumsq = 0.0;

	std::vector<double>  paramVals;
	for (const auto &idx : roiIdx_)
	{
		// Get value from image
		double voxValue = scale * img.voxel(idx);

		if (std::isnan(voxValue))
		{
			stats_.invalidVoxels_++;
			continue;
		}

		if (invert)
		{
			//Can't invert negative values, just skip
			if (voxValue <= 0.0)
			{
				stats_.invalidVoxels_++;
				continue;
			}

			else
				voxValue = 1 / voxValue;
		}

		//Increment sums and save value
		ROI_sum += voxValue;
		ROI_sumsq += voxValue * voxValue;
		paramVals.push_back(voxValue);
		stats_.validVoxels_++;
	}

	//If we haven't got any voxels, return
	if (!stats_.validVoxels_)
		return;

	else if (stats_.validVoxels_ == 1)
	{
		stats_.mean_ = paramVals[0];
		stats_.median_ = paramVals[0];
		stats_.lowerQ_ = paramVals[0];
		stats_.upperQ_ = paramVals[0];

		//std and iqr are 0
		return;
	}

	//Compute summary stats from data values
	//Sort the data arrays
	std::sort(paramVals.begin(), paramVals.end());

	//Compute mean and std
	double validVoxels = (double)stats_.validVoxels_;
	stats_.mean_ = ROI_sum / validVoxels;

	// This is the unbiased estimator for the sd of the parent distribution ... i.e. strong gaussian assumption
	stats_.stddev_ = sqrt((ROI_sumsq - ROI_sum * ROI_sum /
		validVoxels) / (validVoxels - 1));

	//Compute median and IQR
	stats_.median_ = percentile(paramVals, 50);
	stats_.lowerQ_ = percentile(paramVals, 25);
	stats_.upperQ_ = percentile(paramVals, 75);
	stats_.iqr_ = stats_.upperQ_ - stats_.lowerQ_;
}

MDM_API const mdm_ParamSummaryStats::SummaryStats& mdm_ParamSummaryStats::stats() const
{
	return stats_;
}

//
MDM_API void mdm_ParamSummaryStats::writeROISummary(const std::string &roiFile)
{
	std::ofstream roiStream(roiFile);
	if (!roiStream)
	  throw mdm_exception(__func__, boost::format("Failed to open stats file %1%") % roiFile);

	double nVoxels = (double)roiIdx_.size();
	roiStream <<
		"number_of_voxels = " << nVoxels <<
		" volume = " << nVoxels * xmm_ * ymm_ * zmm_;
}

//
MDM_API void mdm_ParamSummaryStats::openNewStatsFile(const std::string &statsFile)
{
	statsOStream_.open(statsFile, std::ios::out);
	if (!statsOStream_)
    throw mdm_exception(__func__, boost::format("Failed to open stats file %1%") % statsFile);

	//Write stats headers
	for (const auto hdr : headers_)
		statsOStream_ << hdr << ",";

	statsOStream_ << "\n";

}

//
MDM_API void mdm_ParamSummaryStats::closeNewStatsFile()
{
	if (statsOStream_)
		statsOStream_.close();

}

//
MDM_API void mdm_ParamSummaryStats::writeStats()
{
	if (!statsOStream_.is_open())
    throw mdm_exception(__func__,
      "Tried to write stats, but no stats file open");

	statsOStream_ <<
		stats_.paramName_ << "," <<
		stats_.validVoxels_ << "," <<
		stats_.invalidVoxels_ << "," <<
		stats_.mean_ << "," <<
		stats_.stddev_ << "," <<
		stats_.median_ << ","<<
		stats_.lowerQ_ << "," <<
		stats_.upperQ_ << "," <<
		stats_.iqr_ << ",\n";

}

//
MDM_API void mdm_ParamSummaryStats::openStatsFile(const std::string &statsFile)
{
	statsIStream_.open(statsFile, std::ios::in);
	if (!statsIStream_)
    throw mdm_exception(__func__, boost::format("Failed to open stats file %1%") % statsFile);

	//Write stats headers
	std::string hdr_in;
	for (const auto hdr : headers_)
	{
		std::getline(statsIStream_, hdr_in, ',');
		if (hdr_in != hdr)
      throw mdm_exception(__func__, boost::format("Incorrect headers in %1%, cannot open") % statsFile);
	}
	//Get rid of final comma to \n part
	std::getline(statsIStream_, hdr_in);
}

//
MDM_API void mdm_ParamSummaryStats::closeStatsFile()
{
	if (statsIStream_)
		statsIStream_.close();
}

//
MDM_API void mdm_ParamSummaryStats::readStats()
{
	if (!statsIStream_.is_open())
    throw mdm_exception(__func__,
      "Tried to read stats, but no stats file open");

	std::getline(statsIStream_, stats_.paramName_, ',');

	std::string val;
	std::getline(statsIStream_, val, ',');
	stats_.validVoxels_ = std::stoi(val);

	std::getline(statsIStream_, val, ',');
	stats_.invalidVoxels_ = std::stoi(val);

	std::getline(statsIStream_, val, ',');
	stats_.mean_ = std::stod(val);

	std::getline(statsIStream_, val, ',');
	stats_.stddev_ = std::stod(val);

	std::getline(statsIStream_, val, ',');
	stats_.median_ = std::stod(val);

	std::getline(statsIStream_, val, ',');
	stats_.lowerQ_ = std::stod(val);

	std::getline(statsIStream_, val, ',');
	stats_.upperQ_ = std::stod(val);

	std::getline(statsIStream_, val, ',');
	stats_.iqr_ = std::stod(val);

	//Get rid of final comma to \n part
	std::getline(statsIStream_, val);
}

//------------------------------------------------------------------------
// Private
//------------------------------------------------------------------------
void mdm_ParamSummaryStats::checkIdx(const mdm_Image3D& img)
{
	if (roiIdx_.empty())
	{
		for (int i = 0; i < img.numVoxels(); i++)
			roiIdx_.push_back(i);
	}

	xmm_ = img.info().Xmm.value();
	ymm_ = img.info().Ymm.value();
	zmm_ = img.info().Zmm.value();
}


/**
 * @date Tue Jan 10 21:14 2016 +0100
 * @author Tiago de Freitas Pereira <tiago.pereira@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/shared_array.hpp>

#include <bob.math/gsvd.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>
#include <bob.math/linear.h>

// Declaration of the external LAPACK function (Divide and conquer SVD)
extern "C" void dggsvd3_(const char *jobu,
                         const char *jobv,
                         const char *jobq,
                         const int *M,
                         const int *N,
                         const int *P,
                         const int *K,
                         const int *L,
                         double *A,
                         const int *lda,
                         double *B,
                         const int *ldb,
                         double *alpha,
                         double *beta,
                         double *U,
                         const int *ldu,
                         double *V,
                         const int *ldv,
                         double *Q,
                         const int *ldq,
                         double *work,
                         const int *lwork,
                         int *iwork,
                         int *info);

void bob::math::gsvd( blitz::Array<double,2>& A,
                      blitz::Array<double,2>& B,
                      blitz::Array<double,2>& U,
                      blitz::Array<double,2>& V,
                      blitz::Array<double,2>& zeroR,
                      blitz::Array<double,2>& Q,
                      blitz::Array<double,2>& X,
                      blitz::Array<double,2>& C,
                      blitz::Array<double,2>& S)
{
  const char jobu = 'U';
  const char jobv = 'V';
  const char jobq = 'Q';

  // Size variables
  const int M = A.extent(0);
  const int N = A.extent(1);
  const int P = B.extent(0);

  const int lda = std::max(1,M);
  const int ldb = std::max(1,P);
  const int ldu = std::max(1,M);
  const int ldv = std::max(1,P);
  const int ldq = std::max(1,N);

  int K = 0; //out
  int L = 0; //out

  // Prepares to call LAPACK function:
  // We will decompose A^T rather than A and B^T rather than B to reduce the required number of copy
  // We recall that FORTRAN/LAPACK is column-major order whereas blitz arrays
  // are row-major order by default.

  //A_lapack = A^T
  blitz::Array<double,2> A_blitz_lapack(bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double* A_lapack = A_blitz_lapack.data();

  //B_lapack = B^T
  blitz::Array<double,2> B_blitz_lapack(bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(B).transpose(1,0)));
  double* B_lapack = B_blitz_lapack.data();

  // U. We will trainpose this one in the end
  double *U_lapack = U.data();

  // V. We will trainpose this one in the end
  double *V_lapack = V.data();

  // For Q, we will return the transpose
  double *Q_lapack = Q.data();

  // In LAPACK C and S is 1-d. Our code makes it diagonal
  blitz::Array<double,1> C_1d(N); C_1d = 0;
  double *C_lapack = C_1d.data();

  blitz::Array<double,1> S_1d(N); S_1d = 0;
  double *S_lapack = S_1d.data();

  const int lwork_query = -1;
  double work_query;

  boost::shared_array<int> iwork(new int[N]);

  int info = 0;
  // A/ Queries the optimal size of the working array

  dggsvd3_(&jobu,
          &jobv,
          &jobq,
          &M,
          &N,
          &P,
          &K,
          &L,
          A_lapack,
          &lda,
          B_lapack,
          &ldb,
          C_lapack,
          S_lapack,
          U_lapack,
          &ldu,
          V_lapack,
          &ldv,
          Q_lapack,
          &ldq,
          &work_query,
          &lwork_query,
          iwork.get(),
          &info);

  if (info != 0)
    throw std::runtime_error("The LAPACK dggsvd3 function returned a non-zero value during the checking");

  // B/ Computes

  const int lwork = static_cast<int>(work_query);
  boost::shared_array<double> work(new double[lwork]);

  dggsvd3_(&jobu,
            &jobv,
           &jobq,
           &M,
           &N,
           &P,
           &K,
           &L,
           A_lapack,
           &lda,
           B_lapack,
           &ldb,
           C_lapack,
           S_lapack,
           U_lapack,
           &ldu,
           V_lapack,
           &ldv,
           Q_lapack,
           &ldq,
           work.get(),
           &lwork,
           iwork.get(),
           &info);
  if (info != 0)
    throw std::runtime_error("The LAPACK dggsvd3 function returned a non-zero value during the computation.");

  /*
  According to the website http://www.netlib.org/lapack/explore-html/d1/d7e/group__double_g_esing_ga4a187519e5c71da3b3f67c85e9baf0f2.html#ga4a187519e5c71da3b3f67c85e9baf0f2

  if (M-K-L >= 0)
                      N-K-L  K    L
        ( 0 R ) = K (  0   R11  R12 )
                  L (  0    0   R22 )

        Where R11, R12, R22, = A(1:K+L,N-K-L+1:N)

   else

                        N-K-L  K   M-K  K+L-M
       ( 0 R ) =     K ( 0    R11  R12  R13  )
                   M-K ( 0     0   R22  R23  )
                 K+L-M ( 0     0    0   R33  )

       where R11, R12, R13, R22, R23 = A(1:M, N-K-L+1:N)
       where R33 = B(M-K+1:L,N+M-K-L+1:N)

  */
  int r = K + L; //Dimension of R
  C.resize(M, r);
  S.resize(P, r);
  S = 0;
  C = 0;
  zeroR.resize(r, N);
  zeroR = 0;
  X.resize(r,N);
  if (M-K-L >= 0){

    //1. First we need the [0 R], which has the shape (N-K-L + K + L, K+L)
    zeroR(blitz::Range(0, r-1), blitz::Range(N-r, N-1)) = A_blitz_lapack.transpose(1,0)(blitz::Range(0,r-1), blitz::Range(N-r,N-1));
    // 2. Now we have to deal with C and S according to http://www.netlib.org/lapack/lug/node36.html
    //    In the end C is m-by-r and is p-by-r, both are real, nonnegative and diagonal and C'C + S'S = I ,
    //    They have the following structure
    //    COPY AND PASTE

    //2.1 Preparing C
    //           K  L
    //   C = K ( I  0 )
    //       L ( 0  C )
    //   M-K-L ( 0  0 )

    // A - Identity part
    if (K>0){
      blitz::Array<double,2> I (K,K); I= 0;
      bob::math::eye_(I);
      C(blitz::Range(0, K-1), blitz::Range(0, K-1)) = I;
    }
    // B - diag(C) part. Here the C is LxL
    // Swaping

    bob::math::swap_(C_1d, iwork.get(), K, std::min(M,r));
    blitz::Array<double,2> C_diag (L,L); C_diag = 0;
    bob::math::diag(C_1d(blitz::Range(K,K+L-1)), C_diag);
    C(blitz::Range(K, M-1), blitz::Range(K, K+L-1)) = C_diag;

    //2.2 Preparing S
    //           K  L
    //D2 =   L ( 0  S )
    //     P-L ( 0  0 )

    // A - diag(S) part
    // Swap
    bob::math::swap_(S_1d, iwork.get(), K, std::min(M,r));
    blitz::Array<double,2> S_diag (L,L); S_diag = 0;
    bob::math::diag(S_1d(blitz::Range(K,K+L-1)), S_diag);
    S(blitz::Range(0, L-1), blitz::Range(K, K+L-1)) = S_diag;

  }
  else{

    //1. First we need the [0 R], which has the shape (N-K-L  K   M-K  K+L-M  ,  k + M-K  K+L-M  )

    //A. First part of R is in A(1:M, N-K-L+1:N)
    zeroR(blitz::Range(0,M-1), blitz::Range(N-K-L, N-1)) = A_blitz_lapack.transpose(1,0)(blitz::Range(0,M-1) , blitz::Range(N-K-L,N-1));

    //B. Second part of R is in B(M-K+1:L,N+M-K-L+1:N)
    zeroR(blitz::Range(M, r - 1), blitz::Range(N-L+M-K,N-1)) = B_blitz_lapack.transpose(1,0)(blitz::Range(M-K, L-1) , blitz::Range(N+M-K-L, N-1));

    //2. Now we have to deal with C and S according to http://www.netlib.org/lapack/lug/node36.html
    //    In the end C is m-by-r and is p-by-r, both are real, nonnegative and diagonal and C'C + S'S = I ,
    //    They have the following structure
    //    COPY AND PASTE

    //2.1 Preparing C, where C=diag( ALPHA(K+1), ... , ALPHA(M) ),
    //             K M-K K+L-M
    //  D1 =   K ( I  0    0   )
    //       M-K ( 0  C    0   )

    // A - Identity part
    if (K>0){
      blitz::Array<double,2> I (K,K); I = 0;
      bob::math::eye_(I);
      C(blitz::Range(0, K-1), blitz::Range(0, K-1)) = I;
    }

    // B - diag(C) part
    // Swaping
    blitz::Array<double,1> C_1d_cropped(M-K); C_1d_cropped = 0;
    C_1d_cropped = C_1d(blitz::Range(K,K+M-1));
    bob::math::swap_(C_1d_cropped, iwork.get(), K, std::min(M,r));
    blitz::Array<double,2> C_diag (M,M); C_diag = 0;
    bob::math::diag(C_1d_cropped, C_diag);
    C(blitz::Range(K,M-1), blitz::Range(K,M-1)) = C_diag;

    //2.2 Preparing S
    //               K M-K K+L-M
    //  D2 =   M-K ( 0  S    0  )
    //       K+L-M ( 0  0    I  )
    //         P-L ( 0  0    0  )

    // A - Identity part
    if (K+L-M>0){
      blitz::Array<double,2> I (K+L-M,K+L-M); I= 0;
      bob::math::eye_(I);
      S(blitz::Range(M-K,L-1), blitz::Range(M,K+L-1)) = I;
    }

    // B - diag(S) part
    // Swaping
    blitz::Array<double,1> S_1d_cropped(M-K); S_1d_cropped = 0;
    S_1d_cropped = S_1d(blitz::Range(K,K+M-1));

    bob::math::swap_(S_1d_cropped, iwork.get(), K, std::min(M,r));
    blitz::Array<double,2> S_diag (M,M); S_diag = 0;
    bob::math::diag(S_1d_cropped, S_diag);
    S(blitz::Range(0,M-K-1), blitz::Range(K,M-1)) = S_diag;
  }

  // Transposing U and V
  blitz::Array<double,2> Ut(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(U).transpose(1,0)));
  U = Ut;

  blitz::Array<double,2> Vt(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(V).transpose(1,0)));
  V = Vt;

  // Swaping U
  bob::math::swap_(U, iwork.get(), K, std::min(M,r));
  // Swaping V
  bob::math::swap_(V, iwork.get(), K, std::min(M,r));

  //Computing X
  bob::math::prod_(zeroR, Q, X);
  blitz::Array<double,2> Xt(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(X).transpose(1,0)));
  X = Xt;
  bob::math::swap_(X, iwork.get(), K, std::min(M,r));

}