//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================

#include <boost/config.hpp>
#include <iostream>
#include <string>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/graph_utility.hpp>

// Use a DIMACS network flow file as stdin.
// max_flow < max_flow.dat
//
// Sample output:
//  c  The total flow:
//  s 13
//
//  c flow values:
//  f 0 6 3
//  f 0 1 6
//  f 0 2 4
//  f 1 5 1
//  f 1 0 0
//  f 1 3 5
//  f 2 4 4
//  f 2 3 0
//  f 2 0 0
//  f 3 7 5
//  f 3 2 0
//  f 3 1 0
//  f 4 5 4
//  f 4 6 0
//  f 5 4 0
//  f 5 7 5
//  f 6 7 3
//  f 6 4 0
//  f 7 6 0
//  f 7 5 0

int
main()
{
  using namespace boost;

  typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
  typedef adjacency_list<listS, vecS, directedS,
    property<vertex_name_t, std::string>,
    property<edge_capacity_t, long,
      property<edge_residual_capacity_t, long,
        property<edge_reverse_t, Traits::edge_descriptor> > >
  > Graph;

  Graph g;

  property_map<Graph, edge_capacity_t>::type
    capacity = get(edge_capacity, g);
  property_map<Graph, edge_reverse_t>::type
    rev = get(edge_reverse, g);
  property_map<Graph, edge_residual_capacity_t>::type
    residual_capacity = get(edge_residual_capacity, g);

  Traits::vertex_descriptor s, t;
  read_dimacs_max_flow(g, capacity, rev, s, t);

  long flow;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // Use non-named parameter version
  property_map<Graph, vertex_index_t>::type
    indexmap = get(vertex_index, g);
  flow = push_relabel_max_flow(g, s, t, capacity, residual_capacity, rev, indexmap);
#else
  flow = push_relabel_max_flow(g, s, t);
#endif

  std::cout << "c  The total flow:" << std::endl;
  std::cout << "s " << flow << std::endl << std::endl;

  std::cout << "c flow values:" << std::endl;
  graph_traits<Graph>::vertex_iterator u_iter, u_end;
  graph_traits<Graph>::out_edge_iterator ei, e_end;
  for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
    for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
      if (capacity[*ei] > 0)
        std::cout << "f " << *u_iter << " " << target(*ei, g) << " "
                  << (capacity[*ei] - residual_capacity[*ei]) << std::endl;

  return 0;
}


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

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

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

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

/*! \file qle/models/infjyparameterization.hpp
    \brief Jarrow Yildrim inflation parameterization
*/

#ifndef quantext_inf_jy_parameterization_hpp
#define quantext_inf_jy_parameterization_hpp

#include <boost/shared_ptr.hpp>
#include <ql/indexes/inflationindex.hpp>
#include <ql/termstructures/inflationtermstructure.hpp>
#include <qle/models/fxbsparametrization.hpp>
#include <qle/models/irlgm1fparametrization.hpp>

namespace QuantExt {

class InfJyParameterization : public Parametrization {
public:
    InfJyParameterization(boost::shared_ptr<Lgm1fParametrization<QuantLib::ZeroInflationTermStructure>> realRate,
                          boost::shared_ptr<FxBsParametrization> index,
                          boost::shared_ptr<QuantLib::ZeroInflationIndex> inflationIndex);

    //! \name Parametrization interface
    //@{
    const QuantLib::Array& parameterTimes(const QuantLib::Size i) const override;
    const boost::shared_ptr<QuantLib::Parameter> parameter(const QuantLib::Size i) const override;
    void update() const override;
    //@}

    //! \name Inspectors
    //@{
    boost::shared_ptr<Lgm1fParametrization<QuantLib::ZeroInflationTermStructure>> realRate() const;
    boost::shared_ptr<FxBsParametrization> index() const;
    boost::shared_ptr<QuantLib::ZeroInflationIndex> inflationIndex() const;
    //@}

    Size numberOfParameters() const override { return 3; }

protected:
    //! \name Parametrization interface
    //@{
    QuantLib::Real direct(const QuantLib::Size i, const QuantLib::Real x) const override;
    QuantLib::Real inverse(const QuantLib::Size i, const QuantLib::Real y) const override;
    //@}

private:
    boost::shared_ptr<Lgm1fParametrization<QuantLib::ZeroInflationTermStructure>> realRate_;
    boost::shared_ptr<FxBsParametrization> index_;
    boost::shared_ptr<QuantLib::ZeroInflationIndex> inflationIndex_;

    //! Check that the indexing.
    void checkIndex(QuantLib::Size i) const;
};

} // namespace QuantExt

#endif


//ARGO: 2016-FEB-09 --> Including headers for I/O for VDW_ssurface grids
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/assign/std/vector.hpp>

#include "../interface/environment.h"
#include "../delphi/delphi_constants.h"
#include "../misc/misc_grid.h"
#include "../space/space.h"

#ifdef PARALLEL_OMP
#include <omp.h>
#endif

using namespace std;
using namespace boost::assign;

