/******************************************************************************
**
** Copyright (C) 2009-2012 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "moleculegeometryoptimizer.h"

#include <boost/make_shared.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <chemkit/atom.h>
#include <chemkit/molecule.h>
#include <chemkit/concurrent.h>
#include <chemkit/cartesiancoordinates.h>

#include "forcefield.h"
#include "integrator.h"

namespace chemkit {

namespace {

class SteepestDescentIntegrator : public Integrator
{
public:
    void integrate() CHEMKIT_OVERRIDE;
};

void SteepestDescentIntegrator::integrate()
{
    CartesianCoordinates *coordinates = this->coordinates();
    boost::shared_ptr<Potential> potential = this->potential();

    if(!potential || !coordinates){
        return;
    }

    // optimization parameters
    Real step = 0.05;
    Real stepConv = 1e-5;
    size_t stepCount = 10;

    // calculate initial energy and gradient
    Real initialEnergy = potential->energy(coordinates);
    std::vector<Vector3> gradient = potential->gradient(coordinates);

    // perform line search
    for(size_t i = 0; i < stepCount; i++){
        // save initial coordinates
        CartesianCoordinates initialCoordinates = *coordinates;

        // move each atom against its gradient
        for(size_t atomIndex = 0; atomIndex < potential->size(); atomIndex++){
            (*coordinates)[atomIndex] += -gradient[atomIndex] * step;
        }

        // calculate new energy
        Real finalEnergy = potential->energy(coordinates);

        // if the final energy is NaN then most likely the
        // simulation exploded so we reset the initial atom
        // positions and then 'wiggle' each atom by one
        // Angstrom in a random direction
        if((boost::math::isnan)(finalEnergy)){
            for(size_t atomIndex = 0; atomIndex < potential->size(); atomIndex++){
                Point3 position = initialCoordinates.position(atomIndex);
                position += Vector3::Random().normalized();
                coordinates->setPosition(atomIndex, position);
            }

            // recalculate gradient
            gradient = potential->gradient(coordinates);

            // continue to next step
            continue;
        }

        if(finalEnergy < initialEnergy && std::abs(finalEnergy - initialEnergy) < stepConv){
            break;
        }
        else if(finalEnergy < initialEnergy){
            // we reduced the energy, so set a bigger step size
            step *= 2;

            // maximum step size is 1
            if(step > 1){
                step = 1;
            }

            // the initial energy for the next step
            // is the final energy of this step
            initialEnergy = finalEnergy;
        }
        else if(finalEnergy > initialEnergy){
            // we went too far, so reset initial atom positions
            *coordinates = initialCoordinates;

            // and reduce step size
            step *= 0.1;
        }
    }
}

} // end anonymous namespace

// === MoleculeGeometryOptimizerPrivate ==================================== //
class MoleculeGeometryOptimizerPrivate
{
public:
    Molecule *molecule;
    boost::shared_ptr<ForceField> forceField;
    std::string forceFieldName;
    std::string errorString;
    boost::shared_ptr<Integrator> integrator;
};

// === MoleculeGeometryOptimizer =========================================== //
/// \class MoleculeGeometryOptimizer moleculegeometryoptimizer.h chemkit/moleculegeometryoptimizer.h
/// \ingroup chemkit-md
/// \brief The MoleculeGeometryOptimizer class performs geometry
///        optimization for a single molecule.
///
/// The MoleculeGeometryOptimizer class is a convenience class meant
/// to simplify the process of setting up a force field and
/// performing an energy minimization run for a single molecule.
///
/// By default the UFF force field is used.
///
/// The easiest way to optimize the geometry for a molecule is to
/// use the optimizeCoordinate() static method as follows:
/// \code
/// MoleculeGeometryOptimizer::optimizeCoordinates(molecule);
/// \endcode
///
/// This class along with the CoordinatePredictor can be used to
/// generate 3D coordinates for a molecule. The following example
/// shows how to create a phenol molecule from its SMILES formula
/// and generate a rough set of 3D coordinates.
/// \code
/// // create phenol molecule from its formula
/// Molecule phenol("c1ccccc1O", "smiles");
///
/// // predict an initial set of 3D coordinate
/// CoordinatePredictor::predictCoordinates(&phenol);
///
/// // optimize the predicted coordinates
/// MoleculeGeometryOptimizer::optimizeCoordinates(&phenol);
/// \endcode
///
/// \see ForceField

// --- Construction and Destruction ---------------------------------------- //
/// Creates a new geometry optimizer for \p molecule.
MoleculeGeometryOptimizer::MoleculeGeometryOptimizer(Molecule *molecule)
    : d(new MoleculeGeometryOptimizerPrivate)
{
    d->molecule = molecule;
    d->forceFieldName = "uff";
    d->integrator = boost::make_shared<SteepestDescentIntegrator>();
}

/// Destroys the geometry optmizer object.
MoleculeGeometryOptimizer::~MoleculeGeometryOptimizer()
{
    delete d;
}

// --- Properties ---------------------------------------------------------- //
/// Sets the molecule for the geometry optimizer to \p molecule.
void MoleculeGeometryOptimizer::setMolecule(Molecule *molecule)
{
    if(molecule != d->molecule){
        d->molecule = molecule;
    }
}

/// Returns the molecule for the geometry optimizer.
Molecule* MoleculeGeometryOptimizer::molecule() const
{
    return d->molecule;
}

/// Sets the force field to use to \p forceField. Returns \c false
/// if \p forceField is not supported.
///
/// \see ForceField
bool MoleculeGeometryOptimizer::setForceField(const std::string &forceField)
{
    d->forceFieldName = forceField;

    return true;
}

/// Returns the name of the force field being used for geometry
/// optimization.
std::string MoleculeGeometryOptimizer::forceField() const
{
    return d->forceFieldName;
}

// --- Energy -------------------------------------------------------------- //
/// Returns the current energy of the force field.
Real MoleculeGeometryOptimizer::energy() const
{
    if(!d->forceField){
        return 0;
    }

    return d->integrator->energy();
}

// --- Optimization -------------------------------------------------------- //
/// Sets up the force field. Returns \c false if an error occurred.
bool MoleculeGeometryOptimizer::setup()
{
    if(!d->molecule){
        d->errorString = "No molecule specified";
        return false;
    }

    d->forceField = boost::shared_ptr<ForceField>(ForceField::create(d->forceFieldName));
    if(!d->forceField){
        d->errorString = "Force field '" + d->forceFieldName + "' is not supported.";
        return false;
    }

    d->integrator->setPotential(d->forceField);
    d->integrator->setCoordinates(d->molecule->coordinates());

    d->forceField->setTopologyFromMolecule(d->molecule);
    if(!d->forceField->setup()){
        d->errorString = "Failed to setup force field.";
        return false;
    }

    return true;
}

/// Performs a single geometry optimization step. Returns \c true if
/// converged. The minimization is considered converged when the
/// root mean square gradient is below the convergance value.
void MoleculeGeometryOptimizer::step()
{
    if(!d->molecule || !d->forceField){
        return;
    }

    // perform a single integration step
    d->integrator->integrate();
}

/// Returns \c true if the optimization algorithm has converged. By
/// default, the algorithm is considered converged when the
/// root-mean-square gradient of the force field falls below \c 0.1.
bool MoleculeGeometryOptimizer::converged()
{
    if(!d->forceField){
        return false;
    }

    // check for convergance
    return d->integrator->rmsg() < 0.1;
}

/// Optimizes the geometry of the molecule. Returns \c true if the
/// optimization algorithm converged.
bool MoleculeGeometryOptimizer::optimize()
{
    if(!setup()){
        return false;
    }

    while(!converged()){
        step();
    }

    // write the optimized coordinates to the molecule
    writeCoordinates();

    return true;
}

/// Writes the optimized coordinates to the molecule.
void MoleculeGeometryOptimizer::writeCoordinates()
{
    if(!d->molecule){
        return;
    }

    const CartesianCoordinates *coordinates = d->integrator->coordinates();

    for(size_t i = 0; i < d->molecule->size(); i++){
        d->molecule->atom(i)->setPosition(coordinates->position(i));
    }
}

// --- Error Handling ------------------------------------------------------ //
/// Returns a string describing the last error that occurred.
std::string MoleculeGeometryOptimizer::errorString() const
{
    return d->errorString;
}

// --- Static Methods ------------------------------------------------------ //
/// Optimizes the geometry of \p molecule.
bool MoleculeGeometryOptimizer::optimizeCoordinates(Molecule *molecule)
{
    return MoleculeGeometryOptimizer(molecule).optimize();
}

/// Runs the optimizeCoordinates() method asynchronously and returns
/// a future containing the result.
///
/// \internal
boost::shared_future<bool> MoleculeGeometryOptimizer::optimizeCoordinatesAsync(Molecule *molecule)
{
  return chemkit::concurrent::run(
      boost::bind(&MoleculeGeometryOptimizer::optimizeCoordinates, molecule));
}

} // end chemkit namespace


/*

 Copyright (c) 2005-2015, University of Oxford.
 All rights reserved.

 University of Oxford means the Chancellor, Masters and Scholars of the
 University of Oxford, having an administrative office at Wellington
 Square, Oxford OX1 2JD, UK.

 This file is part of Chaste.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
 this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
 this list of conditions and the following disclaimer in the documentation
 and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
 contributors may be used to endorse or promote products derived from this
 software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 */

/*
 Jessica Cervi and Raymond J. Spiteri
 Numerical Simulation Laboratory
 University of Saskatchewan
 August 2016
 */

#ifndef THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_
#define THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_

#include "UblasIncludes.hpp"

//#include <iostream>
#include <vector>
#include <petscvec.h>

#include "AbstractBidomainSolver.hpp"
#include "HeartConfig.hpp"
#include "BidomainAssembler.hpp"
#include "BidomainMassMatrixAssembler.hpp"
#include "BidomainStiffnessMatrixAssembler.hpp"
#include "BidomainCorrectionTermAssembler.hpp"
#include "BidomainNeumannSurfaceTermAssembler.hpp"
#include "TimeStepper.hpp"

#include <boost/numeric/ublas/vector_proxy.hpp>

