/**
 * eigen_boost.cpp
 *
 * Tests the cooperation of Eigen and boost::multiprecision.
 * Based on Eigen's test/boostmultiprec.cpp, Copyright (C) 2016 Gael Guennebaud
 */

#include <iostream>

#include <boost/chrono.hpp>

#include <Eigen/Dense>
#include <Eigen/QR>

#undef min
#undef max
#undef isnan
#undef isinf
#undef isfinite

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/number.hpp>
#include <boost/math/special_functions.hpp>

namespace mp = boost::multiprecision;
typedef mp::number<mp::cpp_dec_float<100>, mp::et_on> Real;
typedef Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

int main(int argc, char **argv)
{
	Eigen::MatrixXd abc;
	const int matrixDimension = 50;
	MatrixType a = MatrixType::Random(matrixDimension, matrixDimension);

	Eigen::ColPivHouseholderQR<MatrixType> qrOfA(a);

	std::cout << "QR decomposition " << (qrOfA.info() == Eigen::Success ? "succeeded" : "failed") << std::endl;
	std::cout << "Matrix is invertible: " << (qrOfA.isInvertible() ? "true" : "false") << std::endl;
	std::cout << "Rank = " << qrOfA.rank() << ", abs(determinant) = " << qrOfA.absDeterminant() << std::endl;
	return 0;
}


/*
 Kathpalia, A. and Nagaraj, N., 2019. Data-based intervention approach for Complexity-Causality measure. PeerJ Computer Science, 5, p.e196.
 */

#include "ETC.hpp"
#include <thread>
#include <future>
#include <Eigen/Dense>

struct CCC {

    enum threading {MULTITHREAD, SINGLETHREAD};

    //DC(dx|xpast) = ETC(xpast + dx) - ETC(xpast)
    //equation 5 in the paper
    static double dynamicCC(const ArrayXL &seq, size_t dx, size_t xpast, size_t step, threading threadType=CCC::MULTITHREAD) {
        size_t len = seq.size() -dx - xpast;
        auto calcCall = [&seq, dx, xpast, step, len]()  {
            double valCall=0;
            for(size_t i=0; i < len; i += step) {
                auto window = seq(Eigen::seq(i, i+ xpast + dx - 1));
                //                cout << "all: \n" << window.t() << endl;
                valCall += ETC::calc(window);
            }
            return valCall;
        };
        auto calcCpast = [&seq, xpast, step, len]() {
            double valCpast=0;
            for(size_t i=0; i < len; i += step) {
                ArrayXL window = seq(Eigen::seq(i, i+xpast-1));
                //                cout << "past: \n" << window.t() << endl;
                valCpast += ETC::calc(window);
            }
            return valCpast;
        };
        double Call=0;
        double Cpast=0;
        if (threadType == CCC::MULTITHREAD) {
            auto CallThread = std::async(std::launch::async, [&calcCall]() {
                return calcCall();
            });
            auto CpastThread = std::async(std::launch::async, [&calcCpast]() {
                return calcCpast();
            });
            Call = CallThread.get();
            Cpast = CpastThread.get();
        }else{
            Call = calcCall();
            Cpast = calcCpast();
        }
        int k = ceil(len/(double)step);
        double val = Call - Cpast;
        val = val / k;
        return val;
    }

    //equation 6 in the paper
    static double dynamicCCJoint(const ArrayXL &X, const ArrayXL &Y, size_t dx, size_t past, size_t step) {
        size_t len = X.size() -dx - past;
        int k=1;
        double val=0;
        for(size_t i=0; i < len; i += step) {
            auto CallThread = std::async(std::launch::async, [&X, &Y, dx, past, i]() {
                ArrayXL window1 = X(Eigen::seq(i, i+ past + dx - 1));
                ArrayXL window2 = X(Eigen::seq(i, i+ past + dx - 1));
                window2(Eigen::seq(0,past-1)) = Y(Eigen::seq(i,i+past-1));
//                auto window = seq1.subvec(i, i+ past + dx - 1);
//                cout << "all: \n" << window.t() << endl;
                return ETC::calcJoint(window1, window2);
            });
            auto CpastThread = std::async(std::launch::async, [&X, &Y, past, i]() {
                ArrayXL window1 = X(Eigen::seq(i, i+past-1));
                ArrayXL window2 = Y(Eigen::seq(i, i+past-1));
//                cout << "past: \n" << window.t() << endl;
                return ETC::calcJoint(window1, window2);
            });
            double Call = CallThread.get();
            double Cpast = CpastThread.get();
//            cout << Call << "," << Cpast << endl;
//            cout << "intCC: " << Call << ", " << Cpast << ", " << (Call - Cpast) <<endl;
            val += Call - Cpast;
            k++;
        }
        val = val / (k-1);
        return val;

    }

    //equation 8 in the paper
    static std::tuple<double, unsigned int> CCCausality(const ArrayXL &effectSeq, const ArrayXL &causeSeq, size_t dx, size_t past, size_t step) {
        auto dynCCSeq1Thread = std::async(std::launch::async, [&effectSeq, past, dx, step]() {
            return CCC::dynamicCC(effectSeq, dx, past, step);
        });

        auto dynCCSeqJointThread = std::async(std::launch::async, [&effectSeq, &causeSeq, past, dx, step]() {
            return CCC::dynamicCCJoint(effectSeq, causeSeq, dx, past, step);
        });
        auto dynCCResult = dynCCSeq1Thread.get();
        double dynCC = dynCCResult;
        double dynCCJoint = dynCCSeqJointThread.get();
        double CCC = dynCC - dynCCJoint;
//        cout << "CCC: " << dynCC << ", " << dynCCJoint << endl;
        //CCC mode - see table S1, related to polarity of the terms, and relative magnitude
        unsigned int CCCMode;
        if (dynCC < 0) {
            if (dynCCJoint < 0) {
                if (CCC < 0) {
                    CCCMode = 0;
                }else{
                    CCCMode = 1;
                }

            }else{
                CCCMode = 2;
            }
        }else{
            if (dynCCJoint < 0) {
                CCCMode = 3;
            }else{
                if (CCC < 0) {
                    CCCMode = 4;
                }else{
                    CCCMode = 5;
                }
            }
        }

        return std::make_tuple(CCC, CCCMode);;
    }
};


/*UJ - Uncle Jack
#math #big-numbers

Dear Uncle Jack is willing to give away some of his collectable CDs to his nephews. Among the titles you can find very rare albums of Hard Rock, Classical Music, Reggae and much more; each title is considered to be unique. Last week he was listening to one of his favorite songs, Nobody’s fool, and realized that it would be prudent to be aware of the many ways he can give away the CDs among some of his nephews.

So far he has not made up his mind about the total amount of CDs and the number of nephews. Indeed, a given nephew may receive no CDs at all.

Please help dear Uncle Jack, given the total number of CDs and the number of nephews, to calculate the number of different ways to distribute the CDs among the nephews.

Input

The input consists of several test cases. Each test case is given in a single line of the input by, space separated, integers N (1 <= N <= 1000) and D (0 <= D <= 2500), corresponding to the number of nephews and the number of CDs respectively. The end of the test cases is indicated with N = D = 0.

Output

The output consists of several lines, one per test case, following the order given by the input. Each line has the number of all possible ways to distribute D CDs among N nephews.
Example

Input:
1 20
3 10
0 0

Output:
1
59049

*/

#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>

int main()
{
    using namespace boost::multiprecision;

    cpp_int n;
    int d;

    while (true)
    {
        std::cin >> n >> d;
        if (n == 0) break;

        std::cout << pow(n, d) << std::endl;
    }

    return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <iostream>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

using namespace Eigen;
using std::min;
using std::max;

namespace mmf{

//TODO: use the other sphere class
class SphereSimple
{
  public:
  SphereSimple()
  {}
  ~SphereSimple()
  {}

  /* normal in tangent space around p rotate to the north pole
   * -> the third dimension will always be 0
   * -> return only first 2 dims
   */
  MatrixXf Log_p_2D(const Matrix<float,3,1>& p, const MatrixXf& q)
  {
    return rotate_p2north(p,Log_p(p,q));
  }

  /* rotate points x in tangent plane around north pole down to p
   */
  MatrixXf rotate_north2p(const Matrix<float,3,1>& p, const MatrixXf& xNorth)
  {
    Matrix3f northR = this->north_R_TpS2(p);
    //cout<<"northR"<<endl<<northR<<endl;
    if(xNorth.cols() == 2)
    {
      MatrixXf x(xNorth.rows(),3);
      x = xNorth * northR.transpose().topRows<2>();
      return x;
    }else if (xNorth.rows() == 2){
      MatrixXf x(3,xNorth.cols());
      x = northR.leftCols<2>() * xNorth;
      return x;
    }else{
      assert(false);
      return p;
    }
  }

  /* rotate points x in tangent plane around p to north pole and
   * return 2D coordinates
   */
  MatrixXf rotate_p2north(const Matrix<float,3,1>& p, const MatrixXf& x)
  {
    Matrix3f northR = this->north_R_TpS2(p);
    if(x.cols() == 3)
    {
      //MatrixXf xNorth(x.rows(),2);
      assert((x.row(0)*northR.transpose())(2) < 1e-6);
      return (x * northR.transpose()).leftCols<2>();
    }else if (x.rows() == 3){
//#ifndef NDEBUG
//      cout<< (northR * x.col(0)).transpose()<<endl;
//#endif
      assert((northR * x.col(0))(2) < 1e-3);
      return (northR * x).topRows<2>();
    }else{
      assert(false);
      return Matrix<float,Dynamic,1>::Zero(2);
    }
  }

  /* compute rotation from TpS^2 to north pole on sphere
   */
  Matrix3f north_R_TpS2(const Matrix<float,3,1>& p)
  {
    Matrix<float,3,1> north;
    north << 0.f,0.f,1.f;
    Eigen::Quaternion<float> northQ_TpS2;
    northQ_TpS2.setFromTwoVectors(p,north);
    return northQ_TpS2.toRotationMatrix().cast<float>();
  }

  MatrixXf Log_p(const Matrix<float,3,1>& p, const MatrixXf& q)
  {
    MatrixXf x(q.rows(),q.cols());
    if(q.cols() == 3)
    {
      for (uint32_t i=0; i<q.rows(); ++i)
      {
        float dot = max(-1.0f,min(1.0f,q.row(i).dot(p)));
        float theta = acos(dot);
        float sinc;
        if(theta < 1.e-8)
          sinc = 1.0f;
        else
          sinc = theta/sin(theta);
        x.row(i) = (q.row(i)-p.transpose()*dot)*sinc;
      }
    }else if (q.rows() == 3)
    {
      for (uint32_t i=0; i<q.cols(); ++i)
      {
        float dot = max(-1.0f,min(1.0f,p.dot(q.col(i))));
        float theta = acos(dot);
        float sinc;
        if(theta < 1.e-8)
          sinc = 1.0f;
        else
          sinc = theta/sin(theta);
        x.col(i) = (q.col(i)-p*dot)*sinc;
      }
    }else{
      assert(false);
    }
    return x;
  }

  MatrixXf Exp_p(const Matrix<float,3,1>& p, const MatrixXf& x)
  {
//    assert(p.cols ==1);
    MatrixXf q(x.rows(),x.cols());

    for (uint32_t i=0; i<x.cols(); ++i){
      float theta_i = x.col(i).norm();
      //cout<<"theta "<<theta_i<<endl;
      if (theta_i < 1e-10)
        q.col(i) = p + x.col(i);
      else
        q.col(i) = p*cos(theta_i) + x.col(i)/theta_i *sin(theta_i);
      //cout<<q.col(i).transpose()<<endl;
    }
    return q;
  }

  protected:
};

}


#include "tet.h"

#include "args.hxx"

#include "cuda/cg.cuh"

#include <ctime>

#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>

using namespace CompArch;

// == Geometry data
TetMesh* mesh;

float diffusionTime = 0.001;

void testSolver(size_t startIndex, double t, bool useCSR = false) {
    std::vector<double> distances;
    distances.reserve(mesh->vertices.size());

    std::vector<double> start(mesh->vertices.size(), 0.0);
    start[startIndex] = 1;
    if (t < 0) t = mesh->meanEdgeLength();

    Eigen::VectorXd u0 = Eigen::VectorXd::Map(start.data(), start.size());

    Eigen::VectorXd u(mesh->vertices.size());
    Eigen::VectorXd phi(mesh->vertices.size());
    Eigen::VectorXd divX = Eigen::VectorXd::Random(mesh->vertices.size());
    Eigen::VectorXd ones = Eigen::VectorXd::Ones(divX.size());
    divX -= divX.dot(ones) * ones;

    if (useCSR) {
        cgSolveCSR(u, u0, *mesh, 1e-8, t);
        cgSolveCSR(phi, divX, *mesh, 1e-8, -1);
    } else {
        cgSolve(u, u0, *mesh, 1e-8, t);
        cgSolve(phi, divX, *mesh, 1e-8, -1);
    }

    Eigen::SparseMatrix<double> L    = mesh->weakLaplacian();
    Eigen::SparseMatrix<double> M    = mesh->massMatrix();

    Eigen::SparseMatrix<double> flow = M + t * L;
    cout << "Residual: " << (flow * u  - u0).norm();
    cout << "\tResidual 2: " << (L * phi - divX).norm() << endl;
}

std::vector<double> computeDistances(size_t startIndex, double t, bool useCUDA, bool useCSR=false) {
    std::vector<double> distances;
    distances.reserve(mesh->vertices.size());

    std::vector<double> start(mesh->vertices.size(), 0.0);
    start[startIndex] = 1;
    if (t < 0) t = mesh->meanEdgeLength();

    Eigen::VectorXd u0 = Eigen::VectorXd::Map(start.data(), start.size());
    Eigen::SparseMatrix<double> L    = mesh->weakLaplacian();
    Eigen::SparseMatrix<double> M    = mesh->massMatrix();

    Eigen::VectorXd u(mesh->vertices.size());
    Eigen::SparseMatrix<double> flow = M + t * L;
    if (useCUDA) {
        if (useCSR) {
            cgSolveCSR(u, u0, *mesh, 1e-8, t);
        } else {
            cgSolve(u, u0, *mesh, 1e-8, t);
        }
        double residual = (flow * u - u0).norm();
        if (residual > 1e-5)
            cout << "Residual 1: " << residual << endl;
    } else {
        Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
        solver.compute(flow);
        u = solver.solve(u0);
    }

    Eigen::VectorXd divX = Eigen::VectorXd::Zero(u.size());

    std::vector<Vector3> tetXs;
    for (Tet t : mesh->tets) {
        std::array<Vector3, 4> vertexPositions = mesh->layOutIntrinsicTet(t);

        std::array<double, 4> tetU{u[t.verts[0]], u[t.verts[1]], u[t.verts[2]],
                                   u[t.verts[3]]};
        Vector3 tetGradU = grad(tetU, vertexPositions);
        Vector3 X = tetGradU.normalize();

        tetXs.emplace_back(Vector3{X.x, X.y, X.z});

        std::array<double, 4> tetDivX = div(X, vertexPositions);
        for (size_t i = 0; i < 4; ++i) {
            divX[t.verts[i]] += tetDivX[i];
        }
    }

    Eigen::VectorXd ones = Eigen::VectorXd::Ones(divX.size());
    divX -= divX.dot(ones) * ones;

    Eigen::VectorXd phi(mesh->vertices.size());
    if (useCUDA) {
        if (useCSR) {
            cgSolveCSR(phi, divX, *mesh, 1e-8, -1);
        } else {
            cgSolve(phi, divX, *mesh, 1e-8, -1);
        }
        double residual = (L * phi - divX).norm();
        if (residual > 1e-5)
            cout << "Residual 2: " << residual << endl;
    } else {
        Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
        solver.compute(L);
        phi = solver.solve(divX);
    }

    for (int i = 0; i < phi.size(); ++i) {
        distances[i] = phi[i];
    }

    double minDist = distances[0];
    for (size_t i = 1; i < distances.size(); ++i) {
        minDist = fmin(minDist, distances[i]);
    }
    for (size_t i = 0; i < distances.size(); ++i) {
        distances[i] -= minDist;
        assert(distances[i] >= 0);
    }

    return distances;
}

int main(int argc, char** argv) {

    // Configure the argument parser
    args::ArgumentParser parser("Geometry program");
    args::Positional<std::string> inputFilename(
        parser, "mesh", "Tet mesh (ele file) to be processed.");
    args::Positional<std::string> niceName(
        parser, "name", "Nice name for printed output.");

    // Parse args
    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Help) {
        std::cout << parser;
        return 0;
    } catch (args::ParseError e) {
        std::cerr << e.what() << std::endl;
        std::cerr << parser;
        return 1;
    }

    std::string filename = "../../meshes/TetMeshes/bunny_small.1.ele";
    // Make sure a mesh name was given
    if (inputFilename) {
        filename = args::get(inputFilename);
    }

    std::string descriptionName = filename;
    if (niceName) {
        descriptionName = args::get(niceName);
    }

    mesh = TetMesh::loadFromFile(filename);
    std::cout << descriptionName << "\t" << mesh->tets.size();

    std::cout << endl;
    std::cout << "CSR test: " ;
    testSolver(0, -1, true);
    std::cout << "non CSR test: ";
    testSolver(0, -1, false);
    std::cout << "Done testing " << endl;

    std::clock_t start;
    double duration;

    start = std::clock();
    computeDistances(0, -1, false);
    duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC * 1000;
    std::cout<< "\t" << duration;

    start = std::clock();
    computeDistances(0, -1, true, true);
    duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC * 1000;
    std::cout<< "\tCSR: " << duration;

    start = std::clock();
    computeDistances(0, -1, true, false);
    duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC * 1000;
    std::cout<< "\tmine: " << duration;

    std::cout << std::endl;

    return EXIT_SUCCESS;
}