/*-------------------------ZETA ZETA ZETA ----------------------------
 A whole lot of changes have been made for the zetaSurface purposes.
 Look for if (zetaON == 1 ) conditions and statements when you
 need to put up a different file for zeta-calculation.
---------------------------------------------------------------------*/
void CDelphiSpace::setout()
{
    SGrid <delphi_real>  sqtemp[31],rad2aavtemp[31];
    SGrid <delphi_real> * sq=sqtemp+15;
    SGrid <delphi_real> * rad2aav=rad2aavtemp+15;
    SGrid <delphi_integer>* ioff;   ioff = NULL;

    bool itobig,itest2;
    string strtmp,strtmp1;

    //2011-05-12 Non-standard float variable, thus necessary
    //delphi_real modul,modul2, mod2,modx,mody;

    //2011-05-12 Non-standard type of variable, thus necessary
    //SGrid <delphi_real> xa,xb,xc,xd,xp;
    SGrid <delphi_real> ddist,dxyz,ddxyz,xn;
    //SGrid <delphi_real> tmpvect1,tmpvect2;
    SGrid <delphi_real> rad2av,fxn,vtemp;
    SGrid <delphi_integer> ismin,ismax,idist,ixyz,itest,ixn,i123;

    /*
     * here fRadPrb is not zero only if one wants to map the ext}//ed
     * surf. that has to be done with fRadPrb(1) if solute-solvent
     * interface is concerned imedia = medium number for a object
     * a non-zero entry in iEpsMap indicates an atom # plus 1 (to
     * properly treat re-entrant mid-points later (15 Aug 93)
     */
    delphi_integer iac,ibox,igrdc,i,iv,ix,iy,iz;
    delphi_integer limmax,lim;
    delphi_real dis2min2,dis2min1,distsq,dist,rad,rad2;
    delphi_real rad2a,radmax2,radp2,radtest,radprobe;
    delphi_real radp,temp;

    //argo : atom residue name
    string atom_residue;
    bool isProtein = false;
    vector<string> AA;
    AA += "ALA","ARG","ASN","ASP","ASX","CYS","GLU","GLN","GLX","GLY","HIS","HSE","HSD","ILE","LEU","LYS","MET","PHE","PRO","SER","THR","TRP","TYR","VAL";

    delphi_real extendedRad,extendedRad2;

    #ifdef VERBOSE
    cout << " Creating Van der Waals Epsilon Map: " << endl;
    #endif

    radprobe=0; //this radprobe seems not useful.

    //Parallel, we change this to:
    //epsdim=iNatom+iNObject+2; replaced in space module semi global

    radmax2=0.0;
    itest2=false;

    //ARGO 15-FEB,2016 -> Taken directly from site_writeSite_cube.cpp
    delphi_real coeff = 0.5291772108, stepsize = 1.0/fScale;
    SGrid<delphi_real> origin = (fgBoxCenter-stepsize*optCast<delphi_real,delphi_integer>(iGrid- delphi_integer(1))/2.0)/coeff;

    if (iNatom>0)
    {
        for(ix=1; ix<=iNatom; ix++)
            radmax2=max(radmax2,sDelPhiPDB[ix].radius);

        /*
         * this is probably the best way to do it,depending on which
         * surf. is desired
         */
        temp=max(radprobe,fExternRadius);

        //ARGO
        if ( zetaOn == 1 ) temp=max(temp,zetaDistance);

        #ifdef VERBOSE
        if ( temp == zetaDistance && zetaOn == 1)
        {
            cout << " The box size of individual atoms increased to Surface Distance" << endl;
            cout << " ... Expect Delays ... " << endl;
        }
        #endif

        radmax2=fScale*(radmax2+temp);

        lim=1+radmax2;
        limmax = 12;
        itobig=false;

        if(lim>limmax) itobig=true;
        igrdc=pow((2*lim+1),3);
        ioff = new SGrid <delphi_integer> [igrdc];

        if (!itobig)
        {
            radtest= pow( (radmax2 + 0.5*sqrt(3.0)),2 );
            ibox=-1;

            //2011-05-12 Strange statement. May allocate or may not
            //allocate array that used later in the program
            //irrespectively of itobig value, thus moved array
            //allocation before if condition

            #ifdef PARALLEL_OMP
            #pragma omp for schedule(auto)
            #endif

            for(ix=-lim; ix<=lim; ix++)
            {
                for(iy=-lim; iy<=lim; iy++)
                {
                    for(iz=-lim; iz<=lim; iz++)
                    {
                        //2011-05-13 Replaced by faster operation
                        //2011-05-13 Using operations on coord and
                        //int_coord type variables defined in module
                        //operators_on_coordinates
                        idist=int_coord(ix,iy,iz);
                        dist=float ( optDot(idist,idist) );
                        ddist = dist + float(0.25) + optCast <delphi_real,delphi_integer> (idist);

                        if ((dist<radtest)|| optORLT( ddist,radtest ))
                        {
                            ibox++;
                            ioff[ibox]=idist;
                        }
                    }
                }
            }
        }
    }

    /*
     * set interiors in MOLECULES
     */
    //ARGO: 2016-FEB-09: modified lines here
    //ORIGINIAL: 	if(itest2||itobig) cout <<"setout method 1 " << itest2 << " " << itobig << endl;

    #ifdef VERBOSE
    if(itest2||itobig) cout <<" setout method 1 : itest2 = " << itest2 << " and itobig = " << itobig << endl;
    #endif

    //DoATOMS:
    for( iv=1; iv<=iNatom; iv++)
    {
        /**
         * restore values
         */
        rad= sDelPhiPDB[iv].radius;

        //ARGO: also fetch the resname of the atoms and check if its a protein residue
        atom_residue = sDelPhiPDB[iv].atom_resname;
        isProtein = std::find(AA.begin(), AA.end(), atom_residue) != AA.end();
        isProtein = true; //for the present version 7.0 | Will remove it later

        xn=xn2[iv];

        //2011-05-13 Removed GOTO statement
        if (rad<1.e-6)
            continue;

        //fScale radius to grid
        rad=rad*fScale;
        //ARGO:
        extendedRad = rad + zetaDistance*fScale; //ARGO -> *fscale with zetaDistance to go with other radial distance parameters (e.g. rad,radp,etc.)

        radp=rad+fExternRadius*fScale;
        rad=rad+radprobe*fScale;

        //ARGO
        extendedRad2 = extendedRad*extendedRad;
        rad2=rad*rad;
        radp2=radp*radp;

        /*
         * set dielectric map
         * check if sphere sits within limits of box
         */
        itest2=false;

        ismin=optCast <delphi_integer,delphi_real> (xn-radmax2-1.0);
        ismax=optCast <delphi_integer,delphi_real> (xn+radmax2+1.0);
        itest=ismin;
        ismin=optMin(ismin,iGrid);
        ismin=optMax(ismin, delphi_integer(1));
        if(itest!=ismin) itest2=true;
        itest=ismax;
        ismax=optMin(ismax,iGrid);
        ismax=optMax(ismax, delphi_integer(1));
        if(itest != ismax) itest2=true;

        if (itest2||itobig)   //slow method;
        {
            rad2a = rad2 - 0.25;

            for(iz=ismin.nZ; iz<=ismax.nZ; iz++)
            {
                for(iy=ismin.nY; iy<=ismax.nY; iy++)
                {
                    for(ix=ismin.nX; ix<=ismax.nX; ix++)
                    {
                        ixyz=int_coord(ix,iy,iz);
                        dxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn;
                        distsq=optDot(dxyz,dxyz);
                        dxyz=dxyz+distsq;

                        if (dxyz.nX<rad2a)
                            iepsmp[iz][iy][ix].nX=iv+1+iAtomMed[iv]*epsdim;

                        if (dxyz.nY<rad2a)
                            iepsmp[iz][iy][ix].nY=iv+1+iAtomMed[iv]*epsdim;

                        if (dxyz.nZ<rad2a)
                            iepsmp[iz][iy][ix].nZ=iv+1+iAtomMed[iv]*epsdim;

                        if(distsq<radp2) idebmap[iz][iy][ix] =false;

                        //ARGO: 2016-FEB-09 : if zeta is 'on' then check for that too
                        if (zetaOn == 1 && isProtein)
                            if ( distsq<extendedRad2 ) zetaSurfMap[iz][iy][ix] =false;
                    }
                }
            }
        }
        else  /**faster method;*/
        {
            //IT HAS PROBLEMS!!!! Walter (be careful before using also in multidilectric case!!!&&!isitmd
            //cout << "####faster method:" << endl;
            rad2a=rad2-0.25;

            ixn=optRound(xn);

            fxn=optCast <delphi_real,delphi_integer> (ixn)-xn;
            rad2av=rad2a-fxn;

            for(ix=-lim; ix<=lim; ix++)
            {
                vtemp= double(ix)+fxn;
                sqtemp[ix+15]=vtemp*vtemp;
                rad2aavtemp[ix+15]=rad2a-vtemp;

            }

            if (iNMedia>1&&bOnlyMol)
            {
                for(i=0; i<=ibox; i++)
                {
                    i123=ioff[i];
                    ixyz=ixn+i123;
                    ix=ixyz.nX;
                    iy=ixyz.nY;
                    iz=ixyz.nZ;
                    distsq = sqtemp[i123.nX+15].nX +sqtemp[i123.nY+15].nY + sqtemp[i123.nZ+15].nZ;

                    if (distsq<rad2aavtemp[i123.nX+15].nX)
                    {
                        iac=(iepsmp[ix][iy][iz].nX % epsdim)-1;

                        if (iac==-1||iac>iNatom)
                        {
                            iepsmp[ix][iy][iz].nX=iv+1+iAtomMed[iv]*epsdim;
                        }
                        else
                        {
                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn;
                            ddxyz.nX= ddxyz.nX+0.5;
                            dis2min1=optDot(ddxyz,ddxyz)-rad2;

                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn2[iac];
                            ddxyz.nX= ddxyz.nX+0.5;
                            dis2min2=optDot(ddxyz,ddxyz)-pow((sDelPhiPDB[iac].radius*fScale),2);

                            if (dis2min2>dis2min1) iac=iv;
                            iepsmp[ix][iy][iz].nX=iac+1+iAtomMed[iac]*epsdim;
                        }
                    }

                    if (distsq<rad2aavtemp[i123.nY+15].nY)
                    {
                        iac=(iepsmp[ix][iy][iz].nY % epsdim)-1;
                        if (iac==-1||iac>iNatom)
                        {
                            iepsmp[ix][iy][iz].nY=iv+1+iAtomMed[iv]*epsdim;
                        }
                        else
                        {
                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn;
                            ddxyz.nY= ddxyz.nY+0.5;
                            dis2min1=optDot(ddxyz,ddxyz)-rad2;

                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn2[iac];
                            ddxyz.nY= ddxyz.nY+0.5;
                            dis2min2=optDot(ddxyz,ddxyz)-pow((sDelPhiPDB[iac].radius*fScale),2);

                            if (dis2min2>dis2min1) iac=iv;
                            iepsmp[ix][iy][iz].nY=iac+1+iAtomMed[iac]*epsdim;
                        }
                    }

                    if (distsq<rad2aavtemp[i123.nZ+15].nZ)
                    {
                        iac=(iepsmp[ix][iy][iz].nZ%epsdim)-1;
                        if (iac==-1||iac>iNatom)
                        {
                            iepsmp[ix][iy][iz].nZ=iv+1+iAtomMed[iv]*epsdim;
                        }
                        else
                        {

                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn;
                            ddxyz.nZ= ddxyz.nZ+0.5;
                            dis2min1=optDot(ddxyz,ddxyz)-rad2;

                            ddxyz=optCast <delphi_real,delphi_integer> (ixyz)-xn2[iac];
                            ddxyz.nZ=ddxyz.nZ+0.5;
                            dis2min2=optDot(ddxyz,ddxyz)-pow((sDelPhiPDB[iac].radius*fScale),2);

                            if (dis2min2>dis2min1) iac=iv;
                            iepsmp[ix][iy][iz].nZ=iac+1+iAtomMed[iac]*epsdim;
                        }
                    }

                    if(distsq<radp2) idebmap[ix][iy][iz]=false;

                    //ARGO: 2016-FEB-09 : if zeta is 'on' then check for that too
                    if (zetaOn == 1 && isProtein)
                        if ( distsq<extendedRad2 ) zetaSurfMap[ix][iy][iz] =false;
                }
            }
            else
            {

                #ifdef PARALLEL_OMP
                #pragma omp for schedule(auto)
                #endif

                for(i=0; i<=ibox; i++)
                {
                    i123=ioff[i];
                    ixyz=ixn+i123;
                    ix=ixyz.nX;
                    iy=ixyz.nY;
                    iz=ixyz.nZ;
                    distsq = sq[i123.nX].nX +sq[i123.nY].nY + sq[i123.nZ].nZ;

                    if (distsq<rad2aav[i123.nX].nX)
                        //Lin Li: all indexes -1, because of C++ arrays' indexes start from 0;
                        //Lin Li: because iv start from 0, so +2:
                        iepsmp[iz][iy][ix].nX=iv+1+iAtomMed[iv]*epsdim;

                    if (distsq<rad2aav[i123.nY].nY)
                        iepsmp[iz][iy][ix].nY=iv+1+iAtomMed[iv]*epsdim;

                    if (distsq<rad2aav[i123.nZ].nZ)
                        iepsmp[iz][iy][ix].nZ=iv+1+iAtomMed[iv]*epsdim;

                    if (distsq<radp2) idebmap[iz][iy][ix]=false;

                    //ARGO: 2016-FEB-09 : if zeta is 'on' then check for that too
                    if (zetaOn == 1 && isProtein)
                        if ( distsq<extendedRad2 )
                            zetaSurfMap[iz][iy][ix]=false;
                }
            }
        }
    }

    //ARGO
    if (ioff != NULL) delete [] ioff;

    #ifdef VERBOSE
    cout << " Not Testing iepsmp and idebmap xxx " << endl;
    cout <<" Ending creating Van der Waals Epsilon Map " << endl;
    #endif

    return;

}// void setout;


/*
Copyright 2021 Glen Joseph Fernandes
(glenjofe@gmail.com)

Distributed under the Boost Software License, Version 1.0.
(http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_UNITS_TEST_CLOSE_HPP
#define BOOST_UNITS_TEST_CLOSE_HPP

#include <boost/core/lightweight_test.hpp>
#include <algorithm>
#include <limits>
#include <cmath>

/*
Provide a predicate for BOOST_TEST_WITH that is equivalent to
what was provided by the previous test framework.
*/
class close_to {
public:
    explicit close_to(double f)
        : f_(f) { }

    bool operator()(double l, double r) const {
        return std::abs(l - r) <=
            (std::max)(f_ * (std::max)(std::abs(l), std::abs(r)), 0.);
    }

private:
    double f_;
};

#define BOOST_UNITS_TEST_CLOSE(l,r,f) BOOST_TEST_WITH((l),(r),close_to((f)))

#endif


#include "dakota_global_defs.hpp"
#include "DakotaResponse.hpp"
#include "DistributionParams.hpp"
#include "BasisPolynomial.hpp"
//#include "SharedPolyApproxData.hpp"
#include "nested_sampling.hpp"
#include "SharedOrthogPolyApproxData.hpp"
// Boost.Test
#include <boost/test/minimal.hpp>

//#include <boost/assign/std/vector.hpp>
#include <boost/foreach.hpp>

#include <cassert>
#include <iostream>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/negative_binomial_distribution.hpp>

namespace Dakota {
namespace TestLejaSampling {

/** \brief Create an AleatoryDistParams object such that all variables are
 * uniform in [l_bnd, u_bnd]

  Note this function sets specific distribution parameters
  that are not general. They must coincide with the parameters used
  in generate_samples

 */
Pecos::AleatoryDistParams
initialize_homgeneous_uniform_aleatory_dist_params( short utype,
						    int num_vars ){

  RealVector nuv_means;
  RealVector nuv_std_devs;     RealVector nuv_l_bnds;
  RealVector nuv_u_bnds;       RealVector lnuv_means;
  RealVector lnuv_std_devs;    RealVector lnuv_lambdas;
  RealVector lnuv_zetas;       RealVector lnuv_err_facts;
  RealVector lnuv_l_bnds;      RealVector lnuv_u_bnds;
  RealVector uuv_l_bnds;       RealVector uuv_u_bnds;
  RealVector luuv_l_bnds;      RealVector luuv_u_bnds;
  RealVector tuv_modes;        RealVector tuv_l_bnds;
  RealVector tuv_u_bnds;       RealVector euv_betas;
  RealVector beuv_alphas;      RealVector beuv_betas;
  RealVector beuv_l_bnds;      RealVector beuv_u_bnds;
  RealVector gauv_alphas;      RealVector gauv_betas;
  RealVector guuv_alphas;      RealVector guuv_betas;
  RealVector fuv_alphas;       RealVector fuv_betas;
  RealVector wuv_alphas;       RealVector wuv_betas;
  RealRealMapArray hbuv_prs;   RealVector puv_lambdas;
  RealVector biuv_p_per_tr;    IntVector biuv_num_trials;
  RealVector nbuv_p_per_tr;    IntVector nbuv_num_trials;
  RealVector geuv_p_per_tr;    IntVector hguv_tot_pop;
  IntVector hguv_sel_pop;      IntVector hguv_num_drawn;
  IntRealMapArray hpuiv_prs;   StringRealMapArray hpusv_prs;
  RealRealMapArray hpurv_prs;  RealSymMatrix uv_corr;

  switch (utype){
  case Pecos::STD_UNIFORM:
    uuv_l_bnds.sizeUninitialized( num_vars );
    uuv_u_bnds.sizeUninitialized( num_vars );
    uuv_l_bnds = -1; uuv_u_bnds = 1;
    break;
  case Pecos::POISSON:
    puv_lambdas.sizeUninitialized( num_vars );
    puv_lambdas = 5;
    break;
  case Pecos::BINOMIAL:
    biuv_p_per_tr.sizeUninitialized( num_vars );
    biuv_num_trials.sizeUninitialized( num_vars );
    biuv_p_per_tr = 0.5;
    biuv_num_trials = 20;
    break;
  case Pecos::NEGATIVE_BINOMIAL:
    nbuv_p_per_tr.sizeUninitialized( num_vars );
    nbuv_num_trials.sizeUninitialized( num_vars );
    nbuv_p_per_tr = 0.5;
    nbuv_num_trials = 20;
    break;
  }
  Pecos::AleatoryDistParams adp(nuv_means,
				nuv_std_devs,     nuv_l_bnds,
				nuv_u_bnds,       lnuv_means,
				lnuv_std_devs,    lnuv_lambdas,
				lnuv_zetas,       lnuv_err_facts,
				lnuv_l_bnds,      lnuv_u_bnds,
				uuv_l_bnds,       uuv_u_bnds,
				luuv_l_bnds,      luuv_u_bnds,
				tuv_modes,        tuv_l_bnds,
				tuv_u_bnds,       euv_betas,
				beuv_alphas,      beuv_betas,
				beuv_l_bnds,      beuv_u_bnds,
				gauv_alphas,      gauv_betas,
				guuv_alphas,      guuv_betas,
				fuv_alphas,       fuv_betas,
				wuv_alphas,       wuv_betas,
				hbuv_prs,         puv_lambdas,
				biuv_p_per_tr,    biuv_num_trials,
				nbuv_p_per_tr,    nbuv_num_trials,
				geuv_p_per_tr,    hguv_tot_pop,
				hguv_sel_pop,     hguv_num_drawn,
				hpuiv_prs,        hpusv_prs,
				hpurv_prs,  uv_corr);
  return adp;
  }