//EDIT COMMENTS HERE!!!
/**
 *  A bidomain solver that uses Strang operator splitting of the diffusion (conductivity) term and the reaction
 *  (ionic current) term, instead of solving the full reaction-diffusion PDE. This does NOT refer to operator splitting
 *  of the two PDEs in the bidomain equations. For details see for example Sundnes et al "Computing the Electrical
 *  Activity of the Heart". This solves the PDEs using the SDIRK2O2 method (second order).
 *
 *  The algorithm is, for solving from t=T to T+dt.
 *
 *  (i)   Solve ODEs   dV/dt = Iionic, du/dt = f(u,V)  for t=T to T+dt/2
 *        [giving updated V (internally, and in solution vector) and updated state variables]
 *  (ii)  Solve PDE    dV/dt = div (sigma_i grad V) + div (sigma_i grad phi_e), div (sigma_i grad V) + div ((sigma_i+sigma_e) grad phi_e) = 0
 *        for t=T to dt         [using V from step i, --> updated V]
 *  (iii) Solve ODEs   dV/dt = Iionic, du/dt = f(u,V)  for t=T+dt/2 to T+dt  [using V from step ii, --> final V]
 *
 *  Notes
 *   (a)  Stages (iii) and (i) can normally be solved together in one go, except just before/after printing the voltage to file.
 *        However for simplicity of code this has not been implemented
 *   (b)  Therefore, the effective ODE timestep will be:  min(ode_dt, pde_dt/2), where ode_dt and pde_dt are those
 *        given via HeartConfig.
 *   (c)  This solver is FOR COMPARING ACCURACY, NOT PERFORMANCE. It has not been optimised and may or may not
 *        perform well in parallel.
 *   (d)  Usage: add the follwoing lines to your original code:
 *        HeartConfig::Instance()->SetUseReactionDiffusionOperatorSplittingBidomainSolver();
 * 	  HeartConfig::Instance()->SetUseReactionDiffusionThirdOrderOperatorSplittingBidomainSolver();
 */
template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
class ThirdOrderRDIRKOperatorSplittingBidomainSolver : public AbstractBidomainSolver<ELEMENT_DIM, SPACE_DIM>
{
private:

    /** Coefficient for SDIRK2O3 method */
    double mGamma;

    /** Mass matrix, used to computing the RHS vector (actually: mass-matrix in
     *  voltage-voltage block, zero elsewhere
     */
    Mat mMassMatrix;

    /** Ai matrix, used to computing the RHS vector (actually: Ai-matrix in
     *  voltage-voltage block, zero elsewhere
     */

    Mat mAiMatrix;

    /**
     *  The vector multiplied by the mass matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z1 where b = M z1 + Ai z2.
     */
    Vec mVecForConstructingRhs1;

    /**
     *  The vector multiplied by the Ai matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z2 where b = M z1 + Ai z2.
     */
    Vec mVecForConstructingRhs2;
    /**
     *  The vector multiplied by the mass matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z1 where b = M z1 + Ai z2., Third stage
     */
    Vec mVecForConstructingRhs1_2;

    /**
     *  The vector multiplied by the Ai matrix. Ie, if the linear system to
     *  be solved is Ax=b, this vector is z2 where b = M z1 + Ai z2. Third Stage
     */
    Vec mVecForConstructingRhs2_2;

    /** The bidomain assembler, used to set up the LHS matrix */
    BidomainAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainAssembler;

    /** Assembler for surface integrals coming from any non-zero Neumann boundary conditions */
    BidomainNeumannSurfaceTermAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainNeumannSurfaceTermAssembler;

    /**
     * If using state variable interpolation, points to an assembler to use in
     * computing the correction term to apply to the RHS.
     */
    BidomainCorrectionTermAssembler<ELEMENT_DIM, SPACE_DIM>* mpBidomainCorrectionTermAssembler;

    /**
     *  The linear system that will be set up and solved first as part of the
     *  PDE solve
     */
    //LinearSystem* mpLinearSystem_first_solve;

    /**
     * If using state variable interpolation, points to an assembler to use in
     * computing the correction term to apply to the RHS.
     */
    //BidomainCorrectionTermAssembler<ELEMENT_DIM,SPACE_DIM>* mpBidomainCorrectionTermAssembler;

    /** Overloaded InitialiseForSolve() which calls base version but also
     *  initialises mMassMatrix and mVecForConstructingRhs
     *
     *  @param initialSolution initial solution
     */
    void InitialiseForSolve(Vec initialSolution);

    /**
     *  Implementation of SetupLinearSystem() which uses the assembler to compute the
     *  LHS matrix, but sets up the RHS vector using the mass-matrix (constructed
     *  using a separate assembler) multiplied by a vector
     *
     *  @param currentSolution Solution at current time
     *  @param computeMatrix Whether to compute the matrix of the linear system
     */
    void SetupLinearSystem(Vec currentSolution, bool computeMatrix);

    /**
     *  Called before setting up the linear system, used to solve the cell models for first half timestep (step (i) above)
     *  @param currentSolution the latest solution vector
     */
    void PrepareForSetupLinearSystem(Vec currentSolution);

    /**
     *  Called after solving the linear system, used to solve the cell models for second half timestep (step (iii) above)
     *  @param currentSolution the latest solution vector (ie the solution of the linear system).
     */
    void FollowingSolveLinearSystem(Vec currentSolution);

    /**
     * Called when we are ready to solve the splitted system
     */
    Vec SolveOS(Vec currentSolution);

    /**
     * Whether we are solving the cell system or not
     */
    bool SolvingCellSystem;

    /**
     * When using third-order OS, the substeps for the ODE and PDE system have to be the following
     */
    double ThirdOrderTimeSteps;

    /**
     * Index for the time-step array
     */
    int TimeSubStepIndex;

    /**
     * Index for the whether solving the cell system of not
     */
    int BackwardTimeIntegrationIndex;

    /**
     * Index for DIRK stage
     */
    int DIRKStage;

    /*
     *Stepper
     */
    TimeStepper stepper();

    /*
     *Stage counter for DIRK method
     */
    int currentStage;

public:
    /**
     * Constructor
     *
     * @param bathSimulation Whether the simulation involves a perfusing bath
     * @param pMesh pointer to the mesh
     * @param pTissue pointer to the tissue
     * @param pBoundaryConditions pointer to the boundary conditions
     */
    ThirdOrderRDIRKOperatorSplittingBidomainSolver(bool bathSimulation,
                                              AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
                                              BidomainTissue<SPACE_DIM>* pTissue,
                                              BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM,2>* pBoundaryConditions);

    ~ThirdOrderRDIRKOperatorSplittingBidomainSolver();

    //  void FinaliseLinearSystem_first_solve(Vec existingSolution);

};

#endif /*THIRDORDERRDIRKOPERATORSPLITTINGBIDOMAINSOLVER_HPP_*/


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "BatchToSpaceNd.hpp"

#include "RefWorkloadUtils.hpp"

#include <armnn/Types.hpp>

#include <boost/assert.hpp>

namespace armnn
{

inline unsigned int Offset(const TensorShape& shape, unsigned int batch, unsigned int height, unsigned int width,
        unsigned int channels, const DataLayoutIndexed& dataLayout)
{
    if (dataLayout.GetDataLayout() == DataLayout::NHWC)
    {
        return ((batch * shape[dataLayout.GetHeightIndex()] + height) * shape[dataLayout.GetWidthIndex()] + width) *
               shape[dataLayout.GetChannelsIndex()] + channels;
    }
    else
    {
        return ((batch * shape[dataLayout.GetChannelsIndex()] + channels) *
               shape[dataLayout.GetHeightIndex()] + height) *
               shape[dataLayout.GetWidthIndex()] + width;
    }
}

void BatchToSpaceNd(const DataLayoutIndexed& dataLayout,
                    const TensorInfo& inputTensorInfo,
                    const TensorInfo& outputTensorInfo,
                    const std::vector<unsigned int>& blockShape,
                    const std::vector<std::pair<unsigned int, unsigned int>>& cropsData,
                    const float* inputData,
                    float* outputData)
{
    TensorShape inputShape = inputTensorInfo.GetShape();

    BOOST_ASSERT_MSG(inputShape.GetNumDimensions() == 4, "Expected Input with 4 Dimensions");

    TensorShape outputShape = outputTensorInfo.GetShape();

    BOOST_ASSERT_MSG(outputShape.GetNumDimensions() == 4, "Expected Output with 4 Dimensions");

    const unsigned int inputBatchSize = inputShape[0];
    const unsigned int channels = inputShape[dataLayout.GetChannelsIndex()];

    const unsigned int outputBatchSize = outputShape[0];
    const unsigned int outputHeight = outputShape[dataLayout.GetHeightIndex()];
    const unsigned int outputWidth = outputShape[dataLayout.GetWidthIndex()];

    BOOST_ASSERT_MSG(blockShape.size() > 0, "BlockShape must contain 1 or more entries");

    const unsigned int blockShapeHeight = blockShape[0];
    const unsigned int blockShapeWidth = blockShape[1];

    BOOST_ASSERT_MSG(cropsData.size() > 0, "Crops must contain 1 or more entries");

    const unsigned int cropsTop = cropsData[0].first;
    const unsigned int cropsLeft = cropsData[1].first;

    for (unsigned int inBatch = 0; inBatch < inputBatchSize; ++inBatch)
    {
        const unsigned int outBatch = inBatch % outputBatchSize;
        const unsigned int spatialOffset = inBatch / outputBatchSize;

        for (unsigned int inH = 0; inH < inputTensorInfo.GetShape()[dataLayout.GetHeightIndex()]; ++inH) {
            const unsigned int outH = inH * blockShapeHeight + spatialOffset / blockShapeWidth - cropsTop;

            if (outH >= outputHeight)
            {
                continue;
            }

            for (unsigned int inW = 0; inW < inputTensorInfo.GetShape()[dataLayout.GetWidthIndex()]; ++inW) {
                const unsigned int outW = inW * blockShapeWidth + spatialOffset % blockShapeWidth - cropsLeft;

                if (outW >= outputWidth)
                {
                    continue;
                }

                for (unsigned int c = 0; c < channels; c++)
                {
                    unsigned int outOffset = Offset(outputShape, outBatch, outH, outW, c, dataLayout);
                    unsigned int inOffset = Offset(inputShape, inBatch, inH, inW, c, dataLayout);
                    outputData[outOffset] = inputData[inOffset];
                }
            }
        }
    }
}

} //namespace armnn


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/config/types.hpp>
#include <boost/dispatch/meta/make_integer.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::mpl::_;
using boost::is_same;
using boost::dispatch::meta::make_integer;

