#include <NTL/ZZ_pXFactoring.h>
#include <NTL/ZZXFactoring.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2EXFactoring.h>

NTL_CLIENT

#define TIME_IT(t, action) \
do { \
   double _t0, _t1; \
   long _iter = 1; \
   long _cnt = 0; \
   do { \
      _t0 = GetTime(); \
      for (long _i = 0; _i < _iter; _i++) { action; _cnt++; } \
      _t1 = GetTime(); \
   } while ( _t1 - _t0 < 4 && (_iter *= 2)); \
   t = (_t1 - _t0)/_iter; \
} while(0)

int main()
{
   double t;

   long k = 1000;
   long n = 1000;

   {
      SetSeed(conv<ZZ>(1));
      ZZ p = RandomPrime_ZZ(k);

      ZZ_p::init(p);

      ZZ x, y, z, w, s1, s2;

      SetSeed(conv<ZZ>(2));
      RandomBnd(x, p);

      SetSeed(conv<ZZ>(3));
      RandomBnd(y, p);

      TIME_IT(t, mul(z, x, y));
      cout << "multiply 1000-bit ints: " << t << "\n";

      TIME_IT(t, sqr(w, x));
      cout << "square 1000-bit ints: " << t << "\n";

      TIME_IT(t, rem(w, z, p));
      cout << "remainder 2000/1000-bit ints: " << t << "\n";

      TIME_IT(t, GCD(w, x, y));
      cout << "gcd 1000-bit ints: " << t << "\n";

      TIME_IT(t, XGCD(w, s1, s2, x, y));
      cout << "xgcd 1000-bit ints: " << t << "\n";

      TIME_IT(t, PowerMod(w, x, y, p));
      cout << "power mod 1000-bit ints: " << t << "\n";

      ZZ_pX a, b, c;

      SetSeed(conv<ZZ>(4));
      random(a, n);

      SetSeed(conv<ZZ>(5));
      random(b, n);

      mul(c, a, b);

      TIME_IT(t, mul(c, a, b));
      cout << "multiply degree-1000 poly mod 1000-bit prime: " << t << "\n";

      ZZ_pX f;
      SetSeed(conv<ZZ>(6));
      random(f, n);
      SetCoeff(f, n);

      ZZ_pX A, B;

      SetSeed(conv<ZZ>(7));
      random(A, 2*(deg(f)-1));

      TIME_IT(t, rem(B, A, f));
      cout << "remainder degree-2000/1000 poly mod 1000-bit prime: " << t << "\n";

      ZZ_pXModulus F(f);

      TIME_IT(t, rem(B, A, F));
      cout << "preconditioned remainder degree-2000/1000 poly mod 1000-bit prime: " << t << "\n";

      TIME_IT(t, GCD(a, b));
      cout << "gcd degree-1000 poly mod 1000-bit prime: " << t << "\n";

      ZZX AA = conv<ZZX>(a);
      ZZX BB = conv<ZZX>(b);
      ZZX CC;

      TIME_IT(t, mul(CC, AA, BB));
      cout << "multiply degree-1000 int poly with 1000-bit coeffs: " << t << "\n";

      cout << "\n";
      cout << "factoring degree-1000 poly mod 1000-bit prime...\n";
      TIME_IT(t, CanZass(f, _cnt == 0));
      cout << "...total time = " << t << "\n\n";
   }
   {
      n = 500;
      k = 500;

      SetSeed(conv<ZZ>(8));
      GF2X p = BuildRandomIrred(BuildIrred_GF2X(k));

      GF2E::init(p);

      GF2X x, y, z, w;

      SetSeed(conv<ZZ>(9));
      random(x, deg(p));

      SetSeed(conv<ZZ>(10));
      random(y, deg(p));

      TIME_IT(t, mul(z, x, y));
      cout << "multiply 500-bit GF2Xs: " << t << "\n";

      TIME_IT(t, rem(w, z, p));
      cout << "remainder 1000/500-bit GF2Xs: " << t << "\n";

      TIME_IT(t, GCD(w, x, y));
      cout << "gcd 500-bit GF2Xs: " << t << "\n";

      SetSeed(conv<ZZ>(11));
      GF2X fff;
      random(fff, k);
      SetCoeff(fff, k);

      cout << "\n";
      TIME_IT(t, CanZass(fff, 0));
      cout << "factoring degree-500 GF2X: " << t << "\n";

      TIME_IT(t, GCD(w, x, y));
      cout << "gcd 500-bit GF2X: " << t << "\n";

      GF2EX a, b, c;

      SetSeed(conv<ZZ>(12));
      random(a, n);

      SetSeed(conv<ZZ>(13));
      random(b, n);

      mul(c, a, b);

      TIME_IT(t, mul(c, a, b));
      cout << "multiply degree-500 poly mod 500-bit GF2X: " << t << "\n";

      GF2EX f;
      SetSeed(conv<ZZ>(14));
      random(f, n);
      SetCoeff(f, n);

      GF2EX A, B;

      SetSeed(conv<ZZ>(15));
      random(A, 2*(deg(f)-1));

      TIME_IT(t, rem(B, A, f));
      cout << "remainder degree-1000/500 poly mod 500-bit GF2X: " << t << "\n";

      GF2EXModulus F(f);

      TIME_IT(t, rem(B, A, F));
      cout << "preconditioned remainder degree-1000/500 poly mod 500-bit GF2X: " << t << "\n";

      TIME_IT(t, GCD(a, b));
      cout << "gcd degree-500 poly mod 500-bit GF2X: " << t << "\n";

      f = f >> n/2;
      cout << "\n";
      cout << "factoring degree-500 poly mod 500-bit GF2X...\n";
      TIME_IT(t, CanZass(f, _cnt == 0));
      cout << "\n...total time = " << t << "\n";
   }
}


//
// Created by Gleb Marin on 03.09.2021.
//

#ifndef FLOAT_MATRIX_COOMATRIX_HPP
#define FLOAT_MATRIX_COOMATRIX_HPP

#include <unordered_map>
#include <vector>
#include <ostream>
#include <istream>
#include <algorithm>

#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/algorithm/merge.hpp>
#include <boost/compute/algorithm/reduce_by_key.hpp>
#include <boost/compute/types/struct.hpp>

#include "Utility.hpp"

namespace floatMatrix {
    using T = float;

    namespace cell {
        struct Cell {
            int row;
            int col;
            float data;
        };

        static inline BOOST_COMPUTE_FUNCTION(
                bool, compareCellCoords,
                (Cell a, Cell b), {
                    if (a.row == b.row) {
                        return a.col < b.col;
                    }
                    return a.row < b.row;
                }
        );

        static inline BOOST_COMPUTE_FUNCTION(
                Cell, reduceSameCells,
                (Cell a, Cell b), {
                    Cell reduced = {.row = a.row, .col = a.col, .data = a.data + b.data};
                    return reduced;
                }
        );

        static inline BOOST_COMPUTE_FUNCTION(
                bool, equalCellCoords,
                (Cell a, Cell b), {
                    return a.col == b.col && a.row == b.row;
                }
        );
    }

    class CooMatrix {
    public:
        using CommandQueue = boost::compute::command_queue;
        using Cell = cell::Cell;
        using DeviceCells = boost::compute::vector<Cell>;

        static constexpr T ZERO = static_cast<T>(0);

    private:
        using Row = std::unordered_map<std::size_t, T>;

    public:
        CooMatrix() = default;