  template <typename T>
  void generate_samples( T& rvt, int num_vars, int num_samples,
			 RealMatrix &samples ){
    samples.shapeUninitialized( num_vars, num_samples );
    for(int i=0; i<num_vars; i++)
      for(int j=0; j<num_samples; j++)
	samples(i,j) = rvt();
  }

  template void generate_samples(boost::variate_generator< boost::mt19937,
				 boost::random::uniform_real_distribution<Real> > &rvt,
				 int num_vars, int num_samples,
				 RealMatrix &samples);
  template void generate_samples(boost::variate_generator< boost::mt19937,
				 boost::random::poisson_distribution<> > &rvt,
				 int num_vars, int num_samples,
				 RealMatrix &samples);
  template void generate_samples(boost::variate_generator< boost::mt19937,
				 boost::random::binomial_distribution<> > &rvt,
				 int num_vars, int num_samples,
				 RealMatrix &samples);
  template void generate_samples(boost::variate_generator< boost::mt19937,
				 boost::random::negative_binomial_distribution<> > &rvt,
				 int num_vars, int num_samples,
				 RealMatrix &samples);

  /**  Note this function sets specific distribution parameters
       that are not general. They must coincide with the parameters used
       in initialize_homgeneous_uniform_aleatory_dist_params
  */
void generate_samples(short utype, int num_vars, int num_candidate_samples,
		      int seed, RealMatrix &candidate_samples){
    boost::mt19937 gen;
    gen.seed(seed);

    switch (utype){
    case Pecos::STD_UNIFORM:
      {
	boost::random::uniform_real_distribution<Real> un_dist(-1.,1.);
	boost::variate_generator< boost::mt19937,
	  boost::random::uniform_real_distribution<Real> > un_rvt(gen, un_dist);
	generate_samples( un_rvt, num_vars, num_candidate_samples,
			  candidate_samples );
	break;
      }
    case Pecos::POISSON:
      {
	Real poisson_lambda = 5;
	boost::poisson_distribution<> po_dist(poisson_lambda);
	boost::variate_generator< boost::mt19937,
	  boost::poisson_distribution<> > po_rvt(gen, po_dist);
	generate_samples( po_rvt, num_vars, num_candidate_samples,
			  candidate_samples );
      break;
    }
  case Pecos::BINOMIAL:
    {
      int n=20;
      Real p=0.5;
      boost::random::binomial_distribution<> bi_dist(n,p);
      boost::variate_generator<boost::mt19937,
	boost::random::binomial_distribution<> > bi_rvt(gen, bi_dist);
      generate_samples( bi_rvt, num_vars, num_candidate_samples,
			candidate_samples );
      break;
    }
  case Pecos::NEGATIVE_BINOMIAL:
    {
      int n=20;
      Real p=0.5;
      boost::random::negative_binomial_distribution<> nbi_dist(n,p);
      boost::variate_generator<boost::mt19937,
	boost::random::negative_binomial_distribution<> >
	nbi_rvt(gen, nbi_dist);
      generate_samples( nbi_rvt, num_vars, num_candidate_samples,
			candidate_samples );
    }
    break;
  }
}

void test_leja_sequence_helper(short utype, int num_vars,
			       int num_initial_samples, int num_new_samples,
			       const RealMatrix &candidate_samples){

  // Currently Leja Sampler assumes that the samples are in U space
  // that is the native space of the polynomial. For Legendre polynomials
  // the parameter range is [-1,1]
  Pecos::AleatoryDistParams adp =
    initialize_homgeneous_uniform_aleatory_dist_params(utype, num_vars);
  ShortArray u_types(num_vars,utype);

  // Build polynomial basis using default basis configuration options
  Pecos::BasisConfigOptions bc_options;
  std::vector<Pecos::BasisPolynomial> poly_basis;
  Pecos::SharedOrthogPolyApproxData::construct_basis(u_types, adp, bc_options,
						     poly_basis);

  // Initialize sampling object
  LejaSampler sampler;
  sampler.set_precondition(true);
  sampler.set_polynomial_basis(poly_basis);

  // Generate initial samples
  RealMatrix initial_samples, empty_matrix;
  // The test only works if we use the same degree at every step.
  int num_total_samples = num_initial_samples + num_new_samples;
  sampler.set_total_degree_basis_from_num_samples(num_vars, num_total_samples);
  // First time sampler is called there are no samples we wish to reuse
  // so pass in empty matrix
  sampler.Sampler::enrich_samples( num_vars, empty_matrix, num_initial_samples,
				   candidate_samples, initial_samples );

  // Enrich initial_samples
  RealMatrix enriched_samples;
  sampler.set_total_degree_basis_from_num_samples(num_vars, num_total_samples);
  sampler.Sampler::enrich_samples( num_vars, initial_samples, num_new_samples,
				   candidate_samples, enriched_samples );

  RealMatrix enriched_samples_subset( Teuchos::Copy, enriched_samples, num_vars,
				      num_initial_samples );
  enriched_samples_subset -= initial_samples;
  BOOST_CHECK( enriched_samples_subset.normInf() <
	       10.*std::numeric_limits<double>::epsilon() );

  // Run sampler and generate all points in one go
  RealMatrix combined_samples;
  sampler.set_total_degree_basis_from_num_samples(num_vars, num_total_samples);
  sampler.Sampler::enrich_samples( num_vars, empty_matrix, num_total_samples,
				   candidate_samples, combined_samples );

  // Check samples obtained by running algorithm once are the same
  // as those obtained running the algorithm in steps.
  combined_samples -= enriched_samples;
  BOOST_CHECK( combined_samples.normInf() <
	       10.*std::numeric_limits<double>::epsilon() );
}

void test_uniform_leja_sequence(){
  int num_vars = 2;
  int num_initial_samples = 10;
  int num_new_samples = 20;
  int num_candidate_samples = 10000;
  int seed = 1;

  int num_utype_tests = 2;
  short utypes[] = { Pecos::STD_UNIFORM, Pecos::POISSON };
  RealMatrix candidate_samples;

  for (int i=0; i< num_utype_tests; i++){
    generate_samples( utypes[i], num_vars, num_candidate_samples, seed,
		      candidate_samples );

    test_leja_sequence_helper(utypes[i], num_vars, num_initial_samples,
			      num_new_samples, candidate_samples );
  }
}

} // end namespace TestFieldCovariance
} // end namespace Dakota

// NOTE: Boost.Test framework provides the main progran driver

//____________________________________________________________________________//

int test_main( int argc, char* argv[] )      // note the name!
{
  using namespace Dakota::TestLejaSampling;

  test_uniform_leja_sequence();

  int run_result = 0;
  BOOST_CHECK( run_result == 0 || run_result == boost::exit_success );

  return boost::exit_success;
}


#define ADEPT_STORAGE_THREAD_SAFE

#include <algorithm>
#include <cstdio>
#include <numeric>
#include <array>
#include <thread>
#include <future>
#include <vector>
#include <list>
#include <nvector/nvector_serial.h>  /* serial N_Vector types, fcts., macros */
#include <cvode/cvode.h>            /* prototypes for CVODE fcts., consts. */
#include <string>
#include <sundials/sundials_dense.h>
#include <sunmatrix/sunmatrix_dense.h>
#include <sunlinsol/sunlinsol_dense.h>
#include <cvodes/cvodes.h>             /* prototypes for CVODE fcts., consts.  */
#include <cvode/cvode_direct.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "model.hpp"
#include <adept.h>
#include "reaction.hpp"

using std::array;
using std::copy;
using std::vector;
using std::fill;
using std::string;
using std::endl;
using std::cout;
using adept::adouble;

constexpr double solveTol = 1.0E-8;

static void errorHandler(int, const char *, const char *, char *, void *);
int Jac(double, N_Vector, N_Vector, SUNMatrix, void *, N_Vector, N_Vector, N_Vector);
int JacB(double, N_Vector, N_Vector, N_Vector, SUNMatrix, void *, N_Vector, N_Vector, N_Vector);
int fullModelCVode (const double, const N_Vector, N_Vector, void *);
static int fB(double, N_Vector y, N_Vector yB, N_Vector yBdot, void *user_dataB);
static int fQB(double, N_Vector y, N_Vector yB, N_Vector qBdot, void *user_dataB);

std::mutex print_mutex; // mutex to prevent threads printing on top of each other

typedef Eigen::Matrix<double, Nspecies, Eigen::Dynamic> x0JacM;

extern "C" void fullModel_C(const double * const y_in, double, double *dydt_out, double *rxn_in) {
	std::vector<double> v(rxn_in, rxn_in + Nparams);
	ratesS<double> r(v);

	fullModel(y_in, &r, dydt_out);
}

constexpr bool debug = false;

class solver {
public:
	void *cvode_mem;
	SUNLinearSolver LS;
	N_Vector state, qB, yB;
	SUNMatrix A;
	bool sensi;
	int ncheck, indexB;
	double tret;
	vector<double> params;
	array<double, Nspecies> activities;
	adept::Stack stack;
	double preT;
	const double *preL;