#include "catch.hpp"
#include "functions.hpp"
#include "mwtrans_complex.hpp"
#include "timer.hpp"

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cmath>
#include <complex>
#include <fmt/format.h>

using complex_d = std::complex<double>;
using namespace std::literals::complex_literals;
using namespace boost::math::quadrature;

bool is_equal(complex_d c1, complex_d c2, double eps = 1.0e-8) {
  return std::abs(c1 - c2) < eps;
}

TEST_CASE("Case simple", "[test_complex]") {
  double error;
  double a{0};
  double b{1};
  unsigned int max_depth = 0;
  double tolerance = 0;
  complex_d ret = gauss_kronrod<double, 61>::integrate(
      func_simple, a, b, max_depth, tolerance, &error);
  complex_d exact{0.5, 0.25};
  REQUIRE(is_equal(ret, exact));
}

TEST_CASE("Case 1c", "[test_complex]") {
  double abserr = 1.0e-9;
  double referr = 1.0e-7;
  MWtransIntComplex mwt(0, 0, abserr, referr);
  complex_d ret = mwt.perform(func_1c);
  complex_d exact = -1.0 + 1.0i;
  std::cout << ret << std::endl;
  REQUIRE(is_equal(ret, exact));
}


//  ExoticBSEngineReverse.cpp

//On the tractability of the Brownian Bridge algorithm, 2003, Leobacher, Scheicher
//p.8 - 11

#include <cmath>
#include <algorithm>

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

#include <ExoticBSEngineReverse.h>
#include <stochastic_term.h>

using namespace std;
using namespace boost::numeric::ublas;

void ExoticBSEngineReverse::GetOnePath(MJArray& LogSpotValues)
{
    TheGenerator->GetGaussians(Variates);
    MJArray tmp = StochasticTerm(A, Variates);

    double CurrentLogSpot = LogSpot;

    for (unsigned long j=0; j < NumberOfTimes; j++){
        CurrentLogSpot += Drifts[j];
        CurrentLogSpot += StandardDeviations[j] * tmp[j];
		    LogSpotValues[j] = CurrentLogSpot;
    }
}

ExoticBSEngineReverse::ExoticBSEngineReverse(const Wrapper<PathDependent>& TheProduct_,
                                    const Parameters& R_,
                                    const Parameters& D_,
                                    const Parameters& Vol_,
                                    const Wrapper<RandomBase>& TheGenerator_,
                                    double Spot_,
									bool speed_up_)
                                    :
                                    ExoticEngine(TheProduct_,R_,speed_up_),
                                    TheGenerator(TheGenerator_)
{
    MJArray Times(TheProduct_->GetLookAtTimes());

    NumberOfTimes = Times.size();
	  if (speed_up == true)
		  NumberOfTimes -= 1;

    TheGenerator->ResetDimensionality(NumberOfTimes);

    Drifts.resize(NumberOfTimes);
    StandardDeviations.resize(NumberOfTimes);
    A.resize(NumberOfTimes, NumberOfTimes);

    //double Variance = Vol_.IntegralSquare(0,1);

    Drifts[0] = R_.Integral(0.0,Times[0]) - D_.Integral(0.0,Times[0]) - 0.5 * Vol_.IntegralSquare(0.0,Times[0]);
	StandardDeviations[0] = sqrt(Vol_.IntegralSquare(0.0, 1.0));

    for (unsigned long j=1; j < NumberOfTimes; ++j)
    {
        //double thisVariance = Vol_.IntegralSquare(0,1);
        Drifts[j] = R_.Integral(Times[j-1],Times[j]) - D_.Integral(Times[j-1],Times[j])
                    - 0.5 * Vol_.IntegralSquare(Times[j-1],Times[j]);
		StandardDeviations[j] = sqrt(Vol_.IntegralSquare(0.0, 1.0));
    }

    LogSpot = std::log(Spot_);
    Variates.resize(NumberOfTimes);
    method_ = logscale;

    //--------------------------------------------------------------------------------

    Matrix C(NumberOfTimes, NumberOfTimes);

	/*
    for (size_t i(0); i < NumberOfTimes; ++i)
      for (size_t j(0); j < NumberOfTimes; ++j)
        C[i][j] = max(Times[i],Times[j]);
	*/

	for (size_t i(0); i < NumberOfTimes; ++i){
		for (size_t j(0); j < NumberOfTimes; ++j)
			C[i][j] = TheProduct_->GetLookAtTimes()[j];
	}

	for (size_t j(1); j <NumberOfTimes; ++j){
		for (size_t i(0); i < j; ++i)
			C[i][j] = TheProduct_->GetLookAtTimes()[i];
	}

	//std::cout << C << endl;

    //Permutation matrix - reverse row order last row becoming the first one
    Matrix P(NumberOfTimes, NumberOfTimes);

	for (size_t i(0); i < NumberOfTimes; ++i)
		for (size_t j(0); j < NumberOfTimes; ++j)
			P[i][j] = 0.0;

	for (size_t i(0);i < NumberOfTimes;++i)
		P[NumberOfTimes - 1 - i][i] = 1.0;

	//std::cout << P << endl;

	//Matrix Pinv = inverse(P);

	//Inverse of permutation matrix Pinv = P

	//std::cout << Pinv << endl;

    Matrix D = P * C * P;

	//std::cout << D << endl;

	Matrix E = CholeskyDecomposition(D);

	Matrix F = P * E;

	/*
	//Implement via LU decomposition. Resulting matrix not symmetric

	std::copy(D.begin(), D.end(), A.begin2());

	permutation_matrix<Size> pert(A.size1());

	const Size singular = lu_factorize(A, pert); //overwrite A. Final A contains both L and U triangle
	QL_REQUIRE(singular == 0, "singular matrix given");

	for (size_t i(0); i < NumberOfTimes; ++i)
		for (size_t j(0); j < NumberOfTimes; ++j)
			A(i, j) = P[i][j] * A(i, j);
	*/

	std::copy(F.begin(), F.end(), A.begin2());

	//std::cout << A << endl;

}


// Std includes
#include <cmath>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "fl0w/simple_shear.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;
using TypeFlow = fl0w::SimpleShear<TypeVector, TypeMatrix, TypeRef>;

void test(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    // Expected values
    TypeVector xW;
    xW << 0.0,
          0.0,
         -0.5;
    TypeMatrix xS;
    xS << 0.0, 0.5, 0.0,
          0.5, 0.0, 0.0,
          0.0, 0.0, 0.0;
    // Output
    TypeVector w = flow.getVorticity(x, t);
    TypeMatrix S = flow.getStrain(x, t);
    std::cout << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") = " << std::endl;
    std::cout << w << std::endl;
    std::cout << "flow.getStrain(" << x.transpose() << ", " << t << ") = " << std::endl;
    std::cout << S << std::endl;
    // Test Output
    std::cout << std::endl;
    std::cout << "Test vorticity succeeded : " << (w == xW) << std::endl;
    std::cout << "Test strain succeeded : " << (S == xS) << std::endl;
    std::cout << std::endl;
    // Small temporary additional computations
    std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << ") = " << std::endl;
    std::cout << flow.getJacobian(x, t) << std::endl;
    TypeVector dir;
    dir << 1.0,
           0.0,
           0.0;
    std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << ") * " << dir.transpose() << " = " << std::endl;
    std::cout << flow.getJacobian(x, t).transpose() * dir  << std::endl;
}

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") -> " << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") -> " << flow.getVorticity(x, t).transpose() << std::endl;
    std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") -> " << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