template<typename T> struct test {};

int main()
{
  BOOST_MPL_ASSERT(( is_same<unsigned char      , make_integer<1>::type>                    ));
  BOOST_MPL_ASSERT(( is_same<signed char        , make_integer<1, signed>::type>            ));
  BOOST_MPL_ASSERT(( is_same<test<signed short> , make_integer<2, signed, test<_> >::type>  ));
}


/** test move semantics - run with and without BOOST_UBLAS_MOVE_SEMANTICS defined */

//          Copyright Nasos Iliopoulos, Gunter Winkler 2009.
// 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)

#define BOOST_UBLAS_MOVE_SEMANTICS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas= boost::numeric::ublas;
std::vector<double> a;

ublas::vector<double> doubleit(ublas::vector<double> m)
{
    ublas::vector<double> r;
    r=2.0*m;
    std::cout << "Temporary pointer: " << &r[0] << std::endl;
    return r;
}
template <class T,size_t N>
ublas::bounded_vector<T,N > doubleit(ublas::bounded_vector<T, N> m)
{
    ublas::bounded_vector<T,N> r;
    r=2.0*m;
    std::cout << "Temporary pointer: " << &r[0] << std::endl;
    return r;
}

template <class T,size_t N>
ublas::c_vector<T,N > doubleit(ublas::c_vector<T, N> m)
{
    ublas::c_vector<T,N> r;
    r=2.0*m;
    std::cout << "Temporary pointer: " << &r[0] << std::endl;
    return r;
}

ublas::matrix<double> doubleit(ublas::matrix<double> m)
{
    ublas::matrix<double> r;
    r=2.0*m;
    std::cout << "Temporary pointer r: " << &r(0,0) << std::endl;
    return r;
}
template <class T,size_t N, size_t M>
ublas::bounded_matrix<T,N, M > doubleit(ublas::bounded_matrix<T, N, M> m)
{
    ublas::bounded_matrix<T,N, M> r;
    r=2.0*m;
    std::cout << "Temporary pointer: " <<  &(r(0,0)) << std::endl;
    return r;
}

template <class T,size_t N, size_t M>
ublas::c_matrix<T,N, M > doubleit(ublas::c_matrix<T, N, M> m)
{
    ublas::c_matrix<T,N, M> r;
    r=2.0*m;
    std::cout << "Temporary pointer: " << &(r(0,0)) << std::endl;
    return r;
}

void test1()
{
    std::cout << "vector<double> --------------------------------------------------------------------" << std::endl;
    ublas::vector<double> a(ublas::scalar_vector<double>(2,2.0));
    a = doubleit(a);
    std::cout << "Pointer (must be equal to temp. pointer if move semantics are enabled) : " << &a[0] << std::endl;

    std::cout << a << std::endl;

    std::cout << "bounded_vector<double,2> --------------------------------------------------------------------" << std::endl;
    ublas::bounded_vector<double,2> b(ublas::scalar_vector<double>(2,2.0));
    ublas::bounded_vector<double,2> c;
    noalias(c)=doubleit(b);
    std::cout << "Pointer (bounded_vector swaps by copy this should be different than temp. pointer) : " << &c[0] << std::endl;
    c(1)=0.0;
    std::cout << b << std::endl;
    std::cout << c << std::endl;

    std::cout << "c_vector<double,2> --------------------------------------------------------------------" << std::endl;
    ublas::c_vector<double,2> e=ublas::scalar_vector<double>(2,2.0);
    ublas::c_vector<double,2> f;
    f=doubleit(e);
    std::cout << "Pointer (c_vector swaps by copy this should be different than temp. pointer) : " << &f[0] << std::endl;
    f(1)=0;
    std::cout << e << std::endl;
    std::cout << f << std::endl;

}

void test2() {
    std::cout << "matrix<double> --------------------------------------------------------------------" << std::endl;
    ublas::matrix<double> a(ublas::scalar_matrix<double>(2, 3, 2.0));
    a = doubleit(a);
    std::cout << "Pointer (must be equal to temp. pointer if move semantics are enabled) : " << &(a(0,0)) << std::endl;
    std::cout << a << std::endl;

    std::cout << "bounded_matrix<double,2, 3> --------------------------------------------------------------------" << std::endl;
    ublas::bounded_matrix<double,2, 3> b(ublas::scalar_matrix<double>(2,3, 2.0));
    ublas::bounded_matrix<double,2, 3> c;
    noalias(c)=doubleit(b);
    std::cout << "Pointer (bounded_matrix swaps by copy this should be different than temp. pointer) : " << &(c(0,0)) << std::endl;
    c(1,1)=0.0;
    std::cout << b << std::endl;
    std::cout << c << std::endl;

    std::cout << "c_matrix<double,2 ,3> --------------------------------------------------------------------" << std::endl;
    ublas::c_matrix<double,2, 3> e=ublas::scalar_matrix<double>(2,3, 2.0);
    ublas::c_matrix<double,2, 3> f;
    f=doubleit(e);
    std::cout << "Pointer (c_matrix swaps by copy this should be different than temp. pointer) : " << &(f(0,0)) << std::endl;
    f(1,1)=0;
    std::cout << e << std::endl;
    std::cout << f << std::endl;
}

int main(){
    test1();
    test2();
    return 0;
}


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

#include <iostream>
#include <fstream>
#include <string>
#include <boost/program_options.hpp>

#include <dpMM/dpSubclusterMM.hpp>
#include <dpMM/dirBaseMeasure.hpp>

using namespace Eigen;
using std::string;
namespace po = boost::program_options;

typedef double flt;

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

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("seed", po::value<int>(), "seed for random number generator")
    ("N,N", po::value<int>(), "number of input datapoints")
    ("D,D", po::value<int>(), "number of dimensions of the data")
    ("T,T", po::value<int>(), "iterations")
    ("alpha,a", po::value< vector<double> >()->multitoken(),
      "alpha parameter of the DP (if single value assumes all alpha_i are the "
      "same")
    ("K,K", po::value<int>(), "number of initial clusters ")
    ("base", po::value<string>(),
      "which base measure to use (only DpDir, ")
    ("params,p", po::value< vector<double> >()->multitoken(),
      "parameters of the base measure")
    ("brief", po::value< vector<double> >()->multitoken(),
      "brief parameters of the base measure (ie Delta = delta*I; "
      "theta=t*ones(D)")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different "
      "datapoints)")
    ;

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

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  uint64_t seed = time(0);
  if(vm.count("seed"))
    seed = static_cast<uint64_t>(vm["seed"].as<int>());
  boost::mt19937 rndGen(seed);
  uint32_t K=5;
  if (vm.count("K")) K = vm["K"].as<int>();
  // number of iterations
  uint32_t T=100;
  if (vm.count("T")) T = vm["T"].as<int>();
  uint32_t N=100;
  if (vm.count("N")) N = vm["N"].as<int>();
  uint32_t D=2;
  if (vm.count("D")) D = vm["D"].as<int>();
  cout << "T="<<T<<endl;
  // DP alpha parameter
  VectorXd alpha(K);
  alpha.setOnes(K);
  if (vm.count("alpha"))
  {
    vector<double> params = vm["alpha"].as< vector<double> >();
    if(params.size()==1)
      alpha *= params[0];
    else
      for (uint32_t k=0; k<K; ++k)
        alpha(k) = params[k];
  }
  cout << "alpha="<<alpha.transpose()<<endl;

  shared_ptr<MatrixXd> spx(new MatrixXd(D,N));
  MatrixXd& x(*spx);
  string pathIn ="";
  if(vm.count("input")) pathIn = vm["input"].as<string>();
  if (!pathIn.compare(""))
  {
    for(uint32_t i=0; i<N; ++i)
    {
      if(i<N/3)
      {
        x.col(i) << VectorXd::Zero(D/3), VectorXd::Zero(D/3), VectorXd::Ones(D/3)*100;
      }else if(N/3 <= i && i < 2*N/3){
        x.col(i) << VectorXd::Zero(D/3), VectorXd::Ones(D/3)*100, VectorXd::Zero(D/3);
      }else{
        x.col(i) << VectorXd::Ones(D/3)*100, VectorXd::Zero(D/3), VectorXd::Zero(D/3);
      }
//      x.col(i) /= x.col(i).sum();
    }
    cout<<x<<endl;
  }else{
    cout<<"loading data from "<<pathIn<<endl;
    std::ifstream fin(pathIn.data(),std::ifstream::in);
    for (uint32_t j=0; j<D; ++j)
      for (uint32_t i=0; i<N; ++i)
        fin>>x(j,i);
  }

  // which base distribution
  string base = "DpDir";
  if(vm.count("base")) base = vm["base"].as<string>();