        explicit CooMatrix(const DeviceCells& cells) {
            for (Cell cell : cells) {
                matrix_[cell.row][cell.col] = cell.data;
            }
        }

        template <typename Iterator>
        explicit CooMatrix(Iterator begin, Iterator end, CommandQueue queue) {
            for (auto it = begin; it < end; ++it) {
                const auto& cell = it.read(queue);
                matrix_[cell.row][cell.col] = cell.data;
            }
        }

        explicit CooMatrix(const std::vector<Cell>& cells) {
            for (const auto& cell : cells) {
                matrix_[cell.row][cell.col] = cell.data;
            }
        }

        void set(std::size_t row, std::size_t col, const T& value) noexcept {
            matrix_[row][col] = value;
        }

        [[nodiscard]] T get(std::size_t rowId, std::size_t colId) const noexcept {
            auto rowIt = matrix_.find(rowId);
            if (rowIt == matrix_.cend()) {
                return ZERO;
            }
            const Row& row = rowIt->second;
            auto valIt = row.find(colId);
            if (valIt == row.cend()) {
                return ZERO;
            }
            return valIt->second;
        }

        void clear() noexcept {
            matrix_.clear();
        }

        CooMatrix& add(const CooMatrix& other) {
            for (const auto&[rowId, row] : other.matrix_) {
                for (const auto&[colId, value] : row) {
                    matrix_[rowId][colId] += value;
                    if (matrix_[rowId][colId] == ZERO) {
                        matrix_[rowId].erase(colId);
                    }
                }
            }
            return *this;
        }

        CooMatrix& add(const CooMatrix& other, boost::compute::command_queue& queue) {
            DeviceCells thisCells = toDeviceCells(queue);
            DeviceCells otherCells = other.toDeviceCells(queue);
            boost::compute::sort(thisCells.begin(), thisCells.end(), cell::compareCellCoords, queue);
            boost::compute::sort(otherCells.begin(), otherCells.end(), cell::compareCellCoords, queue);
            DeviceCells merged(thisCells.size() + otherCells.size(), queue.get_context());

            boost::compute::merge(
                    thisCells.begin(), thisCells.end(),
                    otherCells.begin(), otherCells.end(),
                    merged.begin(),
                    cell::compareCellCoords,
                    queue
            );

            DeviceCells reducedValues(merged.size(), queue.get_context());
            DeviceCells reducedKeys(merged.size(), queue.get_context());
            auto [keysEnd, valuesEnd] = boost::compute::reduce_by_key(
                    merged.begin(), merged.end(), merged.begin(),
                    reducedKeys.begin(), reducedValues.begin(),
                    cell::reduceSameCells,
                    cell::equalCellCoords,
                    queue
            );

            return *this = CooMatrix(reducedValues.begin(), valuesEnd, queue);
        }

        [[nodiscard]] std::vector<Cell> toCellsList() const {
            std::vector<Cell> cells;
            for (const auto&[rowId, row] : matrix_) {
                for (const auto&[colId, value] : row) {
                    if (value == ZERO) {
                        continue;
                    }
                    cells.push_back({static_cast<int>(rowId), static_cast<int>(colId), value});
                }
            }
            return cells;
        }

        DeviceCells toDeviceCells(CommandQueue& queue) const {
            std::vector<Cell> cells = toCellsList();
            DeviceCells deviceCells(cells.size(), queue.get_context());
            boost::compute::copy(cells.begin(), cells.end(), deviceCells.begin(), queue);
            return deviceCells;
        }

        bool operator==(const CooMatrix& other) const {
            return isEqSubset(other) && other.isEqSubset(*this);
        }

        friend std::ostream& operator<<(std::ostream& os, const CooMatrix& comp) {
            std::vector<Cell> cells = comp.toCellsList();
            os << cells.size() << '\n';
            for (const Cell& cell : cells) {
                os << cell.row << ' ';
                os << cell.col << ' ';
                os << cell.data << ' ';
            }
            return os;
        }

        friend std::istream& operator>>(std::istream& is, CooMatrix& comp) {
            std::size_t elems;
            is >> elems;
            std::unordered_map<std::size_t, Row> rows(elems);
            for (std::size_t i = 0; i < elems; ++i) {
                std::size_t row, col;
                T data;
                is >> row >> col >> data;
                rows[row][col] = data;
            }
            comp.matrix_ = rows;
            return is;
        }

    private:
        [[nodiscard]] bool isEqSubset(const CooMatrix& other) const {
            for (const auto&[rowId, row] : matrix_) {
                for (const auto&[colId, value] : row) {
                    if (!utility::isEq(other.get(rowId, colId), value)) {
                        std::cerr << "Row: " << rowId << ' ' << "Col: " << colId << std::endl;
                        return false;
                    }
                }
            }
            return true;
        }

        std::unordered_map<std::size_t, Row> matrix_;
    };
}

BOOST_COMPUTE_ADAPT_STRUCT(floatMatrix::cell::Cell, Cell, (row, col, data));

#endif //FLOAT_MATRIX_COOMATRIX_HPP


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

#ifndef percept_math_TransformationMatrix_hpp
#define percept_math_TransformationMatrix_hpp

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

#include <math.h>

#include <Teuchos_ScalarTraits.hpp>

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

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

namespace percept {

  namespace ublas =  boost::numeric::ublas;

  class Math
  {
  public:

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

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

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

    class MyVector : public ubvec
    {
    public:

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

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

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

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

    };

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

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

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

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

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

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

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

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

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

}

#endif


#ifndef NBT_DYNAMICS_ENGINE_HPP
#define NBT_DYNAMICS_ENGINE_HPP

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

#endif


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/vector/arithmetic.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_units
)
{
FCPPT_PP_POP_WARNING
	typedef int unit_type;

	typedef boost::units::quantity<
		boost::units::si::length,
		unit_type
	> length;

	typedef boost::units::quantity<
		boost::units::si::time,
		unit_type
	> time;

	typedef boost::units::quantity<
		boost::units::si::velocity,
		unit_type
	> velocity;

	typedef fcppt::math::vector::static_<
		length,
		2
	> length2;

	typedef fcppt::math::vector::static_<
		time,
		2
	> time2;

	typedef fcppt::math::vector::static_<
		velocity,
		2
	> velocity2;

	length2 const l1(
		length(
			-100
			*
			boost::units::si::meter
		),
		length(
			200
			*
			boost::units::si::meter
		)
	);

	time2 const t1(
		time(
			4
			*
			boost::units::si::second
		),
		time(
			2
			*
			boost::units::si::second
		)
	);

	velocity2 const v1(
		l1
		/
		t1
	);

	BOOST_CHECK_EQUAL(
		v1.x().value(),
		-25
	);

	BOOST_CHECK_EQUAL(
		v1.y().value(),
		100
	);
}


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

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

#include <boost/foreach.hpp>

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

#include <mpi2/mpi2.h>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

	// shut down task managers
	mpi2stop();

	// shut down mpi2 and mpi
	mpi2finalize();

	return 0;
}


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

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

#include "mdm_ParamSummaryStats.h"

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

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

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

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

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

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

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

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

}

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

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

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

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

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

	//Reset the stats
	stats_.reset();

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

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

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

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

			else
				voxValue = 1 / voxValue;
		}

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

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

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

		//std and iqr are 0
		return;
	}

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

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

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

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

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

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

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

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

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

	statsOStream_ << "\n";

}

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

}

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

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

}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