int main () {
    TypeFlow flow;
    flow.create(1.0);
    TypeVector x;
    double t;
    // Init
    x << 0.0, 0.0, 0.0;
    t = 0.0;
    test(flow, x, t);
}


//
// Copyright (c) 2018 Stefan Seefeld
// All rights reserved.
//
// This file is part of Boost.uBLAS. It is made available under the
// Boost Software License, Version 1.0.
// (Consult LICENSE or http://www.boost.org/LICENSE_1_0.txt)

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/program_options.hpp>
#include "../init.hpp"
#include "../benchmark.hpp"
#include <complex>
#include <string>

namespace po = boost::program_options;
namespace ublas = boost::numeric::ublas;
namespace boost { namespace numeric { namespace ublas { namespace benchmark {

template <typename T>
class prod : public benchmark
{
public:
  prod(std::string const &name) : benchmark(name) {}
  virtual void setup(long l)
  {
    init(a, l, 200);
    init(b, l, 200);
  }
  virtual void operation(long l)
  {
    for (int i = 0; i < l; ++i)
      for (int j = 0; j < l; ++j)
      {
	c(i,j) = 0;
	for (int k = 0; k < l; ++k)
	  c(i,j) += a(i,k) * b(k,j);
      }
  }
private:
  ublas::matrix<T> a;
  ublas::matrix<T> b;
  ublas::matrix<T> c;
};

}}}}

namespace bm = boost::numeric::ublas::benchmark;

template <typename T>
void benchmark(std::string const &type)
{
  //  using matrix = ublas::matrix<T, ublas::basic_row_major<>>;
  bm::prod<T> p("ref::prod(matrix<" + type + ">)");
  p.run(std::vector<long>({1, 2, 4, 8, 16, 32, 64, 128, 256, 512}));//, 1024}));
}

int main(int argc, char **argv)
{
  po::variables_map vm;
  try
  {
    po::options_description desc("Matrix product (reference implementation)\n"
                                 "Allowed options");
    desc.add_options()("help,h", "produce help message");
    desc.add_options()("type,t", po::value<std::string>(), "select value-type (float, double, fcomplex, dcomplex)");

    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
      std::cout << desc << std::endl;
      return 0;
    }
  }
  catch(std::exception &e)
  {
    std::cerr << "error: " << e.what() << std::endl;
    return 1;
  }
  std::string type = vm.count("type") ? vm["type"].as<std::string>() : "float";
  if (type == "float")
    benchmark<float>("float");
  else if (type == "double")
    benchmark<double>("double");
  else if (type == "fcomplex")
    benchmark<std::complex<float>>("std::complex<float>");
  else if (type == "dcomplex")
    benchmark<std::complex<double>>("std::complex<double>");
  else
    std::cerr << "unsupported value-type \"" << vm["type"].as<std::string>() << '\"' << std::endl;
}


/*
 * randomNumberGenerator.hpp
 *
 *  Created on: Apr 29, 2016
 *      Author: jhwangbo
 */

#ifndef RANDOMNUMBERGENERATOR_HPP_
#define RANDOMNUMBERGENERATOR_HPP_

// for random sampling
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions.hpp>
#include <cstdlib>
#include <mutex>
#include <Eigen/Core>

namespace rai {

template<typename Dtype>
class RandomNumberGenerator {

 public:

  RandomNumberGenerator() {
//    distributionNormal = new boost::random::normal_distribution<Dtype>(0.0, 1.0);
//    distributionUni = new boost::uniform_real<Dtype>(-1, 1);
//    distributionUni01 = new boost::uniform_real<Dtype>(0, 1);
  }

  ~RandomNumberGenerator() {
//    delete distributionNormal;
//    delete distributionUni;
//    delete distributionUni01;
  }

  /* mean =0, std = 1*/
  Dtype sampleNormal() {
    std::lock_guard<std::mutex> lockModel(rndMutex_);
    auto dist = boost::random::normal_distribution<Dtype>(0.0, 1.0);
    return dist(rngGenerator);
  }

  /* from -1 to 1*/
  Dtype sampleUniform() {
    auto dist = boost::uniform_real<Dtype>(-1, 1);
    std::lock_guard<std::mutex> lockModel(rndMutex_);
    return dist(rngGenerator);
  }

  /* from 0 to 1*/
  Dtype sampleUniform01() {
    auto dist = boost::uniform_real<Dtype>(0, 1);
    std::lock_guard<std::mutex> lockModel(rndMutex_);
    return dist(rngGenerator);
  }

  bool forXPercent(float epsilon) {
    auto dist = boost::uniform_real<Dtype>(0, 1);
    std::lock_guard<std::mutex> lockModel(rndMutex_);
    return dist(rngGenerator) < epsilon;
  }

  int intRand(const int &min, const int &max) {
    {
      std::lock_guard<std::mutex> lockModel(rndMutex_);
      std::uniform_int_distribution<int> distribution(min, max);
      return distribution(rngGenerator);
    }
  }

  /* weighted random sampling where the weights are given by 1, r, r^2, ... */
  int intWeightedRand (const int &max, Dtype weightDecayFtr) {
    Dtype sum = (Dtype(1) - std::pow(weightDecayFtr, max+1)) / (Dtype(1)-weightDecayFtr);
    return std::ceil(log(Dtype(1) - sampleUniform01() * sum * (Dtype(1) - weightDecayFtr)) / log(weightDecayFtr)) - 1;
  }

  template<int dim>
  void sampleVectorInNormalUniform(Dtype *vector) {
    std::lock_guard<std::mutex> lockModel(rndMutex_);
    auto dist = boost::uniform_real<Dtype>(-1, 1);
    for (int i = 0; i < dim; i++)
      vector[i] = dist(rngGenerator);
  }

  template<int dim>
  void sampleInUnitSphere(Dtype *vector) {
    sampleVectorInNormalUniform<dim>(vector);
    Dtype sum = 0.0f;

    for (int i = 0; i < dim; i++)
      sum += vector[i] * vector[i];

    Dtype amplitudeOverSum = pow(std::abs(sampleUniform()), Dtype(1.0) / Dtype(dim)) / sqrtf(sum);

    for (int i = 0; i < dim; i++)
      vector[i] = vector[i] * amplitudeOverSum;
  }

  template<int dim>
  void sampleOnUnitSphere(Dtype *vector) {
    sampleVectorInNormalUniform<dim>(vector);
    Dtype sum = 0.0f;

    for (int i = 0; i < dim; i++)
      sum += vector[i] * vector[i];

    for (int i = 0; i < dim; i++)
      vector[i] = vector[i] / sqrtf(sum);
  }

  template<typename Derived>
  void shuffleSTDVector(std::vector<Derived> &order) {
    boost::variate_generator<boost::mt19937 &, boost::uniform_int<> >
        random_number_shuffler(rngGenerator, boost::uniform_int<>());
    std::random_shuffle(order.begin(), order.end(), random_number_shuffler);
  }

  /* this method is opitmized for memory use. The column should be dynamic size */
  template<typename Derived, int Rows, int Cols>
  void shuffleColumns(Eigen::Matrix<Derived, Rows, Cols> &matrix) {
    int colSize = int(matrix.cols());

    /// sampling the order
    std::vector<int> order;
    std::vector<bool> needSuffling(colSize, true);

    order.resize(colSize);
    for (int i = 0; i < colSize; i++) order[i] = i;
    shuffleSTDVector(order);
    Eigen::Matrix<Derived, Rows, 1> memoryCol(matrix.rows());

    int colID;

    for (int colStartID = 0; colStartID < colSize; colStartID++) {
      if (order[colStartID] == colStartID || !needSuffling[colStartID]) continue;

      colID = colStartID;
      memoryCol = matrix.col(colID);
      do {
        matrix.col(colID) = matrix.col(order[colID]);
        needSuffling[colID] = false;
        colID = order[colID];
      } while (colStartID != order[colID]);
      matrix.col(colID) = memoryCol;
      needSuffling[colID] = false;
    }
  }

  std::vector<unsigned> getNrandomSubsetIdx (unsigned nOfElem, unsigned nOfSubElem) {
    std::vector<unsigned> memoryIdx(nOfSubElem);
    ///// randomly sampling memory indeces
    for (unsigned i = 0; i < nOfSubElem; i++) {
      memoryIdx[i] = intRand(0, nOfElem - 1);
      for (unsigned j = 0; j < i; j++) {
        if (memoryIdx[i] == memoryIdx[j]) {
          i--;
          break;
        }
      }
    }
    return memoryIdx;
  }

  /* this method is opitmized for memory use. The column should be dynamic size */
  template<typename Derived1, int Rows1, int Cols1, typename Derived2, int Rows2, int Cols2>
  void shuffleColumnsOfTwoMatrices(Eigen::Matrix<Derived1, Rows1, Cols1> &matrix1,
                                   Eigen::Matrix<Derived2, Rows2, Cols2> &matrix2) {
//    LOG_IF(FATAL, matrix1.cols() != matrix2.cols()) << "two matrices have different number of columns";

    int colSize = int(matrix1.cols());

    /// sampling the order
    std::vector<int> order;
    std::vector<bool> needSuffling(colSize, true);

    order.resize(colSize);
    for (int i = 0; i < colSize; i++) order[i] = i;
    shuffleSTDVector(order);
    Eigen::Matrix<Derived1, Rows1, 1> memoryCol1(matrix1.rows());
    Eigen::Matrix<Derived2, Rows2, 1> memoryCol2(matrix2.rows());

    int colID;

    for (int colStartID = 0; colStartID < colSize; colStartID++) {
      if (order[colStartID] == colStartID || !needSuffling[colStartID]) continue;

      colID = colStartID;
      memoryCol1 = matrix1.col(colID);
      memoryCol2 = matrix2.col(colID);

      do {
        matrix1.col(colID) = matrix1.col(order[colID]);
        matrix2.col(colID) = matrix2.col(order[colID]);

        needSuffling[colID] = false;
        colID = order[colID];
      } while (colStartID != order[colID]);
      matrix1.col(colID) = memoryCol1;
      matrix2.col(colID) = memoryCol2;
      needSuffling[colID] = false;
    }
  }

  /* this method is opitmized for memory use. The column should be dynamic size */
  template<typename Derived1, int Rows1, int Cols1, typename Derived2, int Rows2, int Cols2, typename Derived3, int Rows3, int Cols3>
  void shuffleColumnsOfThreeMatrices(Eigen::Matrix<Derived1, Rows1, Cols1> &matrix1,
                                     Eigen::Matrix<Derived2, Rows2, Cols2> &matrix2,
                                     Eigen::Matrix<Derived3, Rows3, Cols3> &matrix3) {

    int colSize = int(matrix1.cols());

    /// sampling the order
    std::vector<int> order;
    std::vector<bool> needSuffling(colSize, true);

    order.resize(colSize);
    for (int i = 0; i < colSize; i++) order[i] = i;
    shuffleSTDVector(order);

    Eigen::Matrix<Derived1, Rows1, 1> memoryCol1(matrix1.rows());
    Eigen::Matrix<Derived2, Rows2, 1> memoryCol2(matrix2.rows());
    Eigen::Matrix<Derived3, Rows3, 1> memoryCol3(matrix3.rows());

    int colID;

    for (int colStartID = 0; colStartID < colSize; colStartID++) {
      if (order[colStartID] == colStartID || !needSuffling[colStartID]) continue;

      colID = colStartID;
      memoryCol1 = matrix1.col(colID);
      memoryCol2 = matrix2.col(colID);
      memoryCol3 = matrix3.col(colID);
      do {
        matrix1.col(colID) = matrix1.col(order[colID]);
        matrix2.col(colID) = matrix2.col(order[colID]);
        matrix3.col(colID) = matrix3.col(order[colID]);
        needSuffling[colID] = false;
        colID = order[colID];
      } while (colStartID != order[colID]);
      matrix1.col(colID) = memoryCol1;
      matrix2.col(colID) = memoryCol2;
      matrix3.col(colID) = memoryCol3;
      needSuffling[colID] = false;
    }
  }