//  if(base.compare("DpDir")){
//    // normalize to unit sum
//    int err = 0;
//#pragma omp parallel for
//    for (uint32_t i=0; i<N; ++i)
//      if(fabs(x.col(i).sum() - 1.0) > 1e-1)
//      {
//        err++;
//        cout<<x.col(i).sum() <<endl;
//      }else
//        x.col(i) /= x.col(i).sum();
//    if(err>0) return 0;
//  }

  DpMM<double> *dpmm=NULL;
  if(!base.compare("DpDir"))
  {
  // Dir alpha parameter
    VectorXd gamma(D);
    gamma.setOnes(D);
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      if(params.size() == 1)
        gamma *= params[0];
      else
        for(uint32_t i=0; i<D; ++i)
          gamma(i) = params[i];
      cout <<"gamma="<<gamma<<endl;
    }
    DirMultd dir(gamma, &rndGen);
    shared_ptr<DirMultSampledd> dirSampl(new DirMultSampledd(dir));
    shared_ptr<LrCluster<DirMultSampledd,double> > lrTheta(new
        LrCluster<DirMultSampledd,double>(dirSampl,1.0,&rndGen));
    dpmm = new DpSubclusterMM<DirMultSampledd,double>(alpha(0),
        lrTheta, K, &rndGen);
  }

  string pathOut ="./labels.csv";
  if(vm.count("output"))
    pathOut = vm["output"].as<string>();
  cout<<"output to "<<pathOut<<endl;

  assert(dpmm!=NULL);
  dpmm->initialize(x);

  std::ofstream fout(pathOut.data(),std::ofstream::out);
  std::ofstream foutJointLike((pathOut+"_jointLikelihood.csv").data(),std::ofstream::out);
  std::ofstream foutMeans((pathOut+"_means.csv").data(),std::ofstream::out);
  std::ofstream foutCovs((pathOut+"_covs.csv").data(),std::ofstream::out);

  const VectorXu& z = dpmm->getLabels().transpose();
  for (uint32_t i=0; i<z.size()-1; ++i)
    fout<<z(i)<<" ";
  fout<<z(z.size()-1)<<endl;
  foutJointLike<<dpmm->logJoint()<<endl;
  dpmm->dump(foutMeans,foutCovs);

  for (uint32_t t=0; t<T; ++t)
  {
    cout<<"------------ t="<<t<<" -------------"<<endl;
    //      VectorXd Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
    dpmm->sampleParameters();

    const VectorXu& z = dpmm->getLabels().transpose();
    for (uint32_t i=0; i<z.size()-1; ++i)
      fout<<z(i)<<" ";
    fout<<z(z.size()-1)<<endl;
    foutJointLike<<dpmm->logJoint()<<endl;
    dpmm->dump(foutMeans,foutCovs);

    dpmm->sampleLabels();

    VectorXd Ns = dpmm->getCounts();
    cout<<"--  counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
    cout<<"    K="<<dpmm->getK();
    cout<<"    logJoint= "<<dpmm->logJoint()<<endl;

    dpmm->proposeMerges();
    //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;

    dpmm->proposeSplits();
    //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
    //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;

  }
  fout.close();
  foutJointLike.close();

  MatrixXd logLikes;
  MatrixXu inds = dpmm->mostLikelyInds(10,logLikes);
  cout<<"most likely indices"<<endl;
  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;

  fout.open((pathOut+"mlInds.csv").data(),std::ofstream::out);
  fout<<inds<<endl;
  fout.close();
  fout.open((pathOut+"mlLogLikes.csv").data(),std::ofstream::out);
  fout<<logLikes<<endl;
  fout.close();
};


//  test_special_values.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was extracted by Vicente J. Botet Escriba from Beman Dawes time2_demo.cpp which
was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

using namespace boost::chrono;

void test_special_values()
{
    std::cout << "duration<unsigned>::min().count()  = " << ((duration<unsigned>::min)()).count() << '\n';
    std::cout << "duration<unsigned>::zero().count() = " << duration<unsigned>::zero().count() << '\n';
    std::cout << "duration<unsigned>::max().count()  = " << ((duration<unsigned>::max)()).count() << '\n';
    std::cout << "duration<int>::min().count()       = " << ((duration<int>::min)()).count() << '\n';
    std::cout << "duration<int>::zero().count()      = " << duration<int>::zero().count() << '\n';
    std::cout << "duration<int>::max().count()       = " << ((duration<int>::max)()).count() << '\n';
}

int main()
{
    test_special_values();
    return 0;
}


#define BOOST_TEST_MODULE scan

#include <boost/test/included/unit_test.hpp>
#include <mpl/mpl.hpp>

template<typename T>
class add {
public:
  T operator()(const T &a, const T &b) { return a + b; }
};

template<typename T>
bool scan_func_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)}, y{};
  comm_world.scan(add<T>(), x, y);
  return y == T((N * N + N) / 2);
}

template<typename T>
bool scan_op_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)}, y{};
  comm_world.scan(mpl::plus<T>(), x, y);
  return y == T((N * N + N) / 2);
}

template<typename T>
bool scan_lambda_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)}, y{};
  comm_world.scan([](T a, T b) { return a + b; }, x, y);
  return y == T((N * N + N) / 2);
}

template<typename T>
bool scan_inplace_func_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)};
  comm_world.scan(add<T>(), x);
  return x == T((N * N + N) / 2);
}

template<typename T>
bool scan_inplace_op_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)};
  comm_world.scan(mpl::plus<T>(), x);
  return x == T((N * N + N) / 2);
}

template<typename T>
bool scan_inplace_lambda_test() {
  const mpl::communicator &comm_world = mpl::environment::comm_world();
  int N = comm_world.rank() + 1;
  T x{T(N)};
  comm_world.scan([](T a, T b) { return a + b; }, x);
  return x == T((N * N + N) / 2);
}

BOOST_AUTO_TEST_CASE(reduce) {
  BOOST_TEST(scan_func_test<double>());
  BOOST_TEST(scan_op_test<double>());
  BOOST_TEST(scan_lambda_test<double>());
  BOOST_TEST(scan_inplace_func_test<double>());
  BOOST_TEST(scan_inplace_op_test<double>());
  BOOST_TEST(scan_inplace_lambda_test<double>());
}


#include "galois/Galois.h"
#include "galois/Reduction.h"
#include "galois/Bag.h"
#include "galois/Timer.h"
#include "galois/graphs/LCGraph.h"
#include "galois/ParallelSTL.h"
#include "llvm/Support/CommandLine.h"
#include "galois/runtime/Profile.h"
#include <boost/iterator/transform_iterator.hpp>

const char* name = "Motif Counting";
const char* desc =
    "Counts the edge-induced motifs in a graph using BFS extension";
const char* url = 0;
namespace cll   = llvm::cl;
static cll::opt<std::string> filetype(cll::Positional,
                                      cll::desc("<filetype: txt,adj,mtx,gr>"),
                                      cll::Required);
static cll::opt<std::string>
    filename(cll::Positional, cll::desc("<filename: symmetrized graph>"),
             cll::Required);
static cll::opt<unsigned>
    k("k", cll::desc("max number of vertices in k-motif(default value 3)"),
      cll::init(3));
static cll::opt<unsigned> show("s", cll::desc("print out the details"),
                               cll::init(0));
typedef galois::graphs::LC_CSR_Graph<uint32_t, void>::with_numa_alloc<
    true>::type ::with_no_lockable<true>::type Graph;
typedef Graph::GraphNode GNode;

#define USE_STRUCTURAL
#define EDGE_INDUCED
#define CHUNK_SIZE 256
#include "BfsMining/edge_miner.h"
#include "util.h"

void MotifSolver(EdgeMiner& miner) {
  EmbeddingQueueType in_queue, out_queue; // in&out worklist. double buffering
  miner.init(in_queue);                   // initialize the worklist
  unsigned level = 1;
  galois::gPrint("\n");

  while (level <
         k) { // to get the same output as RStream (which is not complete)
    miner.extend_edge(in_queue, out_queue); // edge extension
    in_queue.swap(out_queue);
    out_queue.clear();

    if (show)
      std::cout << "\n------------------------ Step 2: Aggregation "
                   "------------------------\n";
    QpMapFreq qp_map;           // quick patterns map for counting the frequency
    LocalQpMapFreq qp_localmap; // quick patterns local map for each thread
    galois::do_all(
        galois::iterate(in_queue),
        [&](const EmbeddingType& emb) {
          miner.quick_aggregate_each(
              emb, *(qp_localmap.getLocal())); // quick pattern aggregation
        },
        galois::chunk_size<CHUNK_SIZE>(), galois::steal(),
        galois::no_conflicts(),
        galois::wl<galois::worklists::PerSocketChunkFIFO<CHUNK_SIZE>>(),
        galois::loopname("QuickAggregation"));
    miner.merge_qp_map(qp_localmap, qp_map);

    CgMapFreq cg_map;           // canonical graph map for couting the frequency
    LocalCgMapFreq cg_localmap; // canonical graph local map for each thread
    galois::do_all(
        galois::iterate(qp_map),
        [&](std::pair<QPattern, Frequency> element) {
          miner.canonical_aggregate_each(
              element,
              *(cg_localmap.getLocal())); // canonical pattern aggregation
        },
        galois::chunk_size<CHUNK_SIZE>(), galois::steal(),
        galois::no_conflicts(),
        galois::wl<galois::worklists::PerSocketChunkFIFO<CHUNK_SIZE>>(),
        galois::loopname("CanonicalAggregation"));
    miner.merge_cg_map(cg_localmap, cg_map);
    miner.printout_agg(cg_map);
    galois::gPrint("\n");
    level++;
  }
}

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);
  Graph graph;
  galois::StatTimer Tinit("GraphReadingTime");
  Tinit.start();
  read_graph(graph, filetype, filename);
  Tinit.stop();
  galois::gPrint("num_vertices ", graph.size(), " num_edges ",
                 graph.sizeEdges(), "\n");

  EdgeMiner miner(&graph);
  galois::StatTimer Tcomp("Compute");
  Tcomp.start();
  MotifSolver(miner);
  Tcomp.stop();
  return 0;
}


#ifndef COBRAMODELPARSER_MODELBUILDER_HPP
#define COBRAMODELPARSER_MODELBUILDER_HPP

#include <Eigen/Core>
#include <vector>

#include "CobraModelParser/Model.hpp"

namespace CobraModelParser {

    class ModelBuilder {
    public:

        ModelBuilder &setModelOrigin(const std::string &modelOrigin) {
            this->modelOrigin = modelOrigin;
            return *this;
        }

        ModelBuilder &setModelDescription(const std::string &modelDescription) {
            this->modelDescription = modelDescription;
            return *this;
        }

        ModelBuilder &setData(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) {
            this->A = A;
            this->b = b;
            return *this;
        }

        Model build() {
            Model model;
            model.origin = modelOrigin;
            model.description = modelDescription;
            model.setA(A);
            model.setB(b);
            return model;
        }

    private:
        std::string modelOrigin;
        std::string modelDescription;
        Eigen::VectorXd b;
        Eigen::MatrixXd A;
    };
}

#endif //COBRAMODELPARSER_MODELBUILDER_HPP


