/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Koon, W.S., et al. Dynamical Systems, the Three-Body Problem and Space Mission Design,
 *          2006.
 *      Jet Propulsion Laboratory, NASA. Planets and Pluto: Physical Characteristics.
 *          http://ssd.jpl.nasa.gov/?planet_phys_par, last updated: 5th November, 2008, last
 *          accessed: 22nd November, 2011.
 *      Jet Propulsion Laboratory, NASA. Astrodynamics Constants.
 *          http://ssd.jpl.nasa.gov/?constants, last updated: 6th September, 2011, last accessed:
 *          22nd November, 2011.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/Gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"

namespace tudat
{
namespace unit_tests
{

//! Test if Cartesian state conversion for CRTBP is computed correctly.
BOOST_AUTO_TEST_CASE( testCartesianStateConversionCircularRestrictedThreeBodyProblem )
{
    // Test 1: conversion of dimensionless to dimensional Cartesian state vector for test particle
    // in Sun-Jupiter system [m, m/s] (Koon, 2006).
    {
        // Set distance between Sun and Jupiter [m] [NASA, 2010].
        double distanceSunJupiter = 7.784e11;

        // Set magnitude of velocity of Sun-Jupiter system [m/s].
        double velocitySunJupiter = 13.102e3;

        // Set gravitational parameter of Jupiter [m^3 s^-2].
        double gravitationalParameterJupiter = 6.67259e-11 * 1898.13e24;

        // Set gravitational parameter of Sun [m^3 s^-2].
        double gravitationalParameterSun = 1.32712440018e20;

        // Set mass parameter of Sun-Jupiter system.
        double massParameter = gravitationalParameterJupiter
                / ( gravitationalParameterJupiter + gravitationalParameterSun );

        // Set dimensionless state of the Sun.
        using orbital_element_conversions::xCartesianPositionIndex;
        Eigen::VectorXd dimensionlessStateOfSun = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfSun( xCartesianPositionIndex ) = -massParameter;

        // Declare and set dimensionless state of Jupiter.
        Eigen::VectorXd dimensionlessStateOfJupiter = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfJupiter( xCartesianPositionIndex ) = 1.0 - massParameter;

        // Set dimensionless state of test particle in Sun-Jupiter system.
        using orbital_element_conversions::yCartesianVelocityIndex;
        Eigen::VectorXd dimensionlessStateOfTestParticle = Eigen::VectorXd::Zero( 6 );
        dimensionlessStateOfTestParticle( xCartesianPositionIndex ) = 1.0;
        dimensionlessStateOfTestParticle( yCartesianVelocityIndex ) = 1.0;

        // Convert dimensionless to dimensional Cartesian state vector for test particle [m, m/s].
        Eigen::VectorXd computedDimensionalStateOfTestParticle( 6 );
        computedDimensionalStateOfTestParticle =
                circular_restricted_three_body_problem::convertDimensionlessCartesianStateToDimensionalUnits(
                    dimensionlessStateOfTestParticle, gravitationalParameterSun,
                    gravitationalParameterJupiter, distanceSunJupiter );

        // Check if computed position and velocity matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( distanceSunJupiter,
                                    computedDimensionalStateOfTestParticle( xCartesianPositionIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( velocitySunJupiter,
                                    computedDimensionalStateOfTestParticle( yCartesianVelocityIndex ),
                                    1.0e-2 );
    }
}

//! Test if time conversion for CRTBP is computed correctly.
BOOST_AUTO_TEST_CASE( testTimeConversionCircularRestrictedThreeBodyProblem )
{
    using mathematical_constants::PI;

    // Test 1: conversion of dimensionless to dimensional time for test particle in Sun-Jupiter
    // system [s] (Koon, 2006).
    {
        // Set distance between Sun and Jupiter [m] (NASA, 2010).
        double distanceSunJupiter = 7.784e11;

        // Set gravitational parameter of Jupiter [m^3 s^-2].
        double gravitationalParameterJupiter = 6.67259e-11 * 1898.13e24;

        // Set gravitational parameter of Sun [m^3 s^-2].
        double gravitationalParameterSun = 1.32712440018e20;

        // Compute dimensional time for one complete orbit.
        double computedDimensionalTime =
                circular_restricted_three_body_problem::convertDimensionlessTimeToDimensionalTime(
                    2.0 * PI, gravitationalParameterSun, gravitationalParameterJupiter,
                    distanceSunJupiter );

        // Set expected orbital period of system [s].
        double expectedOrbitalPeriodOfSunJupiterSystem = 3.733e08;

        // Check if computed orbital period matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( expectedOrbitalPeriodOfSunJupiterSystem,
                                    computedDimensionalTime, 1.0e-2 );
    }
}

} // namespace unit_tests
} // namespace tudat


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/math/deltagammavar.hpp
    \brief functions to compute delta or delta-gamma VaR numbers
    \ingroup math
*/

#ifndef quantext_deltagammavar_hpp
#define quantext_deltagammavar_hpp

#include <qle/math/covariancesalvage.hpp>

#include <ql/math/array.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/matrixutilities/choleskydecomposition.hpp>
#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/utilities/disposable.hpp>

// fix for boost 1.64, see https://lists.boost.org/Archives/boost/2016/11/231756.php
#if BOOST_VERSION >= 106400
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/foreach.hpp>

namespace QuantExt {
using namespace QuantLib;

//! function that computes a delta VaR
/*! For a given covariance matrix and a delta vector this function computes a parametric var w.r.t. a given
 * confidence level for multivariate normal risk factors. */
Real deltaVar(const Matrix& omega, const Array& delta, const Real p,
              const CovarianceSalvage& sal = NoCovarianceSalvage());

//! function that computes a delta-gamma normal VaR
/*! For a given a covariance matrix, a delta vector and a gamma matrix this function computes a parametric var
 * w.r.t. a given confidence level. The gamma matrix is taken into account when computing the variance of the PL
 * distirbution, but the PL distribution is still assumed to be normal. */
Real deltaGammaVarNormal(const Matrix& omega, const Array& delta, const Matrix& gamma, const Real p,
                         const CovarianceSalvage& sal = NoCovarianceSalvage());

//! function that computes a delta-gamma VaR using Monte Carlo (single quantile)
/*! For a given a covariance matrix, a delta vector and a gamma matrix this function computes a parametric var
 * w.r.t. a given confidence level. The var quantile is estimated from Monte-Carlo realisations of a second order
 * sensitivity based PL. */
template <class RNG>
Real deltaGammaVarMc(const Matrix& omega, const Array& delta, const Matrix& gamma, const Real p, const Size paths,
                     const Size seed, const CovarianceSalvage& sal = NoCovarianceSalvage());

//! function that computes a delta-gamma VaR using Monte Carlo (multiple quantiles)
/*! For a given a covariance matrix, a delta vector and a gamma matrix this function computes a parametric var
 * w.r.t. a vector of given confidence levels. The var quantile is estimated from Monte-Carlo realisations of a second
 * order sensitivity based PL. */
template <class RNG>
Disposable<std::vector<Real> > deltaGammaVarMc(const Matrix& omega, const Array& delta, const Matrix& gamma,
                                               const std::vector<Real>& p, const Size paths, const Size seed,
                                               const CovarianceSalvage& sal = NoCovarianceSalvage());

namespace detail {
void check(const Real p);
void check(const Matrix& omega, const Array& delta);
void check(const Matrix& omega, const Array& delta, const Matrix& gamma);
template <typename A> Real absMax(const A& a) {
    Real tmp = 0.0;
    BOOST_FOREACH (Real x, a) {
        if (std::abs(x) > tmp)
            tmp = std::abs(x);
    }
    return tmp;
}
} // namespace detail

// implementation

template <class RNG>
Disposable<std::vector<Real> > deltaGammaVarMc(const Matrix& omega, const Array& delta, const Matrix& gamma,
                                               const std::vector<Real>& p, const Size paths, const Size seed,
                                               const CovarianceSalvage& sal) {
    BOOST_FOREACH (Real q, p) { detail::check(q); }
    detail::check(omega, delta, gamma);

    Real num = std::max(detail::absMax(delta), detail::absMax(gamma));
    if (close_enough(num, 0.0)) {
        std::vector<Real> res(p.size(), 0.0);
        return res;
    }

    Matrix L = sal.salvage(omega).second;
    if (L.rows() == 0) {
        L = CholeskyDecomposition(omega, true);
    }

    Real pmin = QL_MAX_REAL;
    BOOST_FOREACH (Real q, p) { pmin = std::min(pmin, q); }

    Size cache = Size(std::floor(static_cast<double>(paths) * (1.0 - pmin) + 0.5)) + 2;
    boost::accumulators::accumulator_set<
        double, boost::accumulators::stats<boost::accumulators::tag::tail_quantile<boost::accumulators::right> > >
        acc(boost::accumulators::tag::tail<boost::accumulators::right>::cache_size = cache);

    typename RNG::rsg_type rng = RNG::make_sequence_generator(delta.size(), seed);

    for (Size i = 0; i < paths; ++i) {
        std::vector<Real> seq = rng.nextSequence().value;
        Array z(seq.begin(), seq.end());
        Array u = L * z;
        acc(DotProduct(u, delta) + 0.5 * DotProduct(u, gamma * u));
    }

    std::vector<Real> res;
    BOOST_FOREACH (Real q, p) {
        res.push_back(boost::accumulators::quantile(acc, boost::accumulators::quantile_probability = q));
    }

    return res;
}

template <class RNG>
Real deltaGammaVarMc(const Matrix& omega, const Array& delta, const Matrix& gamma, const Real p, const Size paths,
                     const Size seed, const CovarianceSalvage& sal) {

    std::vector<Real> pv(1, p);
    return deltaGammaVarMc<RNG>(omega, delta, gamma, pv, paths, seed, sal).front();
}

} // namespace QuantExt

#endif


#include "ezsolver.hpp"

#include <Eigen/Dense>
#include <ezarpack/arpack_worker.hpp>
#include <ezarpack/storages/eigen.hpp>

using namespace Eigen;
using namespace ezarpack;

EzSolver::EzSolver(const Ref<const MatrixXd> &matA,
                   const Ref<const MatrixXd> &matB)
    : ndim_(matA.rows()), matA_(matA), matB_(matB) {}

VectorXcd EzSolver::compute(double sigma, int nev) {
  MatrixXd matL = matA_ - sigma * matB_;
  // auto lu = matL.partialPivLu();
  auto lhh = matL.householderQr();

  using worker_t = arpack_worker<ezarpack::Asymmetric, eigen_storage>;
  using params_t = worker_t::params_t;
  using vector_view_t = worker_t::vector_view_t;
  using vector_const_view_t = worker_t::vector_const_view_t;

  auto o = [&](vector_const_view_t x, vector_view_t y) {
    y = matB_ * x;
    y = lhh.solve(y).eval();
  };
  worker_t worker(ndim_);
  params_t params(nev, params_t::LargestMagnitude, params_t::Ritz);
  worker(o, params);
  auto const &mu = worker.eigenvalues();
  VectorXcd lambda = mu.cwiseInverse().eval();
  lambda.array() += sigma;
  return lambda;
}