  /* this method is opitmized for memory use. The column should be dynamic size */
  template<typename Derived1, int Rows1, int Cols1, typename Derived2, int Rows2, int Cols2, typename Derived3, int Rows3, int Cols3>
  void shuffleColumnsOfFourMatrices(Eigen::Matrix<Derived1, Rows1, Cols1> &matrix1,
                                     Eigen::Matrix<Derived2, Rows2, Cols2> &matrix2,
                                     Eigen::Matrix<Derived3, Rows3, Cols3> &matrix3,
                                     Eigen::Matrix<Derived3, Rows3, Cols3> &matrix4) {

    int colSize = int(matrix1.cols());

    /// sampling the order
    std::vector<int> order;
    std::vector<bool> needSuffling(colSize, true);

    order.resize(colSize);
    for (int i = 0; i < colSize; i++) order[i] = i;
    shuffleSTDVector(order);

    Eigen::Matrix<Derived1, Rows1, 1> memoryCol1(matrix1.rows());
    Eigen::Matrix<Derived2, Rows2, 1> memoryCol2(matrix2.rows());
    Eigen::Matrix<Derived3, Rows3, 1> memoryCol3(matrix3.rows());
    Eigen::Matrix<Derived3, Rows3, 1> memoryCol4(matrix4.rows());

    int colID;

    for (int colStartID = 0; colStartID < colSize; colStartID++) {
      if (order[colStartID] == colStartID || !needSuffling[colStartID]) continue;

      colID = colStartID;
      memoryCol1 = matrix1.col(colID);
      memoryCol2 = matrix2.col(colID);
      memoryCol3 = matrix3.col(colID);
      memoryCol4 = matrix4.col(colID);

      do {
        matrix1.col(colID) = matrix1.col(order[colID]);
        matrix2.col(colID) = matrix2.col(order[colID]);
        matrix3.col(colID) = matrix3.col(order[colID]);
        matrix4.col(colID) = matrix4.col(order[colID]);

        needSuffling[colID] = false;
        colID = order[colID];
      } while (colStartID != order[colID]);
      matrix1.col(colID) = memoryCol1;
      matrix2.col(colID) = memoryCol2;
      matrix3.col(colID) = memoryCol3;
      matrix4.col(colID) = memoryCol4;

      needSuffling[colID] = false;
    }
  }

  /*you can use this method to make the random samples the same*/
  static void seed(uint32_t seed) {
    rngGenerator.seed(seed);
  }

 private:
  static boost::random::mt19937 rngGenerator;
  static std::mutex rndMutex_;
//  boost::random::normal_distribution<Dtype> *distributionNormal;
//  boost::uniform_real<Dtype> *distributionUni, *distributionUni01;

};

}
// Initialize the random number generator with a time-based seed instead of the default one.
template<typename Dtype>
boost::random::mt19937 rai::RandomNumberGenerator<Dtype>::rngGenerator(time(NULL));
template<typename Dtype>
std::mutex rai::RandomNumberGenerator<Dtype>::rndMutex_;

#endif /* RANDOMNUMBERGENERATOR_HPP_ */


#include "MishMesh/geodesics.h"

#include <MishMesh/laplace.h>
#include <MishMesh/utils.h>
#include <MishMesh/macros.h>

#include <Eigen/Eigen>

#include <vector>
#include <cassert>

/**
 * Compute geodesic distances on a mesh using the heat method by Crane, Wischedel and Wardetzky.
 * [Crane, K., Weischedel, C., and Wardetzky, M. (2013). Geodesics in Heat. ACM Trans. Graph. 32, 1-11.]
 * https://dl.acm.org/citation.cfm?id=2516977
 *
 * @param[inout] mesh The mesh.
 * @param[in] start_vh A valid vertex in the mesh, that will be used as start vertex.
 * @param[in] geodesicGeodesicDistanceProperty A mesh property to store the geodesic distances. The method assumes, that the property is already added to the mesh
 * @param[in] t The timestep for the heat diffusion step in the algorithm.
 */
void MishMesh::compute_heat_geodesics(TriMesh &mesh, const TriMesh::VertexHandle start_vh, GeodesicDistanceProperty geodesicGeodesicDistanceProperty, double t) {
	return compute_heat_geodesics(mesh, std::vector<TriMesh::VertexHandle>{start_vh}, geodesicGeodesicDistanceProperty, t);
}

/**
 * Compute geodesic distances on a mesh using the heat method by Crane, Wischedel and Wardetzky.
 * [Crane, K., Weischedel, C., and Wardetzky, M. (2013). Geodesics in Heat. ACM Trans. Graph. 32, 1-11.]
 * https://dl.acm.org/citation.cfm?id=2516977
 *
 * @param[inout] mesh The mesh.
 * @param[in] start_vhs A list of valid vertices in the mesh, that will be used as start vertices.
 * @param[in] geodesicGeodesicDistanceProperty A mesh property to store the geodesic distances. The method assumes, that the property is already added to the mesh
 * @param[in] t The timestep for the heat diffusion step in the algorithm.
 */