/*
 * Copyright (c) DTAI - KU Leuven – All rights reserved.
 * Proprietary, do not copy or distribute without permission.
 * Written by Pieter Robberechts, 2019
 */

#ifndef DECISIONTREE_UTILS_HPP
#define DECISIONTREE_UTILS_HPP

#include <algorithm>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <string>
#include <unordered_map>
#include <vector>
#include <set>
#include <boost/timer/timer.hpp>

// You can change these data type aliases
using VecS = std::vector<std::string>;
using LabelMap = std::map<std::string, std::string>;
using Data = std::vector<std::vector<std::string>>;
struct MetaData {
  VecS labels;
  LabelMap labelMap; //used for checking whether the feature is Numeric or Categorical
};

/**
 * Custom comparator for numeric features
 */
struct NumericComparator{
    explicit NumericComparator(int i, const Data& rows) : data(rows){
        column = i;
    }
    int column;
    const Data& data;
    bool operator() (const size_t vec1, const size_t vec2) const{
        return std::stod(data[vec1][column]) < std::stod(data[vec2][column]);
    }
};

/**
 * Custom comparator for categorical features
 */
struct StringComparator{
        explicit StringComparator(int i, const Data& rows) : data(rows){
            column = i;
        }
        int column;
        const Data& data;
        bool operator() (const size_t vec1, const size_t vec2) const{
            return data[vec1][column] < data[vec2][column];
        }
};

namespace Utils::iterators {

  struct RetrieveKey {
    template<typename T>
      typename T::first_type operator()(T keyValuePair) const {
        return keyValuePair.first;
      }
  };

  struct AddMapValue {
    template<typename S, typename T>
      int operator()(const S previous, const T map) const {
        return previous + map.second;
      }
  };

  template<class InputIt, class T = typename std::iterator_traits<InputIt>::value_type>
    T mostCommon(InputIt begin, InputIt end)
    {
      std::map<T, int> counts;
      for (InputIt it = begin; it != end; ++it) {
        if (counts.find(*it) != counts.end()) {
          ++counts[*it];
        }
        else {
          counts[*it] = 1;
        }
      }
      return std::max_element(counts.begin(), counts.end(),
          [] (const std::pair<T, int>& pair1, const std::pair<T, int>& pair2) {
          return pair1.second < pair2.second;})->first;
    }

  template <typename It>
    auto average(It begin, It end) {
      assert(begin != end);
      auto s = std::accumulate(begin, end, 0.0);
      return s / std::distance(begin, end);
    };
}

namespace Utils::tree {

  template <typename T>
    int mapValueSum(std::unordered_map<T, int> counts) {
      return std::accumulate(begin(counts), std::end(counts), 0, iterators::AddMapValue());
    }

  template<typename T>
    T getMax(std::unordered_map<T, int> counts ) {
      using pairtype = std::pair<T, int>;
      std::pair<T, int> max = *std::max_element(counts.begin(), counts.end(), [] (const pairtype & p1, const pairtype & p2) {
          return p1.second < p2.second;
          });
      return max.first;
    }
}

namespace Utils::print {
  template<typename T>
    void print_vector(const std::vector<T> &vec) {
      if (vec.empty())
        return;

      std::cout << "{ ";
      std::copy(begin(vec), std::end(vec), std::ostream_iterator<T>(std::cout, " "));
      std::cout << "}\n";
    }

  template<typename K, typename V>
    void print_map(const std::unordered_map<K, V> &counter) {
      if (counter.empty())
        return;

      std::cout << "{ ";
      for (const auto& [key, val]: counter) {
        std::cout << key << ": " << val << " ";
      }
      std::cout << "}" << "\n";
    }
}

#endif //DECISIONTREE_UTILS_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_COMPLEX_CONSTANTS_I_HPP_INCLUDED
#define NT2_COMPLEX_CONSTANTS_I_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <complex>

/*!
 * \ingroup nt2_complex
 * \defgroup nt2_complex_i I
 *
 * \par Description
 * Constant I : Imag unit.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/i.hpp>
 * \endcode
 *
 * \alias:
 *    J
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T>
 *     meta::call<tag::I(as_<T>)>::type
 *     I();
 * }
 * \endcode
 *
 *
 * \param T template parameter of I
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    /*!
     * \brief Define the tag I of functor I
     *        in namespace nt2::tag for toolbox complex
    **/
    struct I : ext::constant_<I>
    {
      typedef std::complex<double> default_type;
      typedef ext::constant_<I> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_I( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::I, Site> dispatching_I(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::I, Site>();
    }
    template<class... Args>
    struct impl_I;
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(nt2::tag::I, I)
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(nt2::tag::I, J)
}

#endif


#include <cstdint>
#include <stdexcept>

#include <boost/test/unit_test.hpp>

#include "layout/grid.hpp"

BOOST_AUTO_TEST_SUITE(grid)

BOOST_AUTO_TEST_CASE(calc_grid_dimension_unconstrained_grid) {
  BOOST_REQUIRE_THROW(
      hisui::layout::calc_grid_dimension({.number_of_sources = 0}),
      std::invalid_argument);

  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 4}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 6}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 3};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 7}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 3};
    BOOST_REQUIRE_EQUAL(
        expected, hisui::layout::calc_grid_dimension({.number_of_sources = 9}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 4, .rows = 3};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.number_of_sources = 10}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 4, .rows = 3};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.number_of_sources = 12}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 5, .rows = 4};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.number_of_sources = 17}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 5, .rows = 4};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.number_of_sources = 20}));
  }
}

BOOST_AUTO_TEST_CASE(calc_grid_dimension_unconstrained_dimension_rows) {
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 1, .number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 1, .number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 3};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 1, .number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 2, .number_of_sources = 4}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 3};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 2, .number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 4};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 2, .number_of_sources = 7}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 5};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 2, .number_of_sources = 9}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 6};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_columns = 2, .number_of_sources = 12}));
  }
}

BOOST_AUTO_TEST_CASE(calc_grid_dimension_unconstrained_dimension_columns) {
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 1, .number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 1, .number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 1};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 1, .number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 2, .number_of_sources = 4}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 2, .number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 4, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 2, .number_of_sources = 7}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 5, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_grid_dimension(
                                      {.max_rows = 2, .number_of_sources = 9}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 6, .rows = 2};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_grid_dimension(
                            {.max_rows = 2, .number_of_sources = 12}));
  }
}

BOOST_AUTO_TEST_CASE(calc_grid_dimension_constrained_grid) {
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 1, .max_rows = 1, .number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 1, .number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 2, .number_of_sources = 1}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 1, .max_rows = 1, .number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 1, .number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 2, .number_of_sources = 2}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 1, .max_rows = 1, .number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 1, .number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 2, .number_of_sources = 3}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 2, .number_of_sources = 4}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 1, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 1, .max_rows = 1, .number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 1, .number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 2, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 2, .max_rows = 2, .number_of_sources = 5}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 7, .rows = 1};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 7, .max_rows = 1, .number_of_sources = 9}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 5, .rows = 2};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 7, .max_rows = 2, .number_of_sources = 9}));
  }
  {
    auto expected = hisui::layout::GridDimension{.columns = 3, .rows = 3};
    BOOST_REQUIRE_EQUAL(
        expected,
        hisui::layout::calc_grid_dimension(
            {.max_columns = 7, .max_rows = 3, .number_of_sources = 9}));
  }
}

BOOST_AUTO_TEST_CASE(add_number_of_excluded_cells) {
  BOOST_REQUIRE_THROW(hisui::layout::add_number_of_excluded_cells(
                          {.number_of_sources = 1, .cells_excluded = {3, 1}}),
                      std::invalid_argument);

  BOOST_REQUIRE_EQUAL(0, hisui::layout::add_number_of_excluded_cells(
                             {.number_of_sources = 0, .cells_excluded = {}}));
  BOOST_REQUIRE_EQUAL(1, hisui::layout::add_number_of_excluded_cells(
                             {.number_of_sources = 1, .cells_excluded = {}}));
  BOOST_REQUIRE_EQUAL(2, hisui::layout::add_number_of_excluded_cells(
                             {.number_of_sources = 1, .cells_excluded = {0}}));
  BOOST_REQUIRE_EQUAL(1, hisui::layout::add_number_of_excluded_cells(
                             {.number_of_sources = 1, .cells_excluded = {1}}));
  BOOST_REQUIRE_EQUAL(1, hisui::layout::add_number_of_excluded_cells(
                             {.number_of_sources = 1, .cells_excluded = {1}}));

  BOOST_REQUIRE_EQUAL(3,
                      hisui::layout::add_number_of_excluded_cells(
                          {.number_of_sources = 2, .cells_excluded = {1, 3}}));
  BOOST_REQUIRE_EQUAL(5,
                      hisui::layout::add_number_of_excluded_cells(
                          {.number_of_sources = 3, .cells_excluded = {1, 3}}));
}

BOOST_AUTO_TEST_SUITE_END()


#include <stdint.h>
#include <iostream>
#include <string>
#include <boost/functional/hash.hpp>
#include <boost/format.hpp>
#include <boost/crc.hpp>

#include "consistent_hash_map.hpp"

const char* nodes[] = {
    "192.168.1.100",
    "192.168.1.101",
    "192.168.1.102",
    "192.168.1.103",
    "192.168.1.104"
};

struct vnode_t {
    vnode_t() {}
    vnode_t(std::size_t n,std::size_t v):node_id(n),vnode_id(v) {}

    std::string to_str() const {
        return boost::str(boost::format("%1%-%2%") % nodes[node_id] % vnode_id);
    }

    std::size_t node_id;
    std::size_t vnode_id;

};

struct crc32_hasher {
    uint32_t operator()(const vnode_t& node) {
        boost::crc_32_type ret;
        std::string vnode = node.to_str();
        std::cout<<"vnode:"<<vnode<<std::endl;
        ret.process_bytes(vnode.c_str(),vnode.size());
        return ret.checksum();
    }
    typedef uint32_t result_type;
};