	void commonSetup(vector<double> paramsIn, const double preTin, const double * const preLin) {
		tret = 0.0;
		params = paramsIn;
		preT = preTin;
		preL = preLin;

		// Setup state variable by solving for autocrine
		ratesS<double> rattes(params);
		array<double, Nspecies> y0 = solveAutocrine(&rattes);
		state = N_VNew_Serial(static_cast<long>(Nspecies));
		std::copy_n(y0.data(), Nspecies, NV_DATA_S(state));

		/* Call CVodeCreate to create the solver memory and specify the
		 * Backward Differentiation Formula and the use of a Newton iteration */
		cvode_mem = CVodeCreate(CV_BDF);
		if (cvode_mem == nullptr) {
			throw std::runtime_error(string("Error calling CVodeCreate in solver_setup."));
		}

		CVodeSetErrHandlerFn(cvode_mem, &errorHandler, static_cast<void *>(this));

		// Pass along the parameter structure to the differential equations
		if (CVodeSetUserData(cvode_mem, static_cast<void *>(this)) < 0) {
			throw std::runtime_error(string("Error calling CVodeSetUserData in solver_setup."));
		}

		// Initialize the integrator rhs function in y'=f(t,y), the inital T0, & the initial dependent y
		if (CVodeInit(cvode_mem, fullModelCVode, 0.0, state) < 0) {
			throw std::runtime_error(string("Error calling CVodeInit in solver_setup."));
		}

		// Set the scalar relative and absolute tolerances
		if (CVodeSStolerances(cvode_mem, 1.0E-12, 1.0E-10) < 0) {
			throw std::runtime_error(string("Error calling CVodeSStolerances in solver_setup."));
		}

		A = SUNDenseMatrix(Nspecies, Nspecies);
		LS = SUNDenseLinearSolver(state, A);

		// Call CVDense to specify the CVDENSE dense linear solver
		if (CVDlsSetLinearSolver(cvode_mem, LS, A) < 0) {
			throw std::runtime_error(string("Error calling CVDlsSetLinearSolver in solver_setup."));
		}

		CVDlsSetJacFn(cvode_mem, Jac);

		CVodeSetMaxNumSteps(cvode_mem, 100000);

		// Call CVodeSetConstraints to initialize constraints
		N_Vector constraints = N_VNew_Serial(static_cast<long>(Nspecies));
		N_VConst(1.0, constraints); // all 1's for nonnegative solution values
		if (CVodeSetConstraints(cvode_mem, constraints) < 0) {
			throw std::runtime_error(string("Error calling CVodeSetConstraints in solver_setup."));
		}
		N_VDestroy(constraints);
	}

	solver(vector<double> paramsIn, const double preTin, const double * const preLin) {
		sensi = false;
		commonSetup(paramsIn, preTin, preLin);
	}

	solver(vector<double> paramsIn, array<double, Nspecies> actIn, const double preTin, const double * const preLin) {
		sensi = true;
		std::copy(actIn.begin(), actIn.end(), activities.begin());
		commonSetup(paramsIn, preTin, preLin);

		// CVodeAdjInit to update CVODES memory block by allocting the internal memory needed for backward integration
		// Also setting number of forward solution checkpoints to retain
		if (CVodeAdjInit(cvode_mem, 5000, CV_HERMITE) < 0) {
			throw std::runtime_error(string("Error calling CVodeAdjInit in solver_setup."));
		}
	}

	void backward (double TB1) {
		indexB = 1;
		yB = N_VNew_Serial(Nspecies); // Initialize yB
		qB = N_VNew_Serial(params.size()); // Initialize qB
		std::copy_n(activities.begin(), Nspecies, NV_DATA_S(yB));
		N_VConst(0.0, qB);

		// CVodeCreateB to specify the solution method for the backward problem
		if (CVodeCreateB(cvode_mem, CV_BDF, &indexB) < 0)
			throw std::runtime_error(string("Error calling CVodeCreateB in solver_setup."));

		// Call CVodeInitB to allocate internal memory and initialize the backward problem
		if (CVodeInitB(cvode_mem, indexB, fB, TB1, yB) < 0)
			throw std::runtime_error(string("Error calling CVodeInitB in solver_setup."));

		// Set the scalar relative and absolute tolerances
		if (CVodeSStolerancesB(cvode_mem, indexB, solveTol, solveTol) < 0)
			throw std::runtime_error(string("Error calling CVodeSStolerancesB in solver_setup."));

		// Attach the user data for backward problem
		if (CVodeSetUserDataB(cvode_mem, indexB, static_cast<void *>(this)) < 0)
			throw std::runtime_error(string("Error calling CVodeSetUserDataB in solver_setup."));

		// Call CVDense to specify the CVDENSE dense linear solver
		if (CVodeSetLinearSolverB(cvode_mem, indexB, LS, A) < 0) {
			throw std::runtime_error(string("Error calling CVodeSetLinearSolverB in solver_setup."));
		}

		// Set the user-supplied Jacobian routine JacB
		if (CVodeSetJacFnB(cvode_mem, indexB, JacB) < 0) {
			throw std::runtime_error(string("Error calling CVodeSetJacFnB in solver_setup."));
		}

		// Allocate internal memory and initialize backward quadrature integration
		if (CVodeQuadInitB(cvode_mem, indexB, fQB, qB) < 0) {
			throw std::runtime_error(string("Error calling CVodeQuadInitB in solver_setup."));
		}

		// Whether or not the quadrature variables are to be used in the step size control
		if (CVodeSetQuadErrConB(cvode_mem, indexB, true) < 0) {
			throw std::runtime_error(string("Error calling CVodeSetQuadErrConB in solver_setup."));
		}

		// Specify the scalar relative and absolute tolerances for the backward problem
		if (CVodeQuadSStolerancesB(cvode_mem, indexB, solveTol, solveTol) < 0) {
			throw std::runtime_error(string("Error calling CVodeQuadSStolerancesB in solver_setup."));
		}

		CVodeSetMaxNumStepsB(cvode_mem, indexB, 10000);
	}

	int CVodeRun(const double endT) {
		int returnVal;

		if (endT < tret) {
			cout << "Can't go backwards in forward pass." << std::endl;
			return -1;
		}

		if (sensi) {
			returnVal = CVodeF(cvode_mem, endT, state, &tret, CV_NORMAL, &ncheck);
		} else {
			returnVal = CVode(cvode_mem, endT, state, &tret, CV_NORMAL);
		}

		if (returnVal >= 0 && debug) {
			long nst;
			CVodeGetNumSteps(cvode_mem, &nst);
			cout << "Number of steps: " << nst << std::endl;
			cout << "Final time: " << tret << std::endl;
		}

		if (returnVal < 0) cout << "CVode error in CVode. Code: " << returnVal << std::endl;

		return returnVal;
	}

	ratesS<double> getRates() {
		return ratesS<double>(params);
	}

	double getActivity() {
		return std::inner_product(activities.begin(), activities.end(), NV_DATA_S(state), 0.0);
	}

	int getAdjSens(const double t0B, double *Sout, x0JacM &x0p) {
		Eigen::Map<Eigen::Matrix<double, Nspecies, 1>> St0(NV_DATA_S(yB), Nspecies);
		Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>> Sqt0(NV_DATA_S(qB), params.size());
		Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>> SoutV(Sout, params.size());
		std::copy_n(activities.begin(), Nspecies, NV_DATA_S(yB));
		N_VConst(0.0, qB);

		if (t0B > std::numeric_limits<double>::epsilon()) {
			if (CVodeReInitB(cvode_mem, indexB, t0B, yB) < 0)
				cout << "CVodeReInitB error at 0." << std::endl;

			if (CVodeQuadReInitB(cvode_mem, indexB, qB) < 0)
				cout << "CVodeQuadReInitB error at 0." << std::endl;

			if (CVodeB(cvode_mem, 0.0, CV_NORMAL) < 0)
				cout << "CVodeB error at 0, while integrating back from " << t0B << "." << std::endl;

			CVodeGetB(cvode_mem, indexB, &tret, yB);
			CVodeGetQuadB(cvode_mem, indexB, &tret, qB);
		}

		SoutV = St0.transpose()*x0p - Sqt0.transpose();

		return 0;
	}

	~solver() {
		if (sensi) {
			CVodeSensFree(cvode_mem);
			N_VDestroy_Serial(qB);
			N_VDestroy_Serial(yB);
		}

		N_VDestroy_Serial(state);
		CVodeFree(&cvode_mem);
		SUNLinSolFree(LS);
		SUNMatDestroy(A);
	}
};

void preTreat(double tDiff, std::array<double, Nlig> &ILs, const double *preL) {
	tDiff = fabs(60*tDiff); // Scale this to be on the order of seconds

	for (size_t ii = 0; ii < Nlig; ii++) {
		ILs[ii] = preL[ii] + (ILs[ii] - preL[ii])*exp(-tDiff);
	}
}

// fB routine. Compute fB(t,y,yB).
static int fB(double t, N_Vector y, N_Vector yB, N_Vector yBdot, void *user_data) {
	solver *sMem = static_cast<solver *>(user_data);
	ratesS<double> rattes = sMem->getRates();

	if (t < sMem->preT)
		preTreat(t - sMem->preT, rattes.ILs, sMem->preL);

	std::array<adept::adouble, Nspecies> ya, dydt;

	adept::set_values(&ya[0], Nspecies, NV_DATA_S(y));

	sMem->stack.new_recording();

	// Get the data in the right form
	fullModel(ya.data(), &rattes, dydt.data());

	adouble yOut = 0;
	yOut = -std::inner_product(dydt.begin(), dydt.end(), NV_DATA_S(yB), yOut);

	sMem->stack.independent(&ya[0], Nspecies);
	sMem->stack.dependent(&yOut, 1);

	sMem->stack.jacobian(NV_DATA_S(yBdot));

	return 0;
}

int Jac(double t, N_Vector yv, N_Vector, SUNMatrix J, void *user_data, N_Vector, N_Vector, N_Vector) {
	solver *sMem = static_cast<solver *>(user_data);
	ratesS<double> rattes = sMem->getRates();

	if (t < sMem->preT)
		preTreat(t - sMem->preT, rattes.ILs, sMem->preL);

	// Actually get the Jacobian
	std::array<adept::adouble, Nspecies> y, dydt;

	adept::set_values(&y[0], Nspecies, NV_DATA_S(yv));

	sMem->stack.new_recording();

	// Get the data in the right form
	fullModel(y.data(), &rattes, dydt.data());

	sMem->stack.independent(&y[0], Nspecies);
	sMem->stack.dependent(&dydt[0], Nspecies);

	sMem->stack.jacobian(SM_DATA_D(J));

	return 0;
}

int JacB(double t, N_Vector y, N_Vector a, N_Vector b, SUNMatrix J, void *user_data, N_Vector c, N_Vector d, N_Vector e) {
	Jac(t, y, a, J, user_data, c, d, e);

	Eigen::Map<Eigen::Matrix<double, Nspecies, Nspecies>> jac(SM_DATA_D(J));

	jac = -jac;
	jac.transposeInPlace();

	return 0;
}

// fQB routine. Compute integrand for quadratures
static int fQB(double t, N_Vector y, N_Vector yB, N_Vector qBdot, void *user_dataB) {
	solver *sMem = static_cast<solver *>(user_dataB);
	const size_t Np = sMem->params.size();
	sMem->stack.activate();

	vector<adouble> X(Np);
	array<adouble, Nspecies> dydt;
	adept::set_values(&X[0], Np, sMem->params.data());

	if (t < sMem->preT)
		adept::set_values(&X[0], Nlig, sMem->preL);

	sMem->stack.new_recording();

	ratesS<adouble> rattes = ratesS<adouble>(X);

	// Get the data in the right form
	fullModel(NV_DATA_S(y), &rattes, dydt.data());

	adouble yOut = 0;
	yOut = std::inner_product(dydt.begin(), dydt.end(), NV_DATA_S(yB), yOut);

	sMem->stack.independent(&X[0], Np);
	sMem->stack.dependent(&yOut, 1);

	sMem->stack.jacobian(NV_DATA_S(qBdot));

	return(0);
}