void MishMesh::compute_heat_geodesics(TriMesh &mesh, const std::vector<TriMesh::VertexHandle> start_vhs, GeodesicDistanceProperty geodesicGeodesicDistanceProperty, double t) {
	if(start_vhs.empty()) return;

	mesh.request_face_normals();
	mesh.update_face_normals();

	// Lc is the laplacian without area weighting
	Eigen::SparseMatrix<double> Lc = MishMesh::laplace_matrix(mesh, false, false);

	// Set the initial heat distribution
	Eigen::VectorXd u0 = Eigen::VectorXd::Zero(mesh.n_vertices());
	for(auto start_vh : start_vhs) {
		u0[start_vh.idx()] = 1.0;
	}

	// Calculate the vertex areas as 1/3 of the triangles around a vertex
	Eigen::VectorXd vertex_areas(mesh.n_vertices());
	vertex_areas.setZero();
	for(auto vh : mesh.vertices()) {
		FOR_CVF(f_it, vh) {
			vertex_areas[vh.idx()] += MishMesh::compute_area(mesh, *f_it);
		}
		vertex_areas[vh.idx()] /= 3.0;
	}

	// Calculate A - t*L_c
	Eigen::SparseMatrix<double> A_tLc = -t * Lc;
	assert(A_tLc.rows() == A_tLc.cols());
	for(int i = 0; i < A_tLc.rows(); i++){
		A_tLc.coeffRef(i, i) += vertex_areas[i];
	};
	assert(A_tLc.isCompressed());

	// Compute one heat diffusion timestep
	Eigen::SparseLU<Eigen::SparseMatrix<double>> luSolver;
	luSolver.compute(A_tLc);
	Eigen::VectorXd u = luSolver.solve(u0);

	// Calculate the gradients on the faces
	std::vector<OpenMesh::Vec3d> face_grad_u(mesh.n_faces(), OpenMesh::Vec3d(0,0,0));
	for(auto fh : mesh.faces()) {
		OpenMesh::Vec3d &x = face_grad_u[fh.idx()];
		const OpenMesh::Vec3d &N = mesh.normal(fh);
		FOR_CFV(v_it, fh) {
			auto vh = *v_it;
			auto heh = MishMesh::opposite_halfedge(mesh, fh, vh);
			OpenMesh::Vec3d ei = mesh.point(mesh.to_vertex_handle(heh)) - mesh.point(mesh.from_vertex_handle(heh));
			x += u[v_it->idx()] * (N % ei);
		}
		x /= 2 * MishMesh::compute_area(mesh, fh);
	}

	// Calculate the divergence at the vertices
	Eigen::VectorXd vertex_div_u = Eigen::VectorXd::Zero(mesh.n_vertices());
	for(auto vh : mesh.vertices()) {
		auto pi = mesh.point(vh);
		double &div = vertex_div_u[vh.idx()];
		FOR_CVOH(h_it, vh) {
			// The edge and the next edge belong to a common face,
			// except when the current edge is a boundary edge. In that case
			// we skip it, as it will be the next_heh of another halfedge later.
			auto heh = *h_it;
			if(mesh.is_boundary(heh)) continue;
			auto next_h_it = h_it;
			next_h_it++;
			auto next_heh = *next_h_it;

			assert(mesh.face_handle(heh) == mesh.face_handle(mesh.opposite_halfedge_handle(next_heh)));
			auto fh = mesh.face_handle(heh);

			auto p1 = mesh.point(mesh.to_vertex_handle(heh));
			auto p2 = mesh.point(mesh.to_vertex_handle(next_heh));
			OpenMesh::Vec3d e1 = (p1 - pi);
			OpenMesh::Vec3d e2 = (p2 - pi);
			OpenMesh::Vec3d e3 = (p2 - p1);
			OpenMesh::Vec3d X_face = -face_grad_u[fh.idx()] / face_grad_u[fh.idx()].norm();

			double angle1 = acos((-e3.normalized()) | (-e2.normalized()));
			double angle2 = acos(e3.normalized() | (-e1.normalized()));

			div += (1.0 / tan(angle1)) * (e1 | X_face) + (1.0 / tan(angle2)) * (e2 | X_face);
		}
		div /= 2.0;
	}

	// The distance of the source vertex is always 0, so we can use it as boundary condition
	// to get an unique solution of the poisson equation.
	std::vector<std::pair<Eigen::Index, double>> bc{std::make_pair(start_vhs[0].idx(), 0.0)};
	MishMesh::apply_boundary_conditions(Lc, vertex_div_u, bc);
	luSolver.compute(Lc);
	Eigen::VectorXd phi = luSolver.solve(vertex_div_u);
	for(auto vh : mesh.vertices()) {
		mesh.property(geodesicGeodesicDistanceProperty, vh) = phi[vh.idx()];
	}

	mesh.release_face_normals();
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of the Harris hawks optimization, see
// Harris hawks optimization: Algorithm and applications
// Ali Asghar Heidari, Seyedali Mirjalili, Hossam Faris, Ibrahim Aljarah, Majdi Mafarja, Huiling Chen
// Future Generation Computer Systems,
// DOI: https://doi.org/10.1016/j.future.2019.02.028

// derived from https://github.com/7ossam81/EvoloPy/blob/master/optimizers/HHO.py

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <math.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef double (*callback_type)(int, double[]);

namespace harris_hawks {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(0, 1);
static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static vec zeros(int n) {
	return  Eigen::MatrixXd::Zero(n, 1);
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64& rs) {
	return Eigen::MatrixXd::NullaryExpr( dx, dy, [&](){return distr_01(rs);});
}

static Eigen::MatrixXd normalVec(int dim, pcg64& rs) {
	return Eigen::MatrixXd::NullaryExpr( dim, 1, [&](){return gauss_01(rs);});
}

// wrapper around the fittness function, scales according to boundaries

class Fittness {

public:

    Fittness(callback_type pfunc, const vec& lower_limit,
            const vec& upper_limit) {
        func = pfunc;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    vec getClosestFeasible(const vec& X) const {
        if (lower.size() > 0) {
        	return X.cwiseMin(1.0).cwiseMax(0.0);
        }
        return X;
    }

    double eval(const vec& X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func(n, parg);
        evaluationCounter++;
        return res;
    }

    double value(const vec& X) {
        if (lower.size() > 0)
            return eval(decode(X));
        else
            return eval(X);
    }

	vec decode(const vec& X) const {
		if (lower.size() > 0)
			return (X.array() * scale.array()).matrix() + lower;
		else
			return X;
	}

    vec encode(const vec& X) const {
        if (lower.size() > 0)
        	return (X - lower).array() / scale.array();
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
   callback_type func;
   vec lower;
   vec upper;
   long evaluationCounter;
   vec scale;
};

class HHOptimizer {

public:

    HHOptimizer(long runid_, Fittness* fitfun_, int dim_, int seed_, int popsize_,
            int maxEvaluations_, double stopfitness_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        if (popsize_ > 0)
            popsize = popsize_;
        else
            popsize = 31;
        // termination criteria
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        // Number of iterations already performed.
        iterations = 0;
         // stop criteria
        stop = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        init();
    }

    ~HHOptimizer() {
    	delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    vec levy(int dim) {
        double beta = 1.5;
        double sigma = pow ((tgamma(1+beta)*sin(M_PI*beta/2)/(tgamma((1+beta)/2) * beta * pow(2,((beta-1)/2))) ), (1/beta));
        vec u = 0.01*normalVec(dim, *rs)*sigma;
        vec v = normalVec(dim, *rs).cwiseAbs();
        vec zz = v.array().pow( vec::Constant(dim, 1.0/beta).array());
        vec step = u.cwiseProduct(zz.cwiseInverse());
        return step;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1; fitfun->getEvaluations() < maxEvaluations; iterations++) {

            // fitness of locations
            for (int i = 0; i < popsize; i++) {
                popX.col(i) = fitfun->getClosestFeasible(popX.col(i));
                double y = fitfun->value(popX.col(i)); // compute fitness
                if (!isfinite(y)) {
                    stop = -1;
                    return;
                }
                if (y < bestY) {
                    // update the location of Rabbit
                    bestY = y;
                    bestX = popX.col(i);
                }
            }
            double e1 = 2*(1-((iterations-1)/maxIter)); // factor to decrease the energy of the rabbit

            // Update the location of the harris hawks
           for (int i = 0; i < popsize; i++) {
                double e0 = 2*rnd01()-1;  // -1<e0<1
                vec xi = popX.col(i);
                double escapingEnergy = e1*e0; // escaping energy of rabbit Eq. (3) in the paper

                // -------- Exploration phase Eq. (1) in paper -------------------

                if (abs(escapingEnergy) >= 1) {
                    // harris hawks perch randomly based on 2 strategy:
                    double q = rnd01();
                    int randHawkIndex = (int)(popsize*rnd01());
                    vec xr = popX.col(randHawkIndex);
                    if (q < 0.5)
                        // perch based on other family members
                    	popX.col(i) = xr - rnd01() * (xr - 2*rnd01()*xi).cwiseAbs();

                    else {
                        // perch on a random tall tree (random site inside group's home range)
                        vec xmean = popX.rowwise().mean();
                        vec rvec = vec::Constant(dim, rnd01());
                        popX.col(i) = (bestX - xmean) - rnd01()*rvec;
                    }
                }
                // -------- Exploitation phase -------------------
                else {
                    //Attacking the rabbit using 4 strategies regarding the behavior of the rabbit

                    //phase 1: ----- surprise pounce (seven kills) ----------
                    // multiple, short rapid dives by different hawks

                    double r = rnd01(); // probability of each event

                    if (r >= 0.5 && abs(escapingEnergy) < 0.5)  // Hard besiege Eq. (6) in paper
                        popX.col(i) = bestX - escapingEnergy*(bestX - xi).cwiseAbs();

                    if (r >= 0.5 && abs(escapingEnergy) >= 0.5) {  // Soft besiege Eq. (4) in paper
                        double jumpStrength = 2*(1- rnd01()); // random jump strength of the rabbit
                        popX.col(i) = (bestX-popX.col(i)) - escapingEnergy*(jumpStrength*bestX-xi).cwiseAbs();
                    }
                    // phase 2: --------performing team rapid dives (leapfrog movements)----------

                    if (r < 0.5 && abs(escapingEnergy) >= 0.5) { // Soft besiege Eq. (10) in paper
                        // rabbit try to escape by many zigzag deceptive motions
                        double jumpStrength = 2 * (1-rnd01());
                        vec x1 = fitfun->getClosestFeasible(
                        		bestX-escapingEnergy*(jumpStrength*bestX-xi).cwiseAbs());
                        double y1 = fitfun->value(x1);
                        if (y1 < bestY) // improved move?
                            popX.col(i) = x1;
                        else { // hawks perform levy-based short rapid dives around the rabbit
                            vec x2 = fitfun->getClosestFeasible(
                            		bestX-escapingEnergy*(jumpStrength*bestX-xi).cwiseAbs() +
                            		normalVec(dim, *rs).cwiseProduct(levy(dim)));
                            double y2 = fitfun->value(x2);
                            if ( y2 < bestY )
                                popX.col(i) = x2;
                        }
                    }
                    if (r < 0.5 && abs(escapingEnergy) < 0.5) {  // Hard besiege Eq. (11) in paper
                         double jumpStrength = 2 * (1-rnd01());
                         vec xmean = popX.rowwise().mean();
                         vec x1 = fitfun->getClosestFeasible(
                        		 bestX - escapingEnergy * (jumpStrength * bestX-xmean).cwiseAbs());

                         if (fitfun->value(x1) < bestY) // improved move?
                            popX.col(i) = x1;
                         else { // Perform levy-based short rapid dives around the rabbit
                             vec x2 = fitfun->getClosestFeasible(
                            		 bestX - escapingEnergy * (jumpStrength*bestX - xmean).cwiseAbs() +
                            		 normalVec(dim, *rs).cwiseProduct(levy(dim)));
                            double y2 = fitfun->value(x2);
                            if ( y2 < bestY )
                                popX.col(i) = x2;
                         }
                    }
                }
                if (isfinite(stopfitness) && bestY < stopfitness) {
                    stop = 1;
                    return;
                }
            }
        }
    }

    void init() {
        // initialize the locations of the harris hawks
        popX = uniform(dim, popsize, *rs);
        // initialize the location and energy of the rabbit
        bestX = zeros(popsize);
        bestY = DBL_MAX;
        maxIter = maxEvaluations / popsize;
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
      long runid;
      Fittness* fitfun;
      int popsize; // population size
      int dim;
      int maxEvaluations;
      int maxIter;
      double stopfitness;
      int iterations;
      double guessValue;
      vec guess;
      double bestY;
      vec bestX;
      int stop;
      pcg64* rs;
      mat popX;
      vec popY;
};

}

using namespace harris_hawks;

extern "C" {
    double* optimizeHH_C(long runid, callback_type func, int dim, int seed,
            double *lower, double *upper, int maxEvals,
            double stopfitness, int popsize) {
        int n = dim;
        double *res = new double[n + 4];
        vec lower_limit(n), upper_limit(n);
        bool useLimit = false;
        for (int i = 0; i < n; i++) {
            lower_limit[i] = lower[i];
            upper_limit[i] = upper[i];
            useLimit |= (lower[i] != 0);
            useLimit |= (upper[i] != 0);
        }
        if (useLimit == false) {
            lower_limit.resize(0);
            upper_limit.resize(0);
        }
        Fittness fitfun(func, lower_limit, upper_limit);
        HHOptimizer opt(
            runid,
            &fitfun,
            dim,
            seed,
            popsize,
            maxEvals,
            stopfitness
            );
        try {
            opt.doOptimize();
            vec bestX = fitfun.decode(opt.getBestX());
            double bestY = opt.getBestValue();
            for (int i = 0; i < n; i++)
                res[i] = bestX[i];
            res[n] = bestY;
            res[n+1] = fitfun.getEvaluations();
            res[n+2] = opt.getIterations();
            res[n+3] = opt.getStop();
            return res;
        } catch (std::exception& e) {
            cout << e.what() << endl;
            return res;
        }
    }
}


#pragma once

#include "base.hpp"

#include <Eigen/Geometry>
#include <cmath>
#include <limits>

template <typename Scalar>
class QuaternionElement final
    : public ManifoldElement<QuaternionElement<Scalar>, Scalar, 3>
{
 public:
  using Base = ManifoldElement<QuaternionElement<Scalar>, Scalar, 3>;
  using TangentVec = typename Base::TangentVec;

  using Quat = Eigen::Quaternion<Scalar>;
  using ElementType = Quat;

  QuaternionElement(const Quat &q = Quat::Identity()) : quat_{q} {}

  Quat const &getValue() const { return quat_; }
  void setValue(const Quat &q) { quat_ = q; }

  QuaternionElement operator+(const TangentVec &diff) const override
  {
    return QuaternionElement(quat_ * vec_to_quat(diff));
  }

  TangentVec operator-(const QuaternionElement &q) const override
  {
    return quat_to_vec(q.getValue().conjugate() * quat_);
  }

  static TangentVec angle_axis_to_vec(const Scalar &angle,
                                      const Eigen::Matrix<Scalar, 3, 1> &axis)
  {
    return axis * angle;
  }

  friend std::ostream &operator<<(std::ostream &stream,
                                  const QuaternionElement &element)
  {
    stream << element.quat_.coeffs().transpose();
    return stream;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  static TangentVec quat_to_vec(const Quat &q)
  {
    const auto q_vec_norm = q.vec().norm();
    if(q_vec_norm < 10 * std::numeric_limits<Scalar>::epsilon())
      return TangentVec::Zero();
    else
    {
      const Scalar sign = (q.w() >= 0) ? 1 : -1;
      return 2 * std::atan2(q_vec_norm, sign * q.w()) * sign * q.vec() /
             q_vec_norm;
    }
  }

  static Quat vec_to_quat(const TangentVec &vec)
  {
    const auto v_norm = vec.norm();
    if(v_norm < 10 * std::numeric_limits<Scalar>::epsilon())
      return Quat::Identity();

    Quat q;
    q.w() = std::cos(v_norm / 2);
    q.vec() = std::sin(v_norm / 2) * vec / v_norm;
    return q;
  }

  Quat quat_;
};


#ifndef ALM_PSEUDOINVERSE
#define ALM_PSEUDOINVERSE

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <vector>

namespace math_utils{
  void pseudoInverse(const Eigen::MatrixXd & A,
                     Eigen::MatrixXd & Apinv,
                     double tolerance,
                     unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);

  void pseudoInverse(const Eigen::MatrixXd & A,
                     Eigen::MatrixXd & Apinv,
                     std::vector<double> & singular_values,
                     double tolerance,
                     unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);

  void pseudoInverse(const Eigen::MatrixXd & A,
                     Eigen::JacobiSVD<Eigen::MatrixXd>& svdDecomposition,
                     Eigen::MatrixXd & Apinv,
                     double tolerance,
                     unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);

  void weightedPseudoInverse(const Eigen::MatrixXd & J, const Eigen::MatrixXd & Winv,
                             Eigen::MatrixXd & Jinv, double tolerance);

  void weightedPseudoInverse(const Eigen::MatrixXd & J, const Eigen::MatrixXd & Winv,
                          Eigen::JacobiSVD<Eigen::MatrixXd> & svdDecomposition,
                          Eigen::MatrixXd & Jinv,
                          double tolerance);

}
#endif


#include "solver/constrained_l1_solver.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <glog/logging.h>

#include <algorithm>
#include <string>

#include "math/sparse_cholesky_llt.h"
#include "util/stringprintf.h"

namespace GraphSfM {

ConstrainedL1Solver::ConstrainedL1Solver(
    const Options& options,
    const Eigen::SparseMatrix<double>& A,
    const Eigen::VectorXd& b,
    const Eigen::SparseMatrix<double>& geq_mat,
    const Eigen::VectorXd& geq_vec)
    : options_(options),
      num_l1_residuals_(b.size()),
      num_inequality_constraints_(geq_vec.size()) {
  CHECK_EQ(A.cols(), geq_mat.cols());
  CHECK_EQ(A.rows(), b.rows());
  CHECK_EQ(geq_mat.rows(), geq_vec.rows());

  // Allocate matrix A.
  A_.resize(A.rows() + geq_mat.rows(), A.cols());

  // Iterate over the input mat and geq_mat and store the entries in A.
  std::vector<Eigen::Triplet<double> > triplets;
  triplets.reserve(A.nonZeros() + geq_mat.nonZeros());
  for (int i = 0; i < A.outerSize(); i++) {
    for (Eigen::SparseMatrix<double>::InnerIterator it(A, i); it; ++it) {
      triplets.emplace_back(it.row(), it.col(), it.value());
    }
  }
  // Store the Geq mat below matrix A.
  for (int i = 0; i < geq_mat.outerSize(); i++) {
    // Iterate over inside
    for (Eigen::SparseMatrix<double>::InnerIterator it(geq_mat, i); it; ++it) {
      triplets.emplace_back(A.rows() + it.row(), it.col(), it.value());
    }
  }
  A_.setFromTriplets(triplets.begin(), triplets.end());

  Eigen::SparseMatrix<double> spd_mat(A.cols(), A.cols());
  spd_mat.selfadjointView<Eigen::Upper>().rankUpdate(A_.transpose());

  linear_solver_.Compute(spd_mat);
  CHECK_EQ(linear_solver_.Info(), Eigen::Success);

  // Set the modified b vector.
  b_.resize(b.size() + geq_vec.size());
  b_.head(b.size()) = b;
  b_.tail(geq_vec.size()) = geq_vec;
}

// We create a modified L1 solver such that ||Bx - b|| is minimized under L1
// norm subject to the constraint geq_mat * x > geq_vec. We conveniently
// create this constraint in ADMM terms as:
//
//    minimize f(x) + g(z_1) + h(z_2)
//    s.t. Bx - b - z_1 = 0
//         Cx - c - z_2 = 0
//
// Where f(x) = 0, g(z_1) = |z_1| and h(z_2) is an indicate function for our
// inequality constraint. This can be transformed into the standard ADMM
// formulation as:
//
//    minimize f(x) + g(z)
//    s.t. A * x - d - z = 0
//
// where A = [B;C] and d=[b;c] (where ; is the "stack" operation like matlab)
// This can now be solved in the same form as the L1 minimization, with a
// slightly different z update.
void ConstrainedL1Solver::Solve(Eigen::VectorXd* solution) {
  CHECK_NOTNULL(solution)->resize(A_.cols());
  Eigen::VectorXd& x = *solution;
  Eigen::VectorXd z(A_.rows()), u(A_.rows());
  z.setZero();
  u.setZero();

  Eigen::VectorXd a_times_x(A_.rows()), z_old(z.size()), ax_hat(A_.rows());
  // Precompute some convergence terms.
  const double rhs_norm = b_.norm();
  const double primal_abs_tolerance_eps =
      std::sqrt(A_.rows()) * options_.absolute_tolerance;
  const double dual_abs_tolerance_eps =
      std::sqrt(A_.cols()) * options_.absolute_tolerance;
  VLOG(2) << "Iteration   R norm          S norm          Primal eps      "
             "Dual eps";
  const std::string row_format =
      "  % 4d     % 4.4e     % 4.4e     % 4.4e     % 4.4e";

  // qp_options.max_num_iterations = 100;
  for (int i = 0; i < options_.max_num_iterations; i++) {
    x.noalias() = linear_solver_.Solve(A_.transpose() * (b_ + z - u));

    if (linear_solver_.Info() != Eigen::Success) {
      LOG(ERROR) << "L1 Minimization failed. Could not solve the sparse "
                    "linear system with Cholesky Decomposition";
      return;
    }

    a_times_x.noalias() = A_ * x;
    ax_hat.noalias() = options_.alpha * a_times_x;
    ax_hat.noalias() += (1.0 - options_.alpha) * (z + b_);

    // Update z and set z_old.
    std::swap(z, z_old);
    z.noalias() = ModifiedShrinkage(ax_hat - b_ + u, 1.0 / options_.rho);

    // Update u.
    u.noalias() += ax_hat - z - b_;

    // Compute the convergence terms.
    const double r_norm = (a_times_x - z - b_).norm();
    const double s_norm = (-options_.rho * A_.transpose() * (z - z_old)).norm();
    const double max_norm = std::max({a_times_x.norm(), z.norm(), rhs_norm});
    const double primal_eps =
        primal_abs_tolerance_eps + options_.relative_tolerance * max_norm;
    const double dual_eps = dual_abs_tolerance_eps +
                            options_.relative_tolerance *
                                (options_.rho * A_.transpose() * u).norm();

    // Log the result to the screen.
    VLOG(2) << GraphSfM::StringPrintf(
        row_format.c_str(), i, r_norm, s_norm, primal_eps, dual_eps);
    // Determine if the minimizer has converged.
    if (r_norm < primal_eps && s_norm < dual_eps) {
      break;
    }
  }
}

Eigen::VectorXd ConstrainedL1Solver::ModifiedShrinkage(
    const Eigen::VectorXd& vec, const double kappa) {
  Eigen::VectorXd output(num_l1_residuals_ + num_inequality_constraints_);

  // Get an array for the subset of l1 terms in the input vec.
  Eigen::Map<const Eigen::ArrayXd> l1_array(vec.data(), num_l1_residuals_);
  Eigen::Map<const Eigen::ArrayXd> inequality_array(
      vec.data() + num_l1_residuals_, num_inequality_constraints_);

  // Compute the L1 proximal operator on the L1 terms.
  output.head(num_l1_residuals_).array() =
      (l1_array - kappa).max(0.0) - (-l1_array - kappa).max(0.0);
  // Project the inequality constraints such that geq_mat * x - geq_vec > 0
  output.tail(num_inequality_constraints_).array() = inequality_array.max(0.0);
  return output;
}

}  // namespace GraphSfM


/*
 * Copyright Nick Thompson, 2019
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */

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

using boost::math::rsqrt;

template<typename Real>
void test_rsqrt()
{
    std::cout << "Testing rsqrt on type " << boost::core::demangle(typeid(Real).name()) << "\n";
    using std::sqrt;
    Real x = std::numeric_limits<Real>::min();
    while (x < 10000*std::numeric_limits<Real>::epsilon()) {
        Real expected = 1/sqrt(x);
        Real computed = rsqrt(x);
        if(!CHECK_ULP_CLOSE(expected, computed, 2)) {
            std::cerr << "  1/sqrt(" << x << ") is computed incorrectly.\n";
        }
        x += std::numeric_limits<Real>::epsilon();
    }

    // x ~ 1:
    x = 1;
    while (x < 1 + 1000*std::numeric_limits<Real>::epsilon()) {
        Real expected = 1/sqrt(x);
        Real computed = rsqrt(x);
        if(!CHECK_ULP_CLOSE(expected, computed, 2)) {
            std::cerr << "  1/sqrt(" << x << ") is computed incorrectly.\n";
        }
        x += std::numeric_limits<Real>::epsilon();
    }

    // x ~ 1000:
    x = 1000;
    while (x < 1000 + 1000*1000*std::numeric_limits<Real>::epsilon()) {
        Real expected = 1/sqrt(x);
        Real computed = rsqrt(x);
        if(!CHECK_ULP_CLOSE(expected, computed, 2)) {
            std::cerr << "  1/sqrt(" << x << ") is computed incorrectly.\n";
        }
        x += 1000*std::numeric_limits<Real>::epsilon();
    }

    x = std::numeric_limits<Real>::infinity();
    Real expected = 1/sqrt(x);
    Real computed = rsqrt(x);
    if (!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "Reciprocal square root of infinity not correctly computed.\n";
    }

    x = std::numeric_limits<Real>::max();
    expected = 1/sqrt(x);
    computed = rsqrt(x);
    if (!CHECK_EQUAL(expected, computed)) {
        std::cerr << "Reciprocal square root of std::numeric_limits<Real>::max() not correctly computed.\n";
    }

    if (!CHECK_NAN(rsqrt(std::numeric_limits<Real>::quiet_NaN()))) {
        std::cerr << "Reciprocal square root of std::numeric_limits<Real>::quiet_NaN() is not a NaN.\n";
    }
}

int main()
{
    test_rsqrt<float>();
    test_rsqrt<double>();
    test_rsqrt<long double>();
    test_rsqrt<boost::multiprecision::cpp_bin_float_50>();
    test_rsqrt<boost::multiprecision::cpp_bin_float_100>();

    #ifdef BOOST_HAS_FLOAT128
    test_rsqrt<float128>();
    #endif
    return boost::math::test::report_errors();
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>
#include <mla/matrix/convert.h++>

#include <mla/solvers/CG.h++>

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>,
	mla::matrix::SparseCRS<float>,
	mla::matrix::SparseCRS<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_solvers)

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_1, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	MatrixType A(matrix_size, matrix_size);
	A.setEye();

	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 1.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		b.setValue( i, value);
	}

	mla::cg(A, x, b, 0.01f, 6);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), 1.0f/b.getValue(i), 0.001f);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_2, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	MatrixType A(matrix_size, matrix_size);
	A.setEye();

	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 2.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		b.setValue( i, value);
	}

	mla::cg(A, x, b, 0.01f, 6);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), b.getValue(i), 0.001f);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( solve_unit_3, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	mla::matrix::DenseRowMajor<Scalar> from(matrix_size, matrix_size);
	mla::vector::Dense<Scalar> x(matrix_size), b(matrix_size);

	Scalar value = 2.0f;

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		from.setValue( i, i, value);
		b.setValue( i, (Scalar)1.0f);
	}

	MatrixType A(matrix_size, matrix_size);
	mla::matrix::convert(from, A);

	mla::cg(A, x, b, 0.01f, 6);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), 1.0f/value, 0.001f);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <armadillo>