// MIT License
//
// Copyright (c) 2019 degski
//
// 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 <cmath>
#include <vector>

#include <boost/container/deque.hpp>

#include <SFML/Graphics.hpp>
#include "Extensions.hpp"

namespace sf::CatmullRom {

    using Points = boost::container::deque<Point>;
    using CoordinatesVector = std::vector<Point>;

    template<typename Container>
    float length ( const Container &v_ ) noexcept {

        Vector2f d;
        float return_value = 0.0f;

        for ( std::size_t i = 1, l = v_.size ( ); i < l; ++i ) {

            d = v_ [ i ] - v_ [ i - 1 ];

            return_value += std::sqrtf ( d.x * d.x + d.y * d.y );
        }

        return return_value;
    }

    // CoordinatesVector catmullRom0 ( const Points &points_, const int number_of_points_per_interval_ );

    // Calculate Catmull Rom for a chain of points and return the combined curve. The
    // chains' first and last point are extrapolated from the second and 1-before-
    // last point, respectively...
    CoordinatesVector catmullRom ( const Points & points_, const Int32 number_of_points_per_interval_ ) noexcept;

    // Calculate Catmull Rom for a chain of points and return the combined curve. The
    // chains' first and last point are extrapolated from the second and 1-before-
    // last point, respectively. The returned points are at a distance of distance_
    // (at least) away from each other... 	Requires 3 points or more...
    CoordinatesVector catmullRom ( const Points & points_, float distance_ ) noexcept;
}


//==================================================================================================
/*!
  @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_ACOTD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOTD_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acotd capabilities

    inverse cotangent in degree.

    @par Semantic:

    For every parameter of floating type

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

    Returns the arc @c r in the interval
    \f$[0, 180[\f$ such that <tt>cotd(r) == x</tt>.

    @see acot, acotpi, cotd

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

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

#endif


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

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

#include <bob.math/gsvd.h>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  const int lwork_query = -1;
  double work_query;

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

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

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

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

  // B/ Computes

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

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

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

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

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

   else

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

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

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

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

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

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

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

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

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

  }
  else{

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

}


/**TODO:  Add copyright*/

#define BOOST_TEST_MODULE ModelInterpreter DCG test suite
#include <boost/test/included/unit_test.hpp>
#include <SmartPeak/ml/ModelInterpreterDefaultDevice.h>
#include <SmartPeak/ml/ModelBuilder.h> // comprehensive architecture tests

using namespace SmartPeak;
using namespace std;

Model<float> makeModelFCSum()
{
	/**
	 * Directed Cyclic Graph Toy Network Model
	*/
	Node<float> i1, h1, o1, b1, b2;
	Link l1, l2, l3, lb1, lb2;
	Weight<float> w1, w2, w3, wb1, wb2;
	Model<float> model2;
	// Toy network: 1 hidden layer, fully connected, DCG
	i1 = Node<float>("0", NodeType::input, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	h1 = Node<float>("1", NodeType::hidden, NodeStatus::initialized, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	o1 = Node<float>("2", NodeType::output, NodeStatus::initialized, std::make_shared<ReLUOp<float>>(ReLUOp<float>()), std::make_shared<ReLUGradOp<float>>(ReLUGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	b1 = Node<float>("3", NodeType::bias, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	b2 = Node<float>("4", NodeType::bias, NodeStatus::activated, std::make_shared<LinearOp<float>>(LinearOp<float>()), std::make_shared<LinearGradOp<float>>(LinearGradOp<float>()), std::make_shared<SumOp<float>>(SumOp<float>()), std::make_shared<SumErrorOp<float>>(SumErrorOp<float>()), std::make_shared<SumWeightGradOp<float>>(SumWeightGradOp<float>()));
	// weights
	std::shared_ptr<WeightInitOp<float>> weight_init;
	std::shared_ptr<SolverOp<float>> solver;
	// weight_init.reset(new RandWeightInitOp(1.0)); // No random init for testing
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w1 = Weight<float>("0", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w2 = Weight<float>("1", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	w3 = Weight<float>("2", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb1 = Weight<float>("3", weight_init, solver);
	weight_init = std::make_shared<ConstWeightInitOp<float>>(ConstWeightInitOp<float>(1.0));
	solver = std::make_shared<SGDOp<float>>(SGDOp<float>(0.01, 0.9));
	wb2 = Weight<float>("4", weight_init, solver);
	weight_init.reset();
	solver.reset();
	// links
	l1 = Link("0", "0", "1", "0");
	l2 = Link("1", "1", "2", "1");
	l3 = Link("2", "1", "1", "2"); // cycle
	lb1 = Link("3", "3", "1", "3");
	lb2 = Link("4", "4", "2", "4");
	model2.setId(2);
	model2.addNodes({ i1, h1, o1, b1, b2 });
	model2.addWeights({ w1, w2, w3, wb1, wb2 });
	model2.addLinks({ l1, l2, l3, lb1, lb2 });
	model2.findCycles();
	return model2;
}

BOOST_AUTO_TEST_SUITE(modelInterpreter_DCG)

BOOST_AUTO_TEST_CASE(constructor)
{
	ModelInterpreterDefaultDevice<float>* ptr = nullptr;
	ModelInterpreterDefaultDevice<float>* nullPointer = nullptr;
	ptr = new ModelInterpreterDefaultDevice<float>();
	BOOST_CHECK_NE(ptr, nullPointer);
}

BOOST_AUTO_TEST_CASE(destructor)
{
	ModelInterpreterDefaultDevice<float>* ptr = nullptr;
	ptr = new ModelInterpreterDefaultDevice<float>();
	delete ptr;
}

/**
 * Part 2 test suit for the ModelInterpreter class
 *
 * The following test methods that are
 * required of a standard recurrent neural network
*/

Model<float> model_getNextInactiveLayer = makeModelFCSum();
BOOST_AUTO_TEST_CASE(getNextInactiveLayerWOBiases)
{
  // Toy network: 1 hidden layer, fully connected, DAG
	ModelInterpreterDefaultDevice<float> model_interpreter;

  // initialize nodes
	// NOTE: input and biases have been activated when the model was created

	// get the next hidden layer
	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_getNextInactiveLayer, FP_operations_map, FP_operations_list);

	BOOST_CHECK_EQUAL(FP_operations_map.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_map.at("1"), 0);
	BOOST_CHECK_EQUAL(FP_operations_list.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].source_node->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].weight->getName(), "0");
}

Model<float> model_getNextInactiveLayerBiases = makeModelFCSum();
BOOST_AUTO_TEST_CASE(getNextInactiveLayerBiases)
{
  // Toy network: 1 hidden layer, fully connected, DAG
  // Model<float> model_FC_Sum = makeModelFCSum();
	ModelInterpreterDefaultDevice<float> model_interpreter;

  // initialize nodes
	// NOTE: input and biases have been activated when the model was created

	// get the next hidden layer
	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_getNextInactiveLayerBiases, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_getNextInactiveLayerBiases, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	BOOST_CHECK_EQUAL(FP_operations_map.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_map.at("1"), 0);
	BOOST_CHECK_EQUAL(FP_operations_list.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments.size(), 2);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].source_node->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].weight->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].source_node->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].weight->getName(), "3");
	BOOST_CHECK_EQUAL(sink_nodes_with_biases2.size(), 1);
	BOOST_CHECK_EQUAL(sink_nodes_with_biases2[0], "1");
}