VectorXcd EzSolver::compute_sym(double sigma, int nev) {
  MatrixXd matL = matA_ - sigma * matB_;
  // auto lu = matL.partialPivLu();
  auto lhh = matL.householderQr();

  using worker_t = arpack_worker<ezarpack::Symmetric, eigen_storage>;
  using params_t = worker_t::params_t;
  using vector_view_t = worker_t::vector_view_t;
  using vector_const_view_t = worker_t::vector_const_view_t;

  auto o = [&](vector_const_view_t x, vector_view_t y) {
    y = matB_ * x;
    y = lhh.solve(y).eval();
  };
  worker_t worker(ndim_);
  params_t params(nev, params_t::LargestMagnitude, false);
  worker(o, params);
  auto const &mu = worker.eigenvalues();
  VectorXcd lambda = mu.cwiseInverse().eval();
  lambda.array() += sigma;
  return lambda;
}


#include <mtao/types.hpp>
#include <iostream>
#include<Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>
#include <mtao/logging/timer.hpp>
#include <mtao/eigen/mic0_preconditioner.hpp>
using Mat = mtao::MatrixX<float>;

float permeability = 100.0;
float timestep = 1000.0;

Eigen::SparseMatrix<float> grid_boundary(int ni, int nj, bool dirichlet_boundary) {
    int system_size = ni*nj;
    int usize = (ni+1) * nj;
    int vsize = (nj+1) * ni;
    int offset = usize;

    auto u_ind = [ni,nj](int i, int j) {
        return i + j*(ni + 1);
    };

    auto v_ind = [ni,nj,offset](int i, int j) {
        return i + j*ni + offset;
    };

    std::vector< Eigen::Triplet<float> > triplets;

    Eigen::SparseMatrix<float> D(usize + vsize, system_size);
    for (int j = 0; j < nj ; ++j) {
        for (int i = 0; i < ni ; ++i) {
            int index = i + ni*j;
            if(!(dirichlet_boundary && i == 0)) {
                triplets.push_back(Eigen::Triplet<float>( u_ind(i,j), index, 1.0/ni));
            }
            if(!(dirichlet_boundary && i == ni-1)) {
            triplets.push_back(Eigen::Triplet<float>( u_ind(i+1,j), index, -1.0/ni));
            }

            if(!(dirichlet_boundary && j == 0)) {
            triplets.push_back(Eigen::Triplet<float>(  v_ind(i,j), index, 1.0/nj));
            }
            if(!(dirichlet_boundary && j == ni-1)) {
                triplets.push_back(Eigen::Triplet<float>(  v_ind(i,j+1), index, -1.0/nj));
            }

        }
    }
    D.setFromTriplets(triplets.begin(), triplets.end());
    return D;
}

template <typename SolverType>
void run_test(int N, SolverType& solver, const std::string& name, bool print = false) {
    Mat data = Mat::Random(N,N);
    Eigen::Map<Eigen::VectorXf> theta(data.data(),data.size());
    int size = N*N;
    auto B = grid_boundary(N,N,true);
    auto dirichlet_energy = [&]() -> double {
        return (B * theta).squaredNorm();
    };
    //size ~ dx * dy ~ dx^2
    Eigen::SparseMatrix<float> L = B.transpose() * B;
    Eigen::SparseMatrix<float> A(size,size);

    auto t = mtao::logging::timer(name + "-total");
    for(int i = 0; i < 1; ++i) {
        timestep = std::pow(10,i);
        A.setIdentity();
        A /= timestep;
        A += L;
        //auto t = mtao::logging::timer(name);
        solver.compute(A);
        for(int j = 0; j < 10; ++j) {
            data = Mat::Random(N,N);
            for(int i = 0; i < 10; ++i) {
                theta = solver.solve(theta);
                if(print) {
                    std::cout << dirichlet_energy() << std::endl;
                }
            }
        }
    }

        auto dms = std::chrono::duration_cast<std::chrono::milliseconds>(t.duration());
        std::cout << dms.count() << ",";
}

int main() {
    mtao::logging::make_logger("default",mtao::logging::Level::Info);
    for(int i = 50; i < 1000; i+=10) {
        Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> ldlt_solver;
        Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower|Eigen::Upper> cg_solver;
        Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower|Eigen::Upper, mtao::eigen::preconditioners::MIC0Preconditioner<float>> mic0pcg_solver;
        //run_test(ldlt_solver, "LDLT");
        std::cout << i << ",";
        run_test(i,cg_solver, "CG");
        run_test(i,mic0pcg_solver, "MIC0PCG");
        std::cout << std::endl;
    }
    return 0;

    }


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "ConvolutionTools.hpp"
#include "FFT.hpp"
#include "FluidEigenMappings.hpp"
#include "../public/WindowFuncs.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

struct SpectralMass
{
  index  startBin;
  index  centerBin;
  index  endBin;
  double mass;
};

class OptimalTransport
{

  using ArrayXd = Eigen::ArrayXd;
  using VectorXd = Eigen::VectorXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using ArrayXcd = Eigen::ArrayXcd;
  template <typename T>
  using Ref = Eigen::Ref<T>;
  using TransportMatrix = std::vector<std::tuple<index, index, double>>;
  template <typename T>
  using vector = std::vector<T>;

public:
  void init(ArrayXd A, ArrayXd B)
  {
    mA = A;
    mB = B;
    mS1 = segmentSpectrum(A);
    mS2 = segmentSpectrum(B);
    mTransportMatrix = computeTransportMatrix(mS1, mS2);
    mInitialized = true;
  }

  bool initialized() const { return mInitialized; }

  vector<SpectralMass> segmentSpectrum(const Ref<ArrayXd> magnitude)
  {
    const auto&          epsilon = std::numeric_limits<double>::epsilon();
    vector<SpectralMass> masses;
    ArrayXd              mag = magnitude;
    double               totalMass = mag.sum() + epsilon;
    ArrayXd              invMag = mag * (-1);
    vector<index>        peaks = findPeaks(mag);
    vector<index>        valleys = findPeaks(invMag);
    if (peaks.size() == 0 || valleys.size() == 0) return masses;
    index        nextValley = valleys[0] > peaks[0] ? 0 : 1;
    SpectralMass firstMass{0, peaks[0], valleys[asUnsigned(nextValley)], 0};
    firstMass.mass =
        magnitude.segment(0, valleys[asUnsigned(nextValley)]).sum() / totalMass;
    masses.emplace_back(firstMass);
    for (index i = 1; asUnsigned(i) < (peaks.size() - 1); i++)
    {
      index start = valleys[asUnsigned(nextValley)];
      if (start < 0) start = 0;
      index center = peaks[asUnsigned(i)];
      index end = valleys[asUnsigned(nextValley) + 1];
      if (end > magnitude.size() - 1) end = magnitude.size() - 1;
      if (end < 0 || start > end) continue;
      double mass = magnitude.segment(start, end - start).sum() / totalMass;
      masses.emplace_back(SpectralMass{start, center, end, mass});
      nextValley++;
    }
    if (nextValley < valleys.size() - 1)
    {
      index lastStart = valleys[asUnsigned(nextValley)];
      index lastSize = magnitude.size() - 1 - lastStart;
      if (lastSize < 0) lastSize = 0;
      if (lastSize > magnitude.size() - lastStart - 1)
        lastSize = magnitude.size() - lastStart - 1;
      double lastMass = magnitude.segment(lastStart, lastSize).sum();
      lastMass /= totalMass;
      masses.push_back(SpectralMass{valleys.at(asUnsigned(nextValley)),
                                    peaks.at(peaks.size() - 1),
                                    magnitude.size() - 1, lastMass});
    }
    return masses;
  }

  TransportMatrix computeTransportMatrix(std::vector<SpectralMass> m1,
                                         std::vector<SpectralMass> m2)
  {
    TransportMatrix matrix;
    index           index1 = 0, index2 = 0;
    double          mass1 = m1[0].mass;
    double          mass2 = m2[0].mass;
    while (true)
    {
      if (mass1 < mass2)
      {
        matrix.emplace_back(index1, index2, mass1);
        mDistance += mass1 * std::pow(index1 - index2, 2);
        mass2 -= mass1;

        index1++;
        if (index1 >= asSigned(m1.size())) break;
        mass1 = m1[asUnsigned(index1)].mass;
      }
      else
      {
        matrix.emplace_back(index1, index2, mass2);
        mDistance += mass2 * std::pow(index1 - index2, 2);
        mass1 -= mass2;
        index2++;
        if (index2 >= asSigned(m2.size())) break;
        mass2 = m2[asUnsigned(index2)].mass;
      }
    }
    return matrix;
  }

  void placeMass(const SpectralMass mass, index bin, double scale,
                 Ref<ArrayXd> input, Ref<ArrayXd> output)
  {
    for (index i = mass.startBin; i < mass.endBin; i++)
    {
      index pos = i + bin - mass.centerBin;
      if (pos < 0 || pos >= output.size()) continue;
      double mag = scale * std::abs(input(i));
      output(pos) += mag;
    }
  }

  void interpolate(double interpolation, Eigen::Ref<ArrayXd> out)
  {
    for (auto t : mTransportMatrix)
    {
      SpectralMass m1 = mS1[asUnsigned(std::get<0>(t))];
      SpectralMass m2 = mS2[asUnsigned(std::get<1>(t))];
      index  interpolatedBin = std::lrint((1 - interpolation) * m1.centerBin +
                                         interpolation * m2.centerBin);
      double interpolationFactor = interpolation;
      if (m1.centerBin != m2.centerBin)
      {
        interpolationFactor =
            ((double) interpolatedBin - (double) m1.centerBin) /
            ((double) m2.centerBin - (double) m1.centerBin);
      }
      placeMass(m1, interpolatedBin,
                (1 - interpolation) * std::get<2>(t) / m1.mass, mA, out);
      placeMass(m2, interpolatedBin, interpolation * std::get<2>(t) / m2.mass,
                mB, out);
    }
  }

  std::vector<index> findPeaks(const Ref<ArrayXd> correlation)
  {
    std::vector<index> peaks;
    for (index i = 1; i < correlation.size() - 1; i++)
    {
      if (correlation(i) > correlation(i - 1) &&
          correlation(i) > correlation(i + 1))
      { peaks.push_back(i); }
    }
    return peaks;
  }

  bool                      mInitialized{false};
  TransportMatrix           mTransportMatrix;
  ArrayXd                   mA;
  ArrayXd                   mB;
  std::vector<SpectralMass> mS1;
  std::vector<SpectralMass> mS2;
  double                    mDistance{0};
};
} // namespace algorithm
} // namespace fluid


#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include <limits>
#include <boost/math/constants/constants.hpp>