using namespace std;
using namespace arma;

namespace ic {
using Particion = pair<uvec, uvec>;

// Particionamiento con reposición
vector<Particion> particionar(const mat& datos, int nParticiones, double porcentajeEnt)
{
	vector<Particion> particiones;
	const int nPatronesEnt = datos.n_rows * porcentajeEnt / 100;

    // Creo el vector de índices.
    // Va desde 0 hasta la cantidad de patrones menos 1.
	uvec indices = linspace<uvec>(0, datos.n_rows - 1, datos.n_rows);

	for (int i = 0; i < nParticiones; ++i) {
		indices = shuffle(indices);
        particiones.push_back({indices.head(nPatronesEnt),
                               indices.tail(indices.n_rows - nPatronesEnt)});
	}

	return particiones;
}

// Particionamiento sin reposición
vector<Particion> leaveKOut(const mat& datos, int k)
{
	vector<Particion> particiones;
    const int nParticiones = datos.n_rows / k;
	const uvec indices = shuffle(linspace<uvec>(0, datos.n_rows - 1, datos.n_rows));

	for (int i = 0; i < nParticiones; ++i) {
        uvec indicesPrueba = indices.rows(span(k * i, (i + 1) * k - 1)); // De 0 a k-1, de k a 2k-1, ...
		uvec indicesEnt;

		if (k * i - 1 >= 0)
			indicesEnt.insert_rows(0, indices.rows(span(0, k * i - 1)));

        if ((i + 1) * k <= int(indices.n_elem - 1)) {
            // Si es la última partición, vamos a meter los elementos del final del bloque de datos
            // en la partición de prueba
            if (i == nParticiones - 1)
                indicesPrueba.insert_rows(indicesPrueba.n_elem, indices.rows(span((i + 1) * k, indices.n_elem - 1)));
            else
                indicesEnt.insert_rows(indicesEnt.n_elem, indices.rows(span((i + 1) * k, indices.n_elem - 1)));
        }

		particiones.push_back({indicesEnt, indicesPrueba});
	}

	return particiones;
}

void guardarParticiones(const vector<Particion>& particiones, string rutaCarpeta)
{
	for (unsigned int i = 0; i < particiones.size(); ++i) {
		particiones[i].first.save(rutaCarpeta + "particionEnt" + to_string(i), arma_ascii);
		particiones[i].second.save(rutaCarpeta + "particionPrueba" + to_string(i), arma_ascii);
	}
}

vector<Particion> cargarParticiones(string rutaCarpeta, int nParticiones)
{
	vector<Particion> particiones;
	particiones.resize(nParticiones);

	for (int i = 0; i < nParticiones; ++i) {
		particiones[i].first.load(rutaCarpeta + "particionEnt" + to_string(i));
		particiones[i].second.load(rutaCarpeta + "particionPrueba" + to_string(i));
	}

	return particiones;
}
}


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <NTL/ZZ.h>
#include <helib/polyEval.h>
#include <helib/EncryptedArray.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

#ifdef DEBUG_PRINTOUT
extern helib::SecKey* dbgKey;
extern helib::EncryptedArray* dbgEa;
#endif

namespace {
struct Parameters {
    Parameters(long p, long r, long m, long d, long k, long max_d, long L, bool isMonic) :
        p(p),
        r(r),
        m(m),
        d(d),
        k(k),
        max_d(max_d),
        L(L),
        isMonic(isMonic)
    {};

  const long p;       //   p is the plaintext base
  const long r;       //   r is the lifting
  const long m;       //   m is a specific cyclotomic ring
  const long d;       //   d is the polynomial degree
  const long k;       //   k is the baby-step parameter
  const long max_d;
  const long L;
  const bool isMonic;

    friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
    {
        return os << "{"
            << "p=" << params.p << ","
            << "r=" << params.r << ","
            << "m=" << params.m << ","
            << "d=" << params.d << ","
            << "k=" << params.k << ","
            << "max_d=" << params.max_d << ","
            << "L=" << params.L << ","
            << "isMonic=" << params.isMonic
            << "}";
    };
};

class GTestPolyEval : public ::testing::TestWithParam<Parameters>
{
    protected:
        long p;
        long r;
        long d;
        long max_d;
        long L;
        bool isMonic;
        long m;
        long k;
        helib::Context context;
        long p2r;
        helib::EncryptedArray ea;
        helib::SecKey secretKey;
        const helib::PubKey &publicKey;

        GTestPolyEval() :
            p(GetParam().p),
            r(GetParam().r),
            d(GetParam().d),
            max_d(GetParam().max_d),
            L(GetParam().L),
            isMonic(GetParam().isMonic),
            m(GetParam().m),
            k(GetParam().k),
            context((helib::setDryRun(helib_test::dry), m), p, r),
            p2r(context.alMod.getPPowR()),
            ea((helib::buildModChain(context, L, /*c=*/3), context)),
            secretKey(context),
            publicKey((secretKey.GenSecKey(), secretKey))
            //  addSome1DMatrices(secretKey); // compute key-switching matrices
    {};

    virtual void SetUp() override {
#ifdef DEBUG_PRINTOUT
        helib::dbgEa = &ea;        // for debugging purposes
        helib::dbgKey = &secretKey;
#endif
        if (!helib_test::noPrint) std::cout << (helib::isDryRun()? "* dry run, " : "* ")
        << "degree-"<<d<<", m="<<m<<", L="<<L<<", p^r="<<p2r<<std::endl;
    };