Model<float> model_getNextInactiveLayerCycles = makeModelFCSum();
BOOST_AUTO_TEST_CASE(getNextInactiveLayerCycles)
{
	// Toy network: 1 hidden layer, fully connected, DAG
	// Model<float> model_FC_Sum = makeModelFCSum();
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// initialize nodes
	// NOTE: input and biases have been activated when the model was created

	// get the next hidden layer
	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_getNextInactiveLayerCycles, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_getNextInactiveLayerCycles, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	std::set<std::string> sink_nodes_with_cycles;
	model_interpreter.getNextInactiveLayerCycles(model_getNextInactiveLayerCycles, FP_operations_map, FP_operations_list, sink_nodes_with_cycles);

	BOOST_CHECK_EQUAL(FP_operations_map.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_map.at("1"), 0);
	BOOST_CHECK_EQUAL(FP_operations_list.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments.size(), 3);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].source_node->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].weight->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].source_node->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].weight->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].source_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].weight->getName(), "2");
	BOOST_CHECK_EQUAL(sink_nodes_with_cycles.size(), 1);
  BOOST_CHECK_EQUAL(sink_nodes_with_cycles.count("1"), 1);
}

Model<float> model_pruneInactiveLayerCycles = makeModelFCSum();
BOOST_AUTO_TEST_CASE(pruneInactiveLayerCycles)
{
	// Toy network: 1 hidden layer, fully connected, DAG
	// Model<float> model_FC_Sum = makeModelFCSum();
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// initialize nodes
	// NOTE: input and biases have been activated when the model was created

	// get the next hidden layer
	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_pruneInactiveLayerCycles, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_pruneInactiveLayerCycles, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	std::set<std::string> sink_nodes_with_cycles;
	std::map<std::string, int> FP_operations_map_cycles = FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list_cycles = FP_operations_list;
	model_interpreter.getNextInactiveLayerCycles(model_pruneInactiveLayerCycles, FP_operations_map_cycles, FP_operations_list_cycles, sink_nodes_with_cycles);

	model_interpreter.pruneInactiveLayerCycles(model_pruneInactiveLayerCycles, FP_operations_map, FP_operations_map_cycles, FP_operations_list, FP_operations_list_cycles, sink_nodes_with_cycles);

	BOOST_CHECK_EQUAL(FP_operations_map.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_map.at("1"), 0);
	BOOST_CHECK_EQUAL(FP_operations_list.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments.size(), 3);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].source_node->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[0].weight->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].source_node->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[1].weight->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].source_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_list[0].arguments[2].weight->getName(), "2");
	BOOST_CHECK_EQUAL(sink_nodes_with_cycles.size(), 1);
  BOOST_CHECK_EQUAL(sink_nodes_with_cycles.count("1"), 1);
}

Model<float> model_expandAllForwardPropogationOperations = makeModelFCSum();
BOOST_AUTO_TEST_CASE(expandAllForwardPropogationOperations)
{
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// initialize nodes
	// NOTE: input and biases have been activated when the model was created

	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_expandAllForwardPropogationOperations, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_expandAllForwardPropogationOperations, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	std::set<std::string> sink_nodes_with_cycles;
	std::map<std::string, int> FP_operations_map_cycles = FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list_cycles = FP_operations_list;
	model_interpreter.getNextInactiveLayerCycles(model_expandAllForwardPropogationOperations, FP_operations_map_cycles, FP_operations_list_cycles, sink_nodes_with_cycles);

	model_interpreter.pruneInactiveLayerCycles(model_expandAllForwardPropogationOperations, FP_operations_map, FP_operations_map_cycles, FP_operations_list, FP_operations_list_cycles, sink_nodes_with_cycles);

	std::vector<OperationList<float>> FP_operations_expanded;
	model_interpreter.expandAllForwardPropogationOperations(FP_operations_list, FP_operations_expanded);

	BOOST_CHECK_EQUAL(FP_operations_expanded.size(), 3);
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].arguments[0].source_node->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_expanded[0].arguments[0].weight->getName(), "0");
	BOOST_CHECK_EQUAL(FP_operations_expanded[1].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_expanded[1].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_expanded[1].arguments[0].time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_expanded[1].arguments[0].source_node->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_expanded[1].arguments[0].weight->getName(), "3");
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].result.time_step, 0);
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].result.sink_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].arguments.size(), 1);
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].arguments[0].time_step, 1);
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].arguments[0].source_node->getName(), "1");
	BOOST_CHECK_EQUAL(FP_operations_expanded[2].arguments[0].weight->getName(), "2");
}

Model<float> model_getTensorOperations = makeModelFCSum();
BOOST_AUTO_TEST_CASE(getTensorOperations)
{
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// initialize nodes
	// NOTE: input and biases have been activated when the model was created

	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_getTensorOperations, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_getTensorOperations, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	std::set<std::string> sink_nodes_with_cycles;
	std::map<std::string, int> FP_operations_map_cycles = FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list_cycles = FP_operations_list;
	model_interpreter.getNextInactiveLayerCycles(model_getTensorOperations, FP_operations_map_cycles, FP_operations_list_cycles, sink_nodes_with_cycles);

	model_interpreter.pruneInactiveLayerCycles(model_getTensorOperations, FP_operations_map, FP_operations_map_cycles, FP_operations_list, FP_operations_list_cycles, sink_nodes_with_cycles);

	std::vector<OperationList<float>> FP_operations_expanded;
	model_interpreter.expandAllForwardPropogationOperations(FP_operations_list, FP_operations_expanded);

	std::set<std::string> identified_sink_nodes;
	std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_expanded, identified_sink_nodes, false);

	BOOST_CHECK_EQUAL(identified_sink_nodes.size(), 3);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("1/0"), 1);
	BOOST_CHECK_EQUAL(identified_sink_nodes.count("1/1"), 1);
  BOOST_CHECK_EQUAL(identified_sink_nodes.count("1/2"), 1);
	BOOST_CHECK_EQUAL(tensor_ops.size(), 2);
	BOOST_CHECK_EQUAL(tensor_ops.at("1/0")[0], 0);
  BOOST_CHECK_EQUAL(tensor_ops.at("1/0")[1], 1);
	BOOST_CHECK_EQUAL(tensor_ops.at("1/2")[0], 2);
}