static void errorHandler(int error_code, const char *module, const char *function, char *msg, void *ehdata) {
	if (error_code == CV_WARNING) return;
	solver *sMem = static_cast<solver *>(ehdata);
	ratesS<double> ratt = sMem->getRates();

	std::lock_guard<std::mutex> lock(print_mutex);

	cout << "Internal CVode error in " << function << ", module: " << module << ", error code: " << error_code << std::endl;
	cout << msg << std::endl;
	cout << "Parameters: ";

	for (size_t ii = 0; ii < Nparams; ii++) {
		cout << sMem->params[ii] << "\t";
	}

	ratt.print();

	if (sMem->sensi)
		cout << "Sensitivity enabled." << std::endl;

	cout << std::endl << std::endl;
}

int fullModelCVode(const double t, const N_Vector xx, N_Vector dxxdt, void *user_data) {
	solver *sMem = static_cast<solver *>(user_data);
	ratesS<double> rattes = sMem->getRates();

	if (t < sMem->preT)
		preTreat(t - sMem->preT, rattes.ILs, sMem->preL);

	// Get the data in the right form
	fullModel(NV_DATA_S(xx), &rattes, NV_DATA_S(dxxdt));

	return 0;
}

extern "C" int runCkine (const double * const tps, const size_t ntps, double * const out, const double * const rxnRatesIn, const double preT, const double * const preL) {
	size_t itps = 0;

	std::vector<double> v;

	v = std::vector<double>(rxnRatesIn, rxnRatesIn + Nparams);

	solver sMem(v, preT, preL);

	if (tps[0] + preT < std::numeric_limits<double>::epsilon()) {
		std::copy_n(NV_DATA_S(sMem.state), Nspecies, out);

		itps = 1;
	}

	for (; itps < ntps; itps++) {
		if (sMem.CVodeRun(tps[itps] + preT) < 0) return -1;

		// Copy out result
		std::copy_n(NV_DATA_S(sMem.state), Nspecies, out + Nspecies*itps);
	}

	return 0;
}

x0JacM xNotp (vector<double> &params, adept::Stack *stack) {
	size_t Np = params.size();

	vector<adouble> X(Np);
	adept::set_values(&X[0], Np, params.data());

	stack->new_recording();

	ratesS<adouble> rattes = ratesS<adouble>(X);

	// Get the data in the right form
	std::array<adouble, Nspecies> outAD = solveAutocrine(&rattes);

	stack->independent(&X[0], Np);
	stack->dependent(&outAD[0], Nspecies);

	x0JacM gradZV(Nspecies, Np);
	stack->jacobian(gradZV.data());

	return gradZV;
}

extern "C" int runCkineS (const double * const tps, const size_t ntps, double * const out, double * const Sout, const double * const actV, const double * const rxnRatesIn, const double preT, const double * const preL) {
	size_t itps = 0;

	std::vector<double> v;
	std::array<double, Nspecies> actVv;
	std::copy_n(actV, Nspecies, actVv.begin());

	v = std::vector<double>(rxnRatesIn, rxnRatesIn + Nparams);

	solver sMem(v, actVv, preT, preL);

	if (tps[0] + preT < std::numeric_limits<double>::epsilon()) {
		out[0] = sMem.getActivity();

		itps = 1;
	}

	for (; itps < ntps; itps++) {
		if (sMem.CVodeRun(tps[itps] + preT) < 0) return -1;

		// Copy out result
		out[itps] = sMem.getActivity();
	}

	sMem.backward(tps[ntps-1] + preT);

	x0JacM x0p = xNotp(sMem.params, &sMem.stack);

	// Get sensitivities
	for (int bitps = ntps - 1; bitps >= 0; bitps--) {
		if (sMem.getAdjSens(tps[bitps] + preT, Sout + sMem.params.size()*bitps, x0p)) return -1;
	}

	return 0;
}

extern "C" int runCkineParallel (const double * const rxnRatesIn, const double * const tps, const size_t ntps, size_t nDoses, double *out, const double preT, const double * const preL) {
	int retVal = 1000;
	std::list<std::future<int>> results;

	// Actually run the simulations
	for (size_t ii = 0; ii < nDoses; ii++) {
		results.push_back(std::async(std::launch::async, runCkine, tps, ntps, out + Nspecies*ii*ntps, rxnRatesIn + ii*Nparams, preT, preL));
	}

	// Synchronize all threads
	for (std::future<int> &th:results) retVal = std::min(th.get(), retVal);

	// Get the worst case to return
	return retVal;
}

extern "C" int runCkineSParallel (const double * const rxnRatesIn, const double * const tps, const size_t ntps, const size_t nDoses, double * const out, double * const Sout, double * const actV, const double preT, const double * const preL) {
	int retVal = 1000;
	std::list<std::future<int>> results;

	// Actually run the simulations
	for (size_t ii = 0; ii < nDoses; ii++) {
		results.push_back(std::async(std::launch::async, runCkineS, tps, ntps, out + ii*ntps, Sout + Nparams*ii*ntps, actV, rxnRatesIn + Nparams*ii, preT, preL));
	}

	// Synchronize all threads
	for (std::future<int> &th:results) retVal = std::min(th.get(), retVal);

	// Get the worst case to return
	return retVal;
}


// http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage
#include <boost/foreach.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <ros/ros.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>

class FrameDrawer {
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::CameraSubscriber sub_;
    image_transport::Publisher pub_;
    tf::TransformListener tf_listener_;
    image_geometry::PinholeCameraModel cam_model_;
    std::vector<std::string> frame_ids_;
    CvFont font_;

public:
    FrameDrawer(const std::vector<std::string>& frame_ids)
        : it_(nh_)
        , frame_ids_(frame_ids)
    {
        std::string image_topic = nh_.resolveName("image");
        sub_ = it_.subscribeCamera(image_topic, 1, &FrameDrawer::imageCb, this);
        pub_ = it_.advertise("image_out", 1);
        cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
    }

    void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
        const sensor_msgs::CameraInfoConstPtr& info_msg)
    {
        cv::Mat image;
        cv_bridge::CvImagePtr input_bridge;
        try {
            input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
            image = input_bridge->image;
        } catch (cv_bridge::Exception& ex) {
            ROS_ERROR("[draw_frames] Failed to convert image");
            return;
        }

        cam_model_.fromCameraInfo(info_msg);

        BOOST_FOREACH (const std::string& frame_id, frame_ids_) {
            tf::StampedTransform transform;
            try {
                ros::Time acquisition_time = info_msg->header.stamp;
                ros::Duration timeout(1.0 / 30);
                tf_listener_.waitForTransform(cam_model_.tfFrame(), frame_id,
                    acquisition_time, timeout);
                tf_listener_.lookupTransform(cam_model_.tfFrame(), frame_id,
                    acquisition_time, transform);
            } catch (tf::TransformException& ex) {
                ROS_WARN("[draw_frames] TF exception:\n%s", ex.what());
                return;
            }

            tf::Point pt = transform.getOrigin();
            cv::Point3d pt_cv(pt.x(), pt.y(), pt.z());
            cv::Point2d uv;
            uv = cam_model_.project3dToPixel(pt_cv);

            static const int RADIUS = 3;
            cv::circle(image, uv, RADIUS, CV_RGB(255, 0, 0), -1);
            CvSize text_size;
            int baseline;
            cvGetTextSize(frame_id.c_str(), &font_, &text_size, &baseline);
            CvPoint origin = cvPoint(uv.x - text_size.width / 2,
                uv.y - RADIUS - baseline - 3);
        cv:
            putText(image, frame_id.c_str(), origin, cv::FONT_HERSHEY_SIMPLEX, 12, CV_RGB(255, 0, 0));
        }

        pub_.publish(input_bridge->toImageMsg());
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "draw_frames");
    std::vector<std::string> frame_ids(argv + 1, argv + argc);
    FrameDrawer drawer(frame_ids);
    ros::spin();
}


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

#include <algorithm>
#include <boost/graph/graphviz.hpp>
#include "GraphUtilities.hpp"
#include "gtest/gtest.h"

class GraphTest : public ::testing::Test {
 protected:
  struct VertexData {
    std::string name;
  };
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
                                VertexData>
      Graph;

  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
  typedef std::vector<Edge> Path;

  void SetUp() override {
    const std::string graph_spec("digraph { a -> b; b -> c; c -> d; b -> d;}");
    boost::dynamic_properties dp(boost::ignore_other_properties);
    dp.property("node_id", get(&VertexData::name, g));
    boost::read_graphviz(graph_spec, g, dp);
  }

  Vertex by_vertex_name(const std::string& name) {
    boost::graph_traits<Graph>::vertex_iterator vi, vi_end;
    boost::tie(vi, vi_end) = boost::vertices(g);
    return *std::find_if(vi, vi_end,
                         [&](Vertex v) { return name == g[v].name; });
  };

  // void TearDown() override {}

  Graph g;
};

TEST_F(GraphTest, SameVertexPaths) {
  std::vector<Path> paths;
  pgi::all_paths(by_vertex_name("a"), by_vertex_name("a"), g, [&paths](const Path& p){ paths.push_back(p); });
  ASSERT_EQ(paths.size(), 1);
  EXPECT_EQ(paths.front(), Path());
}

TEST_F(GraphTest, MultiplePaths) {
  std::vector<Path> paths;
  pgi::all_paths(by_vertex_name("a"), by_vertex_name("d"), g, [&paths](const Path& p){ paths.push_back(p); });
  EXPECT_EQ(paths.size(), 2);
}

TEST_F(GraphTest, ZeroPaths) {
  std::vector<Path> paths;
  pgi::all_paths(by_vertex_name("c"), by_vertex_name("a"), g, [&paths](const Path& p){ paths.push_back(p); });
  EXPECT_EQ(paths.size(), 0);
}

TEST_F(GraphTest, ExpectedPathsExist) {
  EXPECT_TRUE(pgi::has_path(by_vertex_name("a"), by_vertex_name("a"), g));
  EXPECT_TRUE(pgi::has_path(by_vertex_name("a"), by_vertex_name("d"), g));
  EXPECT_TRUE(pgi::has_path(by_vertex_name("a"), by_vertex_name("c"), g));
  EXPECT_TRUE(pgi::has_path(by_vertex_name("b"), by_vertex_name("d"), g));
  EXPECT_TRUE(pgi::has_path(by_vertex_name("c"), by_vertex_name("d"), g));
  EXPECT_TRUE(pgi::has_path(by_vertex_name("d"), by_vertex_name("d"), g));
}

TEST_F(GraphTest, ExpectedPathsDoNotExist) {
  EXPECT_FALSE(pgi::has_path(by_vertex_name("d"), by_vertex_name("a"), g));
  EXPECT_FALSE(pgi::has_path(by_vertex_name("c"), by_vertex_name("a"), g));
  EXPECT_FALSE(pgi::has_path(by_vertex_name("b"), by_vertex_name("a"), g));
  EXPECT_FALSE(pgi::has_path(by_vertex_name("c"), by_vertex_name("b"), g));
}

TEST_F(GraphTest, EveryPath) {
  std::vector<Path> paths;
  pgi::all_paths(by_vertex_name("a"), g, [&paths](const Path& p){ paths.push_back(p); });
  EXPECT_EQ(paths.size(), 5);
}