    virtual void TearDown() override
    {
      helib::cleanupGlobals();
    }
};

TEST_P(GTestPolyEval, encryptedPolynomialsEvaluateAtEncryptedPointCorrectly)
{
  NTL::zz_pBak bak; bak.save(); NTL::zz_p::init(p);
  NTL::zz_pXModulus phimX = NTL::conv<NTL::zz_pX>(ea.getPAlgebra().getPhimX());

  // Choose random plaintext polynomials
  NTL::zz_pX pX = NTL::random_zz_pX(deg(phimX)-1);
  NTL::Vec<NTL::zz_pX> ppoly(NTL::INIT_SIZE, d);
  for (long i=0; i<ppoly.length(); i++) random(ppoly[i], deg(phimX)-1);

  // Evaluate the non-encrypted polynomial
  NTL::zz_pX pres = (ppoly.length()>0)? ppoly[ppoly.length()-1] : NTL::zz_pX::zero();
  for (long i=ppoly.length()-2; i>=0; i--) {
    MulMod(pres, pres, pX, phimX);
    pres += ppoly[i];
  }

  // Encrypt the random polynomials
  helib::Ctxt cX(publicKey);
  NTL::Vec<helib::Ctxt> cpoly(NTL::INIT_SIZE, d, cX);

  secretKey.Encrypt(cX, NTL::conv<NTL::ZZX>(pX));

  for (long i=0; i<ppoly.length(); i++)
    secretKey.Encrypt(cpoly[i], NTL::conv<NTL::ZZX>(ppoly[i]));

  // Evaluate the encrypted polynomial
  helib::polyEval(cX, cpoly, cX);

  // Compare the results
  NTL::ZZX ret;
  secretKey.Decrypt(ret, cX);
  NTL::zz_pX cres = NTL::conv<NTL::zz_pX>(ret);
  EXPECT_EQ(cres, pres) << "encrypted poly MISMATCH";
};

TEST_P(GTestPolyEval, evaluatePolynomialOnCiphertext)
{
  // evaluate at random points (at least one co-prime with p)
    std::vector<long> x;
  ea.random(x);
  while (NTL::GCD(x[0],p)!=1) { x[0] = NTL::RandomBnd(p2r); }
  helib::Ctxt inCtxt(publicKey), outCtxt(publicKey);
  ea.encrypt(inCtxt, publicKey, x);

  NTL::ZZX poly;
  for (long i=d; i>=0; i--)
    SetCoeff(poly, i, NTL::RandomBnd(p2r)); // coefficients are random
  if (isMonic) SetCoeff(poly, d);    // set top coefficient to 1

  // Evaluate poly on the ciphertext
  helib::polyEval(outCtxt, poly, inCtxt, k);

  // Check the result
  std::vector<long> y;
  ea.decrypt(outCtxt, secretKey, y);
  for (long i=0; i<ea.size(); i++) {
    EXPECT_EQ(helib::polyEvalMod(poly, x[i], p2r), y[i])
        << "plaintext poly MISMATCH\n";
  }
};

std::vector<Parameters> getParameters()
{
    std::vector<Parameters> allParams;

    //SLOW
    const long p = 7;
    const long r = 2;
          long m = 0;
    const long k = 0;
    long d = 34;

    //FAST
    //const long p = 3;
    //const long r = 2;
    //      long m = 91;
    //const long k = 0;
    //long d = -1;

    const long max_d = (d<=0)? 35 : d;
    const long L = (7+NTL::NextPowerOfTwo(max_d))*30;

    if(m < 2) {
        m = helib::FindM(/*secprm=*/80, L, /*c=*/3, p, 1, 0, m, !helib_test::noPrint);
    }

    // Test both monic and non-monic polynomials of this degree
    if(d >= 0) {
        allParams.push_back(Parameters{p, r, m, d, k, max_d, L, false});
        allParams.push_back(Parameters{p, r, m, d, k, max_d, L,  true});
    } else {
      // Test degrees 1 to 3 and 25 through 35
      for(d = 1; d <= 3; d += 2)
        allParams.push_back(Parameters{p, r, m, d, k, max_d, L, true});
      for(d = 25; d <= 33; d += 2)
        allParams.push_back(Parameters{p, r, m, d, k, max_d, L, true});
    }
    return allParams;
};

INSTANTIATE_TEST_SUITE_P(manyDegrees, GTestPolyEval, ::testing::ValuesIn(getParameters()));

} // namespace


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/algorithms/within.hpp>
#include <boost/geometry/multi/core/point_type.hpp>
#include <boost/geometry/multi/geometries/multi_geometries.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/extensions/nsphere/nsphere.hpp>

int test_main( int , char* [] )
{
    typedef bg::model::d2::point_xy<double> gl_point;
    typedef bg::model::nsphere<gl_point, double> gl_circle;
    typedef bg::model::ring<gl_point> gl_ring;
    typedef bg::model::polygon<gl_point> gl_polygon;
    typedef bg::model::multi_polygon<gl_polygon> gl_multi_polygon;

    gl_circle circle(gl_point(1, 1), 2.5);

    gl_ring ring;
    ring.push_back(gl_point(0,0));
    ring.push_back(gl_point(1,0));
    ring.push_back(gl_point(1,1));
    ring.push_back(gl_point(0,1));
    bg::correct(ring);

    gl_polygon pol;
    pol.outer() = ring;
    gl_multi_polygon multi_polygon;
    multi_polygon.push_back(pol);

    // Multipolygon in circle
    BOOST_CHECK_EQUAL(bg::within(multi_polygon, circle), true);

    multi_polygon.front().outer().insert(multi_polygon.front().outer().begin() + 1, gl_point(10, 10));
    BOOST_CHECK_EQUAL(bg::within(multi_polygon, circle), false);

    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <iterator>

#include <boost/compute/algorithm/max_element.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/geometry.hpp>
#include <boost/compute/iterator/transform_iterator.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace compute = boost::compute;

// this example shows how to use the max_element() algorithm along with
// a transform_iterator and the length() function to find the longest
// 4-component vector in an array of vectors
int main()
{
    using compute::float4_;

    // vectors data
    float data[] = { 1.0f, 2.0f, 3.0f, 0.0f,
                     4.0f, 5.0f, 6.0f, 0.0f,
                     7.0f, 8.0f, 9.0f, 0.0f,
                     0.0f, 0.0f, 0.0f, 0.0f };

    // create device vector with the vector data
    compute::vector<float4_> vector(
        reinterpret_cast<float4_ *>(data),
        reinterpret_cast<float4_ *>(data) + 4
    );

    // find the longest vector
    compute::vector<float4_>::const_iterator iter =
        compute::max_element(
            compute::make_transform_iterator(
                vector.begin(), compute::length<float4_>()
            ),
            compute::make_transform_iterator(
                vector.end(), compute::length<float4_>()
            )
        ).base();

    // print the index of the longest vector
    std::cout << "longest vector index: "
              << std::distance(vector.begin(), iter)
              << std::endl;

    return 0;
}


/* $Id: step-1.cc 23709 2011-05-17 04:34:08Z bangerth $
 *
 * Copyright (C) 1999, 2000, 2001, 2002, 2003, 2005, 2006, 2007, 2009, 2011 by the deal.II authors
 *
 * This file is subject to QPL and may not be  distributed
 * without copyright and license information. Please refer
 * to the file deal.II/doc/license.html for the  text  and
 * further information on this license.
 */

/*
 * This is a modified version for the exercises of the lecture
 * finite elements at University of Hamburg in Summer 2014
 *
 */

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/grid_out.h>
#include <fstream>
#include <cmath>

using namespace dealii;
void first_grid ()

{
  Triangulation<2> triangulation;
  GridGenerator::hyper_cube (triangulation);
  triangulation.refine_global (4);
  std::ofstream out ("grid-1.eps");
  GridOut grid_out;
  grid_out.write_eps (triangulation, out);
}

void second_grid ()
{
  Triangulation<2> triangulation;
  const Point<2> center (1,0);
  const double inner_radius = 0.5,
               outer_radius = 1.0;
  GridGenerator::hyper_shell (triangulation,
                              center, inner_radius, outer_radius,
			      10);
  const HyperShellBoundary<2> boundary_description(center);
  triangulation.set_boundary (0, boundary_description);

  for (unsigned int step=0; step<5; ++step)
    {
      Triangulation<2>::active_cell_iterator
	cell = triangulation.begin_active(),
	endc = triangulation.end();
      for (; cell!=endc; ++cell)
	for (unsigned int v=0;
             v < GeometryInfo<2>::vertices_per_cell;
             ++v)
	{
	  const double distance_from_center
	    = center.distance (cell->vertex(v));

	  if (std::fabs(distance_from_center - inner_radius) < 1e-10)
	  {
	    cell->set_refine_flag ();
	    break;
	  }
	}
      triangulation.execute_coarsening_and_refinement ();
    }
  std::ofstream out ("grid-2.eps");
  GridOut grid_out;
  grid_out.write_eps (triangulation, out);

  triangulation.set_boundary (0);
}

int main ()
{
  first_grid ();
  second_grid ();
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <complex>

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

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

int main(int,char**)
{
  const std::size_t NumDimV = 1;
	const int Nx = 64, Nv = 128, Nb_iter=100;

	field<double,NumDimV> f( boost::extents[Nv][Nx] );

	f.range.v_min = -10.; f.range.v_max = 10;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;
	f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

	const double dt = 1.606*f.step.dv/0.6; //0.5*6.*math::pi<double>()/(Nv*f.range.v_max);

	ublas::vector<double> v (Nv,1.);
  ublas::vector<double> E (Nx,1.),rho(Nx);
  for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  //for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

	const double lx = f.range.x_max-f.range.x_min;
  const double lv = f.range.v_max-f.range.v_min;
	ublas::vector<double> kx(Nx),kv(Nv);
  for ( int i=0  ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/lx; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/lx; }

  for ( int k=0  ; k<Nv/2 ; ++k ) { kv[k]    = 2.*math::pi<double>()*k/lv; }
  for ( int k=-Nv/2 ; k<0 ; ++k ) { kv[Nv+k] = 2.*math::pi<double>()*k/lv; }

  double np = 0.9 , nb = 0.2 , ui = 4.5;
  for (field<double,NumDimV>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,NumDimV>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] = ( std::exp(-0.5*SQ(Vk(k)))*np/std::sqrt(2.*math::pi<double>()) + nb/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)/0.25) )*(1.+0.04*std::cos(0.3*Xi(i)));
      //f[k][i] = std::cos(Xi(i)*0.2)*std::cos(math::pi<double>()*2.*Vk(k)/20);
      //f[k][i] = std::exp( -SQ(Xi(i)-1.)/2. )*std::exp( -Vk(k)*Vk(k)/1.);
      //f[k][i] = ( std::exp(-0.5*SQ(Vk(k)))*np/std::sqrt(2.*math::pi<double>()) )*(1.+0.04*std::cos(0.3*Xi(i)));
    }
  }
  f.write("init.dat");

  poisson<double> poisson_solver(Nx,lx);

  double Tf = 60.;//2*math::pi<double>();
  int i_t=0;

  std::cout << f.size(0) << "x" << f.size(1) << std::endl;
  std::cout << "dt " << dt << std::endl;
  std::cout << "dx " << f.step.dx << std::endl;
  std::cout << "dv " << f.step.dv << std::endl;
  std::cout << "Tf " << Tf << std::endl;

  ublas::vector<double> ee(int(Tf/dt)+1.);
  ublas::vector<double> Emax(int(Tf/dt)+1.);
  ublas::vector<double> H(int(Tf/dt)+1.);

  while ( i_t*dt < Tf ) {
    std::cout<<" \r"<<i_t<<" "<<std::flush;

    field<double,1> f1 = f;
    field<double,1> f2 = f;

    fft::spectrum_ hxf(Nx);
	  for ( std::size_t k=0 ; k<Nv ; ++k ) {
	  	hxf.fft(&(f[k][0]));

	  	for ( std::size_t i=0 ; i<Nx ; ++i ) {
        hxf[i] = std::exp( -I*v(k)*kx[i]*0.5*dt )*hxf[i];
	  	}

	  	hxf.ifft(&(f1[k][0]));
	  }

    fft::spectrum_ hvf(Nv);
    rho = f1.density();
    E = poisson_solver(rho);
    for ( std::size_t i=0 ; i<Nx ; ++i ) {
      std::valarray<double> fi(Nv); for ( std::size_t k=0 ; k<Nv ; ++k ) { fi[k] = f1[k][i]; }
      hvf.fft(&(fi[0]));

      for ( std::size_t k=0 ; k<Nv ; ++k ) {
        hvf[k] = std::exp( -I*E(i)*kv[k]*dt )*hvf[k];
      }

      hvf.ifft(&(fi[0]));
      for ( std::size_t k=0 ; k<Nv ; ++k ) { f2[k][i] = fi[k]; }
    }

	  for ( auto k=0 ; k<f.size(0) ; ++k ) {
	  	hxf.fft(&(f2[k][0]));

	  	for ( auto i=0 ; i<Nx ; ++i ) {
        hxf[i] = std::exp( -I*v(k)*kx[i]*0.5*dt )*hxf[i];
	  	}

	  	hxf.ifft(&(f[k][0]));
	  }

    rho = f.density();
    E = poisson_solver(rho);
    ee(i_t) = 0.;
    for ( auto i=0 ; i<Nx ; ++i ) {
      ee(i_t) += SQ(E(i))*f.step.dx;
    }
    H(i_t) = energy(f,E);
    ++i_t;
	}

  std::cout << " \r" << i_t << std::endl;

  f.write("vp.dat");

  std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<count*dt<<" "<<y; return ss.str(); };
  of.open("ee.dat");
  for ( auto i=0; i<ee.size() ; ++i ) {
    of << i*dt <<" " << ee[i] << "\n";
  }
  of.close();
  of.open("H.dat");
  for ( auto i=0; i<H.size() ; ++i ) {
    of << i*dt <<" " << (H[i]-H[0])/std::abs(H[0]) << "\n";
  }
  of.close();
  of.open("Emax.dat");
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

	return 0;
}