int main(int argc, char const *argv[])
{
    typedef consistent_hash_map<vnode_t,crc32_hasher> consistent_hash_t;
    consistent_hash_t consistent_hash_;

    for(std::size_t i=0;i<5;++i) {
        for(std::size_t j=0;j<100;j++) {
            consistent_hash_.insert(vnode_t(i,j));
        }
    }

{
    std::cout<<"========================================================="<<std::endl;
    std::size_t sums[] = {0,0,0,0,0};
    consistent_hash_t::iterator i = consistent_hash_.begin();
    consistent_hash_t::reverse_iterator j = consistent_hash_.rbegin();
    std::size_t n = i->first + UINT32_MAX - j->first;
    std::cout<<boost::format("vnode:%1%,hash:%2%,contains:%3%")
            % i->second.to_str() % i->first % n << std::endl;
    sums[i->second.node_id] += n;

    uint32_t priv = i->first;
    uint32_t cur;
    consistent_hash_t::iterator end = consistent_hash_.end();
    while(++i != end) {
        cur = i->first;
        n = cur - priv;
        std::cout<<boost::format("vnode:%1%,hash:%2%,contains:%3%")
            % i->second.to_str() % cur % n << std::endl;
        sums[i->second.node_id] += n;
        priv = cur;
    }

    for(std::size_t i=0;i<5;++i) {
        std::cout<<boost::format("node:%1% contains:%2%") % nodes[i] % sums[i] <<std::endl;
    }

}

{
    consistent_hash_t::iterator it;
    it = consistent_hash_.find(290235110);
    std::cout<<boost::format("node:%1%,vnode:%2%,hash:%3%")
                % nodes[it->second.node_id] % it->second.vnode_id % it->first << std::endl;
}

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_POLYNOMIALS_FUNCTION_SIMD_COMMON_POLEVL_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTION_SIMD_COMMON_POLEVL_HPP_INCLUDED
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <boost/fusion/adapted/array.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::polevl_, tag::cpu_,
                        (A0)(A1)(X),
                        ((simd_<arithmetic_<A0>,X>))
                        (fusion_sequence_<A1>)
                        );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::polevl_(tag::simd_<tag::arithmetic_, X> ,
                           tag::fusion_sequence_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>
    struct result<This(A0, A1)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return polevl(tofloat(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::polevl_, tag::cpu_,
		      (A0)(A1)(X),
                        ((simd_<real_<A0>,X>))
                        (fusion_sequence_<A1>)
                        );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::polevl_(tag::simd_<tag::real_, X> ,
                          tag::fusion_sequence_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>
    struct result<This(A0, A1)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      typename A1::const_iterator p = a1.begin();
      A0 ans =  nt2::splat<A0>(*p++);
      do
      ans = fma(ans, a0, nt2::splat<A0>(*p));
      while( ++p !=  a1.end());
      return ans;
    }
  };
} }

#endif
// modified by jt the 05/01/2011


// Filename: mixed_complex.cpp (part of MTL4)

#include <complex>
#include <iostream>
#include <boost/numeric/mtl/operation/extended_complex.hpp>

int main()
{
    std::complex<double>  z(2.0, 3.0);
    std::cout << "2 * z = " << 2 * z << '\n';
    std::cout << "2 + z = " << 2 + z << '\n';
    std::cout << "z / 2 = " << z / 2 << '\n';
    std::cout << "2 / z = " << 2 / z << '\n';
    std::cout << "2 - z = " << 2 - z << '\n';

    return 0;
}


/*Copyright (c) 2021 James Gayvert

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.
*/

/*
 * AOCAP.cpp
 */

#include "AOCAP.h"
#include "grid_radial.h"
#include "bragg.h"
#include <numgrid.h>
#include <omp.h>
#include <chrono>
#include <ctime>
#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <thread>
#include <vector>
#include <cmath>
#include <limits>
#include "BasisSet.h"
#include "gto_ordering.h"
#include "opencap_exception.h"
#include "utils.h"
#include <typeinfo>
#include "boxcap.h"
#include "cap_types.h"

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params)
{
	verify_cap_parameters(params);
	double capx,capy,capz,rcut,radial,angpts;
	std::stringstream capxss(params["cap_x"]);
	std::stringstream capyss(params["cap_y"]);
	std::stringstream capzss(params["cap_z"]);
	std::stringstream rcutss(params["r_cut"]);
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	capxss >> capx;
	capyss >> capy;
	capzss >> capz;
	rcutss >> rcut;
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = params["cap_type"];
	cap_x = capx;
	cap_y= capy;
	cap_z = capz;
	r_cut = rcut;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
    num_atoms = atoms.size();
	if(compare_strings(cap_type,"box"))
		cap_func = box_cap(cap_x,cap_y,cap_z);
	else if(compare_strings(cap_type,"voronoi"))
		cap_func = voronoi_cap(r_cut,atoms);
	else
		opencap_throw("Error: Only box and voronoi caps are implemented natively.");
	if(params.find("do_numerical")!=params.end())
	{
		std::string num_int = params["do_numerical"];
		if(compare_strings(num_int,"true"))
			do_numerical = true;
		else if(compare_strings(num_int,"false") && compare_strings(cap_type,"box"))
			do_numerical = false;
		else
			do_numerical = true;
	}
	else if(compare_strings(cap_type,"box"))
		do_numerical = false;
	else
		do_numerical = true;
}

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params,const std::function<std::vector<double>(std::vector<double> &, std::vector<double> &,
std::vector<double> &, std::vector<double> &)> &custom_cap_func)
{
	verify_cap_parameters(params);
	double radial,angpts;
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = "custom";
	cap_x = 0.0;
	cap_y= 0.0;
	cap_z = 0.0;
	r_cut = 0.0;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
    num_atoms = atoms.size();
	do_numerical = true;
	cap_func = custom_cap_func;
}

void AOCAP::integrate_cap_numerical(Eigen::MatrixXd &cap_mat, BasisSet bs)
{
	// funny business happens with GIL if we try to parallelize over atoms when custom python function is used
	if(cap_type=="custom")
		omp_set_num_threads(1);
    std::cout << "Calculating CAP matrix in AO basis using " << std::to_string(omp_get_max_threads()) << " threads." << std::endl;
    std::cout << std::setprecision(2) << std::scientific  << "Radial precision: " << radial_precision
              << " Angular points: " << angular_points << std::endl;
	size_t num_atoms = atoms.size();
    double x_coords_bohr[num_atoms];
	double y_coords_bohr[num_atoms];
	double z_coords_bohr[num_atoms];
	int nuc_charges[num_atoms];
	for(size_t i=0;i<num_atoms;i++)
	{
		x_coords_bohr[i]=atoms[i].coords[0];
		y_coords_bohr[i]=atoms[i].coords[1];
		z_coords_bohr[i]=atoms[i].coords[2];
		nuc_charges[i]=atoms[i].Z;
		if (atoms[i].Z==0)
			nuc_charges[i]=1; //choose bragg radius for H for ghost atoms
	}
    int min_num_angular_points = angular_points;
    int max_num_angular_points = angular_points;
	#pragma omp parallel for
	for(size_t i=0;i<num_atoms;i++)
	{
        // check parameters
        double alpha_max = bs.alpha_max(atoms[i]);
        std::vector<double> alpha_min = bs.alpha_min(atoms[i]);
        double r_inner = get_r_inner(radial_precision,
                                     alpha_max * 2.0); // factor 2.0 to match DIRAC
        double h = std::numeric_limits<float>::max();
        double r_outer = 0.0;
        for (int l = 0; l <= bs.max_L(); l++)
        {
            if (alpha_min[l] > 0.0)
            {
                r_outer =
                std::max(r_outer,
                         get_r_outer(radial_precision,
                                     alpha_min[l],
                                     l,
                                     4.0 * get_bragg_angstrom(nuc_charges[i])));
                if(r_outer < r_inner)
                {
                    opencap_throw("Error: r_outer < r_inner, grid cannot be allocated for this basis.");
                    //std::cout << "Setting alpha min[l] to 0.01" << std::endl;
                    //alpha_min[l]=0.01;
                }
                else
                {
                    h = std::min(h,
                                 get_h(radial_precision, l, 0.1 * (r_outer - r_inner)));
                    if(r_outer < h)
                    {
                        opencap_throw("Error: r_outer < h, grid cannot be allocated for this basis.");
                    }
                }
            }
        }
        context_t *context = numgrid_new_atom_grid(radial_precision,
		                                 min_num_angular_points,
		                                 max_num_angular_points,
		                                 nuc_charges[i],
		                                 bs.alpha_max(atoms[i]),
		                                 bs.max_L(),
		                                 alpha_min.data());
		int num_points = numgrid_get_num_grid_points(context);
        double *grid_x_bohr = new double[num_points];
        double *grid_y_bohr = new double[num_points];
        double *grid_z_bohr = new double[num_points];
        double *grid_w = new double[num_points];
		double *cap_values = new double[num_points];
        numgrid_get_grid(  context,
                           num_atoms,
                           i,
                           x_coords_bohr,
                           y_coords_bohr,
                           z_coords_bohr,
                           nuc_charges,
                           grid_x_bohr,
                           grid_y_bohr,
                           grid_z_bohr,
                           grid_w);
        int num_radial_points = numgrid_get_num_radial_grid_points(context);
		compute_cap_on_grid(cap_mat,bs,grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points);
	}
}

void AOCAP::compute_cap_on_grid(Eigen::MatrixXd &cap_mat,BasisSet bs,double* x, double* y, double* z,
double *grid_w, int num_points)
{
	Eigen::VectorXd cap_vals; Eigen::MatrixXd bf_values;
	bf_values = Eigen::MatrixXd::Zero(num_points,bs.num_carts());
    std::vector<double> x_vec(x,x+num_points);
    std::vector<double> y_vec(y,y+num_points);
    std::vector<double> z_vec(z,z+num_points);
    std::vector<double> w_vec(grid_w,grid_w+num_points);
    std::vector<double> cap_vec = cap_func(x_vec,y_vec,z_vec,w_vec);
	size_t bf_idx = 0;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(my_shell.l);
		for(size_t j=0;j<my_shell.num_carts();j++)
		{
			std::array<size_t,3> cart = order[j];
			my_shell.evaluate_on_grid(x,y,z,num_points,cart[0],cart[1],cart[2],bf_values.col(bf_idx));
			bf_idx++;
		}
	}
    cap_vals = Eigen::Map<Eigen::VectorXd>(cap_vec.data(),cap_vec.size());
	Eigen::MatrixXd bf_prime;
	bf_prime =  Eigen::MatrixXd::Zero(num_points,bs.num_carts());
	for(size_t i=0;i<bf_prime.cols();i++)
		bf_prime.col(i) = bf_values.col(i).array()*cap_vals.array();
	cap_mat+=bf_prime.transpose()*bf_values;
}