TEST_F(GraphTest, RedirectInEdges) {
  pgi::redirect_in_edges(by_vertex_name("d"), by_vertex_name("a"), g);

  EXPECT_EQ(boost::in_degree(by_vertex_name("d"), g), 0);
  EXPECT_EQ(boost::in_degree(by_vertex_name("a"), g), 2);
}

TEST_F(GraphTest, RedirectOutEdges) {
  pgi::redirect_out_edges(by_vertex_name("a"), by_vertex_name("c"), g);

  EXPECT_EQ(boost::in_degree(by_vertex_name("b"), g), 0);
  EXPECT_EQ(boost::in_degree(by_vertex_name("c"), g), 2);
}


#include "map.h"

#include <boost/assert.hpp>
#include <fstream>
#include <iostream>
#include <sstream>

#include "math.h"
#include "spline.h"

namespace pathplanning {

struct Map::Impl
{
  tk::spline sXSpline;
  tk::spline sYSpline;
  tk::spline sDxSpline;
  tk::spline sDySpline;
};

void
Map::Read(const std::string& filename)
{
  std::ifstream in(filename.c_str(), std::ifstream::in);

  // Deserialize map data from file
  std::string line;
  while (getline(in, line)) {
    std::istringstream iss(line);
    double x;
    double y;
    float s;
    float dx;
    float dy;
    iss >> x;
    iss >> y;
    iss >> s;
    iss >> dx;
    iss >> dy;
    _x.push_back(x);
    _y.push_back(y);
    _s.push_back(s);
    _dx.push_back(dx);
    _dy.push_back(dy);
  }
  BOOST_ASSERT(_x.size() > 0);
  BOOST_ASSERT(_y.size() > 0);

  _s.push_back(_s[1] + _s[_s.size() - 1]);
  _x.push_back(_x[0]);
  _y.push_back(_y[0]);
  _dx.push_back(_dx[0]);
  _dy.push_back(_dy[0]);

  // Smoothing the map waypoints
  _pImpl->sXSpline.set_points(_s, _x);
  _pImpl->sYSpline.set_points(_s, _y);
  _pImpl->sDxSpline.set_points(_s, _dx);
  _pImpl->sDySpline.set_points(_s, _dy);
}

Map::Map()
  : _pImpl(std::make_shared<Impl>())
{}

Map::Map(const std::string& filename)
  : _pImpl(std::make_shared<Impl>())
{
  Read(filename);
}

int
Map::GetClosestWaypoint(double x, double y) const
{
  double minDist = std::numeric_limits<double>::max();
  int closestWaypointIndex = 0;

  for (int i = 0; i < _x.size(); ++i) {
    double dist = GetDistance(x, y, _x[i], _y[i]);
    if (dist < minDist) {
      minDist = dist;
      closestWaypointIndex = i;
    }
  }

  return closestWaypointIndex;
}

// Returns next waypoint of the closest waypoint
int
Map::GetNextWaypoint(double x, double y, double theta) const
{
  int closestWaypointIndex = GetClosestWaypoint(x, y);
  double heading = std::atan2((_y[closestWaypointIndex] - y), (_x[closestWaypointIndex] - x));
  double angle = std::fabs(theta - heading);

  angle = std::min(2.0 * M_PI - angle, angle);

  if (angle > M_PI / 2.0) {
    ++closestWaypointIndex;
    if (closestWaypointIndex == _x.size()) {
      closestWaypointIndex = 0;
    }
  }

  return closestWaypointIndex;
}

std::array<double, 2>
Map::GetSD(double x, double y, double theta) const
{
  int nextWaypointIndex = GetNextWaypoint(x, y, theta);
  int prevWaypointIndex = nextWaypointIndex - 1;

  if (nextWaypointIndex == 0) {
    prevWaypointIndex = _x.size() - 1;
  }

  double n_x = _x[nextWaypointIndex] - _x[prevWaypointIndex];
  double n_y = _y[nextWaypointIndex] - _y[prevWaypointIndex];
  double x_x = x - _x[prevWaypointIndex];
  double x_y = y - _y[prevWaypointIndex];

  // find the projection of x onto n
  double projNorm = (x_x * n_x + x_y * n_y) / (n_x * n_x + n_y * n_y);
  double projX = projNorm * n_x;
  double projY = projNorm * n_y;

  double frenetD = GetDistance(x_x, x_y, projX, projY);

  // see if d value is positive or negative by comparing it to a center point
  double centerX = 1000.0 - _x[prevWaypointIndex];
  double centerY = 2000.0 - _y[prevWaypointIndex];
  double centerToPos = GetDistance(centerX, centerY, x_x, x_y);
  double centerToRef = GetDistance(centerX, centerY, projX, projY);

  if (centerToPos <= centerToRef) {
    frenetD *= -1.0;
  }

  // calculate s value
  double frenetS = 0.0;
  for (int i = 0; i < prevWaypointIndex; ++i) {
    frenetS += GetDistance(_x[i], _y[i], _x[i + 1], _y[i + 1]);
  }

  frenetS += GetDistance(0, 0, projX, projY);

  return { frenetS, frenetD };
}

std::array<double, 2>
Map::GetXY(double s, double d) const
{
  // Default implementation comes with upstream, which provides low resolution.
  //
  // int prevWaypointIndex = -1;

  // while (s > (_s[prevWaypointIndex + 1] - 1e-6) &&
  //        (prevWaypointIndex < static_cast<int>(_s.size() - 1))) {
  //   ++prevWaypointIndex;
  // }

  // int waypoint2 = (prevWaypointIndex + 1) % _x.size();

  // double heading = std::atan2((_y[waypoint2] - _y[prevWaypointIndex]),
  //                             (_x[waypoint2] - _x[prevWaypointIndex]));
  // // the x,y,s along the segment
  // double segS = (s - _s[prevWaypointIndex]);

  // double segX = _x[prevWaypointIndex] + segS * cos(heading);
  // double segY = _y[prevWaypointIndex] + segS * sin(heading);

  // double perpHeading = heading - M_PI / 2.0;

  // double x = segX + d * std::cos(perpHeading);
  // double y = segY + d * std::sin(perpHeading);

  // return { x, y };
  return { GetX(s, d), GetY(s, d) };
}

double
Map::GetX(double s, double d) const
{
  s = std::fmod(s, _s[_s.size() - 1]);
  return _pImpl->sXSpline(s) + d * _pImpl->sDxSpline(s);
}

double
Map::GetY(double s, double d) const
{
  s = std::fmod(s, _s[_s.size() - 1]);
  return _pImpl->sYSpline(s) + d * _pImpl->sDySpline(s);
}

double
GetKMFromMile(double mile)
{
  return mile / 2.24;
}

double
GetMileFromKM(double km)
{
  return km * 2.24;
}
std::array<double, 2>
ComputeFrenetVelocity(const Map& map,
                      const std::array<double, 2>& pos,
                      const std::array<double, 2>& vel,
                      const std::array<double, 2>& sd,
                      const double dt)
{
  using Eigen::Vector2d;
  using std::array;

  Vector2d velDir = { vel[0], vel[1] };
  double speed = velDir.norm();
  if (speed < 1e-6) {
    return { 0.0, 0.0 };
  }

  velDir /= speed;

  // Hard code the local tangential (Frenet) frame by 0.1 meter.
  Vector2d frenetFrameHeadingVec = { sd[0] + 0.1, sd[1] };
  auto tempFrenetFrameHeadingVec = map.GetXY(frenetFrameHeadingVec[0], frenetFrameHeadingVec[1]);

  Vector2d localFrameOrigin = { pos[0], pos[1] };
  Vector2d localFrameDir = { tempFrenetFrameHeadingVec[0], tempFrenetFrameHeadingVec[1] };
  localFrameDir = localFrameDir - localFrameOrigin;
  localFrameDir /= localFrameDir.norm();

  double headingAngle = std::acos(localFrameDir.transpose() * velDir);
  array<double, 2> sdVel = { speed * std::cos(headingAngle), speed * std::sin(headingAngle) };
  return sdVel;
}

} // namespace pathplanning


#include <ros/ros.h>
#include <iostream>
#include <Eigen/Dense>

#include "sensor_msgs/Imu.h"
#include "nav_msgs/Odometry.h"

namespace OdomImuTransform {

class OdomImuTransformer {

public:

    //! Measurement type shortcut definition
    typedef Eigen::Matrix<double, 3, 1> Vector3;
    typedef Eigen::Matrix<double, 3, 3> Matrix3;
    typedef Eigen::Matrix<double, 4, 4> Matrix4;

    OdomImuTransformer(ros::NodeHandle& nh) : _nh(nh) {

    	_sub = _nh.subscribe("/velocity_control/odom", 100, &OdomImuTransformer::OdomCallback, this);
    	_pub = _nh.advertise<sensor_msgs::Imu>("/imu/transformed_data", 1000);
    }

    sensor_msgs::Imu transform(const nav_msgs::Odometry::ConstPtr& msg)
    {
    	sensor_msgs::Imu transformed;
    	transformed.header = msg->header;
    	transformed.header.frame_id = "base_link";

    	transformed.angular_velocity.z = msg->twist.twist.angular.z;

    	if (init_set) {
    		double delta_t = (msg->header.stamp - prev_time).toSec();
    		transformed.linear_acceleration.x = (msg->twist.twist.linear.x - prev_vel) / delta_t;
    	} else {
    		init_set = true;
    	}

    	transformed.linear_acceleration.z = 9.81;

    	prev_vel = msg->twist.twist.linear.x;
    	prev_time = msg->header.stamp;

        return transformed;
    }

    void spin() {
    	while (ros::ok()) {
    		ros::spinOnce();
    	}
    }

private:
    void OdomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    	sensor_msgs::Imu new_msg = transform(msg);
    	_pub.publish(new_msg);
    }

    double prev_vel = 0;
    ros::Time prev_time = ros::Time(0);
    bool init_set = false;

    ros::NodeHandle _nh;
    ros::Publisher _pub;
    ros::Subscriber _sub;
};

}