#define _USE_MATH_DEFINES

#include "fastDonut.h"

#include <Eigen/Core>
#include <Eigen/Dense>

std::vector<std::vector<fast::calPoint>> fast::fastDonut(int center_x, int center_y, int radius, double start_ratio, double end_ratio, double step_angle) {

	if (radius == 0) {
		HVERROR(error, "Invalid box roi range");
	}

	if (step_angle <= 0) {
		HVERROR(error, "Invalid step angle");
	}

	if (start_ratio <= 0 && end_ratio <= 0) {
		HVERROR(error, "Invalid ratio");
	}

	int start_radius = (int)((double)radius * start_ratio);
	int end_radius = (int)((double)radius * end_ratio);

	int distance_radius = (int)fabs(start_radius - end_radius);
	if (distance_radius == 0) {
		HVERROR(error, "Invalid start ratio and end ratio");
	}

	int aligned_distance_radius = distance_radius + (distance_radius % 4);
	int min_radius = start_radius > end_radius ? end_radius : start_radius;
	int max_radius = start_radius > end_radius ? start_radius : end_radius;

	/// Radius SIMD
	std::vector<double> vector_radius; // Radius vector
	vector_radius.resize(aligned_distance_radius * 2);

	int sign = 1;
	if (start_radius > end_radius)
		sign = -1;

	int current_radius = start_radius;
	for (int radius = 0; radius < distance_radius * 2;) {
		vector_radius[radius] = current_radius;
		vector_radius[radius + 1] = current_radius;
		radius += 2;
		current_radius += sign;
	}

	/// Coordinate SIMD
	std::vector<double> vector_cordinate_table = { (double)center_x , (double)center_y, (double)center_x , (double)center_y }; // cordinate vector
	const __m256d simd_coordinate = _mm256_load_pd(vector_cordinate_table.data()); // cordinate simd

	std::vector<std::vector<fast::calPoint>> combine_vertical_xy;
	int doubleDistance = distance_radius * 2;
	int chunk_size = sizeof(double) * 4;

	for (double angle = 0; angle < 360;) {

		/// trigonometric SIMD
		double x_cos = sin(angle * M_PI / 180);
		double y_sine = cos(angle * M_PI / 180);
		std::vector<double> vector_trigonometric_table = { x_cos ,y_sine, x_cos ,y_sine }; // trigonometric vector
		const __m256d simd_trigonometric = _mm256_load_pd(vector_trigonometric_table.data()); // trigonometric simd

		std::vector<fast::calPoint> vertical_xy;
		vertical_xy.resize(aligned_distance_radius);
		void* vertical_xy_ptr = vertical_xy.data();
		for (int simd_skip = 0; simd_skip < doubleDistance;) {

			const __m256d simd_radius = _mm256_load_pd(vector_radius.data() + simd_skip); // radius simd
			__m256d chunk_mul = _mm256_mul_pd(simd_radius, simd_trigonometric);
			__m256d start_result = _mm256_add_pd(chunk_mul, simd_coordinate);
			memcpy((((double*)vertical_xy_ptr) + simd_skip), &start_result, chunk_size);
			simd_skip += 4;
		}
		vertical_xy.resize(distance_radius);
		combine_vertical_xy.push_back(vertical_xy);

		angle += step_angle;
	}

	return combine_vertical_xy;
}


// Effectively recursive lambda in C++, std::function technique
//
// Copyright (c) 2020 Eliah Kagan
//
// Permission to use, copy, modify, and/or distribute this software for any
// purpose with or without fee is hereby granted.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
// SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
// OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
// CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <tuple>
#include <unordered_map>
#include <boost/multiprecision/cpp_int.hpp>

int main()
{
    using boost::multiprecision::cpp_int;
    using Key = std::tuple<cpp_int, unsigned>;

    constexpr auto hash = [](const Key& key) {
        constexpr std::size_t seed {127}, multiplier {131'071};
        const auto& [base, exponent] = key;

        auto code = seed;
        code = code * multiplier + std::hash<cpp_int>{}(base);
        code = code * multiplier + exponent;
        return code;
    };

    std::unordered_map<Key, cpp_int, decltype(hash)> memo;

    std::function<cpp_int(cpp_int, unsigned)>
    my_pow = [&memo, &my_pow](const cpp_int& base, unsigned exponent) {
        if (exponent == 0) return cpp_int{1};

        auto p = memo.find({base, exponent});
        if (p != end(memo)) return p->second;

        auto result = my_pow(base, exponent / 2);
        result *= result;
        if (exponent % 2 != 0) result *= base;

        memo[{base, exponent}] = result;
        return result;
    };

    cpp_int ans = my_pow(7, 1013);
    std::cout << "Computed value: " << ans << '\n';

    cpp_int known = pow(cpp_int{7}, 1013);
    std::cout << "Accepted value: " << known << '\n';

    std::cout << "Correct?  " << std::boolalpha << (ans == known) << '\n';
}


#include <math.h>
#include <iostream>
#include <armadillo>
#include <vector>
#include <complex>
#include <fftw3.h>
#include "mkl.h"

//#include <omp.h>

#include "tensor.h"
#include "tensor.cpp"
#include "cp_als.cpp"
#include "tucker_hosvd.cpp"

using namespace std;

#include "time.h"
double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0; //time:s
};

using namespace std;
using namespace arma;

int main() {
    double t0,t1;
    int I=3;
    int R=0.2*I;
    R=1;
    //    arma_rng::set_seed(1);

    t0=gettime();
    Tensor<float> a(I,I,I);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

    for (int i = 0; i < a.n1; ++i) {
        for (int j = 0; j < a.n2; ++j) {
            for(int k=0; k< a.n3; ++k) {
                a(i,j,k) = randu<float>();
            }
        }
    }
//    arma_rng::set_seed(1);
//    Tensor<float> b(I,I,I);
//    for (int i = 0; i < b.n1; ++i) {
//        for (int j = 0; j < b.n2; ++j) {
//            for(int k=0; k<b.n3; ++k) {
//                b(i,j,k) = randu<float>();
////                cout << a(i,j,k) - b(i,j,k) << endl;
//            }
//        }
//    }

    Tensor<double> b(I,I,I);

//    t0=gettime();
//    tucker_core<float> result;
//    result = hosvd(a,R,R,R);
//    t1=gettime();
//    cout << "time:" <<t1-t0 <<endl;

    t0=gettime();
    cp_mats<float> result;
    result = cp_als(a,R);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

//    t0=gettime();
//        HOSVD(a,R,R,R);
//        hosvd(a,R,R,R);
//        cp_als(a,R);
//        cpals(a,R);
//    t1=gettime();
//    cout << "time:" <<t1-t0 <<endl;

//    Tensor<double> b(2,3,5),d(5,5,5), z(2,3,4),t(5,5,5);
//    cout<<z(0,1,2)<<endl;
//    z=z.zeros(2,3,4);
//    cout<<z(0,1,2)+123<<endl;
//
//    int *c=getsize(b);
//    cout<<c[0]<<endl; //tensor大小
//    cout<<sizeof(a)<<endl;
//    cout<<norm(a)<<endl;
//    cout<<t(1,2,3)<<endl;

//    mat m1 = ten2mat(a,1);
//    cout << m1 << endl;
//    mat m2 = ten2mat(a,2);
//    cout << m2 << endl;
//    mat m3 = ten2mat(a,3);,
//    cout << m3 << endl;

//    cout<<slice(a,0,2)<<endl;
//slice
//    cout<<fiber(a,1,2,2)<<endl;

    return 0;

}


#include <random>
#include <math.h>

#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <cml/cml.h>

#include <framework/misc.h>
#include <framework/vector.h>

namespace fs = boost::filesystem;

// Our global random number generator.
static std::mt19937 rng;

namespace fw {

float distance_between_line_and_point(vector const &start,
    vector const &direction, vector const &point) {
  // see: http://local.wasp.uwa.edu.au/~pbourke/geometry/pointline/
  vector end = start + direction;
  float u = (point[0] - start[0]) * (end[0] - start[0])
      + (point[1] - start[1]) * (end[1] - start[1])
      + (point[2] - start[2]) * (end[2] - start[2]);

  // this is the point on the line closest to 'point'
  vector point_on_line = start + (direction * u);

  // therefore, the distance is just the length of the (point - point_line_to) vector.
  vector vector_to_point = point - point_on_line;
  return vector_to_point.length();
}

// Gets the distance between the given line segment and point. This is different to
// distance_between_line_and_point, which looks at an infinite line, where as this one only looks
// at the given line *segment*.
float distance_between_line_segment_and_point(vector const &start,
    vector const &end, vector const &point) {
  // see: http://local.wasp.uwa.edu.au/~pbourke/geometry/pointline/
  float u = (point[0] - start[0]) * (end[0] - start[0])
      + (point[1] - start[1]) * (end[1] - start[1])
      + (point[2] - start[2]) * (end[2] - start[2]);

  if (u < 0.0f)
    u = 0.0f;
  if (u > 1.0f)
    u = 1.0f;

  // this is the point on the line closest to 'point'
  vector point_on_line = start + ((end - start) * u);

  // therefore, the distance is just the length of the (point - point_line_to) vector.
  vector vector_to_point = point - point_on_line;
  return vector_to_point.length();
}

// Returns the angle, in radians, between a and b
float angle_between(vector const &a, vector const &b) {
  vector lhs = a;
  vector rhs = b;

  float cosangle = cml::dot(lhs.normalize(), rhs.normalize());
  return acos(cosangle);
}

fw::vector point_plane_intersect(vector const &plane_pt,
    vector const &plane_normal, vector const &p_start, vector const &p_dir) {
  // see: http://local.wasp.uwa.edu.au/~pbourke/geometry/planeline/
  vector end = p_start + p_dir;

  float numerator = cml::dot(plane_normal, plane_pt - p_start);
  float denominator = cml::dot(plane_normal, end - p_start);
  float u = numerator / denominator;
  return p_start + (p_dir * u);
}

float random() {
  return static_cast<float>(rng()) / static_cast<float>(rng.max());
}

void random_initialize() {
  std::random_device rd;
  rng.seed(rd());
}

fw::vector get_direction_to(fw::vector const &from, fw::vector const &to,
    float wrap_x, float wrap_z) {
  fw::vector dir = to - from;

  // if we're not wrapping, the direction is just the "simple" direction
  if (wrap_x == 0 && wrap_z == 0)
    return dir;

  // otherwise, we'll also try in the various other directions and return the shortest one
  for (int z = -1; z <= 1; z++) {
    for (int x = -1; x <= 1; x++) {
      fw::vector another_to(to[0] + (x * wrap_x), to[1], to[2] + (z * wrap_z));
      fw::vector another_dir = another_to - from;

      if (another_dir.length_squared() < dir.length_squared())
        dir = another_dir;
    }
  }

  return dir;
}

// Calculates the distance between 'from' and 'to', taking into consideration the fact that
// the world wraps at (wrap_x, wrap_z).
float calculate_distance(fw::vector const &from, fw::vector const &to,
    float wrap_x, float wrap_z) {
  return (get_direction_to(from, to, wrap_x, wrap_z).length());
}

}