Model<float> model_getForwardPropogationLayerTensorDimensions = makeModelFCSum();
BOOST_AUTO_TEST_CASE(getForwardPropogationLayerTensorDimensions)
{
	ModelInterpreterDefaultDevice<float> model_interpreter;

	// initialize nodes
	// NOTE: input and biases have been activated when the model was created

  // Check iteration one with no source/sink/weight tensors already allocated
	std::map<std::string, int> FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list;
	model_interpreter.getNextInactiveLayerWOBiases(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_list);

	std::vector<std::string> sink_nodes_with_biases2;
	model_interpreter.getNextInactiveLayerBiases(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	std::set<std::string> sink_nodes_with_cycles;
	std::map<std::string, int> FP_operations_map_cycles = FP_operations_map;
	std::vector<OperationList<float>> FP_operations_list_cycles = FP_operations_list;
	model_interpreter.getNextInactiveLayerCycles(model_getForwardPropogationLayerTensorDimensions, FP_operations_map_cycles, FP_operations_list_cycles, sink_nodes_with_cycles);

	model_interpreter.pruneInactiveLayerCycles(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_map_cycles, FP_operations_list, FP_operations_list_cycles, sink_nodes_with_cycles);

	std::vector<OperationList<float>> FP_operations_expanded;
	model_interpreter.expandAllForwardPropogationOperations(FP_operations_list, FP_operations_expanded);

	std::set<std::string> identified_sink_nodes;
	std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_expanded, identified_sink_nodes, false);

  std::map<int, int> max_layer_sizes;
  std::map<std::string, int> layer_name_pos;
	std::vector<int> source_layer_sizes, sink_layer_sizes;
	std::vector<std::vector<std::pair<int, int>>> weight_indices;
	std::vector<std::map<std::string, std::vector<std::pair<int, int>>>> shared_weight_indices;
	std::vector<std::vector<float>> weight_values;
	std::vector<bool> make_source_tensors, make_sink_tensors, make_weight_tensors;
  std::vector<int> source_layer_pos, sink_layer_pos;
  int tensor_layers_cnt = 0;
  int weight_layers_cnt = 0;
	model_interpreter.getForwardPropogationLayerTensorDimensions(FP_operations_expanded, tensor_ops, source_layer_sizes, sink_layer_sizes, weight_indices, shared_weight_indices, weight_values, make_source_tensors, make_sink_tensors, make_weight_tensors,
    source_layer_pos, sink_layer_pos, max_layer_sizes, layer_name_pos, tensor_layers_cnt, weight_layers_cnt);

	BOOST_CHECK_EQUAL(source_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(source_layer_sizes[0], 2);
	BOOST_CHECK_EQUAL(source_layer_sizes[1], 1);
	BOOST_CHECK_EQUAL(sink_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(sink_layer_sizes[0], 1);
	BOOST_CHECK_EQUAL(sink_layer_sizes[1], 1);

  BOOST_CHECK_EQUAL(source_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(source_layer_pos.at(0), 1);
  BOOST_CHECK_EQUAL(source_layer_pos.at(1), 0);
  BOOST_CHECK_EQUAL(sink_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(0), 0);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(1), 0);

  BOOST_CHECK_EQUAL(max_layer_sizes.size(), 2);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(0), 0);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(1), 1);

  BOOST_CHECK_EQUAL(layer_name_pos.size(), 0);

	BOOST_CHECK_EQUAL(weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(weight_indices[0].size(), 2);
	BOOST_CHECK_EQUAL(weight_indices[1].size(), 1);
	std::vector<std::vector<std::pair<int, int>>> weight_indices_test1 = {
		{std::make_pair(0,0),std::make_pair(1,0)},
		{std::make_pair(0,0)}
	};
	for (int tensor_iter = 0; tensor_iter < weight_indices_test1.size(); ++tensor_iter) {
		for (int i = 0; i < weight_indices_test1[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].first, weight_indices_test1[tensor_iter][i].first);
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].second, weight_indices_test1[tensor_iter][i].second);
		}
	}

	BOOST_CHECK_EQUAL(shared_weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(shared_weight_indices[0].size(), 0);
	BOOST_CHECK_EQUAL(shared_weight_indices[1].size(), 0);

	BOOST_CHECK_EQUAL(weight_values.size(), 2);
	BOOST_CHECK_EQUAL(weight_values[0].size(), 2);
	BOOST_CHECK_EQUAL(weight_values[1].size(), 1);
	std::vector<std::vector<float>> weight_values_test1 = { {1, 1}, {1} };
	for (int tensor_iter = 0; tensor_iter < weight_values_test1.size(); ++tensor_iter) {
		for (int i = 0; i < weight_values_test1[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_values[tensor_iter][i], weight_values_test1[tensor_iter][i]);
		}
	}

	BOOST_CHECK_EQUAL(make_source_tensors.size(), 2);
	BOOST_CHECK(make_source_tensors[0]);
	BOOST_CHECK(!make_source_tensors[1]);
	BOOST_CHECK_EQUAL(make_sink_tensors.size(), 2);
	BOOST_CHECK(make_sink_tensors[0]);
	BOOST_CHECK(!make_sink_tensors[1]);
	BOOST_CHECK_EQUAL(make_weight_tensors.size(), 2);
	BOOST_CHECK(make_weight_tensors[0]);
	BOOST_CHECK(make_weight_tensors[1]);

	// Check iteration two
	model_getForwardPropogationLayerTensorDimensions.getNodesMap().at("1")->setStatus(NodeStatus::activated);
	FP_operations_map.clear();
	FP_operations_list.clear();
	model_interpreter.getNextInactiveLayerWOBiases(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_list);

	sink_nodes_with_biases2.clear();
	model_interpreter.getNextInactiveLayerBiases(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_list, sink_nodes_with_biases2);

	sink_nodes_with_cycles.clear();
	FP_operations_map_cycles = FP_operations_map;
	FP_operations_list_cycles = FP_operations_list;
	model_interpreter.getNextInactiveLayerCycles(model_getForwardPropogationLayerTensorDimensions, FP_operations_map_cycles, FP_operations_list_cycles, sink_nodes_with_cycles);

	model_interpreter.pruneInactiveLayerCycles(model_getForwardPropogationLayerTensorDimensions, FP_operations_map, FP_operations_map_cycles, FP_operations_list, FP_operations_list_cycles, sink_nodes_with_cycles);

	FP_operations_expanded.clear();
	model_interpreter.expandAllForwardPropogationOperations(FP_operations_list, FP_operations_expanded);

	identified_sink_nodes.clear();
	tensor_ops = model_interpreter.getTensorOperations(FP_operations_expanded, identified_sink_nodes, false);

  max_layer_sizes.clear();
  layer_name_pos.clear();
  source_layer_sizes.clear(); sink_layer_sizes.clear();
  weight_indices.clear();
  shared_weight_indices.clear();
  weight_values.clear();
  make_source_tensors.clear(); make_sink_tensors.clear(); make_weight_tensors.clear();
  source_layer_pos.clear(); sink_layer_pos.clear();
  tensor_layers_cnt = 0; weight_layers_cnt = 0;
  model_interpreter.getForwardPropogationLayerTensorDimensions(FP_operations_expanded, tensor_ops, source_layer_sizes, sink_layer_sizes, weight_indices, shared_weight_indices, weight_values, make_source_tensors, make_sink_tensors, make_weight_tensors,
    source_layer_pos, sink_layer_pos, max_layer_sizes, layer_name_pos, tensor_layers_cnt, weight_layers_cnt);

	BOOST_CHECK_EQUAL(source_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(source_layer_sizes[0], 1);
	BOOST_CHECK_EQUAL(source_layer_sizes[1], 1);
	BOOST_CHECK_EQUAL(sink_layer_sizes.size(), 2);
	BOOST_CHECK_EQUAL(sink_layer_sizes[0], 1);
	BOOST_CHECK_EQUAL(sink_layer_sizes[1], 1);

  BOOST_CHECK_EQUAL(source_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(source_layer_pos.at(0), 0);
  BOOST_CHECK_EQUAL(source_layer_pos.at(1), 1);
  BOOST_CHECK_EQUAL(sink_layer_pos.size(), 2);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(0), 0);
  BOOST_CHECK_EQUAL(sink_layer_pos.at(1), 0);

  BOOST_CHECK_EQUAL(max_layer_sizes.size(), 2);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(0), 0);
  BOOST_CHECK_EQUAL(max_layer_sizes.at(1), 0);

  BOOST_CHECK_EQUAL(layer_name_pos.size(), 0);

	BOOST_CHECK_EQUAL(weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(weight_indices[0].size(), 1);
	BOOST_CHECK_EQUAL(weight_indices[1].size(), 1);
	std::vector<std::vector<std::pair<int,int>>> weight_indices_test2 = {
		{std::make_pair(0,0)},{std::make_pair(0,0) }
	};
	for (int tensor_iter = 0; tensor_iter < weight_indices_test2.size(); ++tensor_iter) {
		for (int i = 0; i < weight_indices_test2[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].first, weight_indices_test2[tensor_iter][i].first);
			BOOST_CHECK_EQUAL(weight_indices[tensor_iter][i].second, weight_indices_test2[tensor_iter][i].second);
		}
	}

	BOOST_CHECK_EQUAL(shared_weight_indices.size(), 2);
	BOOST_CHECK_EQUAL(shared_weight_indices[0].size(), 0);
	BOOST_CHECK_EQUAL(shared_weight_indices[1].size(), 0);

	BOOST_CHECK_EQUAL(weight_values.size(), 2);
	BOOST_CHECK_EQUAL(weight_values[0].size(), 1);
	BOOST_CHECK_EQUAL(weight_values[1].size(), 1);
	std::vector<std::vector<float>> weight_values_test2 = { {1}, {1} };
	for (int tensor_iter = 0; tensor_iter < weight_values_test2.size(); ++tensor_iter) {
		for (int i = 0; i < weight_values_test2[tensor_iter].size(); ++i) {
			BOOST_CHECK_EQUAL(weight_values[tensor_iter][i], weight_values_test2[tensor_iter][i]);
		}
	}

	BOOST_CHECK_EQUAL(make_source_tensors.size(), 2);
	BOOST_CHECK(!make_source_tensors[0]);
	BOOST_CHECK(make_source_tensors[1]);
	BOOST_CHECK_EQUAL(make_sink_tensors.size(), 2);
	BOOST_CHECK(make_sink_tensors[0]);
	BOOST_CHECK(!make_sink_tensors[1]);
	BOOST_CHECK_EQUAL(make_weight_tensors.size(), 2);
	BOOST_CHECK(make_weight_tensors[0]);
	BOOST_CHECK(make_weight_tensors[1]);
}