int main(int argc, char* argv[]) {
	ros::init(argc, argv, "imu_transform");
	ros::NodeHandle n;
	OdomImuTransform::OdomImuTransformer transformer(n);

	//transformer.test();

	transformer.spin();

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_DIVS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_DIVS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_arithmetic
 * \defgroup boost_simd_arithmetic_divs divs
 *
 * \par Description
 * return the saturated  division of the operandsin the input type
 * \par
 * This only differs from standard \c / in division by zero
 * cases, and also for the division of Valmin by -1 that
 * produces Valmax
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/divs.hpp>
 * \endcode
 *
 * \par Alias
 * \arg rdivide
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::divs_(A0,A0)>::type
 *     divs(const A0 & a0,const A0 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of divs
 * \param a1 the second parameter of divs
 *
 * \return a value of the common type of the parameters
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd {
  namespace tag
  {
    /*!
     * \brief Define the tag divs_ of functor divs
     *        in namespace boost::simd::tag for toolbox boost.simd.arithmetic
    **/
    struct divs_ : ext::elementwise_<divs_> { typedef ext::elementwise_<divs_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divs_, divs, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divs_, saturated_div, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divs_, rdivide, 2)

  template<class A0, class A1>
  BOOST_DISPATCH_FORCE_INLINE
  typename dispatch::meta::call<tag::divs_(A0, A1)>::type
  ldivide(A0 const& a0, A1 const& a1)
  {
    return dispatch::make_functor<tag::divs_, A0>::type()(a1, a0);
  }

  template<class A0, class A1>
  BOOST_DISPATCH_FORCE_INLINE
  typename dispatch::meta::call<tag::divs_(A0, A1)>::type
  ldiv(A0 const& a0, A1 const& a1)
  {
    return dispatch::make_functor<tag::divs_, A0>::type()(a1, a0);
  }

} }

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 BOOST_SIMD_IEEE_FUNCTIONS_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_NEXTPOW2_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_nextpow2 nextpow2
 *
 * \par Description
 * Returns the least n such that abs(a0) is less or equal to \f$2^n\f$
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/nextpow2.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::nextpow2_(A0)>::type
 *     nextpow2(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of nextpow2
 *
 * \return a value of the same type as the parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag nextpow2_ of functor nextpow2
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct nextpow2_ : ext::elementwise_<nextpow2_> { typedef ext::elementwise_<nextpow2_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::nextpow2_, nextpow2, 1)
} }

#endif

// modified by jt the 25/12/2010


#define BOOST_TEST_MODULE Species
#include <boost/test/unit_test.hpp>

#include "sycomore/Species.h"
#include "sycomore/units.h"

BOOST_AUTO_TEST_CASE(QuantityConstructorFull)
{
    using namespace sycomore::units;
    sycomore::Species const species(
        1.*ms, 10.*s, 3e-9*m*m/s, (1/1.5)/s, 0.9*rad/s, 1.1);
    BOOST_TEST(species.get_R1() == 1*kHz);
    BOOST_TEST(species.get_T1() == 1*ms);

    BOOST_TEST(species.get_R2() == 0.1*Hz);
    BOOST_TEST(species.get_T2() == 10*s);

    BOOST_TEST(species.get_D()[0] == 3e-9*m*m/s);

    BOOST_TEST(species.get_R2_prime() == (1/1.5)/s);
    BOOST_TEST(species.get_T2_prime() == 1.5*s);

    BOOST_TEST(species.get_delta_omega() == 0.9*rad/s);

    BOOST_TEST(species.w == 1.1);
}

BOOST_AUTO_TEST_CASE(QuantityConstructorPartial)
{
    using namespace sycomore::units;
    sycomore::Species const species(1.*ms, 10.*s);
    BOOST_TEST(species.get_R1() == 1*kHz);
    BOOST_TEST(species.get_T1() == 1*ms);

    BOOST_TEST(species.get_R2() == 0.1*Hz);
    BOOST_TEST(species.get_T2() == 10*s);

    BOOST_TEST(species.get_D()[0] == 0*m*m/s);

    BOOST_TEST(species.get_R2_prime() == 0*Hz);
//    BOOST_TEST(species.get_T2_prime() == INF*s);

    BOOST_TEST(species.get_delta_omega() == 0.*rad/s);

    BOOST_TEST(species.w == 1.);
}

BOOST_AUTO_TEST_CASE(DScalar)
{
    using namespace sycomore::units;
    sycomore::Species species(1.*ms, 10.*s);
    species.set_D(1*um*um/ms);
    sycomore::Array<sycomore::Quantity> const D{
        1*um*um/ms, 0*um*um/ms, 0*um*um/ms,
        0*um*um/ms, 1*um*um/ms, 0*um*um/ms,
        0*um*um/ms, 0*um*um/ms, 1*um*um/ms};
    BOOST_TEST(species.get_D() == D);
}

BOOST_AUTO_TEST_CASE(DTensor)
{
    using namespace sycomore::units;
    sycomore::Species species(1.*ms, 10.*s);
    sycomore::Array<sycomore::Quantity> const D{
        1*um*um/ms, 4*um*um/ms, 7*um*um/ms,
        2*um*um/ms, 5*um*um/ms, 8*um*um/ms,
        3*um*um/ms, 6*um*um/ms, 9*um*um/ms};
    species.set_D(D);
    BOOST_TEST(species.get_D() == D);
}


// Copyright (c) 2019 - present Advanced Micro Devices, Inc. All rights reserved.
//
// 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.

#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <complex>
#include <functional>
#include <iostream>
#include <numeric>
#include <vector>

#include <hip/hip_runtime.h>
#include <hip/hip_vector_types.h>
#include <rocfft.h>

#include "examplekernels.h"
#include "exampleutils.h"

int main(int argc, char* argv[])
{
    std::cout << "rocfft double-precision real/complex transform\n" << std::endl;

    // Length of transform:
    std::vector<size_t> length = {8};

    // Gpu device id:
    int deviceId = 0;

    // Command-line options:
    // clang-format off
    po::options_description desc("rocfft sample command line options");
    desc.add_options()("help,h", "produces this help message")
        ("device", po::value<int>(&deviceId)->default_value(0), "Select a specific device id")
        ("outofplace,o", "Perform an out-of-place transform")
        ("inverse,i", "Perform an inverse transform")
        ("length", po::value<std::vector<size_t>>(&length)->multitoken(),
         "Lengths of the transform separated by spaces");
    // clang-format on
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 0;
    }

    // Placeness for the transform
    const rocfft_result_placement place
        = vm.count("outofplace") ? rocfft_placement_notinplace : rocfft_placement_inplace;
    const bool inplace = place == rocfft_placement_inplace;

    // Direction of transform
    const rocfft_transform_type direction = vm.count("inverse")
                                                ? rocfft_transform_type_real_inverse
                                                : rocfft_transform_type_real_forward;
    const bool forward = direction == rocfft_transform_type_real_forward;

    // Set up the strides and buffer size for the real values:
    std::vector<size_t> rstride = {1};
    for(int i = 1; i < length.size(); ++i)
    {
        // In-place transforms need space for two extra real values in the contiguous
        // direction.
        auto val = (length[i - 1] + ((inplace && i == 1) ? 2 : 0)) * rstride[i - 1];
        rstride.push_back(val);
    }
    // NB: not tight, but hey
    const size_t        real_size = length[length.size() - 1] * rstride[rstride.size() - 1];
    std::vector<double> rdata(real_size); // host storage

    // The complex data length is half + 1 of the real data length in the contiguous
    // dimensions.  Since rocFFT is column-major, this is the first index.
    std::vector<size_t> clength = length;
    clength[0]                  = clength[0] / 2 + 1;
    std::vector<size_t> cstride = {1};
    for(int i = 1; i < clength.size(); ++i)
    {
        cstride.push_back(clength[i - 1] * cstride[i - 1]);
    }
    const size_t complex_size = clength[clength.size() - 1] * cstride[cstride.size() - 1];
    std::vector<std::complex<double>> cdata(complex_size); // host storage

    // Based on the direction, we set the input and output parameters appropriately.
    const size_t isize  = forward ? real_size : complex_size;
    const size_t ibytes = isize * (forward ? sizeof(double) : sizeof(std::complex<double>));
    const std::vector<size_t> ilength = forward ? length : clength;
    const std::vector<size_t> istride = forward ? rstride : cstride;

    const size_t osize  = forward ? complex_size : real_size;
    const size_t obytes = osize * (forward ? sizeof(std::complex<double>) : sizeof(double));
    const std::vector<size_t> olength = forward ? clength : length;
    const std::vector<size_t> ostride = forward ? cstride : rstride;

    // Print information about the transform:
    std::cout << "direction: ";
    if(forward)
        std::cout << "forward\n";
    else
        std::cout << "inverse\n";
    std::cout << "length:";
    for(const auto i : length)
        std::cout << " " << i;
    std::cout << "\n";
    if(inplace)
        std::cout << "in-place transform\n";
    else
        std::cout << "out-of-place transform\n";
    std::cout << "deviceID: " << deviceId << "\n";
    std::cout << "input length:";
    for(auto i : ilength)
        std::cout << " " << i;
    std::cout << "\n";
    std::cout << "input buffer stride:";
    for(auto i : istride)
        std::cout << " " << i;
    std::cout << "\n";
    std::cout << "input buffer size: " << ibytes << "\n";

    std::cout << "output length:";
    for(auto i : olength)
        std::cout << " " << i;
    std::cout << "\n";
    std::cout << "output buffer stride:";
    for(auto i : ostride)
        std::cout << " " << i;
    std::cout << "\n";
    std::cout << "output buffer size: " << obytes << "\n";
    std::cout << std::endl;

    // Set the device:
    hipSetDevice(deviceId);

    // Create HIP device object and initialize data
    // Kernels are provided in examplekernels.h
    hipError_t hip_status = hipSuccess;
    void*      gpu_in     = NULL;
    hip_status            = hipMalloc(&gpu_in, ibytes);
    if(forward)
    {
        initreal(length, istride, gpu_in);
    }
    else
    {
        inithermitiancomplex(length, ilength, istride, gpu_in);
    }
    hipDeviceSynchronize();
    hip_status = hipGetLastError();
    assert(hip_status == hipSuccess);

    // Print the output:
    std::cout << "input:\n";
    if(forward)
    {
        hipMemcpy(rdata.data(), gpu_in, ibytes, hipMemcpyDeviceToHost);
        printbuffer(rdata, ilength, istride, 1, isize);
    }
    else
    {
        hipMemcpy(cdata.data(), gpu_in, ibytes, hipMemcpyDeviceToHost);
        printbuffer(cdata, ilength, istride, 1, isize);

        // Check that the buffer is Hermitian symmetric:
        check_symmetry(cdata, length, istride, 1, isize);
    }

    // rocfft_status can be used to capture API status info
    rocfft_status rc = rocfft_status_success;

    // Create the a descrition struct to set data layout:
    rocfft_plan_description gpu_description = NULL;
    rc                                      = rocfft_plan_description_create(&gpu_description);
    assert(rc == rocfft_status_success);
    rc = rocfft_plan_description_set_data_layout(
        gpu_description,
        // input data format:
        forward ? rocfft_array_type_real : rocfft_array_type_hermitian_interleaved,
        // output data format:
        forward ? rocfft_array_type_hermitian_interleaved : rocfft_array_type_real,
        NULL,
        NULL,
        istride.size(), // input stride length
        istride.data(), // input stride data
        0, // input batch distance
        ostride.size(), // output stride length
        ostride.data(), // output stride data
        0); // ouptut batch distance
    // We can also pass "NULL" instead of a description; rocFFT will use reasonable
    // default parameters.  If the data isn't contiguous, we need to set strides, etc,
    // using the description.

    // Create the FFT plan:
    rocfft_plan gpu_plan = NULL;
    rc                   = rocfft_plan_create(&gpu_plan,
                            place,
                            direction,
                            rocfft_precision_double,
                            length.size(), // Dimension
                            length.data(), // lengths
                            1, // Number of transforms
                            gpu_description); // Description
    assert(rc == rocfft_status_success);

    // Get the execution info for the fft plan (in particular, work memory requirements):
    rocfft_execution_info planinfo = NULL;
    rc                             = rocfft_execution_info_create(&planinfo);
    assert(rc == rocfft_status_success);
    size_t workbuffersize = 0;
    rc                    = rocfft_plan_get_work_buffer_size(gpu_plan, &workbuffersize);
    assert(rc == rocfft_status_success);

    // If the transform requires work memory, allocate a work buffer:
    void* wbuffer = NULL;
    if(workbuffersize > 0)
    {
        hip_status = hipMalloc(&wbuffer, workbuffersize);
        assert(hip_status == hipSuccess);
        rc = rocfft_execution_info_set_work_buffer(planinfo, wbuffer, workbuffersize);
        assert(rc == rocfft_status_success);
    }

    // If the transform is out-of-place, allocate the output buffer as well:
    void* gpu_out = inplace ? gpu_in : NULL;
    if(!inplace)
    {
        hip_status = hipMalloc(&gpu_out, obytes);
        assert(hip_status == hipSuccess);
    }

    // Execute the GPU transform:
    rc = rocfft_execute(gpu_plan, // plan
                        (void**)&gpu_in, // in_buffer
                        (void**)&gpu_out, // out_buffer
                        planinfo); // execution info

    // Get the output from the device and print to cout:
    std::cout << "output:\n";
    if(forward)
    {
        hipMemcpy(cdata.data(), gpu_out, obytes, hipMemcpyDeviceToHost);
        printbuffer(cdata, olength, ostride, 1, osize);
    }
    else
    {
        hipMemcpy(rdata.data(), gpu_out, obytes, hipMemcpyDeviceToHost);
        printbuffer(rdata, olength, ostride, 1, osize);
    }

    // Clean up: free GPU memory:
    hipFree(gpu_in);
    if(!inplace)
    {
        hipFree(gpu_out);
    }
    if(wbuffer != NULL)
    {
        hipFree(wbuffer);
    }

    // Clean up: destroy plans:
    rocfft_execution_info_destroy(planinfo);
    rocfft_plan_description_destroy(gpu_description);
    rocfft_plan_destroy(gpu_plan);

    return 0;
}