void AOCAP::compute_ao_cap_mat(Eigen::MatrixXd &cap_mat, BasisSet &bs)
{
	if(!do_numerical)
	{
		std::cout << "CAP integrals will be computed analytically." << std::endl;
		eval_box_cap_analytical(cap_mat,bs);
		return;
	}
	else
		integrate_cap_numerical(cap_mat,bs);
}

void AOCAP::eval_box_cap_analytical(Eigen::MatrixXd &cap_mat, BasisSet &bs)
{
	double boxlength[3] = {cap_x,cap_y,cap_z};
	size_t bf1_idx = 0;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell shell1 = bs.basis[i];
		std::vector<std::array<size_t,3>> order1 = opencap_carts_ordering(shell1.l);
		for(size_t j=0;j<shell1.num_carts();j++)
		{
			std::array<size_t,3> l1 = order1[j];
			size_t bf2_idx = 0;
			for (size_t q=0;q<bs.basis.size();q++)
			{
				Shell shell2 = bs.basis[q];
				std::vector<std::array<size_t,3>> order2 = opencap_carts_ordering(shell2.l);
				for(size_t k=0;k<shell2.num_carts();k++)
				{
					std::array<size_t,3> l2 = order2[k];
					cap_mat(bf1_idx+j,bf2_idx+k) = integrate_box_cap(shell1,shell2,l1,l2,boxlength);
				}
				bf2_idx += shell2.num_carts();
			}
		}
		bf1_idx += shell1.num_carts();
	}
}

void AOCAP::verify_cap_parameters(std::map<std::string,std::string> &parameters)
{
	std::vector<std::string> missing_keys;
	if(parameters.find("cap_type")==parameters.end())
		opencap_throw("Error: Missing cap_type keyword.");
	if(compare_strings(parameters["cap_type"],"box"))
	{
		if(parameters.find("cap_x")==parameters.end())
			missing_keys.push_back("cap_x");
		if(parameters.find("cap_y")==parameters.end())
			missing_keys.push_back("cap_y");
		if (parameters.find("cap_z")==parameters.end())
			missing_keys.push_back("cap_z");
	}
	else if (compare_strings(parameters["cap_type"],"voronoi"))
	{
		if(parameters.find("r_cut")==parameters.end())
			missing_keys.push_back("r_cut");
	}
	else if(compare_strings(parameters["cap_type"],"custom"))
		;
	else
		opencap_throw("Error: only box and voronoi CAPs supported.");
	if(missing_keys.size()!=0)
	{
		std::string error_str = "Missing CAP keywords: ";
		for (auto key: missing_keys)
			error_str+=key+" ";
		opencap_throw(error_str);
	}
	std::map<std::string, std::string> defaults = {{"radial_precision", "14"}, {"angular_points", "590"}};
	for (const auto &pair:defaults)
	{
		if(parameters.find(pair.first)==parameters.end())
			parameters[pair.first]=pair.second;
	}
}


/**
* This file is part of DSO.
*
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
*/

/*
 * KFBuffer.cpp
 *
 *  Created on: Jan 7, 2014
 *      Author: engelj
 */

#include "FullSystem/FullSystem.h"

#include "stdio.h"
#include "util/globalFuncs.h"
#include <Eigen/LU>
#include <algorithm>
#include "IOWrapper/ImageDisplay.h"
#include "util/globalCalib.h"
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

#include "FullSystem/ResidualProjections.h"
#include "OptimizationBackend/EnergyFunctional.h"
#include "OptimizationBackend/EnergyFunctionalStructs.h"

#include "FullSystem/HessianBlocks.h"

namespace dso
{
int PointFrameResidual::instanceCounter = 0;

long runningResID=0;

PointFrameResidual::PointFrameResidual(){assert(false); instanceCounter++;}

PointFrameResidual::~PointFrameResidual(){assert(efResidual==0); instanceCounter--; delete J;}

PointFrameResidual::PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_) :
	point(point_),
	host(host_),
	target(target_)
{
	efResidual=0;
	instanceCounter++;
	resetOOB();
	J = new RawResidualJacobian();
	assert(((long)J)%16==0);

	isNew=true;
}

double PointFrameResidual::linearize(CalibHessian* HCalib)
{
	state_NewEnergyWithOutlier=-1;

	if(state_state == ResState::OOB)
		{ state_NewState = ResState::OOB; return state_energy; }

	FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]);
	float energyLeft=0;
	const Eigen::Vector3f* dIl = target->dI;
	//const float* const Il = target->I;
	const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;
	const Vec3f &PRE_KtTll = precalc->PRE_KtTll;
	const Mat33f &PRE_RTll_0 = precalc->PRE_RTll_0;
	const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0;
	const float * const color = point->color;
	const float * const weights = point->weights;

	Vec2f affLL = precalc->PRE_aff_mode;
	float b0 = precalc->PRE_b0_mode;

	Vec6f d_xi_x, d_xi_y;
	Vec4f d_C_x, d_C_y;
	float d_d_x, d_d_y;
	{
		float drescale, u, v, new_idepth;
		float Ku, Kv;
		Vec3f KliP;

		if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0,HCalib,
				PRE_RTll_0,PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth))
			{ state_NewState = ResState::OOB; return state_energy; }

		centerProjectedTo = Vec3f(Ku, Kv, new_idepth);

		// diff d_idepth
		d_d_x = drescale * (PRE_tTll_0[0]-PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl();
		d_d_y = drescale * (PRE_tTll_0[1]-PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl();

		// diff calib
		d_C_x[2] = drescale*(PRE_RTll_0(2,0)*u-PRE_RTll_0(0,0));
		d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0(2,1)*u-PRE_RTll_0(0,1)) * HCalib->fyli();
		d_C_x[0] = KliP[0]*d_C_x[2];
		d_C_x[1] = KliP[1]*d_C_x[3];

		d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0(2,0)*v-PRE_RTll_0(1,0)) * HCalib->fxli();
		d_C_y[3] = drescale*(PRE_RTll_0(2,1)*v-PRE_RTll_0(1,1));
		d_C_y[0] = KliP[0]*d_C_y[2];
		d_C_y[1] = KliP[1]*d_C_y[3];

		d_C_x[0] = (d_C_x[0]+u)*SCALE_F;
		d_C_x[1] *= SCALE_F;
		d_C_x[2] = (d_C_x[2]+1)*SCALE_C;
		d_C_x[3] *= SCALE_C;

		d_C_y[0] *= SCALE_F;
		d_C_y[1] = (d_C_y[1]+v)*SCALE_F;
		d_C_y[2] *= SCALE_C;
		d_C_y[3] = (d_C_y[3]+1)*SCALE_C;

		d_xi_x[0] = new_idepth*HCalib->fxl();
		d_xi_x[1] = 0;
		d_xi_x[2] = -new_idepth*u*HCalib->fxl();
		d_xi_x[3] = -u*v*HCalib->fxl();
		d_xi_x[4] = (1+u*u)*HCalib->fxl();
		d_xi_x[5] = -v*HCalib->fxl();

		d_xi_y[0] = 0;
		d_xi_y[1] = new_idepth*HCalib->fyl();
		d_xi_y[2] = -new_idepth*v*HCalib->fyl();
		d_xi_y[3] = -(1+v*v)*HCalib->fyl();
		d_xi_y[4] = u*v*HCalib->fyl();
		d_xi_y[5] = u*HCalib->fyl();
	}

	{
		J->Jpdxi[0] = d_xi_x;
		J->Jpdxi[1] = d_xi_y;

		J->Jpdc[0] = d_C_x;
		J->Jpdc[1] = d_C_y;

		J->Jpdd[0] = d_d_x;
		J->Jpdd[1] = d_d_y;

	}

	float JIdxJIdx_00=0, JIdxJIdx_11=0, JIdxJIdx_10=0;
	float JabJIdx_00=0, JabJIdx_01=0, JabJIdx_10=0, JabJIdx_11=0;
	float JabJab_00=0, JabJab_01=0, JabJab_11=0;

	float wJI2_sum = 0;

	for(int idx=0;idx<patternNum;idx++)
	{
		float Ku, Kv;
		if(!projectPoint(point->u+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv))
			{ state_NewState = ResState::OOB; return state_energy; }

		projectedTo[idx][0] = Ku;
		projectedTo[idx][1] = Kv;

        Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
        float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]);

		float drdA = (color[idx]-b0);
		if(!std::isfinite((float)hitColor[0]))
		{ state_NewState = ResState::OOB; return state_energy; }

		float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm()));
        w = 0.5f*(w + weights[idx]);

		float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
		energyLeft += w*w*hw *residual*residual*(2-hw);

		{
			if(hw < 1) hw = sqrtf(hw);
			hw = hw*w;

			hitColor[1]*=hw;
			hitColor[2]*=hw;

			J->resF[idx] = residual*hw;

			J->JIdx[0][idx] = hitColor[1];
			J->JIdx[1][idx] = hitColor[2];
			J->JabF[0][idx] = drdA*hw;
			J->JabF[1][idx] = hw;

			JIdxJIdx_00+=hitColor[1]*hitColor[1];
			JIdxJIdx_11+=hitColor[2]*hitColor[2];
			JIdxJIdx_10+=hitColor[1]*hitColor[2];

			JabJIdx_00+= drdA*hw * hitColor[1];
			JabJIdx_01+= drdA*hw * hitColor[2];
			JabJIdx_10+= hw * hitColor[1];
			JabJIdx_11+= hw * hitColor[2];

			JabJab_00+= drdA*drdA*hw*hw;
			JabJab_01+= drdA*hw*hw;
			JabJab_11+= hw*hw;

			wJI2_sum += hw*hw*(hitColor[1]*hitColor[1]+hitColor[2]*hitColor[2]);

			if(setting_affineOptModeA < 0) J->JabF[0][idx]=0;
			if(setting_affineOptModeB < 0) J->JabF[1][idx]=0;

		}
	}

	J->JIdx2(0,0) = JIdxJIdx_00;
	J->JIdx2(0,1) = JIdxJIdx_10;
	J->JIdx2(1,0) = JIdxJIdx_10;
	J->JIdx2(1,1) = JIdxJIdx_11;
	J->JabJIdx(0,0) = JabJIdx_00;
	J->JabJIdx(0,1) = JabJIdx_01;
	J->JabJIdx(1,0) = JabJIdx_10;
	J->JabJIdx(1,1) = JabJIdx_11;
	J->Jab2(0,0) = JabJab_00;
	J->Jab2(0,1) = JabJab_01;
	J->Jab2(1,0) = JabJab_01;
	J->Jab2(1,1) = JabJab_11;

	state_NewEnergyWithOutlier = energyLeft;

	if(energyLeft > std::max<float>(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2)
	{
		energyLeft = std::max<float>(host->frameEnergyTH, target->frameEnergyTH);
		state_NewState = ResState::OUTLIER;
	}
	else
	{
		state_NewState = ResState::IN;
	}

	state_NewEnergy = energyLeft;
	return energyLeft;
}