namespace ob = ompl::base;
namespace oc = ompl::control;

void propagate(const oc::SpaceInformation *si, const ob::State *state, const oc::Control* control, const double duration, ob::State *result) {
    static double timeStep = .01;
    int nsteps = ceil(duration / timeStep);
    double dt = duration / nsteps;
    const double *u = control->as<oc::RealVectorControlSpace::ControlType>()->values;

    ob::CompoundStateSpace::StateType& s = *result->as<ob::CompoundStateSpace::StateType>();
    ob::SE2StateSpace::StateType& se2 = *s.as<ob::SE2StateSpace::StateType>(0);
    ob::RealVectorStateSpace::StateType& velocity = *s.as<ob::RealVectorStateSpace::StateType>(1);
    // ob::DiscreteStateSpace::StateType& gear = *s.as<ob::DiscreteStateSpace::StateType>(2);

    si->getStateSpace()->copyState(result, state);
    for (int i = 0; i < nsteps; i++) {
        se2.setX(se2.getX() + dt * velocity.values[0] * cos(se2.getYaw()));
        se2.setY(se2.getY() + dt * velocity.values[0] * sin(se2.getYaw()));
        se2.setYaw(se2.getYaw() + dt * u[0]);
        // velocity.values[0] = velocity.values[0] + dt * (u[1]*gear.value);
        velocity.values[0] = velocity.values[0] + dt * u[1];

        // 'guards' - conditions to change gears
        // if (gear.value > 0)
        // {
        //     if (gear.value < 3 && velocity.values[0] > 10*(gear.value + 1))
        //         gear.value++;
        //     else if (gear.value > 1 && velocity.values[0] < 10*gear.value)
        //         gear.value--;
        // }

        if (!si->satisfiesBounds(result))
            return;
    }
}

// The free space consists of two narrow corridors connected at right angle.
// To make the turn, the car will have to downshift.
bool isStateValid(const oc::SpaceInformation *si, const ob::State *state) {
    const ob::SE2StateSpace::StateType *se2 = state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
    // return si->satisfiesBounds(state) && (se2->getX() < -80. || se2->getY() > 80.);

    // check if bounds are satisfied
    // if (si->satisfiesBounds(state)) {
        if ((se2->getX() > -60. && se2->getX() < -30 && se2->getY() > -60. && se2->getY() < -30) ||
            (se2->getX() > -60. && se2->getX() < -30 && se2->getY() > 30. && se2->getY() < 60) ||
            (se2->getX() > 30. && se2->getX() < 60 && se2->getY() > 30. && se2->getY() < 60)) {
            return false;
        } else {
            return true;
        }
    // } else {
    //     return false;
    // }
}

int main(int, char**) {
    // plan for hybrid car in SE(2) with discrete gears
    ob::StateSpacePtr SE2(new ob::SE2StateSpace());
    ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1));
    // set the range for gears: [-1,3] inclusive
    // ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3));
    ob::StateSpacePtr stateSpace = SE2 + velocity;

    // set the bounds for the R^2 part of SE(2)
    ob::RealVectorBounds bounds(2);
    bounds.setLow(-100);
    bounds.setHigh(100);
    SE2->as<ob::SE2StateSpace>()->setBounds(bounds);

    // set the bounds for the velocity
    ob::RealVectorBounds velocityBound(1);
    velocityBound.setLow(0);
    velocityBound.setHigh(60);
    velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);

    // create start and goal states
    ob::ScopedState<> start(stateSpace);
    ob::ScopedState<> goal(stateSpace);

    // Both start and goal are states with high velocity with the car in third gear.
    // However, to make the turn, the car cannot stay in third gear and will have to
    // shift to first gear.
    start[0] = -79.6228; //position
    start[1] = -81.93; //position
    start[2] = -0.911333; // orientation
    // start[3] = 40.; // velocity
    // start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

    goal[0] = -50; // position
    goal[1] = -50; //position
    // goal[2] = 0; // orientation
    // goal[3] = 40.; // velocity
    // goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear

    oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));

    // set the bounds for the control manifold
    ob::RealVectorBounds cbounds(2);
    // bounds for steering input
    cbounds.setLow(0, -3.);
    cbounds.setHigh(0, 3.);
    // bounds for brake/gas input
    cbounds.setLow(1, -20.);
    cbounds.setHigh(1, 20.);
    cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);

    oc::SimpleSetup setup(cmanifold);
    setup.setStartAndGoalStates(start, goal, 5.);
    setup.setStateValidityChecker(boost::bind(&isStateValid, setup.getSpaceInformation().get(), _1));
    setup.setStatePropagator(boost::bind(&propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
    setup.getSpaceInformation()->setPropagationStepSize(.1);
    setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);

    // try to solve the problem
    if (setup.solve(30)) {
        // print the (approximate) solution path: print states along the path
        // and controls required to get from one state to the next
        oc::PathControl& path(setup.getSolutionPath());

        // print out full state on solution path
        // (format: x, y, theta, v, u0, u1, dt)
        for (unsigned int i = 0; i < path.getStateCount(); ++i) {
            const ob::State* state = path.getState(i);
            const ob::SE2StateSpace::StateType *se2 = state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
            const ob::RealVectorStateSpace::StateType *velocity = state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);

            std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw() << ' ' << velocity->values[0] << ' ';
            if (i == 0)
                // null controls applied for zero seconds to get to start state
                std::cout << "0 0 0";
            else {
                // print controls and control duration needed to get from state i-1 to state i
                const double* u = path.getControl(i - 1)->as<oc::RealVectorControlSpace::ControlType>()->values;
                std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i - 1);
            }
            std::cout << std::endl;
        }
        if (!setup.haveExactSolutionPath()) {
            std::cout << "Solution is approximate. Distance to actual goal is " << setup.getProblemDefinition()->getSolutionDifference() << std::endl;
        }
    }

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// median.hpp
//
//  Copyright 2006 Eric Niebler, Olivier Gygi. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_MEDIAN_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_MEDIAN_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/p_square_cumul_dist.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // median_impl
    //
    /**
        @brief Median estimation based on the \f$P^2\f$ quantile estimator

        The \f$P^2\f$ algorithm is invoked with a quantile probability of 0.5.
    */
    template<typename Sample>
    struct median_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type result_type;

        median_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            return p_square_quantile_for_median(args);
        }
    };
    ///////////////////////////////////////////////////////////////////////////////
    // with_density_median_impl
    //
    /**
        @brief Median estimation based on the density estimator

        The algorithm determines the bin in which the \f$0.5*cnt\f$-th sample lies, \f$cnt\f$ being
        the total number of samples. It returns the approximate horizontal position of this sample,
        based on a linear interpolation inside the bin.
    */
    template<typename Sample>
    struct with_density_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        with_density_median_impl(Args const &args)
          : sum(numeric::average(args[sample | Sample()], (std::size_t)1))
          , is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                std::size_t cnt = count(args);
                range_type histogram = density(args);
                typename range_type::iterator it = histogram.begin();
                while (this->sum < 0.5 * cnt)
                {
                    this->sum += it->second * cnt;
                    ++it;
                }
                --it;
                float_type over = numeric::average(this->sum - 0.5 * cnt, it->second * cnt);
                this->median = it->first * over + (it + 1)->first * (1. - over);
            }

            return this->median;
        }

    private:
        mutable float_type sum;
        mutable bool is_dirty;
        mutable float_type median;
    };

    ///////////////////////////////////////////////////////////////////////////////
    // with_p_square_cumulative_distribution_median_impl
    //
    /**
        @brief Median estimation based on the \f$P^2\f$ cumulative distribution estimator

        The algorithm determines the first (leftmost) bin with a height exceeding 0.5. It
        returns the approximate horizontal position of where the cumulative distribution
        equals 0.5, based on a linear interpolation inside the bin.
    */
    template<typename Sample>
    struct with_p_square_cumulative_distribution_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        with_p_square_cumulative_distribution_median_impl(dont_care)
          : is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                range_type histogram = p_square_cumulative_distribution(args);
                typename range_type::iterator it = histogram.begin();
                while (it->second < 0.5)
                {
                    ++it;
                }
                float_type over = numeric::average(it->second - 0.5, it->second - (it - 1)->second);
                this->median = it->first * over + (it + 1)->first * ( 1. - over );
            }

            return this->median;
        }
    private:

        mutable bool is_dirty;
        mutable float_type median;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::median
// tag::with_densisty_median
// tag::with_p_square_cumulative_distribution_median
//
namespace tag
{
    struct median
      : depends_on<p_square_quantile_for_median>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::median_impl<mpl::_1> impl;
    };
    struct with_density_median
      : depends_on<count, density>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_density_median_impl<mpl::_1> impl;
    };
    struct with_p_square_cumulative_distribution_median
      : depends_on<p_square_cumulative_distribution>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_p_square_cumulative_distribution_median_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::median
// extract::with_density_median
// extract::with_p_square_cumulative_distribution_median
//
namespace extract
{
    extractor<tag::median> const median = {};
    extractor<tag::with_density_median> const with_density_median = {};
    extractor<tag::with_p_square_cumulative_distribution_median> const with_p_square_cumulative_distribution_median = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(median)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(with_density_median)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(with_p_square_cumulative_distribution_median)
}

using extract::median;
using extract::with_density_median;
using extract::with_p_square_cumulative_distribution_median;

// median(with_p_square_quantile) -> median
template<>
struct as_feature<tag::median(with_p_square_quantile)>
{
    typedef tag::median type;
};

// median(with_density) -> with_density_median
template<>
struct as_feature<tag::median(with_density)>
{
    typedef tag::with_density_median type;
};

// median(with_p_square_cumulative_distribution) -> with_p_square_cumulative_distribution_median
template<>
struct as_feature<tag::median(with_p_square_cumulative_distribution)>
{
    typedef tag::with_p_square_cumulative_distribution_median type;
};

// for the purposes of feature-based dependency resolution,
// with_density_median and with_p_square_cumulative_distribution_median
// provide the same feature as median
template<>
struct feature_of<tag::with_density_median>
  : feature_of<tag::median>
{
};

template<>
struct feature_of<tag::with_p_square_cumulative_distribution_median>
  : feature_of<tag::median>
{
};

// So that median can be automatically substituted with
// weighted_median when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::median>
{
    typedef tag::weighted_median type;
};

template<>
struct feature_of<tag::weighted_median>
  : feature_of<tag::median>
{
};

// So that with_density_median can be automatically substituted with
// with_density_weighted_median when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::with_density_median>
{
    typedef tag::with_density_weighted_median type;
};

template<>
struct feature_of<tag::with_density_weighted_median>
  : feature_of<tag::with_density_median>
{
};

// So that with_p_square_cumulative_distribution_median can be automatically substituted with
// with_p_square_cumulative_distribution_weighted_median when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::with_p_square_cumulative_distribution_median>
{
    typedef tag::with_p_square_cumulative_distribution_weighted_median type;
};