//  Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/special_functions/fibonacci.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/fibonacci.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::fibonacci<float>(f));
   check_result<double>(boost::math::fibonacci<double>(d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::fibonacci<long double>(l));
#endif

   check_result<float>(boost::math::unchecked_fibonacci<float>(f));
   check_result<double>(boost::math::unchecked_fibonacci<double>(d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::unchecked_fibonacci<long double>(l));
#endif
}


#define BOOST_TEST_MODULE "test_read_directional_contact_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/input/read_local_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_directional_contact_cosine_go_contact)
{
    mjolnir::LoggerManager::set_default_logger("test_read_directional_contact_interaction.log");
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type = traits_type::real_type;
    const real_type tol = 1e-7;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
          interaction        = "DirectionalContact"
          potentials.angle1   = "Cosine"
          potentials.angle2   = "Cosine"
          potentials.contact  = "GoContact"
          topology           = "none"
          parameters = [
          {indices = [0, 1, 2, 3], angle1 = {v0 = 0.0, k = -10.0, n = 1}, angle2 = {v0 = 1.0, k = -20.0, n = 2}, contact = {v0 = 2.0, k = 1.0}},
          ]
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv =
                dynamic_cast<
                    mjolnir::DirectionalContactInteraction<
                        traits_type, mjolnir::CosinePotential<real_type>,
                        mjolnir::CosinePotential<real_type>,
                        mjolnir::GoContactPotential<real_type>
                        >*
                >(base.get()); //check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
        const auto indices_potentials_tuple = derv->potentials().at(0);
        const auto angle1_pot  = std::get<1>(indices_potentials_tuple);
        const auto angle2_pot  = std::get<2>(indices_potentials_tuple);
        const auto contact_pot = std::get<3>(indices_potentials_tuple);

        BOOST_TEST(angle1_pot.v0()  == real_type(0.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle1_pot.k()   == real_type(-10.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle1_pot.n()   == std::int32_t(1));
        BOOST_TEST(angle2_pot.v0()  == real_type(1.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle2_pot.k()   == real_type(-20.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle2_pot.n()   == std::int32_t(2));
        BOOST_TEST(contact_pot.k()  == real_type(1.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(contact_pot.v0() == real_type(2.0), boost::test_tools::tolerance(tol));
    }
}

BOOST_AUTO_TEST_CASE(read_directional_contact_cosine_gaussian)
{
    mjolnir::LoggerManager::set_default_logger("test_read_directional_contact_interaction.log");
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type = traits_type::real_type;
    const real_type tol = 1e-7;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
          interaction        = "DirectionalContact"
          potentials.angle1   = "Cosine"
          potentials.angle2   = "Cosine"
          potentials.contact  = "Gaussian"
          topology           = "none"
          parameters = [
          {indices = [0, 1, 2, 3], angle1 = {v0 = 0.0, k = -10.0, n = 1}, angle2 = {v0 = 0.0, k = -20.0, n = 2}, contact = {v0 = 1.0, k = 1.0, sigma = 5.0}},
          ]
        )"_toml;

        const auto base = mjolnir::read_local_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv =
                dynamic_cast<
                    mjolnir::DirectionalContactInteraction<
                        traits_type, mjolnir::CosinePotential<real_type>,
                        mjolnir::CosinePotential<real_type>,
                        mjolnir::GaussianPotential<real_type>
                        >*
                >(base.get()); //check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));
        const auto indices_potentials_tuple = derv->potentials().at(0);
        const auto angle1_pot  = std::get<1>(indices_potentials_tuple);
        const auto angle2_pot  = std::get<2>(indices_potentials_tuple);
        const auto contact_pot = std::get<3>(indices_potentials_tuple);

        BOOST_TEST(angle1_pot.v0()  == real_type(0.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle1_pot.k()   == real_type(-10.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle1_pot.n()   == std::int32_t(1));
        BOOST_TEST(angle2_pot.v0()  == real_type(0.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle2_pot.k()   == real_type(-20.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(angle2_pot.n()   == std::int32_t(2));
        BOOST_TEST(contact_pot.k()  == real_type(1.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(contact_pot.v0() == real_type(1.0), boost::test_tools::tolerance(tol));
        BOOST_TEST(contact_pot.sigma() == real_type(5.0), boost::test_tools::tolerance(tol));
    }
}


/*
 *  pose.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-10-27
 *
 *  Class to store and make calculations on a robot arm,
 *  including the angle of the grippers.
 *
 */

#ifndef IROB_TOOL_POSE_HPP_
#define IROB_TOOL_POSE_HPP_

#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/JointState.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <irob_msgs/ToolPose.h>
#include <irob_msgs/ToolPoseStamped.h>
#include <ros/ros.h>
#include <irob_utils/utils.hpp>

namespace saf {

class ToolPose {

   public:

   	struct Distance {
    	double cartesian;
    	double angle;
    	double jaw;

    	Distance operator*=(double);
    	Distance operator/=(double);
    	Distance operator*(double) const;
    	Distance operator/(double) const;

    	friend std::ostream& operator<<(std::ostream&, const Distance&);
  	};

    Eigen::Affine3d transform;
   	double jaw;

    ToolPose();
    ToolPose(double, double, double, double, double, double, double, double);
    ToolPose(const ToolPose&);
    ToolPose(const irob_msgs::ToolPose&);
    ToolPose(const irob_msgs::ToolPoseStamped&);
    ToolPose(const Eigen::Vector3d&, const Eigen::Quaterniond&, double);
    ToolPose(const Eigen::Affine3d&, double );
    ToolPose(const Eigen::Translation3d&, const Eigen::Quaterniond&, double );
    ToolPose(const geometry_msgs::Transform&, double);
    ToolPose(const geometry_msgs::TransformStamped&, double);

    irob_msgs::ToolPose toRosToolPose() const;

    void swap(ToolPose&);
    ToolPose operator=(const ToolPose&);

    ToolPose interpolate(double, const ToolPose&) const;

   	bool isNaN() const;

    Distance dist(const ToolPose&) const;
    Distance dist(const Eigen::Affine3d&) const;
   	Distance dist(const Eigen::Vector3d&) const;
    Distance dist(const Eigen::Quaterniond&) const;
   	Distance dist(double) const;

    friend std::ostream& operator<<(std::ostream&, const ToolPose&);
    friend std::istream& operator>>(std::istream&, ToolPose&);

    template<typename T_typ>
    friend ToolPose operator*(
          const T_typ& T,
          const ToolPose& p)
    {
      ToolPose ret(T * p.transform, p.jaw);
      return ret;
    }
};

// Template specializations from utils.hpp

template<> inline ToolPose interpolate(double a, const ToolPose& x1, const ToolPose& x2) {
  return x1.interpolate(a, x2);
}

template<> inline double distanceEuler(const ToolPose& x1, const ToolPose& x2) {
  ToolPose::Distance d = x1.dist(x2);
  double weighted_cartesian = std::abs(d.cartesian) * 10000.0;
  double weighted_angle = std::abs(d.angle);
  double weighted_jaw = radToDeg(std::abs(d.jaw));
  if (weighted_cartesian >= weighted_angle
      && weighted_cartesian >= weighted_jaw)
    return std::abs(d.cartesian);
  if (weighted_angle >= weighted_jaw)
    // in degrees, should be converted to rad?
    return std::abs(d.angle);
  return std::abs(d.jaw);
}

template <>
inline ToolPose unwrapMsg(const irob_msgs::ToolPose& msg){
  ToolPose ret(msg);
  return ret;
}

template <>
inline ToolPose unwrapMsg(const irob_msgs::ToolPoseStamped& msg){
  ToolPose ret(msg);
  return ret;
}

template <>
inline irob_msgs::ToolPose wrapToMsg(const ToolPose& data){
  return data.toRosToolPose();
}

template <>
inline ToolPose makeNaN(){
  ToolPose ret(std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN(),
           std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline bool isnan(const ToolPose& d)
{
  Eigen::Translation3d v(d.transform.translation());
  Eigen::Quaterniond q(d.transform.rotation());
  return (std::isnan(v.x())
          || std::isnan(v.y())
          || std::isnan(v.z())
          || std::isnan(q.x())
          || std::isnan(q.y())
          || std::isnan(q.z())
          || std::isnan(q.w())
          || std::isnan(d.jaw));
}

}
#endif


#include <boost/graph/boykov_kolmogorov_max_flow.hpp>


//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
#define __pinocchio_joint_revolute_unbounded_unaligned_hpp__

#include "pinocchio/fwd.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/rotation.hpp"
#include "pinocchio/math/matrix.hpp"

#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options = 0> struct JointRevoluteUnboundedUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 2,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;

    typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
    typedef BiasZeroTpl<Scalar,Options> Bias_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
  };

  template<typename Scalar, int Options>
  struct traits< JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits <JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataRevoluteUnboundedUnalignedTpl
  : public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataRevoluteUnboundedUnalignedTpl() {}

    template<typename Vector3Like>
    JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : M(1)
    , S(axis)
    , v(axis,(Scalar)NAN)
    , U(), Dinv(), UDinv()
    {}

    static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); }
    std::string shortname() const { return classname(); }

  }; // struct JointDataRevoluteUnboundedUnalignedTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);

  template<typename _Scalar, int _Options>
  struct JointModelRevoluteUnboundedUnalignedTpl
  : public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;

    typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointModelRevoluteUnboundedUnalignedTpl() {}

    JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x,
                                            const Scalar & y,
                                            const Scalar & z)
    : axis(x,y,z)
    {
      axis.normalize();
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }

    template<typename Vector3Like>
    JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }

    JointDataDerived createData() const { return JointDataDerived(axis); }

    using Base::isEqual;
    bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar OtherScalar;
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
      & q = qs.template segment<NQ>(idx_q());

      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);

      toRotationMatrix(axis,ca,sa,data.M.rotation());
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());
      data.v.w = (Scalar)vs[idx_v()];
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
      data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

    // data
    ///
    /// \brief 3d main axis of the joint.
    ///
    Vector3 axis;
  }; // struct JointModelRevoluteUnboundedUnalignedTpl

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__