/*
The following test the expected `tensor_ops_steps` and `FP_operations` for more
complicated model structures
*/

template<typename TensorT>
void makeModelLSTM(Model<TensorT>& model, const int& n_inputs, int n_blocks = 2, int n_cells = 2, bool specify_layers = false)
{
  model.setId(0);
  model.setName("LSTM");

  ModelBuilder<TensorT> model_builder;

  // Add the inputs
  std::vector<std::string> node_names_input = model_builder.addInputNodes(model, "Input", "Input", n_inputs, specify_layers);

  // Add the LSTM layer
  std::vector<std::string> node_names = model_builder.addLSTM(model, "LSTM", "LSTM", node_names_input, n_blocks, n_cells,
    std::shared_ptr<ActivationOp<TensorT>>(new ReLUOp<float>()), std::shared_ptr<ActivationOp<TensorT>>(new ReLUGradOp<float>()),
    std::make_shared<SumOp<TensorT>>(SumOp<TensorT>()), std::make_shared<SumErrorOp<TensorT>>(SumErrorOp<TensorT>()), std::make_shared<SumWeightGradOp<TensorT>>(SumWeightGradOp<TensorT>()),
    //std::make_shared<RandWeightInitOp<TensorT>>(RandWeightInitOp<TensorT>(0.4)),
    std::shared_ptr<WeightInitOp<TensorT>>(new RangeWeightInitOp<TensorT>(0.0, 1.0)),
    std::make_shared<AdamOp<TensorT>>(AdamOp<TensorT>(0.0005, 0.9, 0.999, 1e-8)),
    0.0f, 0.0f, true, true, 1, specify_layers);

  // Add a final output layer (Specify the layer name to ensure the output is always on its own tensor!!!)
  node_names = model_builder.addFullyConnected(model, "Output", "Output", node_names, 1,
    std::make_shared<LinearOp<TensorT>>(LinearOp<float>()),
    std::make_shared<LinearGradOp<TensorT>>(LinearGradOp<float>()),
    std::make_shared<SumOp<TensorT>>(SumOp<TensorT>()),
    std::make_shared<SumErrorOp<TensorT>>(SumErrorOp<TensorT>()),
    std::make_shared<SumWeightGradOp<TensorT>>(SumWeightGradOp<TensorT>()),
    std::make_shared<RandWeightInitOp<TensorT>>(RandWeightInitOp<TensorT>(node_names.size(), 2)),
    std::make_shared<AdamOp<TensorT>>(AdamOp<TensorT>(0.001, 0.9, 0.999, 1e-8)), 0.0f, 0.0f, true, true);

  for (const std::string& node_name : node_names)
    model.getNodesMap().at(node_name)->setType(NodeType::output);

  if (!model.checkCompleteInputToOutput())
    std::cout << "Model input and output are not fully connected!" << std::endl;
}