template<>
struct feature_of<tag::with_p_square_cumulative_distribution_weighted_median>
  : feature_of<tag::with_p_square_cumulative_distribution_median>
{
};

}} // namespace boost::accumulators

#endif


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

#pragma once

#include <stdexcept>
#include <armadillo>
#include <armalogp/arma.extensions.hpp>
#include <armalogp/arma.math.hpp>

namespace armalogp {

  template<double LOGF(double), typename T, typename U, typename V>
  double normal_logp(const T& x, const U& mu, const V& tau) {
    return arma::accu(0.5*LOGF(0.5*tau/arma::datum::pi) - 0.5 * arma::schur_prod(tau, square(x - mu)));
  }

  template<double LOGF(double), typename T, typename U, typename V>
  double uniform_logp(const T& x, const U& lower, const V& upper) {
    return (arma::any(arma::vectorise(x < lower)) || arma::any(arma::vectorise(x > upper))) ? -std::numeric_limits<double>::infinity() : -arma::accu(LOGF(upper - lower));
  }

  template<double LOGF(double), typename T, typename U, typename V>
  double gamma_logp(const T& x, const U& alpha, const V& beta) {
    return arma::any(arma::vectorise(x < 0)) ?
      -std::numeric_limits<double>::infinity() :
      arma::accu(arma::schur_prod((alpha - 1.0),LOGF(x)) - arma::schur_prod(beta,x) - lgamma(alpha) + arma::schur_prod(alpha,LOGF(beta)));
  }

  template<double LOGF(double), typename T, typename U, typename V>
  double beta_logp(const T& x, const U& alpha, const V& beta) {
    const double one = 1.0;
    return arma::any(arma::vectorise(x <= 0)) || arma::any(arma::vectorise(x >= 1)) || arma::any(arma::vectorise(alpha <= 0)) || arma::any(arma::vectorise(beta <= 0)) ?
      -std::numeric_limits<double>::infinity() :
      arma::accu(lgamma(alpha+beta) - lgamma(alpha) - lgamma(beta) + arma::schur_prod((alpha-one),LOGF(x)) + arma::schur_prod((beta-one),LOGF(one-x)));
  }

  template<double LOGF(double)>
  double categorical_logp(const arma::ivec& x, const arma::mat& p) {
    if(arma::any(arma::vectorise(p <= 0)) || arma::any(arma::vectorise(p >= 1)) || arma::any(arma::vectorise(x < 0)) || arma::any(arma::vectorise(x >= p.n_cols))) {
      return -std::numeric_limits<double>::infinity();
    }
    // replace w/ call to p.elems later
    double ans(0);
    for(unsigned int i = 0; i < x.n_rows; i++) {
      ans += LOGF(p(i,x[i]));
    }
    return ans;
  }

  template<double LOGF(double)>
  double categorical_logp(const arma::ivec& x, const arma::vec& p) {
    if(arma::any(arma::vectorise(p <= 0)) || arma::any(arma::vectorise(p >= 1)) || arma::any(arma::vectorise(x < 0)) || arma::any(arma::vectorise(x >= p.n_elem))) {
      return -std::numeric_limits<double>::infinity();
    }
    // replace w/ call to p.elems later
    double ans(0);
    for(unsigned int i = 0; i < x.n_rows; i++) {
      ans += LOGF(p(x[i]));
    }
    return ans;
  }

  template<double LOGF(double)>
  double categorical_logp(const int x, const arma::vec& p) {
    return LOGF(p[x]);
  }

  template<double LOGF(double), typename T, typename U, typename V>
  double binomial_logp(const T& x, const U& n, const V& p) {
    if(arma::any(arma::vectorise(p <= 0)) || arma::any(arma::vectorise(p >= 1)) || arma::any(arma::vectorise(x < 0))  || arma::any(arma::vectorise(x > n))) {
      return -std::numeric_limits<double>::infinity();
    }
    return arma::accu(arma::schur_prod(x,LOGF(p)) + arma::schur_prod((n-x),LOGF(1-p)) + arma::factln(n) - arma::factln(x) - arma::factln(n-x));
  }

  template<double LOGF(double), typename T, typename U>
  double bernoulli_logp(const T& x, const U& p) {
    if( arma::any(arma::vectorise(p <= 0)) || arma::any(arma::vectorise(p >= 1)) || arma::any(arma::vectorise(x < 0))  || arma::any(arma::vectorise(x > 1)) ) {
      return -std::numeric_limits<double>::infinity();
    } else {
      return arma::accu(arma::schur_prod(x,LOGF(p)) + arma::schur_prod((1-x), LOGF(1-p)));
    }
  }

  template<double LOGF(double), typename T, typename U>
  double poisson_logp(const T& x, const U& mu) {
    if( arma::any(arma::vectorise(mu < 0)) || arma::any(arma::vectorise(x < 0))) {
      return -std::numeric_limits<double>::infinity();
    } else {
      return arma::accu(schur(x,LOGF(mu)) - mu - factln(x));
    }
  }

  template<double LOGF(double), typename T, typename U>
  double exponential_logp(const T& x, const U& lambda) {
    if(!arma::all(arma::vectorise(x > 0)) || !arma::all(arma::vectorise(lambda > 0))) {
        return -std::numeric_limits<double>::infinity();
    }
    return arma::accu(LOGF(lambda) - arma::schur_prod(lambda, x));
  }

  template<double LOGF(double), typename T, typename U>
  double multivariate_normal_chol_logp(const T& x, const U& mu, const arma::mat& R) {
    static double log_2pi = LOGF(2 * arma::datum::pi);
    double ldet = LOGF(cholesky_determinant(R));
    return -0.5 * (x.n_elem * log_2pi + ldet + mahalanobis_chol(x,mu,R));
  }

  // sigma denotes cov matrix rather than precision matrix
  template<double LOGF(double), typename T, typename U>
  double multivariate_normal_sigma_logp(const T& x, const U& mu, const arma::mat& sigma) {
    arma::mat R;
    bool chol_succeeded = chol(R,sigma);
    if(!chol_succeeded) { return -std::numeric_limits<double>::infinity(); }

    return multivariate_normal_chol_logp<LOGF>(x, mu, R);
  }

  // sigma denotes cov matrix rather than precision matrix
  template<double LOGF(double)>
  double multivariate_normal_sigma_logp(const arma::mat& x, const arma::vec& mu, const arma::mat& sigma) {
    arma::mat R;
    bool chol_succeeded = chol(R,sigma);
    if(!chol_succeeded) { return -std::numeric_limits<double>::infinity(); }
    const arma::rowvec mu_r = mu.t();
    double ans(0);
    for(size_t i = 0; i < x.n_rows; i++) {
      ans += multivariate_normal_chol_logp<LOGF>(x.row(i), mu_r, R);
    }
    return ans;
  }

  template<double LOGF(double)>
  double multivariate_normal_chol_logp(const arma::mat& x, const arma::vec& mu, const arma::mat& R) {
    const arma::rowvec mu_r = mu.t();
    double ans(0);
    for(size_t i = 0; i < x.n_rows; i++) {
      ans += multivariate_normal_chol_logp<LOGF>(x.row(i), mu_r, R);
    }
    return ans;
  }