void PointFrameResidual::debugPlot()
{
	if(state_state==ResState::OOB) return;
	Vec3b cT = Vec3b(0,0,0);

	if(freeDebugParam5==0)
	{
		float rT = 20*sqrt(state_energy/9);
		if(rT<0) rT=0; if(rT>255)rT=255;
		cT = Vec3b(0,255-rT,rT);
	}
	else
	{
		if(state_state == ResState::IN) cT = Vec3b(255,0,0);
		else if(state_state == ResState::OOB) cT = Vec3b(255,255,0);
		else if(state_state == ResState::OUTLIER) cT = Vec3b(0,0,255);
		else cT = Vec3b(255,255,255);
	}

	for(int i=0;i<patternNum;i++)
	{
		if((projectedTo[i][0] > 2 && projectedTo[i][1] > 2 && projectedTo[i][0] < wG[0]-3 && projectedTo[i][1] < hG[0]-3 ))
			target->debugImage->setPixel1((float)projectedTo[i][0], (float)projectedTo[i][1],cT);
	}
}

void PointFrameResidual::applyRes(bool copyJacobians)
{
	if(copyJacobians)
	{
		if(state_state == ResState::OOB)
		{
			assert(!efResidual->isActiveAndIsGoodNEW);
			return;	// can never go back from OOB
		}
		if(state_NewState == ResState::IN)// && )
		{
			efResidual->isActiveAndIsGoodNEW=true;
			efResidual->takeDataF();
		}
		else
		{
			efResidual->isActiveAndIsGoodNEW=false;
		}
	}

	setState(state_NewState);
	state_energy = state_NewEnergy;
}
}


// Copyright (c) 2012-2013 The IXCoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

//
// Unit tests for canonical signatures
//

#include "script.h"
#include "util.h"
#include "data/sig_noncanonical.json.h"
#include "data/sig_canonical.json.h"

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>
#include "json/json_spirit_writer_template.h"
#include <openssl/ecdsa.h>

using namespace std;
using namespace json_spirit;

// In script_tests.cpp
extern Array read_json(const std::string& jsondata);

BOOST_AUTO_TEST_SUITE(canonical_tests)

// OpenSSL-based test for canonical signature (without test for hashtype byte)
bool static IsCanonicalSignature_OpenSSL_inner(const std::vector<unsigned char>& vchSig)
{
    if (vchSig.size() == 0)
        return false;
    const unsigned char *input = &vchSig[0];
    ECDSA_SIG *psig = NULL;
    d2i_ECDSA_SIG(&psig, &input, vchSig.size());
    if (psig == NULL)
        return false;
    unsigned char buf[256];
    unsigned char *pbuf = buf;
    unsigned int nLen = i2d_ECDSA_SIG(psig, NULL);
    if (nLen != vchSig.size()) {
        ECDSA_SIG_free(psig);
        return false;
    }
    nLen = i2d_ECDSA_SIG(psig, &pbuf);
    ECDSA_SIG_free(psig);
    return (memcmp(&vchSig[0], &buf[0], nLen) == 0);
}

// OpenSSL-based test for canonical signature
bool static IsCanonicalSignature_OpenSSL(const std::vector<unsigned char> &vchSignature) {
    if (vchSignature.size() < 1)
        return false;
    if (vchSignature.size() > 127)
        return false;
    if (vchSignature[vchSignature.size() - 1] & 0x7C)
        return false;

    std::vector<unsigned char> vchSig(vchSignature);
    vchSig.pop_back();
    if (!IsCanonicalSignature_OpenSSL_inner(vchSig))
        return false;
    return true;
}

BOOST_AUTO_TEST_CASE(script_canon)
{
    Array tests = read_json(std::string(json_tests::sig_canonical, json_tests::sig_canonical + sizeof(json_tests::sig_canonical)));

    BOOST_FOREACH(Value &tv, tests) {
        string test = tv.get_str();
        if (IsHex(test)) {
            std::vector<unsigned char> sig = ParseHex(test);
            BOOST_CHECK_MESSAGE(IsCanonicalSignature(sig, SCRIPT_VERIFY_STRICTENC), test);
            BOOST_CHECK_MESSAGE(IsCanonicalSignature_OpenSSL(sig), test);
        }
    }
}

BOOST_AUTO_TEST_CASE(script_noncanon)
{
    Array tests = read_json(std::string(json_tests::sig_noncanonical, json_tests::sig_noncanonical + sizeof(json_tests::sig_noncanonical)));

    BOOST_FOREACH(Value &tv, tests) {
        string test = tv.get_str();
        if (IsHex(test)) {
            std::vector<unsigned char> sig = ParseHex(test);
            BOOST_CHECK_MESSAGE(!IsCanonicalSignature(sig, SCRIPT_VERIFY_STRICTENC), test);
            BOOST_CHECK_MESSAGE(!IsCanonicalSignature_OpenSSL(sig), test);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/math/Unit.h>
#include <rl/mdl/Dynamic.h>
#include <rl/mdl/XmlFactory.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		rl::mdl::XmlFactory factory;
		std::shared_ptr<rl::mdl::Dynamic> dynamic = std::dynamic_pointer_cast<rl::mdl::Dynamic>(factory.create(argv[1]));

		rl::math::Vector q(dynamic->getDofPosition());
		rl::math::Vector qd(dynamic->getDof());
		rl::math::Vector qdd(dynamic->getDof());

		for (std::size_t i = 0; i < dynamic->getDofPosition(); ++i)
		{
			q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]);
		}

		for (std::size_t i = 0; i < dynamic->getDof(); ++i)
		{
			qd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2 + dynamic->getDofPosition()]);
			qdd(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2 + dynamic->getDofPosition() + dynamic->getDof()]);
		}

		std::cout << "===============================================================================" << std::endl;

		// forward position

		dynamic->setPosition(q);
		dynamic->forwardPosition();
		const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
		rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
		std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward velocity

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->forwardVelocity();
		std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// J

		rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
		dynamic->calculateJacobian(J, false);
		std::cout << "J = " << std::endl << J << std::endl;

		// J * qd

		rl::math::Vector Jqd = J * qd;
		std::cout << "xd = J * qd = " << Jqd.transpose() << std::endl;

		// J^{-1}

		rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianInverse(J, invJ);
		std::cout << "J^{-1} = " << std::endl << invJ << std::endl;

		// J^{-1} * xd

		rl::math::Vector invJxd = invJ * Jqd;
		std::cout << "qd = J^{-1} * xd = " << invJxd.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward acceleration

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->forwardVelocity();
		dynamic->forwardAcceleration();
		std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// Jd * qd

		rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
		dynamic->calculateJacobianDerivative(Jdqd, false);
		std::cout << "Jd * qd = " << Jdqd.transpose() << std::endl;

		// J * qdd + Jd * qd

		rl::math::Vector JqddJdqd = J * qdd + Jdqd;
		std::cout << "xdd = J * qdd + Jd * qd = " << JqddJdqd.transpose() << std::endl;

		// J^{-1} * (xdd - Jd * qd)

		rl::math::Vector invJxddJdqd = invJ * (JqddJdqd - Jdqd);
		std::cout << "qdd = J^{-1} * (xdd - Jd * qd) = " << invJxddJdqd.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// inverse dynamics

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setAcceleration(qdd);
		dynamic->inverseDynamics();
		std::cout << "tau = " << dynamic->getTorque().transpose() << std::endl;

		rl::math::Vector tau = dynamic->getTorque();

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// M

		rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrix(M);
		std::cout << "M = " << std::endl << M << std::endl;

		// V

		rl::math::Vector V(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->calculateCentrifugalCoriolis(V);
		std::cout << "V = " << V.transpose() << std::endl;

		// G

		rl::math::Vector G(dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateGravity(G);
		std::cout << "G = " << G.transpose() << std::endl;

		// M * qdd + V + G

		rl::math::Vector MqddVG = M * qdd + V + G;
		std::cout << "tau = M * qdd + V + G = " << MqddVG.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;

		// forward dynamics

		dynamic->setPosition(q);
		dynamic->setVelocity(qd);
		dynamic->setTorque(tau);
		dynamic->forwardDynamics();
		std::cout << "qdd = " << dynamic->getAcceleration().transpose() << std::endl;

		std::cout << "-------------------------------------------------------------------------------" << std::endl;

		// M^{-1}

		rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
		dynamic->setPosition(q);
		dynamic->calculateMassMatrixInverse(invM);
		std::cout << "M^{-1} = " << std::endl << invM << std::endl;

		// V

		std::cout << "V = " << V.transpose() << std::endl;

		// G

		std::cout << "G = " << G.transpose() << std::endl;

		// M^{-1} * (tau - V - G)

		rl::math::Vector invMtauVG = invM * (tau - V - G);
		std::cout << "qdd = M^{-1} * (tau - V - G) = " << invMtauVG.transpose() << std::endl;

		std::cout << "===============================================================================" << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}