BOOST_AUTO_TEST_CASE(makeModelLSTM1)
{
  ModelInterpreterDefaultDevice<float> model_interpreter;

  // Determine the tensor_ops_steps and FP_operations for the manually specified layer case
  Model<float> model_test;
  model_test.findCycles();
  makeModelLSTM(model_test, 2, 1, 2, true);

  int iter_test = 0;
  std::vector<OperationList<float>> FP_operations_expanded_test;
  model_interpreter.getFPOpsOoO_(model_test, FP_operations_expanded_test, iter_test);

  std::set<std::string> identified_sink_nodes_test;
  std::map<std::string, std::vector<int>> tensor_ops_test = model_interpreter.getTensorOperations(FP_operations_expanded_test, identified_sink_nodes_test, true);

  // Determine the tensor_ops_steps and FP_operations for the manually specified layer case
  Model<float> model;
  model.findCycles();
  makeModelLSTM(model, 2, 1, 2, false);

  int iter = 0;
  std::vector<OperationList<float>> FP_operations_expanded;
  model_interpreter.getFPOpsOoO_(model, FP_operations_expanded, iter);

  std::set<std::string> identified_sink_nodes;
  std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_expanded, identified_sink_nodes, false);

  BOOST_CHECK_EQUAL(iter_test, iter);
  BOOST_CHECK(tensor_ops_test == tensor_ops);
  BOOST_CHECK(identified_sink_nodes_test == identified_sink_nodes);
  BOOST_CHECK_EQUAL(FP_operations_expanded_test.size(), FP_operations_expanded.size());
  if (tensor_ops_test == tensor_ops && identified_sink_nodes_test == identified_sink_nodes && FP_operations_expanded_test.size() == FP_operations_expanded.size()) {
    for (int i = 0; i < FP_operations_expanded_test.size(); ++i) {
      BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].result.sink_node->getName(), FP_operations_expanded[i].result.sink_node->getName());
      BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].result.time_step, FP_operations_expanded[i].result.time_step);
      for (int j = 0; j < FP_operations_expanded_test[i].arguments.size(); ++j) {
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].source_node->getName(), FP_operations_expanded[i].arguments[j].source_node->getName());
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].weight->getName(), FP_operations_expanded[i].arguments[j].weight->getName());
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].time_step, FP_operations_expanded[i].arguments[j].time_step);
      }
    }
  }
}
BOOST_AUTO_TEST_CASE(makeModelLSTM2)
{
  ModelInterpreterDefaultDevice<float> model_interpreter;

  // Determine the tensor_ops_steps and FP_operations for the manually specified layer case
  Model<float> model_test;
  model_test.findCycles();
  makeModelLSTM(model_test, 2, 4, 2, true);

  int iter_test = 0;
  std::vector<OperationList<float>> FP_operations_expanded_test;
  model_interpreter.getFPOpsOoO_(model_test, FP_operations_expanded_test, iter_test);

  std::set<std::string> identified_sink_nodes_test;
  std::map<std::string, std::vector<int>> tensor_ops_test = model_interpreter.getTensorOperations(FP_operations_expanded_test, identified_sink_nodes_test, true);

  // Determine the tensor_ops_steps and FP_operations for the manually specified layer case
  Model<float> model;
  model.findCycles();
  makeModelLSTM(model, 2, 4, 2, false);

  int iter = 0;
  std::vector<OperationList<float>> FP_operations_expanded;
  model_interpreter.getFPOpsOoO_(model, FP_operations_expanded, iter);

  std::set<std::string> identified_sink_nodes;
  std::map<std::string, std::vector<int>> tensor_ops = model_interpreter.getTensorOperations(FP_operations_expanded, identified_sink_nodes, false);

  BOOST_CHECK_EQUAL(iter_test, iter);
  BOOST_CHECK(tensor_ops_test == tensor_ops);
  BOOST_CHECK(identified_sink_nodes_test == identified_sink_nodes);
  BOOST_CHECK_EQUAL(FP_operations_expanded_test.size(), FP_operations_expanded.size());
  if (tensor_ops_test == tensor_ops && identified_sink_nodes_test == identified_sink_nodes && FP_operations_expanded_test.size() == FP_operations_expanded.size()) {
    for (int i = 0; i < FP_operations_expanded_test.size(); ++i) {
      BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].result.sink_node->getName(), FP_operations_expanded[i].result.sink_node->getName());
      BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].result.time_step, FP_operations_expanded[i].result.time_step);
      for (int j = 0; j < FP_operations_expanded_test[i].arguments.size(); ++j) {
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].source_node->getName(), FP_operations_expanded[i].arguments[j].source_node->getName());
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].weight->getName(), FP_operations_expanded[i].arguments[j].weight->getName());
        BOOST_CHECK_EQUAL(FP_operations_expanded_test[i].arguments[j].time_step, FP_operations_expanded[i].arguments[j].time_step);
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * \file ToneStackFilter.hxx
 */

#include <boost/math/tools/polynomial.hpp>

#include <ATK/EQ/ToneStackFilter.h>