  template<double LOGF(double)>
  double wishart_logp(const arma::mat& X, const arma::mat& tau, const unsigned int n) {
    if(X.n_cols != X.n_rows || tau.n_cols != tau.n_rows || X.n_cols != tau.n_rows || X.n_cols > n) { return -std::numeric_limits<double>::infinity(); }
    const double lg2 = LOGF(2.0);
    const int k = X.n_cols;
    const double dx(arma::det(X));
    const double db(arma::det(tau));
    if(dx <= 0 || db <= 0) { return -std::numeric_limits<double>::infinity(); }

    const double ldx(LOGF(dx));
    const double ldb(LOGF(db));
    const arma::mat bx(X * tau);
    const double tbx = arma::trace(bx);

    double cum_lgamma(0);
    for(size_t i = 0; i < X.n_rows; ++i) {
      cum_lgamma += lgamma((n + 1)/2.0);
    }
    return (n - k - 1)/2 * ldx + (n/2.0)*ldb - 0.5*tbx - (n*k/2.0)*lg2 - cum_lgamma;
  }

} // namespace armalogp


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing mantissa capabilities

    Returns the signed mantissa of the floating input.

    @par Semantic:

    @code
    auto r = mantissa(x);
    @endcode

    is similar to:

    @code
    auto r = x*pow(2, -exponent(x));
    @endcode

    @par Note
    The @ref exponent e and signed @ref mantissa m of a floating point entry a are related by
    \f$a = m\times 2^e\f$, with |m| \f$\in[1, 2[\f$.

    @see frexp, pow, exponent

  **/
  Value mantissa(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/mantissa.hpp>
#include <boost/simd/function/simd/mantissa.hpp>

#endif


#include <boost/program_options.hpp>

#include <iostream>
#include <fstream>
#include <cassert>
#include <list>
#include <vector>

// https://doc.cgal.org/latest/Triangulation_3/
// https://doc.cgal.org/latest/Kernel_23/index.html#Chapter_2D_and_3D_Geometry_Kernel
// http://doc.cgal.org/latest/Triangulation_3/classTriangulationTraits__3.html
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

// https://doc.cgal.org/latest/Triangulation_3/classCGAL_1_1Triangulation__3.html
// https://doc.cgal.org/latest/Triangulation_3/classCGAL_1_1Delaunay__triangulation__3.html
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_data_structure_3.h>

// The doc for Point is a little tricky to find. Delaunay and Triangulation pages
// don't link here:
// https://doc.cgal.org/latest/Kernel_23/classCGAL_1_1Point__3.html
// http://doc.cgal.org/latest/Kernel_23/classCGAL_1_1Cartesian.html
// http://www.cgal.org/FAQ.html

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_3<K>                   Triangulation;
typedef Triangulation::Point                                Point;

// these two functions are for my readvtk function.
Triangulation::Vertex_handle
add_vertex(Triangulation &T, std::string& vname, double x, double y, double z)
{
  // vertex name doesn't have any meaning in this context.
  return T.insert(Point(x,y,z));
}

void add_edge(Triangulation &T,
               Triangulation::Vertex_handle s,
               Triangulation::Vertex_handle t)
{
  // no edges to add. CGAL does that.
}

#include <readvtk.hxx>

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

    namespace po = boost::program_options;
    po::options_description desc("Usage");

    std::string filename;
    desc.add_options()
        ("help", "produce help message")
        ("filename", po::value<std::string>(&filename)->default_value(""),
         "filename containing input points");

    po::variables_map opts;
    po::store(po::parse_command_line(argc, argv, desc), opts);

    try {
        po::notify(opts);
    } catch (std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
        return 1;
    }

    if (filename == "") {
      std::cerr << "please provide a vtk file with the --filename <file> option" << std::endl;
      exit(-1);
    }

    std::ifstream input(filename.c_str());
    Triangulation T;

    /* if you just have a file with a list of points, you can do this:
       std::list<Point> L;
       Point p ;
       Triangulation T;
       while (input >> p) {
         T.insert(p);
         L.push_front(p);
       }
       // if you already have a list of points:
       // Triangulation T(L.begin(), L.end());

    */

    readvtk<Triangulation,Triangulation::Vertex_handle>(input, T);

    // at this point, the trianulation is already done. Just have to extract
    // the computed lines.
    Triangulation::size_type n = T.number_of_vertices();

    assert( T.is_valid() ); // checking validity of T

    std::cout << "# vtk DataFile Version 1.0\n";
    std::cout << "3D triangulation data\n";
    std::cout << "ASCII\n";
    std::cout << std::endl;
    std::cout << "DATASET POLYDATA\n";

    // according to http://doc.cgal.org/latest/TDS_3/classTriangulationDataStructure__3.html
    // Note that the triangulation data structure has one more
    // vertex than an associated geometric triangulation, if there
    // is one, since the infinite vertex is a standard vertex and
    // is thus also counted.
    // I am omitting this one by using the finite_vertices/edges iterators below
    int numvertices = T.number_of_vertices();
    std::cout << "POINTS " << numvertices << " float\n";

    // the APIs I'm using below are documented here:
    // http://doc.cgal.org/latest/Triangulation_3/classCGAL_1_1Triangulation__3.html

    // as I iterate over the vertices, I assign ids to each vertex. Seems like there'd
    // be an id method, but I haven't found it.
    std::map<Point, int> vertex_ids;
    Triangulation::Finite_vertices_iterator viter;
    for (viter =  T.finite_vertices_begin();
         viter != T.finite_vertices_end();
         viter++) {
      Triangulation::Triangulation_data_structure::Vertex v = *viter;
      // in the line below, looking up the point (the [] part) will add a new
      // element to the map. That's why I subtract 1.
      vertex_ids[v.point()] = vertex_ids.size() - 1;
      std::cout << v.point() << std::endl;
    }

    std::cout << "LINES " << T.number_of_finite_edges() << " "
              << 3*T.number_of_finite_edges() << std::endl;

    Triangulation::Finite_edges_iterator iter;
    for(iter =  T.finite_edges_begin();
        iter != T.finite_edges_end();
        iter++) {
      // edges are not represented as edges in CGAL triangulation graphs.
      // Instead, they are stored in faces/cells.

      Triangulation::Triangulation_data_structure::Edge e = *iter;
      Triangulation::Triangulation_data_structure::Cell_handle c = e.first;
      int i = e.second;
      int j = e.third;

      Triangulation::Triangulation_data_structure::Vertex_handle a = c->vertex(i);
      Point pa = a->point();
      Point pb = c->vertex(j)->point();
      int ida, idb;
      if (vertex_ids.find(pa) == vertex_ids.end()) {
        std::cout << "didn't find " << pa << std::endl;
        ida = 0;
      } else {
        ida = vertex_ids[pa];
      }
      if (vertex_ids.find(pb) == vertex_ids.end()) {
        std::cout << "didn't find " << pb << std::endl;
        idb = 0;
      } else {
        idb = vertex_ids[pb];
      }
      std::cout << "2 " << ida << " " << idb << std::endl;
    }

    return 0;
}


#include <cmath>
#include <cstring>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <esn/exceptions.hpp>
#include <esn/network_nsli.h>
#include <esn/network_nsli.hpp>
#include <network_nsli.h>

namespace ESN {

    std::unique_ptr< Network > CreateNetwork(
        const NetworkParamsNSLI & params )
    {
        return std::unique_ptr< NetworkNSLI >( new NetworkNSLI( params ) );
    }

    NetworkNSLI::NetworkNSLI( const NetworkParamsNSLI & params )
        : mParams( params )
        , mIn( params.inputCount )
        , mWIn( params.neuronCount, params.inputCount )
        , mWInScaling( params.inputCount )
        , mWInBias( params.inputCount )
        , mX( params.neuronCount )
        , mW( params.neuronCount, params.neuronCount )
        , mOut( params.outputCount )
        , mWOut( params.outputCount, params.neuronCount )
        , mWFB()
        , mWFBScaling()
        , mAdaptiveFilter( params.neuronCount,
            params.onlineTrainingForgettingFactor,
            params.onlineTrainingInitialCovariance )
    {
        if ( params.inputCount <= 0 )
            throw std::invalid_argument(
                "NetworkParamsNSLI::inputCount must be not null" );
        if ( params.neuronCount <= 0 )
            throw std::invalid_argument(
                "NetworkParamsNSLI::neuronCount must be not null" );
        if ( params.outputCount <= 0 )
            throw std::invalid_argument(
                "NetworkParamsNSLI::outputCount must be not null" );
        if ( !( params.leakingRateMin > 0.0 &&
                params.leakingRateMin <= 1.0 ) )
            throw std::invalid_argument(
                "NetworkParamsNSLI::leakingRateMin must be within "
                "interval (0,1]" );
        if ( !( params.leakingRateMax > 0.0 &&
                params.leakingRateMax <= 1.0 ) )
            throw std::invalid_argument(
                "NetworkParamsNSLI::leakingRateMax must be within "
                "interval (0,1]" );
        if ( params.leakingRateMin > params.leakingRateMax )
            throw std::invalid_argument(
                "NetworkParamsNSLI::leakingRateMin must be less then or "
                "equal to NetworkParamsNSLI::leakingRateMax" );
        if ( !( params.connectivity > 0.0f &&
                params.connectivity <= 1.0f ) )
            throw std::invalid_argument(
                "NetworkParamsNSLI::connectivity must be within "
                "interval (0,1]" );

        mWIn = Eigen::MatrixXf::Random(
            params.neuronCount, params.inputCount );

        Eigen::MatrixXf randomWeights =
            ( Eigen::MatrixXf::Random( params.neuronCount,
                params.neuronCount ).array().abs()
                    <= params.connectivity ).cast< float >() *
            Eigen::MatrixXf::Random( params.neuronCount,
                params.neuronCount ).array();
        if ( params.useOrthonormalMatrix )
        {
            auto svd = randomWeights.jacobiSvd(
                Eigen::ComputeFullU | Eigen::ComputeFullV );
            mW = ( svd.matrixU() * svd.matrixV() ).sparseView();
        }
        else
        {
            float spectralRadius =
                randomWeights.eigenvalues().cwiseAbs().maxCoeff();
            mW = ( randomWeights / spectralRadius *
                params.spectralRadius ).sparseView() ;
        }

        mWInScaling = Eigen::VectorXf::Constant( params.inputCount, 1.0f );
        mWInBias = Eigen::VectorXf::Zero( params.inputCount );

        mWOut = Eigen::MatrixXf::Zero(
            params.outputCount, params.neuronCount );

        if (params.hasOutputFeedback)
        {
            mWFB = Eigen::MatrixXf::Random(
                params.neuronCount, params.outputCount);
            mWFBScaling = Eigen::VectorXf::Constant(
                params.outputCount, 1.0f);
        }

        mLeakingRate = ( Eigen::ArrayXf::Random( params.neuronCount ) *
            ( mParams.leakingRateMax - mParams.leakingRateMin ) +
            ( mParams.leakingRateMin + mParams.leakingRateMax ) ) / 2.0f;
        mOneMinusLeakingRate = 1.0f - mLeakingRate.array();

        mIn = Eigen::VectorXf::Zero( params.inputCount );
        mX = Eigen::VectorXf::Random( params.neuronCount );
        mOut = Eigen::VectorXf::Zero( params.outputCount );
    }

    NetworkNSLI::~NetworkNSLI()
    {
    }

    void NetworkNSLI::SetInputs( const std::vector< float > & inputs )
    {
        if ( inputs.size() != mIn.rows() )
            throw std::invalid_argument( "Wrong size of the input vector" );
        mIn = (Eigen::Map<Eigen::VectorXf>(
            const_cast<float*>(inputs.data()), inputs.size()) +
            mWInBias).cwiseProduct(mWInScaling);
    }

    void NetworkNSLI::SetInputScalings(
        const std::vector< float > & scalings )
    {
        if ( scalings.size() != mParams.inputCount )
            throw std::invalid_argument(
                "Wrong size of the scalings vector" );
        mWInScaling = Eigen::Map< Eigen::VectorXf >(
            const_cast< float * >( scalings.data() ), scalings.size() );
    }

    void NetworkNSLI::SetInputBias(
        const std::vector< float > & bias )
    {
        if ( bias.size() != mParams.inputCount )
            throw std::invalid_argument(
                "Wrong size of the scalings vector" );
        mWInBias = Eigen::Map< Eigen::VectorXf >(
            const_cast< float * >( bias.data() ), bias.size() );
    }

    void NetworkNSLI::SetFeedbackScalings(
        const std::vector< float > & scalings )
    {
        if (!mParams.hasOutputFeedback)
            throw std::logic_error(
                "Trying to set up feedback scaling for a network "
                "which doesn't have an output feedback");
        if ( scalings.size() != mParams.outputCount )
            throw std::invalid_argument(
                "Wrong size of the scalings vector" );
        mWFBScaling = Eigen::Map< Eigen::VectorXf >(
            const_cast< float * >( scalings.data() ), scalings.size() );
    }

    void NetworkNSLI::Step( float step )
    {
        if ( step <= 0.0f )
            throw std::invalid_argument(
                "Step size must be positive value" );

        auto tanh = [] ( float x ) -> float { return std::tanh( x ); };

        #define TEMP mWIn * mIn + mW * mX

        #define CALC_X(val) \
            mX = mOneMinusLeakingRate.cwiseProduct(mX) + \
                mLeakingRate.cwiseProduct(val).unaryExpr(tanh)

        #define CALC_X_WITH_FB(val) \
            if (mParams.hasOutputFeedback) \
                CALC_X(TEMP + val); \
            else \
                CALC_X(TEMP)

        if ( mParams.linearOutput )
        {
            CALC_X_WITH_FB(
                mWFB * mOut.unaryExpr(tanh).cwiseProduct(mWFBScaling));
            mOut = mWOut * mX;
        }
        else
        {
            CALC_X_WITH_FB(
                mWFB * mOut.cwiseProduct(mWFBScaling));
            mOut = ( mWOut * mX ).unaryExpr( tanh );
        }

        #undef TEMP
        #undef CALC_X
        #undef CALC_X_WITH_FB

        auto isnotfinite =
            [] (float n) -> bool { return !std::isfinite(n); };
        if (mOut.unaryExpr(isnotfinite).any())
            throw OutputIsNotFinite();
    }

    void NetworkNSLI::CaptureTransformedInput(
        std::vector< float > & input )
    {
        if ( input.size() != mParams.inputCount )
            throw std::invalid_argument(
                "Size of the vector must be equal to "
                "the number of inputs" );
        for ( int i = 0; i < mParams.inputCount; ++ i )
            input[ i ] = mIn( i );
    }

    void NetworkNSLI::CaptureActivations(
        std::vector< float > & activations )
    {
        if ( activations.size() != mParams.neuronCount )
            throw std::invalid_argument(
                "Size of the vector must be equal "
                "actual number of neurons" );

        for ( int i = 0; i < mParams.neuronCount; ++ i )
            activations[ i ] = mX( i );
    }

    void NetworkNSLI::CaptureOutput( std::vector< float > & output )
    {
        if ( output.size() != mParams.outputCount )
            throw std::invalid_argument(
                "Size of the vector must be equal "
                "actual number of outputs" );

        for ( int i = 0; i < mParams.outputCount; ++ i )
            output[ i ] = mOut( i );
    }

    void NetworkNSLI::Train(
        const std::vector< std::vector< float > > & inputs,
        const std::vector< std::vector< float > > & outputs )
    {
        if ( inputs.size() == 0 )
            throw std::invalid_argument(
                "Number of samples must be not null" );
        if ( inputs.size() != outputs.size() )
            throw std::invalid_argument(
                "Number of input and output samples must be equal" );
        const unsigned kSampleCount = inputs.size();

        Eigen::MatrixXf matX( mParams.neuronCount, kSampleCount );
        Eigen::MatrixXf matY( mParams.outputCount, kSampleCount );
        for ( int i = 0; i < kSampleCount; ++ i )
        {
            SetInputs( inputs[i] );
            Step( 0.1f );
            matX.col( i ) = mX;
            matY.col( i ) = Eigen::Map< Eigen::VectorXf >(
                const_cast< float * >( outputs[i].data() ),
                    mParams.outputCount );
        }

        Eigen::MatrixXf matXT = matX.transpose();

        mWOut = ( matY * matXT * ( matX * matXT ).inverse() );
    }

    void NetworkNSLI::TrainOnline( const std::vector< float > & output,
        bool forceOutput )
    {
        for ( unsigned i = 0; i < mParams.outputCount; ++ i )
        {
            Eigen::VectorXf w = mWOut.row( i ).transpose();
            if ( mParams.linearOutput )
                mAdaptiveFilter.Train( w, mOut( i ), output[i], mX );
            else
                mAdaptiveFilter.Train( w, std::atanh( mOut( i ) ),
                    std::atanh( output[i] ), mX );
            mWOut.row( i ) = w.transpose();
        }

        if ( forceOutput )
            mOut = Eigen::Map< Eigen::VectorXf >(
                const_cast< float * >( output.data() ),
                mParams.outputCount );
    }

} // namespace ESN

#define SIZEOF_MEMBER( structure, member ) \
    sizeof( ( ( structure * ) 0 )->member )

void * esnCreateNetworkNSLI( esnNetworkParamsNSLI * params )
{
    static_assert( ( sizeof( esnNetworkParamsNSLI ) -
        SIZEOF_MEMBER( esnNetworkParamsNSLI, structSize ) ) ==
        sizeof( ESN::NetworkParamsNSLI ),
        "Wrong size of esnNetworkParamsNSLI" );

    if ( params->structSize != sizeof( esnNetworkParamsNSLI ) )
        throw std::invalid_argument(
            "esnNetworkParamsNSLI::structSize must be equal the "
            "sizeof( esnNetworkParamsNSLI )" );

    ESN::NetworkParamsNSLI p;
    std::memcpy( &p, reinterpret_cast< char * >( params ) +
        SIZEOF_MEMBER( esnNetworkParamsNSLI, structSize ),
        sizeof( ESN::NetworkParamsNSLI ) );

    return new ESN::NetworkNSLI( p );
}

#undef SIZEOF_MEMBER


#ifndef STACK_HH
#define STACK_HH

// 2020: Changes by Benjamin Huth
// - templated all classes

// StackT classes
//
// These are the low-level classes that implement feed-forward and
// recurrent neural networks. All the Eigen-dependant code in this
// library should live in this file.
//
// To keep the Eigen code out of the high-level interface, the STL ->
// Eigen ``preprocessor'' classes are also defined here.
//
// The ordering of classes is as follows:
//  - Feed-forward Stack class
//  - Feed-forward Layer classes
//  - RecurrentStackT class
//  - Recurrent layers
//  - Activation functions
//  - Various utility functions

#include "Exceptions.hh"
#include "NNLayerConfig.hh"

#include <Eigen/Dense>

#include <vector>
#include <functional>

namespace lwt {

  template<typename T>
  using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;

  template<typename T>
  using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  template<typename T>
  using ArrayX = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;

  template<typename T>
  class ILayerT;

  template<typename T>
  class IRecurrentLayerT;

  class FittableLWTNN;

  // ______________________________________________________________________
  // Feed forward Stack class

  template<typename T>
  class StackT
  {
    friend class FittableLWTNN;

  public:
    // constructor for dummy net
    StackT();
    // constructor for real net
    StackT(size_t n_inputs, const std::vector<LayerConfig>& layers,
          size_t skip_layers = 0);
    ~StackT();

    // make non-copyable for now
    StackT(StackT&) = delete;
    StackT& operator=(StackT&) = delete;

    VectorX<T> compute(VectorX<T>) const;
    size_t n_outputs() const;

  private:
    // returns the size of the next layer
    size_t add_layers(size_t n_inputs, const LayerConfig&);
    size_t add_dense_layers(size_t n_inputs, const LayerConfig&);
    size_t add_normalization_layers(size_t n_inputs, const LayerConfig&);
    size_t add_highway_layers(size_t n_inputs, const LayerConfig&);
    size_t add_maxout_layers(size_t n_inputs, const LayerConfig&);
    std::vector<ILayerT<T>*> m_layers;
    size_t m_n_outputs;
  };

  using Stack = StackT<double>;

  // _______________________________________________________________________
  // Feed-forward layers

  template<typename T>
  class ILayerT
  {
  public:
    virtual ~ILayerT() {}
    virtual VectorX<T> compute(const VectorX<T>&) const = 0;
  };

  using ILayer = ILayerT<double>;

  template<typename T>
  class DummyLayerT: public ILayerT<T>
  {
  public:
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  };

  using DummyLayer = DummyLayerT<double>;

  template<typename T>
  class UnaryActivationLayerT: public ILayerT<T>
  {
  public:
    UnaryActivationLayerT(ActivationConfig);
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  private:
    std::function<T(T)> m_func;
  };

  using UnaryActivationLayer = UnaryActivationLayerT<double>;

  template<typename T>
  class SoftmaxLayerT: public ILayerT<T>
  {
  public:
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  };

  using SoftmaxLayer = SoftmaxLayerT<double>;

  template<typename T>
  class BiasLayerT: public ILayerT<T>
  {
    friend class FittableLWTNN;

  public:
    BiasLayerT(const VectorX<T>& bias);
    template<typename U> BiasLayerT(const std::vector<U>& bias);
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  private:
    VectorX<T> m_bias;
  };

  using BiasLayer = BiasLayerT<double>;

  template<typename T>
  class MatrixLayerT: public ILayerT<T>
  {
    friend class FittableLWTNN;

  public:
    MatrixLayerT(const MatrixX<T>& matrix);
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  private:
    MatrixX<T> m_matrix;
  };

  using MatrixLayer = MatrixLayerT<double>;

  template<typename T>
  class MaxoutLayerT: public ILayerT<T>
  {
    friend class FittableLWTNN;

  public:
    typedef std::pair<MatrixX<T>, VectorX<T>> InitUnit;
    MaxoutLayerT(const std::vector<InitUnit>& maxout_tensor);
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  private:
    std::vector<MatrixX<T>> m_matrices;
    MatrixX<T> m_bias;
  };

  using MaxoutLayer = MaxoutLayerT<double>;

  /// Normalization layer ///
  /// https://arxiv.org/abs/1502.03167 ///
  template<typename T>
  class NormalizationLayerT : public ILayerT<T>
  {
    friend class FittableLWTNN;

  public:
    NormalizationLayerT(const VectorX<T>& W,const VectorX<T>& b);
    virtual VectorX<T> compute(const VectorX<T>&) const override;

  private:
    VectorX<T> _W;
    VectorX<T> _b;

  };

  using NormalizationLayer = NormalizationLayerT<double>;

  //http://arxiv.org/pdf/1505.00387v2.pdf
  template<typename T>
  class HighwayLayerT: public ILayerT<T>
  {
    friend class FittableLWTNN;

  public:
    HighwayLayerT(const MatrixX<T>& W,
                 const VectorX<T>& b,
                 const MatrixX<T>& W_carry,
                 const VectorX<T>& b_carry,
                 ActivationConfig activation);
    virtual VectorX<T> compute(const VectorX<T>&) const override;
  private:
    MatrixX<T> m_w_t;
    VectorX<T> m_b_t;
    MatrixX<T> m_w_c;
    VectorX<T> m_b_c;
    std::function<T(T)> m_act;
  };

  using HighwayLayer = HighwayLayerT<double>;

  // ______________________________________________________________________
  // Recurrent StackT

  template<typename T>
  class RecurrentStackT
  {
  public:
    RecurrentStackT(size_t n_inputs, const std::vector<LayerConfig>& layers);
    ~RecurrentStackT();
    RecurrentStackT(RecurrentStackT&) = delete;
    RecurrentStackT& operator=(RecurrentStackT&) = delete;
    MatrixX<T> scan(MatrixX<T> inputs) const;
    size_t n_outputs() const;
  private:
    std::vector<IRecurrentLayerT<T>*> m_layers;
    size_t add_lstm_layers(size_t n_inputs, const LayerConfig&);
    size_t add_gru_layers(size_t n_inputs, const LayerConfig&);
    size_t add_embedding_layers(size_t n_inputs, const LayerConfig&);
    size_t m_n_outputs;
  };

  using RecurrentStack = RecurrentStackT<double>;

  // This is the old RecurrentStack. Should probably absorb this into
  // the high-level interface in LightweightRNN, since all it does is
  // provide a slightly higher-level interface to a network which
  // combines recurrent + ff layers.
  template<typename T>
  class ReductionStackT
  {
  public:
    ReductionStackT(size_t n_in, const std::vector<LayerConfig>& layers);
    ~ReductionStackT();
    ReductionStackT(ReductionStackT&) = delete;
    ReductionStackT& operator=(ReductionStackT&) = delete;
    VectorX<T> reduce(MatrixX<T> inputs) const;
    size_t n_outputs() const;
  private:
    RecurrentStackT<T>* m_recurrent;
    StackT<T>* m_stack;
  };

  using ReductionStack = ReductionStackT<double>;

  // __________________________________________________________________
  // Recurrent layers

  template<typename T>
  class IRecurrentLayerT
  {
  public:
    virtual ~IRecurrentLayerT() {}
    virtual MatrixX<T> scan( const MatrixX<T>&) const = 0;
  };

  using IRecurrentLayer = IRecurrentLayerT<double>;

  template<typename T>
  class EmbeddingLayerT : public IRecurrentLayerT<T>
  {
  public:
    EmbeddingLayerT(int var_row_index, MatrixX<T> W);
    virtual ~EmbeddingLayerT() {};
    virtual MatrixX<T> scan( const MatrixX<T>&) const override;

  private:
    int m_var_row_index;
    MatrixX<T> m_W;
  };

  using EmbeddingLayer = EmbeddingLayerT<double>;

  /// long short term memory ///
  template<typename T> struct LSTMStateT;
  using LSTMState = LSTMStateT<double>;

  template<typename T>
  class LSTMLayerT : public IRecurrentLayerT<T>
  {
  public:
    LSTMLayerT(ActivationConfig activation,
              ActivationConfig inner_activation,
              MatrixX<T> W_i, MatrixX<T> U_i, VectorX<T> b_i,
              MatrixX<T> W_f, MatrixX<T> U_f, VectorX<T> b_f,
              MatrixX<T> W_o, MatrixX<T> U_o, VectorX<T> b_o,
              MatrixX<T> W_c, MatrixX<T> U_c, VectorX<T> b_c);

    virtual ~LSTMLayerT() {};
    virtual MatrixX<T> scan( const MatrixX<T>&) const override;
    void step( const VectorX<T>& input, LSTMState& ) const;

  private:
    std::function<T(T)> m_activation_fun;
    std::function<T(T)> m_inner_activation_fun;

    MatrixX<T> m_W_i;
    MatrixX<T> m_U_i;
    VectorX<T> m_b_i;

    MatrixX<T> m_W_f;
    MatrixX<T> m_U_f;
    VectorX<T> m_b_f;

    MatrixX<T> m_W_o;
    MatrixX<T> m_U_o;
    VectorX<T> m_b_o;

    MatrixX<T> m_W_c;
    MatrixX<T> m_U_c;
    VectorX<T> m_b_c;

    int m_n_outputs;
  };

  using LSTMLayer = LSTMLayerT<double>;

  /// gated recurrent unit ///
  template<typename T> struct GRUStateT;
  using GRUState = GRUStateT<double>;

  template<typename T>
  class GRULayerT : public IRecurrentLayerT<T>
  {
  public:
    GRULayerT(ActivationConfig activation,
             ActivationConfig inner_activation,
             MatrixX<T> W_z, MatrixX<T> U_z, VectorX<T> b_z,
             MatrixX<T> W_r, MatrixX<T> U_r, VectorX<T> b_r,
             MatrixX<T> W_h, MatrixX<T> U_h, VectorX<T> b_h);

    virtual ~GRULayerT() {};
    virtual MatrixX<T> scan( const MatrixX<T>&) const override;
    void step( const VectorX<T>& input, GRUState& ) const;

  private:
    std::function<T(T)> m_activation_fun;
    std::function<T(T)> m_inner_activation_fun;

    MatrixX<T> m_W_z;
    MatrixX<T> m_U_z;
    VectorX<T> m_b_z;

    MatrixX<T> m_W_r;
    MatrixX<T> m_U_r;
    VectorX<T> m_b_r;

    MatrixX<T> m_W_h;
    MatrixX<T> m_U_h;
    VectorX<T> m_b_h;

    int m_n_outputs;
  };

  using GRULayer = GRULayerT<double>;

  // ______________________________________________________________________
  // Activation functions

  // note that others are supported but are too simple to
  // require a special function
  template<typename T> T nn_sigmoidT( T x );
  template<typename T> T nn_hard_sigmoidT( T x );
  template<typename T> T nn_tanhT( T x );
  template<typename T> T nn_reluT( T x );

  double nn_sigmoid( double x );
  double nn_hard_sigmoid( double x );
  double nn_tanh( double x );
  double nn_relu( double x );

  template<typename T>
  class ELUT
  {
  public:
    ELUT(T alpha);
    T operator()(T) const;
  private:
    T m_alpha;
  };

  using ELU = ELUT<double>;

  template<typename T>
  class LeakyReLUT
  {
  public:
    LeakyReLUT(T alpha);
    T operator()(T) const;
  private:
    T m_alpha;
  };

  using LeakyReLU = LeakyReLUT<double>;

  template<typename T>
  class SwishT
  {
  public:
    SwishT(T alpha);
    T operator()(T) const;
  private:
    T m_alpha;
  };

  using Swish = SwishT<double>;

  template<typename T> std::function<T(T)> get_activationT(lwt::ActivationConfig);
  std::function<double(double)> get_activation(lwt::ActivationConfig);

  // WARNING: you own this pointer! Only call when assigning to member data!
  template<typename T> ILayerT<T>* get_raw_activation_layerT(ActivationConfig);
  ILayer* get_raw_activation_layer(ActivationConfig);

  // ______________________________________________________________________
  // utility functions

  // functions to build up basic units from vectors
  template<typename T1, typename T2> MatrixX<T1> build_matrixT(const std::vector<T2>& weights, size_t n_inputs);
  template<typename T1, typename T2> VectorX<T1> build_vectorT(const std::vector<T2>& bias);

  MatrixX<double> build_matrix(const std::vector<double>& weights, size_t n_inputs);
  VectorX<double> build_vector(const std::vector<double>& bias);

  // consistency checks
  void throw_if_not_maxout(const LayerConfig& layer);
  void throw_if_not_dense(const LayerConfig& layer);
  void throw_if_not_normalization(const LayerConfig& layer);

  // LSTM component for convenience in some layers
  template<typename T>
  struct DenseComponentsT
  {
    MatrixX<T> W;
    MatrixX<T> U;
    VectorX<T> b;
  };

  using DenseComponents = DenseComponentsT<double>;

  template<typename T> DenseComponentsT<T> get_componentT(const lwt::LayerConfig& layer, size_t n_in);
  DenseComponents get_component(const lwt::LayerConfig& layer, size_t n_in);
}

#include "Stack.txx"

#endif // STACK_HH


#ifndef _HOMOMORPHINE_SEAL_BACKED_H_
#define _HOMOMORPHINE_SEAL_BACKED_H_

#include <iostream>
#include <utility>
#include <sstream>
#include <string>
#include <math.h>
#include <seal/seal.h>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup.hpp>

#include "util.hpp"
#include "backend.hpp"
#include "arithmetic_backend.hpp"
#include "constants.hpp"

using namespace seal;
using namespace std;
using namespace boost::archive::iterators;

namespace homomorphine
{

  //! SEAL algorithms
  enum class SealAlgorithm {
    BFV,     /*!< SEAL BFV backend */
    CKKS,    /*!< SEAL CKKS backend */
    UNKNOWN  /*!< Unknown algorithm type - usually an error */
  };

  /*! /brief SEAL backend
   *
   * This class is an implementation of the SEAL backend.
   */
  class SealBackend : public ArithmeticBackend
  {
    private:
      std::shared_ptr<SEALContext> context;               /*!< SEAL context object */
      KeyGenerator *keygen = nullptr;                     /*!< SEAL key generator */
      SealAlgorithm algorithm;                            /*!< SEAL algorithm type (BFV, or CKKS) */
      EncryptionParameters *encryption_params = nullptr;  /*!< SEAL specific encryption parameters */
      PublicKey public_key;                               /*!< SEAL public key */
      SecretKey secret_key;                               /*!< SEAL secret key */
      RelinKeys relin_keys;                               /*!< SEAL relinearization keys */
      Ciphertext cipher;                                  /*!< cipher */
      double scale;                                       /*!< bit-precision of encoding, used by CKKS encryption implementation */

      /*!
       * Get SEAL algorithm type from algorithm name
       *
       * \param name algorithm name
       * \return algorithm type
       */
      SealAlgorithm getAlgorithmType(string name);

      /*!
       * Initialize BFV specific stuff of SEAL backend
       */
      void initBFV();

      /*!
       * Initialize CKKS specific stuff of SEAL backend
       */
      void initCKKS();

      /*!
       * Encode vector of values using BFV algorithm
       *
       * \param values vector of values
       * \return plaintext representation of values
       */
      Plaintext encodeWithBFV(vector<long> values);

      /*!
       * Encode single value using BFV algorithm
       *
       * \param value value
       * \return plaintext representation of value
       */
      Plaintext encodeWithBFV(long value);

      /*!
       * Encode vector of values using CKKS algorithm
       *
       * \param values vector of values
       * \return plaintext representation of values
       */
      Plaintext encodeWithCKKS(vector<long> values);

      /*!
       * Encode single value using CKKS algorithm
       *
       * \param value value
       * \return plaintext representation of value
       */
      Plaintext encodeWithCKKS(long value);

      /*!
       * Decode value from plaintext using BFV algorithm
       *
       * \param plain_result plaintext representation of value
       * \return value
       */
      long decodeWithBFV(Plaintext plain_result);

      /*!
       * Decode vector of values from plaintext using BFV algorithm
       *
       * \param plain_result plaintext representation of value
       * \return value
       */
      vector<long> decodeValuesWithBFV(Plaintext plain_result);

      /*!
       * Decode value from plaintext using CKKS algorithm
       *
       * \param plain_result plaintext representation of value
       * \return vector of values
       */
      long decodeWithCKKS(Plaintext plain_result);

      /*!
       * Decode vector of value from plaintext using CKKS algorithm
       *
       * \param plain_result plaintext representation of value
       * \return vector of values
       */
      vector<long> decodeValuesWithCKKS(Plaintext plain_result);

    public:

      /*!
       * SEAL backend cleanup
       */
      ~SealBackend();

      /*!
       * Initializes the SEAL backend
       */
      void init();

      /*!
       * Sets the specific SEAL algorithm implementation
       * that backend provides (currently, BFV and CKKS)
       *
       * \param algorithm homomorphic encryption algorithm
       */
      void setAlgorithm(string algorithm);

      /*!
       * Sets the specific SEAL algorithm implementation
       * that backend provides using the name of algorithm
       *
       * \param algorithm homomorphic encryption algorithm
       */
      void setAlgorithm(SealAlgorithm algorithm);

      /*!
       * Generates the public/secret key pair
       */
      void generateKeys();

      /*!
       * Returns the UUEncoded public key
       *
       * \return public key
       */
      string getPublicKey();

      /*!
       * Writes the public key to a stream
       *
       * \param stream public key stream
       */
      void writePublicKeyToStream(ostream& stream);

      /*!
       * Returns the UUEncoded secret key
       *
       * \return secret key
       */
      string getSecretKey();

      /*!
       * Writes the secret key to a stream
       *
       * \param stream secret key stream
       */
      void writeSecretKeyToStream(ostream& stream);

      /*!
       * Returns the pair of UUEncoded public and secret keys
       *
       * \return pair of public and secret keys
       */
      pair<string, string> getKeys();

      /*!
       * Sets the public key
       *
       * \param public_key UUEncoded public key
       */
      void setPublicKey(string public_key);

      /*!
       * Sets the public key from stream
       *
       * \param stream public key binary stream
       */
      void readPublicKeyFromStream(istream &stream);

      /*!
       * Sets the secret key
       *
       * \param secret_key UUEncoded secret key
       */
      void setSecretKey(string secret_key);

      /*!
       * Sets the secret key from stream
       *
       * \param stream secret key binary stream
       */
      void readSecretKeyFromStream(istream &stream);

      /*!
       * Sets the both public and secret keys
       *
       * \param public_key UUEncoded public key
       * \param secret_key UUEncoded secret key
       */
      void setKeys(string public_key, string secret_key);

       /*!
       * Returns the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \return UUEncoded cipher
       */
      string getCipher();

      /*!
       * Writes the cipher to output stream
       *
       * \param stream output stream
       */
      void writeCipherToStream(ostream& stream);

      /*!
       * Sets the UUEncoded cipher containing ecrypted value, or vector of values
       *
       * \param cipher UUEncoded cipher
       */
      void setCipher(string cipher);

      /*!
       * Reads the cipher from input stream
       *
       * \param stream cipher stream
       */
      void readCipherFromStream(istream &stream);

      /*!
       * Encrypts the vector of values using the public key
       *
       * \param values vector of values
       * \return UUEncoded cipher
       */
      void encrypt(vector<long> values);

      /*!
       * Encrypts the single value using the public key
       *
       * \param value value
       * \return UUEncoded cipher
       */
      void encrypt(long value);

      /*!
       * Decrypts the vector of values using the secret key
       *
       * \return vector of decrypted values
       */
      vector<long> decryptValues();

      /*!
       * Decrypts the single value using the secret key
       *
       * \return decrypted value
       */
      long decrypt();

      /*!
       * Adds the vector of values to encrypted vector of values
       *
       * \param values vector of values
       */
      void add(vector<long> values);

      /*!
       * Adds the value to encrypted value
       *
       * \param value value
       */
      void add(long value);

      /*!
       * Negates a single encrypted value, or a vector of encrypted values
       */
      void negate();

      /*!
       * Multiplies the vector of values with the encrypted vector of values
       *
       * \param values vector of values
       */
      void multiply(vector<long> values);

      /*!
       * Multiplies the value with the encrypted value
       *
       * \param value value
       */
      void multiply(long value);
  };
}

#endif


#include "quadrature/angular/level_symmetric_gaussian.h"

#include <deal.II/base/exceptions.h>
#include <deal.II/base/quadrature_lib.h>

namespace bart {

namespace quadrature {

namespace angular {

LevelSymmetricGaussian::LevelSymmetricGaussian(bart::quadrature::Order order)
    : order_(order.get()) {
  AssertThrow(order_ >= 2,
              dealii::ExcMessage("Error in constructor of "
                                 "LevelSymmetricGaussian order must be >= 2"))
  AssertThrow(order_ % 2 == 0,
              dealii::ExcMessage("Error in constructor of "
                                 "LevelSymmetricGaussian order must be even"))
  AssertThrow(order_ <= 16,
              dealii::ExcMessage("Error in constructor of "
                                 "LevelSymmetricGaussian order must be <= 16"))
}

std::vector<std::pair<CartesianPosition<3>, Weight>>
LevelSymmetricGaussian::GenerateSet() const {
  std::vector<std::pair<CartesianPosition<3>, Weight>> generated_set;
  const int n_points = order_/2;

  // Gaussian quadrature for theta, the lowest level has order_ points but we
  // will only use half of the quadrature
  dealii::QGauss<1> gaussian_quadrature(order_);

  // For each quadrature point in the lowest level, we will create equally
  // spaced points in phi
  for (int level = 0; level < n_points; ++level) {
    int n_points_this_level = level + 1;
    // mu is just the gaussian quadrature point
    double mu = 1 - gaussian_quadrature.point(level)[0] * 2;
    // points in phi will be equally spaced from 0 to pi/2
    double dphi = M_PI/(2*n_points_this_level);
    // weights on this level are equal to the gaussian times 2PI/total points in
    // 2PI. Here we have n_points_this_level*4 total points
    double weight =
        gaussian_quadrature.weight(level) * M_PI/(n_points_this_level);

    for (int j = 0; j < n_points_this_level; ++j) {
      double phi = (j + 0.5)*dphi;
      double x = std::sqrt(1 - mu*mu) * std::cos(phi);
      double y = std::sqrt(1 - mu*mu) * std::sin(phi);
      double z = mu;
      std::array<double, 3> position{x,y,z};
      generated_set.emplace_back(std::make_pair(CartesianPosition<3>(position),
                                                Weight(weight)));
    }
  }
  return generated_set;
}

} // namespace angular

} // namespace quadrature

} //namespace bart


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "utils/multinomialCoefficients.hpp"

// n=1; r=1

BOOST_AUTO_TEST_CASE( multinomialCoefficient_1 )
{
    std::vector< unsigned int > input = {1};
    BOOST_CHECK_EQUAL( 1, multinomial_coefficient( input ) );
}

// n=2; r=2

BOOST_AUTO_TEST_CASE( multinomialCoefficient_0_2 )
{
    std::vector< unsigned int > input = {0, 2};
    BOOST_CHECK_EQUAL( 1, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_1_1 )
{
    std::vector< unsigned int > input = {1, 1};
    BOOST_CHECK_EQUAL( 2, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_2_0 )
{
    std::vector< unsigned int > input = {2, 0};
    BOOST_CHECK_EQUAL( 1, multinomial_coefficient( input ) );
}

// n=3; r=3 (partial)

BOOST_AUTO_TEST_CASE( multinomialCoefficient_3_0_0 )
{
    std::vector< unsigned int > input = {3, 0, 0};
    BOOST_CHECK_EQUAL( 1, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_2_1_0 )
{
    std::vector< unsigned int > input = {2, 1, 0};
    BOOST_CHECK_EQUAL( 3, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_2_0_1 )
{
    std::vector< unsigned int > input = {2, 0, 1};
    BOOST_CHECK_EQUAL( 3, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_1_1_1 )
{
    std::vector< unsigned int > input = {1, 1, 1};
    BOOST_CHECK_EQUAL( 6, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_0_2_1 )
{
    std::vector< unsigned int > input = {0, 2, 1};
    BOOST_CHECK_EQUAL( 3, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_0_1_2 )
{
    std::vector< unsigned int > input = {0, 1, 2};
    BOOST_CHECK_EQUAL( 3, multinomial_coefficient( input ) );
}

BOOST_AUTO_TEST_CASE( multinomialCoefficient_0_0_3 )
{
    std::vector< unsigned int > input = {0, 0, 3};
    BOOST_CHECK_EQUAL( 1, multinomial_coefficient( input ) );
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/core/lightweight_test.hpp>

#include <boost/gil/image.hpp>
#include <boost/gil/image_processing/hough_transform.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>

#include <cstddef>
#include <iostream>
#include <limits>
#include <vector>

namespace gil = boost::gil;

template <typename Rasterizer>
void exact_fit_test(std::ptrdiff_t radius, gil::point_t offset, Rasterizer rasterizer)
{
    std::vector<gil::point_t> circle_points(rasterizer.point_count(radius));
    rasterizer(radius, offset, circle_points.begin());
    // const std::ptrdiff_t diameter = radius * 2 - 1;
    const std::ptrdiff_t width = offset.x + radius + 1;
    const std::ptrdiff_t height = offset.y + radius + 1;
    gil::gray8_image_t image(width, height);
    auto input = gil::view(image);

    for (const auto& point : circle_points)
    {
        input(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    using param_t = gil::hough_parameter<std::ptrdiff_t>;
    const auto radius_parameter = param_t{radius, 0, 1};
    // const auto x_parameter = param_t::from_step_count(offset.x, neighborhood, half_step_count);
    // const auto y_parameter = param_t::from_step_count(offset.y, neighborhood, half_step_count);
    const auto x_parameter = param_t{offset.x, 0, 1};
    const auto y_parameter = param_t{offset.y, 0, 1};

    std::vector<gil::gray16_image_t> output_images(
        radius_parameter.step_count,
        gil::gray16_image_t(x_parameter.step_count, y_parameter.step_count));
    std::vector<gil::gray16_view_t> output_views(radius_parameter.step_count);
    std::transform(output_images.begin(), output_images.end(), output_views.begin(),
                   [](gil::gray16_image_t& img)
                   {
                       return gil::view(img);
                   });
    gil::hough_circle_transform_brute(input, radius_parameter, x_parameter, y_parameter,
                                      output_views.begin(), rasterizer);
    if (output_views[0](0, 0) != rasterizer.point_count(radius))
    {
        std::cout << "accumulated value: " << static_cast<int>(output_views[0](0, 0))
                  << " expected value: " << rasterizer.point_count(radius) << "\n\n";
    }
    BOOST_TEST(output_views[0](0, 0) == rasterizer.point_count(radius));
}

int main()
{
    const int test_dim_length = 20;
    for (std::ptrdiff_t radius = 5; radius < test_dim_length; ++radius)
    {
        for (std::ptrdiff_t x_offset = radius; x_offset < radius + test_dim_length; ++x_offset)
        {
            for (std::ptrdiff_t y_offset = radius; y_offset < radius + test_dim_length; ++y_offset)
            {

                exact_fit_test(radius, {x_offset, y_offset}, gil::midpoint_circle_rasterizer{});
                exact_fit_test(radius, {x_offset, y_offset},
                               gil::trigonometric_circle_rasterizer{});
            }
        }
    }

    return boost::report_errors();
}


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

#ifndef BOOST_MATH_CCMATH_FPCLASSIFY
#define BOOST_MATH_CCMATH_FPCLASSIFY

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isfinite.hpp>

namespace boost::math::ccmath {

template <typename T, std::enable_if_t<!std::is_integral_v<T>, bool> = true>
inline constexpr int fpclassify(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return boost::math::ccmath::isnan(x) ? FP_NAN :
               boost::math::ccmath::isinf(x) ? FP_INFINITE :
               boost::math::ccmath::abs(x) == T(0) ? FP_ZERO :
               boost::math::ccmath::abs(x) > 0 && boost::math::ccmath::abs(x) < (std::numeric_limits<T>::min)() ? FP_SUBNORMAL : FP_NORMAL;
    }
    else
    {
        using std::fpclassify;
        return fpclassify(x);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr int fpclassify(Z x)
{
    return boost::math::ccmath::fpclassify(static_cast<double>(x));
}

}

#endif // BOOST_MATH_CCMATH_FPCLASSIFY


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

map<int, int> memorize;
int f(int n) {
    if (memorize.count(n)) {
        return memorize[n];
    }
    int x = (n % 2 == 0) ? n / 2 : 3 * n + 1;
    memorize[n] = x;
    return x;
}

int main(void) {
    int s;
    cin >> s;

    int i;
    set<int> ans;
    ans.insert(s);
    for(i = 2; ; ++i) {
        s = f(s);
        if(ans.count(s)) break;
        ans.insert(s);
    }
    cout << i << endl;
    return 0;
}