// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

// This file was modified by Oracle on 2016.
// Modifications copyright (c) 2016, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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)

#ifndef BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/size_t.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
    BOOST_MPL_ASSERT_MSG((false),
                         NOT_IMPLEMENTED_FOR_THIS_DIMENSION,
                         (mpl::size_t<Dimension>));
};

template <>
struct cross_product<2>
{
    template <typename P1, typename P2, typename ResultP>
    static inline void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();
        assert_dimension<ResultP, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        set<0>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

template <>
struct cross_product<3>
{
    template <typename P1, typename P2, typename ResultP>
    static inline void apply(P1 const& p1, P2 const& p2, ResultP& result)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();
        assert_dimension<ResultP, 3>();

        set<0>(result, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(result, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(result, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

*/
template <typename ResultP, typename P1, typename P2>
inline ResultP cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<ResultP>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<P2>) );

    ResultP result;
    detail::cross_product<dimension<ResultP>::value>::apply(p1, p2, result);
    return result;
}

/*!
\brief Computes the cross product of two vectors.
\details All vectors should have the same dimension, 3 or 2.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector

\qbk{[heading Examples]}
\qbk{[cross_product] [cross_product_output]}
*/
template <typename P>
inline P cross_product(P const& p1, P const& p2)
{
    BOOST_CONCEPT_ASSERT((concepts::Point<P>));
    BOOST_CONCEPT_ASSERT((concepts::ConstPoint<P>));

    P result;
    detail::cross_product<dimension<P>::value>::apply(p1, p2, result);
    return result;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_CROSS_PRODUCT_HPP


/*
 * simulated_telemetry_server.cpp
 *
 *  Created on: Dec 5, 2018
 *      Author: ttw2xk
 */

#include "f1_datalogger/car_data/car_data.h"
#include <boost/program_options.hpp>
#include <iostream>
#include <boost/asio.hpp>
#include <memory>
#include <thread>
#include <math.h>
#include <boost/math/constants/constants.hpp>
namespace po = boost::program_options;
void exit_with_help(po::options_description& desc)
{
        std::stringstream ss;
        ss << "F1 Simulated Telemetry Server. Command line arguments are as follows:" << std::endl;
        desc.print(ss);
        std::printf("%s", ss.str().c_str());
        exit(0); // @suppress("Invalid arguments")
}
int main(int argc, char** argv) {
	using boost::asio::ip::udp;
	using namespace deepf1;
        unsigned int BUFLEN = 1289;
        unsigned int UDP_BUFLEN = BUFLEN;
        unsigned int sleep_time;
        unsigned int packet_size = sizeof(UDPPacket);

        std::string address, port;
        po::options_description desc("Allowed Options");

        try{
                desc.add_options()
                ("help,h", "Displays options and exits")
                ("address,a", po::value<std::string>(&address)->default_value("127.0.0.1"), "IPv4 Address to send data to")
                ("port_number,p", po::value<std::string>(&port)->default_value("20777"), "Port number to send data to")
                ("sleep_time,s", po::value<unsigned int>(&sleep_time)->default_value(100), "Number of milliseconds to sleep between simulated packets")
                ;
	        po::variables_map vm;
                po::store(po::parse_command_line(argc, argv, desc), vm);
                po::notify(vm);
                if (vm.find("help") != vm.end()) {
                        exit_with_help(desc);
                }
        }catch(boost::exception& e){
                exit_with_help(desc);
        }
        boost::asio::io_service io_service;
        udp::resolver resolver(io_service);
        udp::resolver::query query(udp::v4(), address, port);
        udp::endpoint receiver_endpoint = *resolver.resolve(query);
        udp::socket socket(io_service);
        socket.open(udp::v4());

        std::shared_ptr<UDPPacket> data(new UDPPacket);
        float fake_time = 0;
        float dt = 1E-3*((float)sleep_time);
        float period = 5.0;
        float freq=1/period;
	float pi = boost::math::constants::pi<float>();
        while (true) {
                data->m_time = fake_time;
                data->m_steer = sin(2*pi*freq*fake_time);
                data->m_throttle = sin(2*pi*freq*fake_time + pi /3.0);
                data->m_brake = sin(2*pi*freq*fake_time + 2.0*pi /3.0);
                std::cout<<"Sending fake UDP data"<<std::endl;
                // std::cout<<"fake_time: "<<fake_time<<std::endl;
                // std::cout<<"Steering: "<<data->m_steer<<std::endl;
                // std::cout<<"Throttle: "<<data->m_throttle<<std::endl;
                // std::cout<<"B:rake "<<data->m_brake<<std::endl;
                socket.send_to(boost::asio::buffer(boost::asio::buffer(data.get(), packet_size)), receiver_endpoint);
                fake_time += dt;
                std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
        return 0;
		/* */
}


#include "simulate.h"

#include <algorithm>
#include <exception>
#include <climits>
#include <cmath>
#include <utility>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/students_t.hpp>

using namespace std;

const boost::math::normal STANDARD_NORMAL(0, 1);

double testBalance(const arma::Col<double> &z, const arma::Col<double> &t) {
  if (z.n_rows != t.n_rows) throw length_error("Column vectors must be the same length.");
  int N = t.n_rows;             // population size
  int n = 0;                    // draws
  int K = 0;                    // "success" events
  int k = 0;                    // observed successes
  for (int i = 0; i < N; ++i) {
    if (!(t(i) == 0 || t(i) == 1)) throw logic_error("Treatment must be a vector of 0s and 1s.");
    if (!(z(i) == 0 || z(i) == 1)) throw logic_error("Covariate must be a vector of 0s and 1s.");
    if (t(i) == 1) ++n;         // assigning to treatment is draw
    if (z(i) == 1) ++K;         // number of possible success events
    if (t(i) == 1 && z(i) == 1) ++k; // observed successes
  }
  boost::math::hypergeometric_distribution<> X(K, n, N);
  // find Prob(|X - K/N*n| >= |k - K/N*n|) = Prob(X <= K/N*n - |k - K/N*n|) + Prob(X >= K/N*n + |k - K/N*n|)
  double p = ((double) K)/N;
  double mean = p*n;
  double delta = abs(k - p*n);
  double pValue = 0;
  int L = mean - delta;
  int U = mean + delta - 1; // complement is not inclusive
  return U < L ? 1 : boost::math::cdf(X, L) + boost::math::cdf(boost::math::complement(X, U));
}

double calculateVariance(int N, double alpha, double power) {
  double z = boost::math::quantile(boost::math::complement(STANDARD_NORMAL, alpha/2)); // critical value
  double lower = 1;
  double upper = 100000000;
  double mid = (upper + lower)/2;
  while (upper - lower > 0.00000001) {
    double val = boost::math::cdf(STANDARD_NORMAL, -z + sqrt(N)/sqrt(mid)) + boost::math::cdf(STANDARD_NORMAL, -z - sqrt(N)/sqrt(mid));
    if (val > power) {
      lower = mid;
    } else {
      upper = mid;
    }
    mid = (upper + lower)/2;
  }
  return mid;
}

pair<double, double> calculateBetaPValue(const arma::mat &Z, const arma::Col<double> &Y,
                                         double sigma, bool varianceKnown) {
  arma::mat ZZ = Z.t()*Z;
  arma::mat beta = arma::solve(ZZ, Z.t()*Y);
  arma::Col<double> e(ZZ.n_rows, arma::fill::zeros); e(0) = 1;
  arma::mat inverseFirstColumn = arma::solve(ZZ, e);
  if (varianceKnown) {
    double standardDeviation = sigma*sqrt(inverseFirstColumn(0));
    double t = abs(beta(0))/standardDeviation;
    return make_pair(2*cdf(STANDARD_NORMAL, -t), beta(0));
  } else {
    int df = Y.n_rows - beta.n_rows;            // degrees of freedom
    double s = arma::norm(Y - Z*beta)/sqrt(df); // sample standard deviation
    double standardDeviation = s*sqrt(inverseFirstColumn(0));
    double t = abs(beta(0))/standardDeviation;
    boost::math::students_t tDist(df);
    return make_pair(2*cdf(tDist, -t), beta(0));
  }
}

tuple<double, double, int, int, double, double> simulate(const arma::Col<double> &Y, const vector<int> X,
                                                         double sigma, bool varianceKnown,
                                                         arma::mat &Z, mt19937_64 &rng,
                                                         bool interceptTerm) {
  bernoulli_distribution bernoulli(0.5);
  int N = X.size();
  Z.fill(0);
  // bestColumns[k] keeps track of the k + 1 or k + 2 columns that produce the smallest p-value depending on interceptTerm
  vector<arma::uvec> bestColumns; bestColumns.reserve(N - 1);
  if (interceptTerm) { // make intercept term last column of Z
    fill(Z.begin_col(N - 1), Z.end_col(N - 1), 1);
    copy(X.begin(), X.end(), Z.begin_col(0));
    bestColumns.push_back(arma::uvec{0, (unsigned long long) N - 1ULL});
  } else {
    copy(X.begin(), X.end(), Z.begin_col(0));
    bestColumns.push_back(arma::uvec{0});
  }
  // bestPValues[k] corresponds to p-value if the columns bestColumns[k] are used
  vector<pair<double, double>> bestPValues; bestPValues.reserve(N - 1);
  bestPValues.push_back(calculateBetaPValue(Z.cols(bestColumns.front()), Y, sigma, varianceKnown));
  if (bestPValues.front().first <= 0.05) {
    return make_tuple(bestPValues.front().first, bestPValues.front().second, 0, 0, -1, bestPValues.front().first);
  } else {                    // need more covariates
    bool done = false;
    int smallestSubsetSize = INT_MAX;
    /* add covariates one-by-one, we always include the treatment
     * if we're using the intercept two covariates are included by default
     */
    for (int j = 1; j < N - 2 || (j == N - 2 && !interceptTerm); ++j) {
      for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
      if (!interceptTerm) {
        while (arma::rank(Z) <= j) {
          for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
        }
      } else { // offset rank by 1 for intercept term
        while (arma::rank(Z) <= j + 1) {
          for (int k = 0; k < N; ++k) Z(k, j) = bernoulli(rng);
        }
      }
      for (int k = j; k >= 1; --k) { // loop through subset sizes, k is the number of additional covariates
        pair<double, double> newPValue;
        if (k == j) {           // use all available covariates
          bestColumns.emplace_back(bestColumns.back().n_rows + 1); // add one more to biggest subset
          for (int l = 0; l < bestColumns.back().n_rows - 1; ++l) {
            bestColumns.back()(l) = bestColumns[j - 1](l); // copy over from original subset
          }
          bestColumns.back()(bestColumns.back().n_rows - 1) = j; // add new covariate
          newPValue = calculateBetaPValue(Z.cols(bestColumns.back()), Y, sigma, varianceKnown);
          bestPValues.push_back(newPValue);
        } else {                // make a new subset of same size with new covariate
          arma::uvec columnSubset(bestColumns[k].n_rows);
          for (int l = 0; l < columnSubset.n_rows - 1; ++l)
            columnSubset(l) = bestColumns[k - 1](l); // copy over from smaller subset
          columnSubset(columnSubset.n_rows - 1) = j; // add new covariate
          newPValue = calculateBetaPValue(Z.cols(columnSubset), Y, sigma, varianceKnown);
          if (bestPValues[k].first > newPValue.first) { // if better subset replace
            bestPValues[k] = newPValue;
            bestColumns[k] = columnSubset;
          }
        }
        if (newPValue.first <= 0.05) { // stop when we reach significance
          done = true;
          smallestSubsetSize = k;
        }
      }
      if (done) {
        // compute balance p value in special case that only 1 covariate was needed
        double balancePValue = -1;
        if (smallestSubsetSize == 1 && !interceptTerm) {
          balancePValue = testBalance(Z.col(bestColumns[1](1)), Z.col(0));
        } else if (smallestSubsetSize == 1 && interceptTerm) {
          balancePValue = testBalance(Z.col(bestColumns[1](2)), Z.col(0));
        }
        return make_tuple(bestPValues.front().first, bestPValues[smallestSubsetSize].second,
                          j, smallestSubsetSize, balancePValue, bestPValues[smallestSubsetSize].first);
      }
    }
  }
  return make_tuple(bestPValues.front().first, bestPValues.front().second, -1, -1, -1, bestPValues.front().first);
}


/*
 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:
};

}


//  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;

}


/*
 * 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_;
};


#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;
}
}


/* $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;
}


// 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());
}

}


#include "gradient_sh.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <complex>
#include <fmt/format.h>
#include <fmt/ostream.h>
#include <fstream>
#include <memory>
#include <utility>

using namespace Eigen;
using namespace std::complex_literals;

using std::exp;
using std::pow;
using std::sqrt;
using complex_d = std::complex<double>;

const double PI = 3.14159265358979323846;

namespace grad_sh {

GRTCoeff::GRTCoeff(const Ref<const ArrayXXd> model, const double freq,
                   const double c)
    : z_(model.col(1)), rho_(model.col(2)), beta_(model.col(3)),
      alpha_(model.col(4)), mu_(rho_ * beta_.pow(2)), nl_(model.rows()),
      angfreq_(2.0 * PI * freq), c_(c), nv_(VectorXcd::Zero(nl_)),
      e_(nl_, Matrix2cd::Zero()), t_d_(VectorXcd::Zero(nl_)),
      r_ud_(VectorXcd::Zero(nl_)), r_du_(VectorXcd::Zero(nl_)),
      t_u_(VectorXcd::Zero(nl_)), gt_d_(VectorXcd::Zero(nl_)),
      gr_ud_(VectorXcd::Zero(nl_)), gr_du_(VectorXcd::Zero(nl_ + 1)),
      gt_u_(VectorXcd::Zero(nl_)), cd_(VectorXcd::Zero(nl_)),
      cu_(VectorXcd::Zero(nl_)) {

  initialize_nv();
  initialize_E();

  compute_rtc();
  compute_grtc();
  compute_CdCu();
}

void GRTCoeff::initialize_E() {
  for (int i = 0; i < nl_; ++i) {
    complex_d e21 = -mu_(i) * nv_(i);
    e_[i] << 1.0, 1.0, e21, -e21;
  }
}

void GRTCoeff::initialize_nv() {
  for (int i = 0; i < nl_; ++i) {
    complex_d val =
        std::sqrt(pow(angfreq_ / c_, 2) - pow(angfreq_ / beta_(i), 2));
    if (val.real() < 0) {
      val = -val;
    }
    nv_(i) = val;
  }
}

complex_d GRTCoeff::get_Ad(const double z, const int ind_layer) const {
  return exp(-nv_(ind_layer) * (z - z_(ind_layer)));
}

complex_d GRTCoeff::get_Au(const double z, const int ind_layer) const {
  return exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
}

complex_d GRTCoeff::get_Ad_der(const double z, const int ind_layer) const {
  return -nv_(ind_layer) * exp(-nv_(ind_layer) * (z - z_(ind_layer)));
}

complex_d GRTCoeff::get_Au_der(const double z, const int ind_layer) const {
  if (ind_layer == nl_ - 1) {
    return 0.0;
  }

  return nv_(ind_layer) * exp(-nv_(ind_layer) * (z_(ind_layer + 1) - z));
}

void GRTCoeff::compute_rtc() {
  for (int i = 1; i < nl_ - 1; ++i) {
    auto &e0 = e_[i - 1];
    auto &e1 = e_[i];

    Matrix2cd mat1;
    mat1 << e1(0, 0), -e0(0, 1), e1(1, 0), -e0(1, 1);

    // complex_d ad = exp(-nv_(i - 1) * (z_(i) - z_(i - 1)));
    // complex_d au = exp(-nv_(i) * (z_(i + 1) - z_(i)));
    complex_d ad = get_Ad(z_(i), i - 1);
    complex_d au = get_Au(z_(i), i);
    Matrix2cd mat2;
    mat2 << e0(0, 0) * ad, -e1(0, 1) * au, e0(1, 0) * ad, -e1(1, 1) * au;

    Matrix2cd result = mat1.inverse() * mat2;
    t_d_(i) = result(0, 0);
    r_ud_(i) = result(0, 1);
    r_du_(i) = result(1, 0);
    t_u_(i) = result(1, 1);
  }

  int i = nl_ - 1;
  auto &e0 = e_[i - 1];
  auto &e1 = e_[i];
  Matrix2cd mat1;
  mat1 << e1(0, 0), -e0(0, 1), e1(1, 0), -e0(1, 1);
  // complex_d ad = exp(-nv_(i - 1) * (z_(i) - z_(i - 1)));
  complex_d ad = get_Ad(z_(nl_ - 1), nl_ - 2);
  Matrix<complex_d, 2, 1> mat2;
  mat2 << e0(0, 0) * ad, e0(1, 0) * ad;

  Matrix<complex_d, 2, 1> result = mat1.inverse() * mat2;
  t_d_(i) = result(0, 0);
  r_du_(i) = result(1, 0);
}

void GRTCoeff::compute_grtc() {
  for (int i = nl_ - 1; i >= 1; --i) {
    gt_d_(i) = 1.0 / (1.0 - r_ud_(i) * gr_du_(i + 1)) * t_d_(i);
    gr_du_(i) = r_du_(i) + t_u_(i) * gr_du_(i + 1) * gt_d_(i);
  }
  auto &e0 = e_[0];
  // complex_d au = exp(-nv_(0) * (z_(1) - z_(0)));
  complex_d au = get_Au(z_(0), 0);
  gr_ud_(0) = -1.0 / e0(1, 0) * e0(1, 1) * au;
  for (int i = 1; i < nl_; ++i) {
    gt_u_(i) = 1.0 / (1.0 - r_du_(i) * gr_ud_(i - 1)) * t_u_(i);
    gr_ud_(i) = r_ud_(i) + t_d_(i) * gr_ud_(i - 1) * gt_u_(i);
  }
}

void GRTCoeff::compute_CdCu() {
  cd_(0) = 1.0;
  cu_(0) = gr_du_(1) * cd_(0);
  for (int i = 1; i < nl_ - 1; ++i) {
    cd_(i) = gt_d_(i) * cd_(i - 1);
    cu_(i) = gr_du_(i + 1) * cd_(i);
  }
  cd_(nl_ - 1) = gt_d_(nl_ - 1) * cd_(nl_ - 2);
}

IntegralLayer::IntegralLayer(const Ref<const ArrayXXd> model, const double freq,
                             const double c)
    : grtc_(std::make_unique<GRTCoeff>(model, freq, c)), nl_(model.rows()),
      k_(2.0 * PI * freq / c), pvel_(c), z_(model.col(1)), beta_(model.col(3)),
      rho_(model.col(2)), mu_(rho_ * beta_ * beta_), nv_(grtc_->nv_),
      thickness_(nl_ - 1), cd_(grtc_->cd_), cu_(grtc_->cu_),
      matP_u_u_(nl_, Array22cd::Zero()), matP_uc_u_(nl_, Array22cd::Zero()),
      matP_uc_uc_(nl_, Array22cd::Zero()), matP_du_du_(nl_, Array22cd::Zero()),
      matP_duc_du_(nl_, Array22cd::Zero()),
      matP_duc_duc_(nl_, Array22cd::Zero()),
      sigma_x_sigma_top_(nl_, Array22cd::Zero()),
      sigmac_x_sigma_top_(nl_, Array22cd::Zero()),
      sigma_x_sigmac_top_(nl_, Array22cd::Zero()),
      sigmac_x_sigmac_top_(nl_, Array22cd::Zero()),
      sigma_x_sigma_bottom_(nl_, Array22cd::Zero()),
      sigmac_x_sigma_bottom_(nl_, Array22cd::Zero()),
      sigma_x_sigmac_bottom_(nl_, Array22cd::Zero()),
      sigmac_x_sigmac_bottom_(nl_, Array22cd::Zero()),
      int_ut2_(ArrayXd::Zero(nl_)), int_dut2_(ArrayXd::Zero(nl_)) {
  for (int i = 0; i < nl_ - 1; ++i) {
    thickness_(i) = z_(i + 1) - z_(i);
  }

  initialize_P();
  initialize_sigma();

  integrate_ut2();
  integrate_dut2();
}

void IntegralLayer::initialize_P() {
  for (auto i = 0; i < nl_; ++i) {
    const complex_d nv = nv_(i);
    const complex_d nvc = std::conj(nv_(i));

    matP_u_u_[i] << 1.0 / (-nv - nv), 1.0, 1.0, 1.0 / (nv + nv);
    matP_uc_u_[i] << 1.0 / (-nvc - nv), 1.0 / (-nvc + nv), 1.0 / (nvc - nv),
        1.0 / (nvc + nv);
    matP_uc_uc_[i] << 1.0 / (-nvc - nvc), 1.0, 1.0, 1.0 / (nvc + nvc);
    matP_du_du_[i] << nv / (-2.0), -nv * nv, -nv * nv, nv / 2.0;
    matP_duc_du_[i] << nvc * nv / (-nvc - nv), -nvc * nv / (-nvc + nv),
        -nvc * nv / (nvc - nv), nvc * nv / (nvc + nv);
    matP_duc_duc_[i] << nvc / (-2.0), -nvc * nvc, -nvc * nvc, nvc / 2.0;

    if (pvel_ > beta_(i)) {
      matP_uc_u_[i](0, 0) = 1.0;
      matP_uc_u_[i](1, 1) = 1.0;
      matP_duc_du_[i](0, 0) = nvc * nv;
      matP_duc_du_[i](1, 1) = nvc * nv;
    } else {
      matP_uc_u_[i](0, 1) = 1.0;
      matP_uc_u_[i](1, 0) = 1.0;
      matP_duc_du_[i](0, 1) = -nvc * nv;
      matP_duc_du_[i](1, 0) = -nvc * nv;
    }
  }
}

void IntegralLayer::initialize_sigma() {
  MatrixXcd sigma_bottom = MatrixXcd::Zero(2, nl_);
  MatrixXcd sigma_top = MatrixXcd::Zero(2, nl_);
  for (auto i = 0; i < nl_ - 1; ++i) {
    sigma_bottom.col(i) << cd_(i), cu_(i) * exp(-nv_(i) * thickness_(i));
    sigma_top.col(i) << cd_(i) * exp(-nv_(i) * thickness_(i)), cu_(i);
  }
  sigma_bottom.col(nl_ - 1) << cd_(nl_ - 1), 0;
  sigma_top.col(nl_ - 1).fill(0);

  for (auto id_layer = 0; id_layer < nl_; ++id_layer) {
    for (auto i = 0; i < 2; ++i) {
      for (auto j = 0; j < 2; ++j) {
        sigma_x_sigma_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * sigma_top(j, id_layer);
        sigmac_x_sigma_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * sigma_top(j, id_layer);
        sigma_x_sigmac_top_[id_layer](i, j) =
            sigma_top(i, id_layer) * conj(sigma_top(j, id_layer));
        sigmac_x_sigmac_top_[id_layer](i, j) =
            conj(sigma_top(i, id_layer)) * conj(sigma_top(j, id_layer));

        sigma_x_sigma_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * sigma_bottom(j, id_layer);
        sigmac_x_sigma_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * sigma_bottom(j, id_layer);
        sigma_x_sigmac_bottom_[id_layer](i, j) =
            sigma_bottom(i, id_layer) * conj(sigma_bottom(j, id_layer));
        sigmac_x_sigmac_bottom_[id_layer](i, j) =
            conj(sigma_bottom(i, id_layer)) * conj(sigma_bottom(j, id_layer));
      }
    }
    if (id_layer != nl_ - 1) {
      sigma_x_sigma_top_[id_layer](0, 1) *= thickness_(id_layer);
      sigma_x_sigma_top_[id_layer](1, 0) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](0, 1) *= thickness_(id_layer);
      sigmac_x_sigmac_top_[id_layer](1, 0) *= thickness_(id_layer);
      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_top_[id_layer](0, 0) =
            conj(cd_(id_layer)) * cd_(id_layer) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 1) =
            conj(cu_(id_layer)) * cu_(id_layer) * thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 0) =
            cd_(id_layer) * conj(cd_(id_layer)) * thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 1) =
            cu_(id_layer) * conj(cu_(id_layer)) * thickness_(id_layer);
      } else {
        sigmac_x_sigma_top_[id_layer](0, 1) *= thickness_(id_layer);
        sigmac_x_sigma_top_[id_layer](1, 0) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](0, 1) *= thickness_(id_layer);
        sigma_x_sigmac_top_[id_layer](1, 0) *= thickness_(id_layer);
      }
      sigma_x_sigma_bottom_[id_layer](0, 1) = 0.;
      sigma_x_sigma_bottom_[id_layer](1, 0) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](0, 1) = 0.;
      sigmac_x_sigmac_bottom_[id_layer](1, 0) = 0.;

      if (pvel_ > beta_(id_layer)) {
        sigmac_x_sigma_bottom_[id_layer](0, 0) = 0.;
        sigmac_x_sigma_bottom_[id_layer](1, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 1) = 0.;
      } else {
        sigmac_x_sigma_bottom_[id_layer](0, 1) = 0.;
        sigmac_x_sigma_bottom_[id_layer](1, 0) = 0.;
        sigma_x_sigmac_bottom_[id_layer](0, 1) = 0.;
        sigma_x_sigmac_bottom_[id_layer](1, 0) = 0.;
      }
    }
  }
}

double IntegralLayer::intker_ut2_top(int id_layer) {
  Array22cd auu = matP_u_u_[id_layer] * sigma_x_sigma_top_[id_layer];
  Array22cd aucu = matP_uc_u_[id_layer] * sigmac_x_sigma_top_[id_layer];
  Array22cd aucuc = matP_uc_uc_[id_layer] * sigmac_x_sigmac_top_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_ut2_bottom(int id_layer) {
  Array22cd auu = matP_u_u_[id_layer] * sigma_x_sigma_bottom_[id_layer];
  Array22cd aucu = matP_uc_u_[id_layer] * sigmac_x_sigma_bottom_[id_layer];
  Array22cd aucuc = matP_uc_uc_[id_layer] * sigmac_x_sigmac_bottom_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_dut2_top(int id_layer) {
  Array22cd auu = matP_du_du_[id_layer] * sigma_x_sigma_top_[id_layer];
  Array22cd aucu = matP_duc_du_[id_layer] * sigmac_x_sigma_top_[id_layer];
  Array22cd aucuc = matP_duc_duc_[id_layer] * sigmac_x_sigmac_top_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

double IntegralLayer::intker_dut2_bottom(int id_layer) {
  Array22cd auu = matP_du_du_[id_layer] * sigma_x_sigma_bottom_[id_layer];
  Array22cd aucu = matP_duc_du_[id_layer] * sigmac_x_sigma_bottom_[id_layer];
  Array22cd aucuc = matP_duc_duc_[id_layer] * sigmac_x_sigmac_bottom_[id_layer];
  complex_d result = auu.sum() + 2.0 * aucu.sum() + aucuc.sum();
  return result.real() / 4.0;
}

void IntegralLayer::integrate_ut2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_ut2_(id_layer) = intker_ut2_top(id_layer) - intker_ut2_bottom(id_layer);
  }
}

void IntegralLayer::integrate_dut2() {
  for (int id_layer = 0; id_layer < nl_; ++id_layer) {
    int_dut2_(id_layer) =
        intker_dut2_top(id_layer) - intker_dut2_bottom(id_layer);
  }
}

double IntegralLayer::compute_I1() {
  ArrayXd ker = rho_ * int_ut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

double IntegralLayer::compute_I2() {
  ArrayXd ker = mu_ * int_ut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

double IntegralLayer::compute_I3() {
  ArrayXd ker = mu_ * int_dut2_;
  double i2 = 0.5 * ker.sum();
  return i2;
}

ArrayXd IntegralLayer::compute_kvs() {
  ArrayXd kvs(nl_);
  for (int i = 0; i < nl_; ++i) {
    kvs(i) =
        0.5 * rho_(i) * beta_(i) * (int_ut2_(i) + int_dut2_(i) / pow(k_, 2));
  }
  return kvs;
}

GradientSH::~GradientSH() = default;

ArrayXd GradientSH::compute(const double freq, const double c) const {
  IntegralLayer intl(model_, freq, c);
  double I2 = intl.compute_I2();
  ArrayXd kvs = intl.compute_kvs();

  kvs *= c / I2;

  return kvs;
}

} // namespace grad_sh


//
// Copyright 2014 Mitsubishi Electric Research Laboratories All
// Rights Reserved.
//
// Permission to use, copy and modify this software and its
// documentation without fee for educational, research and non-profit
// purposes, is hereby granted, provided that the above copyright
// notice, this paragraph, and the following three paragraphs appear
// in all copies.
//
// To request permission to incorporate this software into commercial
// products contact: Director; Mitsubishi Electric Research
// Laboratories (MERL); 201 Broadway; Cambridge, MA 02139.
//
// IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT,
// INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
// LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
// DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF
// SUCH DAMAGES.
//
// MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
// "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE,
// SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
//
#pragma once
//
// Note:
// If you use dsyevh3 library [http://www.mpi-hd.mpg.de/personalhomes/globes/3x3/],
// this function can be accelerated by uncommenting the following line
//#define USE_DSYEVH3
//
#ifdef USE_DSYEVH3
	#include "dsyevh3/dsyevh3.h"
#else
	#include <Eigen/Core>
	#include <Eigen/Dense>
#endif

#include <cmath>
#include <limits>

namespace LA {
	//s[0]<=s[1]<=s[2], V[:][i] correspond to s[i]
	inline static bool eig33sym(double K[3][3], double s[3], double V[3][3])
	{
#ifdef USE_DSYEVH3
		double tmpV[3][3];
		if(dsyevh3(K, tmpV, s)!=0) return false;

		int order[]={0,1,2};
		for(int i=0; i<3; ++i) {
			for(int j=i+1; j<3; ++j) {
				if(s[i]>s[j]) {
					double tmp=s[i];
					s[i]=s[j];
					s[j]=tmp;
					int tmpor=order[i];
					order[i]=order[j];
					order[j]=tmpor;
				}
			}
		}
		V[0][0]=tmpV[0][order[0]]; V[0][1]=tmpV[0][order[1]]; V[0][2]=tmpV[0][order[2]];
		V[1][0]=tmpV[1][order[0]]; V[1][1]=tmpV[1][order[1]]; V[1][2]=tmpV[1][order[2]];
		V[2][0]=tmpV[2][order[0]]; V[2][1]=tmpV[2][order[1]]; V[2][2]=tmpV[2][order[2]];
#else
		//below we did not specify row major since it does not matter, K==K'
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(
			Eigen::Map<Eigen::Matrix3d>(K[0], 3, 3) );
		Eigen::Map<Eigen::Vector3d>(s,3,1)=es.eigenvalues();
		//below we need to specify row major since V!=V'
		Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>>(V[0],3,3)=es.eigenvectors();
#endif
		return true;
	}
}//end of namespace LA


/*****************************************************************************
*
* Copyright (C) 2015-2017 by Synge Todo <wistaria@phy.s.u-tokyo.ac.jp>
*
* 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)
*
*****************************************************************************/

// Calculating free energy density of square lattice Ising model by the transfer matrix method

#include <cmath>
#include <vector>
#include <boost/array.hpp>
#include <lattice/square.hpp>
#include <lse/exp_number.hpp>

#ifndef ISING_SQUARE_TRANSFER_MATRIX_HPP
#define ISING_SQUARE_TRANSFER_MATRIX_HPP

namespace ising {
namespace square {

struct transfer_matrix {
public:
  typedef lse::exp_double exp_double;
  template<typename VEC>
  static exp_double product_D(double beta, std::vector<double> const& inter_x,
                                   std::vector<double> const& field, VEC& v) {
    int width = inter_x.size();
    int dim = 1 << width;
    exp_double normal = 1;
    std::vector<boost::array<double, 2> > weight(width);
    for (int b = 0; b < width; ++b) {
      double offset = std::abs(beta * inter_x[b]);
      normal *= lse::exp_value(offset);
      weight[b][0] = std::exp(beta * inter_x[b] - offset);
      weight[b][1] = std::exp(-beta * inter_x[b] - offset);
    }
    for (int c = 0; c < dim; ++c) {
      double elem = v[c];
      for (int b = 0; b < width; ++b) {
        int s0 = b;
        int s1 = (b+1) % width;
        elem *= weight[b][((c >> s0) & 1) ^ ((c >> s1) & 1)];
      }
      v[c] = elem;
    }
    if (field.size()) {
      for (int s = 0; s < width; ++s) {
        double offset = std::abs(beta * field[s]);
        normal *= lse::exp_value(offset);
        weight[s][0] = std::exp(beta * field[s] - offset);
        weight[s][1] = std::exp(-beta * field[s] - offset);
      }
      for (int c = 0; c < dim; ++c) {
        double elem = v[c];
        for (int s = 0; s < width; ++s) elem *= weight[s][((c >> s) & 1)];
        v[c] = elem;
      }
    }
    return normal;
  }

  template<typename VEC>
  static exp_double product_D(double beta, std::vector<double> const& inter_x, VEC& v) {
    std::vector<double> field(0);
    return product_D(beta, inter_x, field, v);
  }

  template<typename VEC>
  static exp_double product_U(double beta, std::vector<double> const& inter_y, VEC& v) {
    int width = inter_y.size();
    int dim = 1 << width;
    exp_double normal = 1;
    boost::array<double, 2> weight;
    for (int s = 0; s < width; ++s) {
      double offset = std::abs(beta * inter_y[s]);
      normal *= lse::exp_value(offset);
      weight[0] = std::exp(beta * inter_y[s] - offset);
      weight[1] = std::exp(-beta * inter_y[s] - offset);
      for (int c0 = 0; c0 < dim; ++c0) {
        if (((c0 >> s) & 1) == 0) {
          int c1 = c0 ^ (1 << s);
          double v0 = v[c0];
          double v1 = v[c1];
          v[c0] = weight[0] * v0 + weight[1] * v1;
          v[c1] = weight[0] * v1 + weight[1] * v0;
        }
      }
    }
    return normal;
  }

  static double free_energy(double beta, lattice::square const& lat,
                            std::vector<double> const& inter,
                            std::vector<double> const& field = std::vector<double>(0)) {
    int Lx = lat.get_length_x();
    int Ly = lat.get_length_y();
    std::vector<double> inter_x(Lx), inter_y(Lx), field_x(field.size() > 0 ? Lx : 0);
    int dim = 1 << Lx;
    std::vector<double> v(dim);
    exp_double sum = 0;
    for (int i = 0; i < dim; ++i) {
      exp_double weight = 1;
      for (int j = 0; j < dim; ++j) v[j] = 0;
      v[i] = 1;
      for (int y = 0; y < Ly; ++y) {
        for (int x = 0; x < Lx; ++x) {
          inter_x[x] = inter[2 * (Lx * y + x)];
          inter_y[x] = inter[2 * (Lx * y + x) + 1];
        }
        if (field_x.size()) {
          for (int x = 0; x < Lx; ++x) {
            field_x[x] = field[Lx * y + x];
          }
        }
        weight *= ising::square::transfer_matrix::product_D(beta, inter_x, field_x, v);
        weight *= ising::square::transfer_matrix::product_U(beta, inter_y, v);
      }
      weight *= v[i];
      sum += weight;
    }
    return -log(sum) / beta;
  }

  static double free_energy(double beta, lattice::square const& lat, double J, double H = 0.0) {
    std::vector<double> inter(lat.num_bonds(), J);
    std::vector<double> field((H != 0.0) ? lat.num_sites() : 0, H);
    return free_energy(beta, lat, inter, field);
  }

  static double free_energy_density(double beta, lattice::square const& lat,
                                    std::vector<double> const& inter,
                                    std::vector<double> const& field = std::vector<double>(0)) {
    return free_energy(beta, lat, inter, field) / lat.num_sites();
  }

  static double free_energy_density(double beta, lattice::square const& lat, double J,
                                    double H = 0.0) {
    return free_energy(beta, lat, J, H) / lat.num_sites();
  }
};

} // end namespace square
} // end namespace ising

#endif // ISING_SQUARE_TRANSFER_MATRIX_HPP


/*
 * bulirsch_stoer.cpp
 *
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2011-2012 Karsten Ahnert
 *
 * 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)
 */

#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>

#include <boost/array.hpp>
#include <boost/ref.hpp>

#include <boost/numeric/odeint/config.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef boost::array< double , 1 > state_type;

/*
 * x' = ( - x*sin t  + 2 tan x ) y
 * with x( pi/6 ) = 2/sqrt(3) the analytic solution is 1/cos t
 */

void rhs( const state_type &x , state_type &dxdt , const double t )
{
    dxdt[0] = ( - x[0] * sin( t ) + 2.0 * tan( t ) ) * x[0];
}

void rhs2( const state_type &x , state_type &dxdt , const double t )
{
    dxdt[0] = sin(t);
}

ofstream out;

void write_out( const state_type &x , const double t )
{
    out << t << '\t' << x[0] << endl;
}

int main()
{
    bulirsch_stoer_dense_out< state_type > stepper( 1E-8 , 0.0 , 0.0 , 0.0 );
    bulirsch_stoer< state_type > stepper2( 1E-8 , 0.0 , 0.0 , 0.0 );

    state_type x = {{ 2.0 / sqrt(3.0) }};

    double t = M_PI/6.0;
    //double t = 0.0;
    double dt = 0.01;
    double t_end = M_PI/2.0 - 0.1;
    //double t_end = 100.0;

    out.open( "bs.dat" );
    out.precision(16);
    integrate_const( stepper , rhs , x , t , t_end , dt , write_out );
    out.close();

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs2.dat" );
    out.precision(16);
    integrate_adaptive( stepper , rhs , x , t , t_end , dt , write_out );
    out.close();

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs3.dat" );
    out.precision(16);
    integrate_adaptive( stepper2 , rhs , x , t , t_end , dt , write_out );
    out.close();

    typedef runge_kutta_dopri5< state_type > dopri5_type;
    typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
    typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;

    dense_output_dopri5_type dopri5 = make_dense_output( 1E-9 , 1E-9 , dopri5_type() );

    x[0] = 2.0 / sqrt(3.0);

    out.open( "bs4.dat" );
    out.precision(16);
    integrate_adaptive( dopri5 , rhs , x , t , t_end , dt , write_out );
    out.close();

}