namespace ATK
{
  template<typename DataType>
  ToneStackCoefficients<DataType>::ToneStackCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels)
  {
  }

  template<typename DataType>
  ToneStackCoefficients<DataType>::ToneStackCoefficients(ToneStackCoefficients&& other)
  :Parent(std::move(other)), R1(other.R1), R2(other.R2), R3(other.R3), R4(other.R4), C1(other.C1), C2(other.C2), C3(other.C3), low(other.low), middle(other.middle), high(other.high)
  {

  }

  template<typename DataType>
  void ToneStackCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    CoeffDataType tempm[2] = {static_cast<CoeffDataType>(-2) * input_sampling_rate, static_cast<CoeffDataType>(2) * input_sampling_rate};
    CoeffDataType tempp[2] = {static_cast<CoeffDataType>(1), static_cast<CoeffDataType>(1)};
    boost::math::tools::polynomial<CoeffDataType> poly1(tempm, 1);
    boost::math::tools::polynomial<CoeffDataType> poly2(tempp, 1);

    boost::math::tools::polynomial<CoeffDataType> b;
    boost::math::tools::polynomial<CoeffDataType> a;

    b = poly2 * poly2 * poly1 * (high*C1*R1 + middle*C3*R3 + low*(C1*R2 + C2*R2) + (C1*R3 + C2*R3));
    b += poly2 * poly1 * poly1 * (high*(C1*C2*R1*R4 + C1*C3*R1*R4) - middle*middle*(C1*C3*R3*R3 + C2*C3*R3*R3) + middle*(C1*C3*R1*R3 + C1*C3*R3*R3 + C2*C3*R3*R3)
      + low*(C1*C2*R1*R2 + C1*C2*R2*R4 + C1*C3*R2*R4) + low*middle*(C1*C3*R2*R3 + C2*C3*R2*R3)
      + (C1*C2*R1*R3 + C1*C2*R3*R4 + C1*C3*R3*R4));
    b += poly1 * poly1 * poly1 * (low*middle*(C1*C2*C3*R1*R2*R3 + C1*C2*C3*R2*R3*R4) - middle*middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4)
      + middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4) + high*C1*C2*C3*R1*R3*R4 - high*middle*C1*C2*C3*R1*R3*R4
      + high*low*C1*C2*C3*R1*R2*R4);

    a = poly2 * poly2 * poly2;
    a += poly2 * poly2 * poly1 * ((C1*R1 + C1*R3 + C2*R3 + C2*R4 + C3*R4) + middle*C3*R3 + low*(C1*R2 + C2*R2));
    a += poly2 * poly1 * poly1 * (middle*(C1*C3*R1*R3 - C2*C3*R3*R4 + C1*C3*R3*R3 + C2*C3*R3*R3)
      + low*middle*(C1*C3*R2*R3 + C2*C3*R2*R3) - middle*middle*(C1*C3*R3*R3 + C2*C3*R3*R3) + low*(C1*C2*R2*R4 + C1*C2*R1*R2 + C1*C3*R2*R4 + C2*C3*R2*R4)
      + (C1*C2*R1*R4 + C1*C3*R1*R4 + C1*C2*R3*R4 + C1*C2*R1*R3 + C1*C3*R3*R4 + C2*C3*R3*R4));
    a += poly1 * poly1 * poly1 * (low*middle*(C1*C2*C3*R1*R2*R3 + C1*C2*C3*R2*R3*R4) - middle*middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4)
      + middle*(C1*C2*C3*R3*R3*R4 + C1*C2*C3*R1*R3*R3 - C1*C2*C3*R1*R3*R4)
      + low*C1*C2*C3*R1*R2*R4 + C1*C2*C3*R1*R3*R4);

    for(gsl::index i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(gsl::index i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_low(CoeffDataType low)
  {
    if(low < 0 || low > 1)
    {
      throw std::out_of_range("Low is outside the interval [0,1]");
    }
    this->low = low;

    setup();
  }

  template<typename DataType_>
  typename ToneStackCoefficients<DataType_>::CoeffDataType ToneStackCoefficients<DataType_>::get_low() const
  {
    return low;
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_middle(CoeffDataType middle)
  {
    if(middle < 0 || middle > 1)
    {
      throw std::out_of_range("Middle is outside the interval [0,1]");
    }
    this->middle = middle;

    setup();
  }

  template<typename DataType_>
  typename ToneStackCoefficients<DataType_>::CoeffDataType ToneStackCoefficients<DataType_>::get_middle() const
  {
    return middle;
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_high(CoeffDataType high)
  {
    if(high < 0 || high > 1)
    {
      throw std::out_of_range("high is outside the interval [0,1]");
    }
    this->high = high;

    setup();
  }

  template<typename DataType_>
  typename ToneStackCoefficients<DataType_>::CoeffDataType ToneStackCoefficients<DataType_>::get_high() const
  {
    return high;
  }

  template<typename DataType>
  IIRFilter<ToneStackCoefficients<DataType> > ToneStackCoefficients<DataType>::buildBassmanStack()
  {
    IIRFilter<ToneStackCoefficients<DataType> > filter;
    filter.set_coefficients(static_cast<CoeffDataType>(250e3), static_cast<CoeffDataType>(1e6), static_cast<CoeffDataType>(25e3), static_cast<CoeffDataType>(45e3),
      static_cast<CoeffDataType>(250e-12), static_cast<CoeffDataType>(20e-9), static_cast<CoeffDataType>(20e-9));
    return std::move(filter);
  }

  template<typename DataType>
  IIRFilter<ToneStackCoefficients<DataType> > ToneStackCoefficients<DataType>::buildJCM800Stack()
  {
    IIRFilter<ToneStackCoefficients<DataType> > filter;
    filter.set_coefficients(static_cast<CoeffDataType>(220e3), static_cast<CoeffDataType>(1e6), static_cast<CoeffDataType>(22e3), static_cast<CoeffDataType>(33e3),
      static_cast<CoeffDataType>(470e-12), static_cast<CoeffDataType>(22e-9), static_cast<CoeffDataType>(22e-9));
    return std::move(filter);
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_coefficients(CoeffDataType R1, CoeffDataType R2, CoeffDataType R3, CoeffDataType R4, CoeffDataType C1, CoeffDataType C2, CoeffDataType C3)
  {
    this->R1 = R1;
    this->R2 = R2;
    this->R3 = R3;
    this->R4 = R4;
    this->C1 = C1;
    this->C2 = C2;
    this->C3 = C3;
  }
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int x, y, cnt = 1; cin >> x >> y;
    while(true) {
        if (x * 2 > y) break;
        else {
            cnt++;
            x *= 2;
        }
    }
    cout << cnt << endl;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_

#define BOOST_CHRONO_HEADER_ONLY

#if !defined(TEST_MPZ) && !defined(TEST_TOMMATH) && !defined(TEST_CPP_INT)
#  define TEST_MPZ
#  define TEST_TOMMATH
#  define TEST_CPP_INT
#endif

#ifdef TEST_MPZ
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_TOMMATH
#include <boost/multiprecision/tommath.hpp>
#endif
#ifdef TEST_CPP_INT
#include <boost/multiprecision/cpp_int.hpp>
#endif
#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/chrono.hpp>
#include <map>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

private:
   typename Clock::time_point m_start;
};

unsigned allocation_count = 0;

void *(*alloc_func_ptr) (size_t);
void *(*realloc_func_ptr) (void *, size_t, size_t);
void (*free_func_ptr) (void *, size_t);

void *alloc_func(size_t n)
{
   ++allocation_count;
   return (*alloc_func_ptr)(n);
}

void free_func(void * p, size_t n)
{
   (*free_func_ptr)(p, n);
}

void * realloc_func(void * p, size_t old, size_t n)
{
   ++allocation_count;
   return (*realloc_func_ptr)(p, old, n);
}

#ifdef TEST_MPZ
boost::chrono::duration<double> test_miller_rabin_gmp()
{
   using namespace boost::random;
   using namespace boost::multiprecision;

   stopwatch<boost::chrono::high_resolution_clock> c;

   independent_bits_engine<mt11213b, 256, mpz_int> gen;

   for(unsigned i = 0; i < 1000; ++i)
   {
      mpz_int n = gen();
      mpz_probab_prime_p(n.backend().data(), 25);
   }
   return c.elapsed();
}
#endif

std::map<std::string, double> results;
double min_time = (std::numeric_limits<double>::max)();

template <class IntType>
boost::chrono::duration<double> test_miller_rabin(const char* name)
{
   using namespace boost::random;

   stopwatch<boost::chrono::high_resolution_clock> c;

   independent_bits_engine<mt11213b, 256, IntType> gen;
   //
   // We must use a different generator for the tests and number generation, otherwise
   // we get false positives.
   //
   mt19937 gen2;
   unsigned result_count = 0;

   for(unsigned i = 0; i < 1000; ++i)
   {
      IntType n = gen();
      if(boost::multiprecision::miller_rabin_test(n, 25, gen2))
         ++result_count;
   }
   boost::chrono::duration<double> t = c.elapsed();
   double d = t.count();
   if(d < min_time)
      min_time = d;
   results[name] = d;
   std::cout << "Time for " << std::setw(30) << std::left << name << " = " << d << std::endl;
   std::cout << "Number of primes found = " << result_count << std::endl;
   return t;
}

void generate_quickbook()
{
   std::cout << "[table\n[[Integer Type][Relative Performance (Actual time in parenthesis)]]\n";

   std::map<std::string, double>::const_iterator i(results.begin()), j(results.end());

   while(i != j)
   {
      double rel = i->second / min_time;
      std::cout << "[[" << i->first << "][" << rel << "(" << i->second << "s)]]\n";
      ++i;
   }

   std::cout << "]\n";
}

int main()
{
   using namespace boost::multiprecision;
#ifdef TEST_CPP_INT
   test_miller_rabin<number<cpp_int_backend<>, et_off> >("cpp_int (no Expression templates)");
   test_miller_rabin<cpp_int>("cpp_int");
   test_miller_rabin<number<cpp_int_backend<128> > >("cpp_int (128-bit cache)");
   test_miller_rabin<number<cpp_int_backend<256> > >("cpp_int (256-bit cache)");
   test_miller_rabin<number<cpp_int_backend<512> > >("cpp_int (512-bit cache)");
   test_miller_rabin<number<cpp_int_backend<1024> > >("cpp_int (1024-bit cache)");
   test_miller_rabin<int1024_t>("int1024_t");
   test_miller_rabin<checked_int1024_t>("checked_int1024_t");
#endif
#ifdef TEST_MPZ
   test_miller_rabin<number<gmp_int, et_off> >("mpz_int (no Expression templates)");
   test_miller_rabin<mpz_int>("mpz_int");
   std::cout << "Time for mpz_int (native Miller Rabin Test) = " << test_miller_rabin_gmp() << std::endl;
#endif
#ifdef TEST_TOMMATH
   test_miller_rabin<number<boost::multiprecision::tommath_int, et_off> >("tom_int (no Expression templates)");
   test_miller_rabin<boost::multiprecision::tom_int>("tom_int");
#endif

   generate_quickbook();

   return 0;
}


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[rtree_iterative_query

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

// just for output
#include <iostream>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point;
    typedef point value;
    typedef bgi::rtree< value, bgi::linear<16> > rtree_t;

    // create the rtree using default constructor
    rtree_t rtree;

    // create some values
    for ( double f = 0 ; f < 10 ; f += 1 )
    {
        // insert new value
        rtree.insert(point(f, f));
    }

    // query point
    point pt(5.1, 5.1);

    // iterate over nearest Values
    for ( rtree_t::const_query_iterator
            it = rtree.qbegin(bgi::nearest(pt, 100)) ;
            it != rtree.qend() ;
            ++it )
    {
        double d = bg::distance(pt, *it);

        std::cout << bg::wkt(*it) << ", distance= " << d << std::endl;

        // break if the distance is too big
        if ( d > 2 )
        {
            std::cout << "break!" << std::endl;
            break;
        }
    }

    return 0;
}

//]