// ---------------------------------------------------------------------
//
// Copyright (c) 2018 - 2020 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/boundary_mesh.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/explicit_system.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/LEInteractor.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// Elasticity model data.
namespace ModelData
{
// Tether (penalty) force functions.
static double kappa_s = 1.0e6;
static double eta_s = 0.0;
System *x_solid_system, *u_solid_system;
void
tether_force_function(VectorValue<double>& F,
                      const TensorValue<double>& /*FF*/,
                      const libMesh::Point& x_bndry, // x_bndry gives current   coordinates on the boundary mesh
                      const libMesh::Point& X_bndry, // X_bndry gives reference coordinates on the boundary mesh
                      Elem* const elem,
                      const vector<const vector<double>*>& var_data,
                      const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                      double /*time*/,
                      void* /*ctx*/)
{
    // tether_force_function() is called on elements of the boundary mesh.  Here
    // we look up the element in the solid mesh that the current boundary
    // element was extracted from.
    const Elem* const interior_parent = elem->interior_parent();

    // We define "arbitrary" velocity and displacement fields on the solid mesh.
    // Here we look up their values.
    std::vector<double> x_solid(NDIM, 0.0), u_solid(NDIM, 0.0);
    for (unsigned int d = 0; d < NDIM; ++d)
    {
        x_solid[d] = x_solid_system->point_value(d, X_bndry, interior_parent);
        u_solid[d] = u_solid_system->point_value(d, X_bndry, interior_parent);
    }

    // Look up the velocity of the boundary mesh.
    const std::vector<double>& u_bndry = *var_data[0];

    // The tether force is proportional to the mismatch between the positions
    // and velocities.
    for (unsigned int d = 0; d < NDIM; ++d)
    {
        F(d) = kappa_s * (x_solid[d] - x_bndry(d)) + eta_s * (u_solid[d] - u_bndry[d]);
    }
    return;
} // tether_force_function
} // namespace ModelData
using namespace ModelData;

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/

int
main(int argc, char* argv[])
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && app_initializer->getVisItDataWriter();
#ifdef LIBMESH_HAVE_EXODUS_API
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
#else
        const bool uses_exodus = false;
        if (!app_initializer->getExodusIIFilename().empty())
        {
            plog << "WARNING: libMesh was compiled without Exodus support, so no "
                 << "Exodus output will be written in this program.\n";
        }
#endif
        const string exodus_solid_filename = "solid_output.ex2"; // app_initializer->getExodusIIFilename();
        const string exodus_bndry_filename = "bndry_output.ex2"; // app_initializer->getExodusIIFilename();

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();
        const string restart_read_dirname = app_initializer->getRestartReadDirectory();
        const int restart_restore_num = app_initializer->getRestartRestoreNumber();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Create a simple FE mesh.
        Mesh solid_mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.5;
        if (NDIM == 2 && (elem_type == "TRI3" || elem_type == "TRI6"))
        {
#ifdef LIBMESH_HAVE_TRIANGLE
            const int num_circum_nodes = ceil(2.0 * M_PI * R / ds);
            for (int k = 0; k < num_circum_nodes; ++k)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes);
                solid_mesh.add_point(libMesh::Point(R * cos(theta), R * sin(theta)));
            }
            TriangleInterface triangle(solid_mesh);
            triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
            triangle.elem_type() = Utility::string_to_enum<ElemType>(elem_type);
            triangle.desired_area() = 1.5 * sqrt(3.0) / 4.0 * ds * ds;
            triangle.insert_extra_points() = true;
            triangle.smooth_after_generating() = true;
            triangle.triangulate();
#else
            TBOX_ERROR("ERROR: libMesh appears to have been configured without support for Triangle,\n"
                       << "       but Triangle is required for TRI3 or TRI6 elements.\n");
#endif
        }
        else
        {
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = 2.0 * M_PI * R / ds;
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(solid_mesh, R, r, Utility::string_to_enum<ElemType>(elem_type));
        }

        // Ensure nodes on the surface are on the analytic boundary.
        MeshBase::element_iterator el_end = solid_mesh.elements_end();
        for (MeshBase::element_iterator el = solid_mesh.elements_begin(); el != el_end; ++el)
        {
            Elem* const elem = *el;
            for (unsigned int side = 0; side < elem->n_sides(); ++side)
            {
                const bool at_mesh_bdry = !elem->neighbor_ptr(side);
                if (!at_mesh_bdry) continue;
                for (unsigned int k = 0; k < elem->n_nodes(); ++k)
                {
                    if (!elem->is_node_on_side(k, side)) continue;
                    Node& n = elem->node_ref(k);
                    n = R * n.unit();
                }
            }
        }
        solid_mesh.prepare_for_use();

        BoundaryMesh bndry_mesh(solid_mesh.comm(), solid_mesh.mesh_dimension() - 1);
        solid_mesh.boundary_info->sync(bndry_mesh);
        bndry_mesh.prepare_for_use();

        kappa_s = input_db->getDouble("KAPPA_S");
        eta_s = input_db->getDouble("ETA_S");

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"));
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           &bndry_mesh,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           /*register_for_restart*/ true,
                           restart_read_dirname,
                           restart_restore_num);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator);
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Configure the IBFE solver.
        ib_method_ops->initializeFEEquationSystems();
        std::vector<int> vars(NDIM);
        for (unsigned int d = 0; d < NDIM; ++d) vars[d] = d;
        vector<SystemData> sys_data(1, SystemData(IBFEMethod::VELOCITY_SYSTEM_NAME, vars));
        IBFEMethod::LagBodyForceFcnData body_fcn_data(tether_force_function, sys_data);
        ib_method_ops->registerLagBodyForceFunction(body_fcn_data);
        EquationSystems* bndry_equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();

        // Setup solid systems.
        std::unique_ptr<EquationSystems> solid_equation_systems(new EquationSystems(solid_mesh));
        x_solid_system = &solid_equation_systems->add_system<ExplicitSystem>("position");
        u_solid_system = &solid_equation_systems->add_system<ExplicitSystem>("velocity");
        Order order = SECOND;
        FEFamily family = LAGRANGE;
        for (int d = 0; d < NDIM; ++d)
        {
            x_solid_system->add_variable("X_" + std::to_string(d), order, family);
        }
        for (int d = 0; d < NDIM; ++d)
        {
            u_solid_system->add_variable("U_" + std::to_string(d), order, family);
        }
        solid_equation_systems->init();

        // Set up the position vector.
        //
        // \todo There needs to be an API so that this can be handled by the IBFEMethod class implementation.
        {
            MeshBase& mesh = solid_equation_systems->get_mesh();
            System& X_system = solid_equation_systems->get_system("position");
            const unsigned int X_sys_num = X_system.number();
            NumericVector<double>& X_coords = *X_system.solution;
            for (MeshBase::node_iterator it = mesh.local_nodes_begin(); it != mesh.local_nodes_end(); ++it)
            {
                Node* n = *it;
                if (n->n_vars(X_sys_num))
                {
                    TBOX_ASSERT(n->n_vars(X_sys_num) == NDIM);
                    const libMesh::Point& X = *n;
                    libMesh::Point x = X;
                    for (unsigned int d = 0; d < NDIM; ++d)
                    {
                        const int dof_index = n->dof_number(X_sys_num, d, 0);
                        X_coords.set(dof_index, x(d));
                    }
                }
            }
            X_coords.close();
            X_system.get_dof_map().enforce_constraints_exactly(X_system, &X_coords);
            copy_and_synch(*X_system.solution, *X_system.current_local_solution);
        }

        x_solid_system->assemble_before_solve = false;
        x_solid_system->assemble();

        u_solid_system->assemble_before_solve = false;
        u_solid_system->assemble();

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        // Create Eulerian body force function specification objects.
        if (input_db->keyExists("ForcingFunction"))
        {
            Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
                "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
            time_integrator->registerBodyForceFunction(f_fcn);
        }

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            time_integrator->registerVisItDataWriter(visit_data_writer);
        }
        std::unique_ptr<ExodusII_IO> exodus_solid_io(uses_exodus ? new ExodusII_IO(solid_mesh) : NULL);
        std::unique_ptr<ExodusII_IO> exodus_bndry_io(uses_exodus ? new ExodusII_IO(bndry_mesh) : NULL);

        // Check to see if this is a restarted run to append current exodus files
        if (uses_exodus)
        {
            const bool from_restart = RestartManager::getManager()->isFromRestart();
            exodus_solid_io->append(from_restart);
            exodus_bndry_io->append(from_restart);
        }

        // Initialize hierarchy configuration and data on all patches.
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();
        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
            if (uses_exodus)
            {
                exodus_solid_io->write_timestep(
                    exodus_solid_filename, *solid_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
                exodus_bndry_io->write_timestep(
                    exodus_bndry_filename, *bndry_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double dt = 0.0;
        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();

            // Setup the position and velocity vector.
            //
            // \todo There needs to be an API so that this can be handled by the IBFEMethod class implementation.
            {
                DenseVector<double> U(NDIM);
                U(1) = -1.0;
                MeshBase& mesh = solid_equation_systems->get_mesh();
                System& X_system = solid_equation_systems->get_system("position");
                const unsigned int X_sys_num = X_system.number();
                NumericVector<double>& X_coords = *X_system.solution;
                System& U_system = solid_equation_systems->get_system("velocity");
                const unsigned int U_sys_num = U_system.number();
                NumericVector<double>& U_coords = *U_system.solution;
                for (MeshBase::node_iterator it = mesh.local_nodes_begin(); it != mesh.local_nodes_end(); ++it)
                {
                    Node* n = *it;
                    if (n->n_vars(X_sys_num))
                    {
                        TBOX_ASSERT(n->n_vars(X_sys_num) == NDIM);
                        const libMesh::Point& X = *n;
                        for (unsigned int d = 0; d < NDIM; ++d)
                        {
                            const int dof_index = n->dof_number(U_sys_num, d, 0);
                            X_coords.set(dof_index, X(d) + loop_time * U(d));
                            U_coords.set(dof_index, U(d));
                        }
                    }
                }
                X_coords.close();
                X_system.get_dof_map().enforce_constraints_exactly(X_system, &X_coords);
                copy_and_synch(X_coords, *X_system.current_local_solution);
                U_coords.close();
                U_system.get_dof_map().enforce_constraints_exactly(U_system, &U_coords);
                copy_and_synch(U_coords, *U_system.current_local_solution);
            }

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            time_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                if (uses_visit)
                {
                    time_integrator->setupPlotData();
                    visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                }
                if (uses_exodus)
                {
                    exodus_solid_io->write_timestep(exodus_solid_filename,
                                                    *solid_equation_systems,
                                                    iteration_num / viz_dump_interval + 1,
                                                    loop_time);
                    exodus_bndry_io->write_timestep(exodus_bndry_filename,
                                                    *bndry_equation_systems,
                                                    iteration_num / viz_dump_interval + 1,
                                                    loop_time);
                }
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
                ib_method_ops->writeFEDataToRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
        }

        // Cleanup Eulerian boundary condition specification objects (when
        // necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown
} // main


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <vector>
#include<list>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
using namespace boost;
namespace py = pybind11;

// supports DELOCX and DELOCY without copying matrix A
// "Serializes" a map. That means, giving an index for every pixel.
// For example: [0123]
// 				[4567]
#define LOC(y,x,Arows) ((y)*(Arows+1)+(x))
// Deserialising. Returns the x or y coordinate for a given index.
#define DELOCY(i,Arows) ((i)/(Arows+1))
#define DELOCX(i,Arows) ((i)%(Arows+1))

// Typedefs for graph_t, vertex_descriptor, edge_descriptor, Edge,
// and out_edge_iterator.
typedef adjacency_list < listS, vecS, directedS,
    no_property, property < edge_weight_t, double > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
typedef std::pair<int, int> Edge;
typedef graph_traits < graph_t >::out_edge_iterator out_edge_iterator;

/*
  Global state, needs mlock!
*/

struct tState
{
    graph_t g;						                        // the graph
    property_map<graph_t, edge_weight_t>::type weightmap;   // the weights
    std::vector<vertex_descriptor> p;		                // predecessor map
    std::vector<double> d;				                    // distance map
    vertex_descriptor global_s;				      // cache the start of dijkstra
    int Arows;
} ;

// The graph

std::vector<tState> gState;

int py_graphFromBitmap(py::array_t<uint8_t> pyo_image, uint8_t walkable=0)
{
    auto A  = pyo_image.mutable_unchecked<2>();
    tState state;

	// Create a matrix from the bitmap and store the number of rows for LOC/DELOC
    state.Arows = A.shape(0);
    std::cout << "Rows:" << state.Arows << ", Columns: " << A.shape(1) << "\n";

    // prepare the graph and search
    // This is the full graph. All points are vertices
    // hence, some vertices are disconnected and make their
    // own connected component, e.g., black pixels
    graph_t _g(A.shape(0)*A.shape(1));
    state.g = _g;
    // Define macro to check, whether an edge can be added from
    // i,j to i+k, j+l
    auto CHECK_EDGE = [&](int i,int j,int k,int l) { return (
	i+k >= 0 && i+k < A.shape(0) &&
	j+l >= 0 && j+l < A.shape(1) &&
	A(i,j) == walkable &&
	A(i+k,j+l) == walkable );};

	// Iterate through the map and add edges, if possible
    int e=0;
    for (int i=0; i< A.shape(0); i++) {
        for (int j=0; j< A.shape(1); j++) {
            // NOTE!!!
            // left      0 -1
            // right     0  1
            // up       -1  0
            // down      1  0
            if (CHECK_EDGE(i,j,-1,0))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,+1,0))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,0,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i,j-1,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,0,1))
                add_edge(LOC(i,j,state.Arows),LOC(i,j+1,state.Arows),1,state.g);
            // DIAGONALS
            if (CHECK_EDGE(i,j,-1,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j-1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,-1,1))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j+1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,1,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j-1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,1,1))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j+1,state.Arows),sqrt(2),state.g);
        }
    }

    std::cout << "Added " << num_vertices(state.g) << " vertices and "
				  << num_edges(state.g)<<" edges.\n";

	// Store the state
    gState.push_back(state);
    int index = gState.size()-1;
    std::cout << "State is " << index << std::endl;
    return index;

}

// -------------
// pure C++ code
// -------------

std::vector<int> multiply(const std::vector<double>& input)
{
  std::vector<int> output(input.size());

  for ( size_t i = 0 ; i < input.size() ; ++i )
    output[i] = 10*static_cast<int>(input[i]);

  return output;
}

// ----------------
// Python interface
// ----------------

struct predicate_violated {};
/*This function actually performs our homotopy test*/
bool py_empty(py::array_t<uint8_t> pyo_image, py::array_t<double> pyo_points, uint8_t allowed=0)
{
    auto image  = pyo_image.mutable_unchecked<2>();
    auto points = pyo_points.unchecked<2>();

    const int polyCorners = points.shape(0);
    std::vector<double> nodeX;
    nodeX.reserve(polyCorners);
    try {
    for (size_t pixelY=0; pixelY < image.shape(1); pixelY++)
    {
	int i,j;

    auto polyX = [&](int row) {return points(row,0);};
    auto polyY = [&](int row) {return points(row,1);};

	nodeX.clear(); j=polyCorners-1;
	for (i=0; i<polyCorners; i++)
	{
	    if (polyY(i)<(double) pixelY && polyY(j)>=(double) pixelY
		||  polyY(j)<(double) pixelY && polyY(i)>=(double) pixelY)
	    {
		nodeX.push_back( (polyX(i)+(pixelY-polyY(i))/(polyY(j)-polyY(i))*(polyX(j)-polyX(i))));
	   }
	  j=i;
    } // for i ranging polyCorners
    std::sort(nodeX.begin(), nodeX.end());

    const auto IMAGE_LEFT=0;
    const auto IMAGE_RIGHT = image.shape(1);

     for (i=0; i<nodeX.size(); i+=2) {
	if   (nodeX[i  ]>=IMAGE_RIGHT) break;

	if   (nodeX[i+1]> IMAGE_LEFT ) {
	  if (nodeX[i  ]< IMAGE_LEFT ) nodeX[i  ]=IMAGE_LEFT ;
	  if (nodeX[i+1]> IMAGE_RIGHT) nodeX[i+1]=IMAGE_RIGHT;
	  for (auto pixelX=(int)nodeX[i]; pixelX<=(int) (nodeX[i+1]+0.5); pixelX++)
	      if (image(pixelX,pixelY) != allowed)
	         throw(predicate_violated());
	  }
	  }
   } // for y
   }catch(predicate_violated v)
    {
    return false;

   }
   return true;

 }

void py_dijkstra(int handle, py::array_t<uint32_t> pyo_p)
{
    tState *state = &gState[handle];  // @TODO: bound check? else crash
    auto p = pyo_p.unchecked<1>();

    vertex_descriptor s = LOC(p(0),p(1),state->Arows);
    state->global_s = s;

    std::cout << "Search: " << s << "\n";
    std::cout << "Using " << num_vertices(state->g) << " vertices and "
				  << num_edges(state->g) << " edges.\n";
    state->weightmap = get(edge_weight, state->g);	// weightmap
    state->p.resize(num_vertices(state->g));		// predecessor
    state->d.resize(num_vertices(state->g));		// distances

	// Perform dijkstra
    dijkstra_shortest_paths(state->g, s,
		predecessor_map(boost::make_iterator_property_map(state->p.begin(), get(boost::vertex_index, state->g))).
		distance_map(boost::make_iterator_property_map(state->d.begin(), get(boost::vertex_index, state->g))));
    std::cout << "Did it " << std::endl;
}

py::array_t<uint32_t> py_getpath(int handle, py::array_t<uint32_t> pyo_p, double penalty=1.0)
{
    tState *state = &gState[handle];  // @TODO: bound check? else crash
    auto p = pyo_p.unchecked<1>();

    vertex_descriptor e = LOC(p(0),p(1),state->Arows);
    if (state->p[e] == e) { // no path found
	  return (py::array_t<uint32_t>(0));
    }
    // Get the shortest path from s to e

    std::list<vertex_descriptor> shortest_path;
    for(vertex_descriptor v = e;;v=state->p[v] ) {
        shortest_path.push_front(v);
        if ( penalty != 1) {
			edge_descriptor e1; bool found;
			tie(e1, found) = edge(state->p[v], v, state->g);
			if(found) {
//				octave_stdout << e1 << " : " << get(state->weightmap, e1);
				put(state->weightmap, e1, get(state->weightmap, e1)*2);
//				octave_stdout << " -> " << get(state->weightmap, e1) << "\n";
			}
		}
        if(state->p[v] == v)
            break;
    }
// allocate py::array (to pass the result of the C++ function to Python)
  // correctly transfer with a capsule
  uint32_t *result_ptr = new uint32_t[shortest_path.size()*2];
  uint32_t *ptr = result_ptr;
  for(const auto pt:shortest_path) {
        *ptr++ =  DELOCY(pt,state->Arows);
        *ptr++  = DELOCX(pt,state->Arows);
    }

      py::capsule free_when_done(result_ptr, [](void *f) {
            uint32_t *foo = reinterpret_cast<uint32_t *>(f);
            //std::cerr << "Element [0] = " << foo[0] << "\n";
            //std::cerr << "freeing memory @ " << f << "\n";
            delete[] foo;
        });

   return py::array_t<uint32_t>(
            shortest_path.size()*2, result_ptr,free_when_done); // numpy array references this parent
 }

void py_getweights(int handle, py::array_t<double> pyo_image)
{
    auto weights  = pyo_image.mutable_unchecked<2>();
    tState *state = &gState[handle];  // @TODO: bound check? else crash

    for (auto r =0; r < weights.shape(0); r++)
      for (auto c = 0; c < weights.shape(1); c++)
      {
	  vertex_descriptor v = LOC(r,c,state->Arows);
	  out_edge_iterator out_i, out_end;
	  edge_descriptor e;
	  double sum = 0;
	  double N = 0;
	  for (tie(out_i, out_end) = out_edges(v, state->g); out_i != out_end; ++out_i)
	  {
			sum += get(state->weightmap, *out_i);
			N++;
	  }
	  weights(r,c) = (N==0)?0:sum/N;
      }
}
/*This function is for debugging, but it is not needed*/

void py_fill(py::array_t<uint8_t> pyo_image, py::array_t<double> pyo_points)
{
    auto image  = pyo_image.mutable_unchecked<2>();
    auto points = pyo_points.unchecked<2>();

    const int polyCorners = points.shape(0);
    std::vector<double> nodeX;
    nodeX.reserve(polyCorners);

    for (size_t pixelY=0; pixelY < image.shape(1); pixelY++)
    {
//	std::cout << "scanning " << pixelY << std::endl;
    int nodes = 0; int i,j;

    auto polyX = [&](int row) {return points(row,0);};
    auto polyY = [&](int row) {return points(row,1);};

	nodeX.clear(); j=polyCorners-1;
	for (i=0; i<polyCorners; i++)
	{
	    if (polyY(i)<(double) pixelY && polyY(j)>=(double) pixelY
		||  polyY(j)<(double) pixelY && polyY(i)>=(double) pixelY)
	    {
		nodeX.push_back( (polyX(i)+(pixelY-polyY(i))/(polyY(j)-polyY(i))*(polyX(j)-polyX(i))));
	   }
	  j=i;
	  // now we have the X of the scanline
    } // for i ranging polyCorners
    std::sort(nodeX.begin(), nodeX.end());
    //std::cout << "On Scanline " << pixelY << " we have " << std::endl;
  //  Fill the pixels between node pairs.

    const auto IMAGE_LEFT=0;
    const auto IMAGE_RIGHT = image.shape(1);

     for (i=0; i<nodeX.size(); i+=2) {
	if   (nodeX[i  ]>=IMAGE_RIGHT) break;

	if   (nodeX[i+1]> IMAGE_LEFT ) {
	  if (nodeX[i  ]< IMAGE_LEFT ) nodeX[i  ]=IMAGE_LEFT ;
	  if (nodeX[i+1]> IMAGE_RIGHT) nodeX[i+1]=IMAGE_RIGHT;
	  for (auto pixelX=(int)nodeX[i]; pixelX<=(int) (nodeX[i+1]+0.5); pixelX++)
	      image(pixelX,pixelY) = 1;
	  }
	  }

    } // for y

}

/*// wrap C++ function with NumPy array IO
py::array_t<int> py_multiply(py::array_t<double, py::array::c_style | py::array::forcecast> array)
{
  // allocate std::vector (to pass to the C++ function)
  std::vector<double> array_vec(array.size());

  // copy py::array -> std::vector
  std::memcpy(array_vec.data(),array.data(),array.size()*sizeof(double));

  // call pure C++ function
  std::vector<int> result_vec = multiply(array_vec);

  // allocate py::array (to pass the result of the C++ function to Python)
  auto result        = py::array_t<int>(array.size());
  auto result_buffer = result.request();
  int *result_ptr    = (int *) result_buffer.ptr;

  // copy std::vector -> py::array
  std::memcpy(result_ptr,result_vec.data(),result_vec.size()*sizeof(int));

  return result;
  }
  */

// wrap as Python module
PYBIND11_MODULE(cfsrouting,m)
{
  m.doc() = "pybind11 example plugin";

//  m.def("multiply", &py_multiply, "Convert all entries of an 1-D NumPy-array to int and multiply by 10");
//  m.def("test", &py_multiply, "Convert all entries of an 1-D NumPy-array to int and multiply by 10");
  m.def ("fill", &py_fill,py::arg().noconvert(), py::arg().none());
  m.def("empty", &py_empty);
  m.def("graphFromBitmap", &py_graphFromBitmap);
  m.def("dijkstra", &py_dijkstra);
  m.def("getpath", &py_getpath);
  m.def("getweights", &py_getweights, py::arg().noconvert(),py::arg().noconvert());
}

/*template<typename vtype>
py::array wrap(vtype v)
{
   return py::array(v.size(),v.data()); // does a copy
}

PYBIND11_MODULE(spatialfacet,m) {
    py::class_<SpatialFacetMiner>(m, "SpatialFacetMiner")
    .def(py::init<>())
    .def("add_database", &SpatialFacetMiner::add_database)
    .def("query", [](SpatialFacetMiner &m,std::string query_string, int first, int max, int check_at_least){return m.query(query_string,first,max,check_at_least, false);})
    .def("query_with_data", [](SpatialFacetMiner &m,std::string query_string, int first, int max, int check_at_least){return m.query(query_string,first,max,check_at_least, true);})
    .def("getSpyData", [](SpatialFacetMiner &m){
	auto &spy = m.getSpy();
	return py::make_tuple(
	    wrap(spy.coords[0]),
	    wrap(spy.coords[1]),
	    wrap(spy.docids),
	    wrap(spy.weights)
	 );
	 })
   .def("getSpyStringData",[](SpatialFacetMiner &m){
      auto &spy = m.getSpy();
      return py::make_tuple(spy.value1, spy.values);

      })

    .def ("augment",[](SpatialFacetMiner &m, std::string query_string,
		       py::array_t<double, py::array::c_style | py::array::forcecast> documents,
		       int n_terms)
    {
	    std::vector<int> stl_documents;
	    auto r = documents.unchecked<1>();
	    for (py::ssize_t i =0; i < r.shape(0); i++)
	      stl_documents.push_back(r(i));

	    std::vector<string> terms; std::vector<double> weights; std::string query_out;
//    void augment_query_from_documents(std::string query_string, std::vector<int> documents, int n_terms,
//				      std::vector<std::string> &terms, std::vector<double> &weights, std::string &query_out)

	    m.augment_query_from_documents(query_string, stl_documents,n_terms, terms, weights, query_out);
	   return py::make_tuple(terms, weights, query_out);

    });

      ;

}
*/


#if defined (_MSC_VER) && !defined (_WIN64)
#pragma warning(disable:4244) // boost::number_distance::distance()
                              // converts 64 to 32 bits integers
#endif

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <string>

#include <boost/function_output_iterator.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Classification.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/jet_estimate_normals.h>
#include <CGAL/Shape_detection/Region_growing.h>
#include <CGAL/Real_timer.h>

#ifdef CGAL_LINKED_WITH_TBB
typedef CGAL::Parallel_tag   Concurrency_tag;
#else
typedef CGAL::Sequential_tag Concurrency_tag;
#endif

typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3                Point;
typedef Kernel::Iso_cuboid_3           Iso_cuboid_3;

typedef CGAL::Point_set_3<Point> Point_set;

typedef Point_set::Point_map                   Pmap;
typedef Point_set::Vector_map                  Vmap;
typedef Point_set::Property_map<int>           Imap;
typedef Point_set::Property_map<unsigned char> UCmap;

typedef CGAL::Shape_detection::Point_set::Sphere_neighbor_query<Kernel, Point_set, Pmap>                Neighbor_query;
typedef CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<Kernel, Point_set, Pmap, Vmap> Region_type;
typedef CGAL::Shape_detection::Region_growing<Point_set, Neighbor_query, Region_type>                   Region_growing;

namespace Classification = CGAL::Classification;

typedef Classification::Label_handle   Label_handle;
typedef Classification::Feature_handle Feature_handle;
typedef Classification::Label_set      Label_set;
typedef Classification::Feature_set    Feature_set;

typedef Classification::Local_eigen_analysis                                 Local_eigen_analysis;
typedef Classification::Point_set_feature_generator<Kernel, Point_set, Pmap> Feature_generator;
typedef Classification::Cluster<Point_set, Pmap>                             Cluster;

int main (int argc, char** argv)
{
  std::string filename        = "data/b9.ply";
  std::string filename_config = "data/b9_clusters_config.gz";

  if (argc > 1)
    filename = argv[1];
  if (argc > 2)
    filename_config = argv[2];

  std::ifstream in (filename.c_str(), std::ios::binary);
  Point_set pts;

  std::cerr << "Reading input" << std::endl;
  in >> pts;

  std::cerr << "Estimating normals" << std::endl;
  CGAL::Real_timer t;
  t.start();
  pts.add_normal_map();
  CGAL::jet_estimate_normals<Concurrency_tag> (pts, 12);
  t.stop();
  std::cerr << "Done in " << t.time() << " second(s)" << std::endl;
  t.reset();

  Feature_set pointwise_features;

  std::cerr << "Generating pointwise features" << std::endl;
  t.start();
  Feature_generator generator (pts, pts.point_map(), 5); // using 5 scales

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.begin_parallel_additions();
#endif

  generator.generate_point_based_features (pointwise_features);
  generator.generate_normal_based_features (pointwise_features, pts.normal_map());

#ifdef CGAL_LINKED_WITH_TBB
  pointwise_features.end_parallel_additions();
#endif

  t.stop();
  std::cerr << "Done in " << t.time() << " second(s)" << std::endl;

  ///////////////////////////////////////////////////////////////////
  //! [Cluster]

  std::cerr << "Detecting planes and creating clusters" << std::endl;
  t.start();

  const double search_sphere_radius  = 1.0;
  const double max_distance_to_plane = 1.0;
  const double max_accepted_angle    = 25.0;
  const std::size_t min_region_size  = 10;

  Neighbor_query neighbor_query (
    pts,
    search_sphere_radius,
    pts.point_map());
  Region_type region_type (
    pts,
    max_distance_to_plane, max_accepted_angle, min_region_size,
    pts.point_map(), pts.normal_map());
  Region_growing region_growing (
    pts, neighbor_query, region_type);

  std::vector<Cluster> clusters;
  region_growing.detect
    (boost::make_function_output_iterator
     ([&](const std::vector<std::size_t>& region) -> void {

        // Create a new cluster.
        Classification::Cluster<Point_set, Pmap> cluster (pts, pts.point_map());
        for (const std::size_t idx : region)
          cluster.insert(idx);
        clusters.push_back(cluster);
      }));

  t.stop();
  std::cerr << clusters.size() << " clusters created in "
            << t.time() << " second(s)" << std::endl;
  t.reset();

  //! [Cluster]
  ///////////////////////////////////////////////////////////////////

  std::cerr << "Computing cluster features" << std::endl;

  ///////////////////////////////////////////////////////////////////
  //! [Eigen]

  Local_eigen_analysis eigen = Local_eigen_analysis::create_from_point_clusters (clusters);

  //! [Eigen]
  ///////////////////////////////////////////////////////////////////

  t.start();

  ///////////////////////////////////////////////////////////////////
  //! [Features]

  Feature_set features;

#ifdef CGAL_LINKED_WITH_TBB
  features.begin_parallel_additions();
#endif

  // First, compute means of features.
  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
    features.add<Classification::Feature::Cluster_mean_of_feature> (clusters, pointwise_features[i]);

#ifdef CGAL_LINKED_WITH_TBB
  features.end_parallel_additions();
  features.begin_parallel_additions();
#endif

  // Then, compute variances of features (and remaining cluster features).
  for (std::size_t i = 0; i < pointwise_features.size(); ++ i)
    features.add<Classification::Feature::Cluster_variance_of_feature> (clusters,
                                                                        pointwise_features[i], // i^th feature
                                                                        features[i]);          // mean of i^th feature

  features.add<Classification::Feature::Cluster_size> (clusters);
  features.add<Classification::Feature::Cluster_vertical_extent> (clusters);

  for (std::size_t i = 0; i < 3; ++ i)
    features.add<Classification::Feature::Eigenvalue> (clusters, eigen, (unsigned int)(i));

#ifdef CGAL_LINKED_WITH_TBB
  features.end_parallel_additions();
#endif

  //! [Features]
  ///////////////////////////////////////////////////////////////////

  t.stop();

  // Add types.
  Label_set labels;
  Label_handle ground     = labels.add ("ground");
  Label_handle vegetation = labels.add ("vegetation");
  Label_handle roof       = labels.add ("roof");

  std::vector<int> label_indices(clusters.size(), -1);

  std::cerr << "Using ETHZ Random Forest Classifier" << std::endl;
  Classification::ETHZ_random_forest_classifier classifier (labels, features);

  std::cerr << "Loading configuration" << std::endl;
  std::ifstream in_config (filename_config, std::ios_base::in | std::ios_base::binary);
  classifier.load_configuration (in_config);

  std::cerr << "Classifying" << std::endl;
  t.reset();
  t.start();
  Classification::classify<Concurrency_tag> (clusters, labels, classifier, label_indices);
  t.stop();

  std::cerr << "Classification done in " << t.time() << " second(s)" << std::endl;
  return EXIT_SUCCESS;
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// associated header
#include "Tasks/QPContacts.h"

// includes
// std
#include <stdexcept>

// Eigen
#include <Eigen/Geometry>

// boost
#include <boost/math/constants/constants.hpp>

namespace tasks
{

namespace qp
{

/// @throw std::domain_error if points is not a valid points index.
void checkRange(int point, const std::vector<Eigen::Vector3d> & points)
{
  if(point < 0 || point >= static_cast<int>(points.size()))
  {
    std::ostringstream str;
    str << "invalid point index: must be in the range [0," << points.size() << "[";
    throw std::domain_error(str.str());
  }
}

/**
 *													FrictionCone
 */

FrictionCone::FrictionCone(const Eigen::Matrix3d & frame, int nrGen, double mu, double dir) : generators(nrGen)
{
  Eigen::Vector3d normal(frame.row(2));
  Eigen::Vector3d tan(dir * frame.row(0));
  double angle = std::atan(mu);

  Eigen::Vector3d gen = Eigen::AngleAxisd(angle, tan) * normal;
  double step = (boost::math::constants::pi<double>() * 2.) / nrGen;

  for(int i = 0; i < nrGen; ++i)
  {
    generators[i] = Eigen::AngleAxisd(dir * step * i, normal) * gen;
  }
}

/**
 *													ContactId
 */

ContactId::ContactId() : r1Index(-1), r2Index(-1), r1BodyName(""), r2BodyName(""), ambiguityId(-1) {}

ContactId::ContactId(int r1I, int r2I, const std::string & r1BName, const std::string & r2BName, int ambId)
: r1Index(r1I), r2Index(r2I), r1BodyName(r1BName), r2BodyName(r2BName), ambiguityId(ambId)
{
}

bool ContactId::operator==(const ContactId & cId) const
{
  return r1Index == cId.r1Index && r2Index == cId.r2Index && r1BodyName == cId.r1BodyName
         && r2BodyName == cId.r2BodyName && ambiguityId == cId.ambiguityId;
}

bool ContactId::operator!=(const ContactId & cId) const
{
  return !((*this) == cId);
}

bool ContactId::operator<(const ContactId & cId) const
{
  return r1Index < cId.r1Index || (r1Index == cId.r1Index && r1BodyName < cId.r1BodyName)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index < cId.r2Index)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index == cId.r2Index
             && r2BodyName < cId.r2BodyName)
         || (r1Index == cId.r1Index && r1BodyName == cId.r1BodyName && r2Index == cId.r2Index
             && r2BodyName == cId.r2BodyName && ambiguityId < cId.ambiguityId);
}

/**
 *													UnilateralContact
 */

UnilateralContact::UnilateralContact(int r1I,
                                     int r2I,
                                     const std::string & r1BName,
                                     const std::string & r2BName,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu), r2Cone(),
  X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

UnilateralContact::UnilateralContact(int r1I,
                                     int r2I,
                                     const std::string & r1BName,
                                     const std::string & r2BName,
                                     int ambId,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName, ambId), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu),
  r2Cone(), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

UnilateralContact::UnilateralContact(const ContactId & cId,
                                     std::vector<Eigen::Vector3d> r1P,
                                     const Eigen::Matrix3d & r1Frame,
                                     const sva::PTransformd & Xbb,
                                     int nrGen,
                                     double mu,
                                     const sva::PTransformd & Xbcf)
: contactId(cId), r1Points(std::move(r1P)), r2Points(), r1Cone(r1Frame, nrGen, mu), r2Cone(), X_b1_b2(Xbb),
  X_b1_cf(Xbcf)
{
  construct(r1Frame, nrGen, mu);
}

Eigen::Vector3d UnilateralContact::force(const Eigen::VectorXd & lambda,
                                         int /* point */,
                                         const FrictionCone & cone) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());

  for(std::size_t i = 0; i < cone.generators.size(); ++i)
  {
    F += cone.generators[i] * lambda(i);
  }

  return F;
}

Eigen::Vector3d UnilateralContact::force(const Eigen::VectorXd & lambda, const FrictionCone & cone) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());
  int pos = 0;

  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    F += force(lambda.segment(pos, nrLambda(i)), i, cone);
    pos += nrLambda(i);
  }

  return F;
}

sva::ForceVecd UnilateralContact::force(const Eigen::VectorXd & lambda,
                                        const std::vector<Eigen::Vector3d> & p,
                                        const FrictionCone & c) const
{
  sva::ForceVecd F_b(Eigen::Vector6d::Zero());
  int pos = 0;

  for(int i = 0; i < int(p.size()); ++i)
  {
    // force at point p in frame b
    sva::ForceVecd F_p_b(Eigen::Vector3d::Zero(), force(lambda.segment(pos, nrLambda(i)), i, c));
    // F_b += xlt(r_b_p)^T F_p_b
    F_b += sva::PTransformd(p[i]).transMul(F_p_b);
    pos += nrLambda(i);
  }

  return F_b;
}

int UnilateralContact::nrLambda(int /* point */) const
{
  return static_cast<int>(r1Cone.generators.size());
}

int UnilateralContact::nrLambda() const
{
  int totalLambda = 0;
  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    totalLambda += nrLambda(i);
  }
  return totalLambda;
}

Eigen::Vector3d UnilateralContact::sForce(const Eigen::VectorXd & lambda, int point, const FrictionCone & cone) const
{
  checkRange(point, r1Points);
  if(static_cast<int>(lambda.rows()) != nrLambda(point))
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << nrLambda(point) << ") gived (" << lambda.rows()
        << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, point, cone);
}

Eigen::Vector3d UnilateralContact::sForce(const Eigen::VectorXd & lambda, const FrictionCone & cone) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, cone);
}

sva::ForceVecd UnilateralContact::sForce(const Eigen::VectorXd & lambda,
                                         const std::vector<Eigen::Vector3d> & r_b_pi,
                                         const FrictionCone & c_b) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, r_b_pi, c_b);
}

int UnilateralContact::sNrLambda(int point) const
{
  checkRange(point, r1Points);
  return nrLambda(point);
}

void UnilateralContact::construct(const Eigen::MatrixXd & r1Frame, int nrGen, double mu)
{
  // compute points in b2 coordinate
  r2Points.reserve(r1Points.size());
  for(const Eigen::Vector3d & p : r1Points)
  {
    r2Points.push_back((sva::PTransformd(p) * X_b1_b2.inv()).translation());
  }

  // compute points frame in b2 coordinate
  // Eigen::Matrix3d r2Frame = (X_b1_b2*sva::PTransformd(Eigen::Matrix3d(r1Frame))).rotation();
  Eigen::Matrix3d r2Frame = (sva::PTransformd(Eigen::Matrix3d(r1Frame)) * X_b1_b2.inv()).rotation();

  // create the b2 cone
  // We take the oppostie frame because force are opposed
  r2Cone = FrictionCone(-r2Frame, nrGen, mu, -1.);
}

/**
 *													BilateralContact
 */

BilateralContact::BilateralContact(int r1I,
                                   int r2I,
                                   const std::string & r1BName,
                                   const std::string & r2BName,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()),
  r2Cones(r1Points.size()), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(int r1I,
                                   int r2I,
                                   const std::string & r1BName,
                                   const std::string & r2BName,
                                   int ambId,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(r1I, r2I, r1BName, r2BName, ambId), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()),
  r2Cones(r1Points.size()), X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(const ContactId & cId,
                                   std::vector<Eigen::Vector3d> r1P,
                                   const std::vector<Eigen::Matrix3d> & r1Frames,
                                   const sva::PTransformd & Xbb,
                                   int nrGen,
                                   double mu,
                                   const sva::PTransformd & Xbcf)
: contactId(cId), r1Points(std::move(r1P)), r2Points(), r1Cones(r1Points.size()), r2Cones(r1Points.size()),
  X_b1_b2(Xbb), X_b1_cf(Xbcf)
{
  construct(r1Frames, nrGen, mu);
}

BilateralContact::BilateralContact(const UnilateralContact & c)
: contactId(c.contactId), r1Points(c.r1Points), r2Points(c.r2Points), r1Cones(c.r1Points.size(), c.r1Cone),
  r2Cones(c.r1Points.size(), c.r2Cone), X_b1_b2(c.X_b1_b2), X_b1_cf(c.X_b1_cf)
{
}

Eigen::Vector3d BilateralContact::force(const Eigen::VectorXd & lambda,
                                        int point,
                                        const std::vector<FrictionCone> & cones) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());

  for(std::size_t i = 0; i < cones[point].generators.size(); ++i)
  {
    F += cones[point].generators[i] * lambda(i);
  }

  return F;
}

Eigen::Vector3d BilateralContact::force(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & cones) const
{
  Eigen::Vector3d F(Eigen::Vector3d::Zero());
  int pos = 0;

  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    F += force(lambda.segment(pos, nrLambda(i)), i, cones);
    pos += nrLambda(i);
  }

  return F;
}

sva::ForceVecd BilateralContact::force(const Eigen::VectorXd & lambda,
                                       const std::vector<Eigen::Vector3d> & p,
                                       const std::vector<FrictionCone> & c) const
{
  sva::ForceVecd F_b(Eigen::Vector6d::Zero());
  int pos = 0;

  for(int i = 0; i < int(p.size()); ++i)
  {
    // force at point p in frame b
    sva::ForceVecd F_p_b(Eigen::Vector3d::Zero(), force(lambda.segment(pos, nrLambda(i)), i, c));
    // F_b += xlt(r_b_p)^T F_p_b
    F_b += sva::PTransformd(p[i]).transMul(F_p_b);
    pos += nrLambda(i);
  }

  return F_b;
}

int BilateralContact::nrLambda(int point) const
{
  return static_cast<int>(r1Cones[point].generators.size());
}

int BilateralContact::nrLambda() const
{
  int totalLambda = 0;
  for(int i = 0; i < int(r1Points.size()); ++i)
  {
    totalLambda += nrLambda(i);
  }
  return totalLambda;
}

Eigen::Vector3d BilateralContact::sForce(const Eigen::VectorXd & lambda,
                                         int point,
                                         const std::vector<FrictionCone> & cones) const
{
  checkRange(point, r1Points);
  if(static_cast<int>(lambda.rows()) != nrLambda(point))
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << nrLambda(point) << ") gived (" << lambda.rows()
        << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, point, cones);
}

Eigen::Vector3d BilateralContact::sForce(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & cones) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, cones);
}

sva::ForceVecd BilateralContact::sForce(const Eigen::VectorXd & lambda,
                                        const std::vector<Eigen::Vector3d> & r_b_pi,
                                        const std::vector<FrictionCone> & c_pi_b) const
{
  int totalLambda = nrLambda();

  if(static_cast<int>(lambda.rows()) != totalLambda)
  {
    std::ostringstream str;
    str << "number of lambda and generator mismatch: expected (" << totalLambda << ") gived (" << lambda.rows() << ")";
    throw std::domain_error(str.str());
  }

  return force(lambda, r_b_pi, c_pi_b);
}

int BilateralContact::sNrLambda(int point) const
{
  checkRange(point, r1Points);
  return nrLambda(point);
}

void BilateralContact::construct(const std::vector<Eigen::Matrix3d> & r1Frames, int nrGen, double mu)
{
  assert(r1Points.size() == r1Frames.size());

  r2Points.reserve(r1Points.size());
  sva::PTransformd X_b2_b1(X_b1_b2.inv());
  for(std::size_t i = 0; i < r1Points.size(); ++i)
  {
    // compute point i in b2 coordinate
    sva::PTransformd X_b1_p(r1Frames[i], r1Points[i]);
    sva::PTransformd X_b2_p = X_b1_p * X_b2_b1;
    r2Points.push_back(X_b2_p.translation());

    // construct r1 cone
    r1Cones[i] = FrictionCone(r1Frames[i], nrGen, mu);
    // create the b2 cone
    // We take the oppostie frame because force are opposed
    r2Cones[i] = FrictionCone(-X_b2_p.rotation(), nrGen, mu, -1.);
  }
}

} // namespace qp

} // namespace tasks


#include <CGAL/algorithm.h>
#include <CGAL/function_objects.h>
#include <vector>
#include <iostream>
#include <boost/functional.hpp>

int main()
{
  std::vector< int > v;
  v.push_back(3);
  v.push_back(5);
  v.push_back(2);
  std::cout << "min_odd = "
            << *CGAL::min_element_if(v.begin(),
                                     v.end(),
                                     [](int i){ return (i%2) > 0; })
            << std::endl;
  return 0;
}


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

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// Copyright (c) 2016-2018 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/formulas/authalic_radius_sqr.hpp>
#include <boost/geometry/formulas/eccentricity_sqr.hpp>

#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Geographic area calculation
\ingroup strategies
\details Geographic area calculation by trapezoidal rule plus integral
         approximation that gives the ellipsoidal correction
\tparam FormulaPolicy Formula used to calculate azimuths
\tparam SeriesOrder The order of approximation of the geodesic integral
\tparam Spheroid The spheroid model
\tparam CalculationType \tparam_calculation
\author See
- Danielsen JS, The area under the geodesic. Surv Rev 30(232): 61–66, 1989
- Charles F.F Karney, Algorithms for geodesics, 2011 https://arxiv.org/pdf/1109.4448.pdf

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
\* [link geometry.reference.srs.srs_spheroid srs::spheroid]
}
*/
template
<
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
    // Switch between two kinds of approximation(series in eps and n v.s.series in k ^ 2 and e'^2)
    static const bool ExpandEpsN = true;
    // LongSegment Enables special handling of long segments
    static const bool LongSegment = false;

    //Select default types in case they are not set

public:
    template <typename Geometry>
    struct result_type
        : strategy::area::detail::result_type
            <
                Geometry,
                CalculationType
            >
    {};

protected :
    struct spheroid_constants
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::value,
                typename geometry::radius_type<Spheroid>::type,
                CalculationType
            >::type calc_t;

        Spheroid m_spheroid;
        calc_t const m_a2;  // squared equatorial radius
        calc_t const m_e2;  // squared eccentricity
        calc_t const m_ep2; // squared second eccentricity
        calc_t const m_ep;  // second eccentricity
        calc_t const m_c2;  // squared authalic radius

        inline spheroid_constants(Spheroid const& spheroid)
            : m_spheroid(spheroid)
            , m_a2(math::sqr(get_radius<0>(spheroid)))
            , m_e2(formula::eccentricity_sqr<calc_t>(spheroid))
            , m_ep2(m_e2 / (calc_t(1.0) - m_e2))
            , m_ep(math::sqrt(m_ep2))
            , m_c2(formula_dispatch::authalic_radius_sqr
                    <
                        calc_t, Spheroid, srs_spheroid_tag
                    >::apply(m_a2, m_e2))
        {}
    };

public:
    template <typename Geometry>
    class state
    {
        friend class geographic;

        typedef typename result_type<Geometry>::type return_type;

    public:
        inline state()
            : m_excess_sum(0)
            , m_correction_sum(0)
            , m_crosses_prime_meridian(0)
        {}

    private:
        inline return_type area(spheroid_constants const& spheroid_const) const
        {
            return_type result;

            return_type sum = spheroid_const.m_c2 * m_excess_sum
                   + spheroid_const.m_e2 * spheroid_const.m_a2 * m_correction_sum;

            // If encircles some pole
            if (m_crosses_prime_meridian % 2 == 1)
            {
                std::size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = return_type(2.0)
                         * geometry::math::pi<return_type>()
                         * spheroid_const.m_c2
                         * return_type(times_crosses_prime_meridian)
                         - geometry::math::abs(sum);

                if (geometry::math::sign<return_type>(sum) == 1)
                {
                    result = - result;
                }

            }
            else
            {
                result = sum;
            }

            return result;
        }

        return_type m_excess_sum;
        return_type m_correction_sum;

        // Keep track if encircles some pole
        std::size_t m_crosses_prime_meridian;
    };

public :
    explicit inline geographic(Spheroid const& spheroid = Spheroid())
        : m_spheroid_constants(spheroid)
    {}

    template <typename PointOfSegment, typename Geometry>
    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      state<Geometry>& st) const
    {
        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {
            typedef geometry::formula::area_formulas
                <
                    typename result_type<Geometry>::type,
                    SeriesOrder, ExpandEpsN
                > area_formulas;

            typename area_formulas::return_type_ellipsoidal result =
                     area_formulas::template ellipsoidal<FormulaPolicy::template inverse>
                                             (p1, p2, m_spheroid_constants);

            st.m_excess_sum += result.spherical_term;
            st.m_correction_sum += result.ellipsoidal_term;

            // Keep track whenever a segment crosses the prime meridian
            if (area_formulas::crosses_prime_meridian(p1, p2))
            {
                st.m_crosses_prime_meridian++;
            }
        }
    }

    template <typename Geometry>
    inline typename result_type<Geometry>::type
        result(state<Geometry> const& st) const
    {
        return st.area(m_spheroid_constants);
    }

private:
    spheroid_constants m_spheroid_constants;

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <>
struct default_strategy<geographic_tag>
{
    typedef strategy::area::geographic<> type;
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP


#define BOOST_TEST_MODULE pcraster model_engine fopointarray
#include <boost/test/unit_test.hpp>
#include "com_math.h"
#include "calc_foarrayimplmanual.h"
#include "calc_fopointarray.h"
#include "calc_fopointimpl.h"

BOOST_AUTO_TEST_CASE(test1)
{
  using namespace calc;

#include "calc_fopointarrayimpl.inc"

{
 INT4  l[3]={1,2,3};
 float  r[3];
 pcr::setMV(l[1]);
 foAr_c_4_2_s_4.f(r, l,3);
 BOOST_CHECK(r[0]==1);
 BOOST_CHECK(pcr::isMV(r[1]));
 BOOST_CHECK(r[2]==3);
}
 {
  float l[4] = { 10, 4, 8, 0 };
  float r[4] = { 4, 10, 0, 9 };
  pcr::setMV(l[3]);
  pcr::setMV(r[2]);
  UINT1 res[4];
  foAr_gt_f.ss(res,l,r,4);
  BOOST_CHECK(res[0]==1);
  BOOST_CHECK(res[1]==0);
  BOOST_CHECK(res[2]==MV_UINT1);
  BOOST_CHECK(res[3]==MV_UINT1);
 }
{
  float l[4] = { 10, 4, 8, 0 };
  float r[1] = { 4 };
  pcr::setMV(l[3]);
  UINT1 res[4];
  foAr_gt_f.sn(res,l,r,4);
  BOOST_CHECK(res[0]==1);
  BOOST_CHECK(res[1]==0);
  BOOST_CHECK(res[2]==1);
  BOOST_CHECK(res[3]==MV_UINT1);
 }
{
  float l[1] = { 4 };
  float r[4] = { 10, 4, 2, 0 };
  pcr::setMV(r[3]);
  UINT1 res[4];
  foAr_gt_f.ns(res,l,r,4);
  BOOST_CHECK(res[0]==0);
  BOOST_CHECK(res[1]==0);
  BOOST_CHECK(res[2]==1);
  BOOST_CHECK(res[3]==MV_UINT1);
 }
{
 {
  float l[2] = { 10, 4 };
  float r[2] = { 4,  0 };
  foAr_mod_f.ss(l,r,2);
  BOOST_CHECK(l[0]==2);
  BOOST_CHECK(pcr::isMV(l[1]));
  BOOST_CHECK(r[0]==4);
  BOOST_CHECK(r[1]==0);
 }
 {
  float l[1] = { 10 };
  float r[2] = { 4,  0 };
  foAr_mod_f.ns(l,r,2);
  BOOST_CHECK(l[0]==10);
  BOOST_CHECK(r[0]==2);
  BOOST_CHECK(pcr::isMV(r[1]));
 }
 {
  float l[2] = { 10, 4 };
  float r[1] = {  0 };
  bool catched(false);
  try {
    foAr_mod_f.sn(l,r,2);
  } catch(const DomainError& ) {
    catched=true;
  }
  BOOST_CHECK(catched);
 }
}
{
 {
  float l[3]={-1,2,4};
  float r[3]={-1,2,4};
  pcr::setMV(r[1]);
  foAr_badd_f.ss( l,r,3);
  BOOST_CHECK(l[0]==-2);
  BOOST_CHECK(r[0]==-1);
  BOOST_CHECK(pcr::isMV(l[1]));
  BOOST_CHECK(pcr::isMV(r[1]));
  BOOST_CHECK(l[2]==8);
  BOOST_CHECK(r[2]==4);
 }
 {
  float l[1]={10};
  float r[3]={1,2,4};
  pcr::setMV(r[1]);
  foAr_badd_f.ns( l,r,3);
  BOOST_CHECK(l[0]==10);
  BOOST_CHECK(r[0]==11);
  BOOST_CHECK(pcr::isMV(r[1]));
  BOOST_CHECK(r[2]==14);
 }
 {
  float l[1]={10};
  float r[3]={1,2,4};
  pcr::setMV(r[1]);
  foAr_badd_f.sn(r,l,3);  // swap as above
  BOOST_CHECK(l[0]==10);
  BOOST_CHECK(r[0]==11);
  BOOST_CHECK(pcr::isMV(r[1]));
  BOOST_CHECK(r[2]==14);
 }
}
{
 float l[3]={-1,2,4};
 pcr::setMV(l[1]);
 foAr_sqrt_f.f( l,3);
 BOOST_CHECK(pcr::isMV(l[1]));
 BOOST_CHECK(l[2]==2);
 BOOST_CHECK(pcr::isMV(l[0]));
}
{
 float l[3]={1,2,3};
 foAr_umin_f.f( l,3);
 BOOST_CHECK(l[0]==-1);
 BOOST_CHECK(l[1]==-2);
 BOOST_CHECK(l[2]==-3);
}
{
 INT4 l[3]={5,0,MV_INT4};
 UINT1 r[3];
 foAr_c_4_2_b_4.f(r, l,3);
 BOOST_CHECK(r[0]==1);
 BOOST_CHECK(r[1]==0);
 BOOST_CHECK(r[2]==MV_UINT1);
 BOOST_CHECK(l[0]==5);
 BOOST_CHECK(l[1]==0);
 BOOST_CHECK(l[2]==MV_INT4);
}
{ // pcrcalc337a
    REAL4 r= -0.047F;
    REAL4 l= 1.5F;
    BOOST_CHECK(special::pow_domainIll(r,l));
}
}

BOOST_AUTO_TEST_CASE(testAggregate)
{
  using namespace calc;

 {
  UINT1 r,i[4]={MV_UINT1,4,0,3};
  AggregateArray< MapTotal<UINT1> > f;
  f.fImpl(&r,i,4);
  BOOST_CHECK(r==7);
 }
 {
  INT4 r,i[4]={MV_INT4,4,0,3};
  AggregateArray< MapMaximum<INT4> > f;
  f.fImpl(&r,i,4);
  BOOST_CHECK(r == 4);
 }
}


#ifndef TENSOR_HH
#define TENSOR_HH

#include <type_traits>
#include <Eigen/Dense>
#include "../ElasticityTensor.hh"
#include "../Flattening.hh"
#include "../SymmetricMatrix.hh"
#include "../Types.hh"
#include "EnergyTraits.hh"

template<typename _Real,
         size_t t_N,
         typename _Storage_t,
         typename _ConstStorageRef_t,
         typename _Derived>
_Real
doubleContract(const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t>& lhs,
               const Eigen::MatrixBase<_Derived>& rhs)
{
    // Note: Some template metaprogramming should be used to test that the
    // one scalar is convertible into the other and make the return type
    // the type that is greater for the convertible relation.
    static_assert(std::is_same<_Real, typename _Derived::Scalar>::value,
                  "Different scalar types between the operand is not supported");

    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime &&
                    _Derived::RowsAtCompileTime == t_N,
                  "");

    // Note: This can be optimized by using the fact that lhs is symmetric
    _Real e = 0;
    for (size_t i = 0; i < t_N; ++i) {
        for (size_t j = 0; j < t_N; ++j) {
            e += lhs(i, j) * rhs(i, j);
        }
    }

    return e;
}

template<typename _Real,
         size_t t_N,
         typename _Storage_t,
         typename _ConstStorageRef_t,
         typename _Derived>
_Real
doubleContract(const Eigen::MatrixBase<_Derived>& lhs,
               const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t>& rhs)
{
    return doubleContract(rhs, lhs);
}

template<typename _Derived>
void symmetrize(Eigen::MatrixBase<_Derived>& m) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    m = 0.5 * (m + m.transpose()).eval();
}

template<typename EigenType>
using SMVType = SymmetricMatrixValue<typename EigenType::Scalar,
                                     EigenType::RowsAtCompileTime>;

template<typename _Derived>
SMVType<_Derived>
symmetrized(const Eigen::MatrixBase<_Derived> &A) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    return SMVType<_Derived>(0.5 * (A + A.transpose()), typename SMVType<_Derived>::skip_validation());
}

template<typename _Derived>
SMVType<_Derived>
symmetrized_x2(const Eigen::MatrixBase<_Derived> &A) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetrization only makes sense for square matrices");
    return SMVType<_Derived>(A + A.transpose(), typename SMVType<_Derived>::skip_validation());
}

template<typename _Derived>
bool isSymmetric(const Eigen::MatrixBase<_Derived>& matrix) {
    static_assert(_Derived::RowsAtCompileTime == _Derived::ColsAtCompileTime,
                  "Symmetry check only makes sense for square matrices");

    for (size_t col = 0; col < _Derived::ColsAtCompileTime; ++col) {
        for (size_t row = 0; row <= col; ++row) {
            if (std::abs(matrix(row, col) - matrix(col, row)) > 1e-13)
                return false;
        }
    }
    return true;
}

// Compute the scalar product of two matrices A : B
template<typename _Derived1, typename _Derived2>
typename _Derived1::Scalar doubleContract(const Eigen::MatrixBase<_Derived1>& A, const Eigen::MatrixBase<_Derived2>& B)
{
    static_assert((int(_Derived1::RowsAtCompileTime) == int(_Derived2::RowsAtCompileTime)) &&
                  (int(_Derived1::ColsAtCompileTime) == int(_Derived2::ColsAtCompileTime)), "Dimensions of A and B must match to compute A : B");
    return (A.transpose() * B).trace();
}

template<typename _Real, size_t _Dim, typename Derived>
SymmetricMatrixValue<_Real, _Dim>
doubleContract(const ElasticityTensor<_Real, _Dim>& A, const Eigen::MatrixBase<Derived>& B)
{
    SymmetricMatrixValue<_Real, _Dim> result;
    for (size_t i = 0; i < _Dim; ++i)
        for (size_t j = i; j < _Dim; ++j)
            for (size_t k = 0; k < _Dim; ++k)
                for (size_t l = 0; l < _Dim; ++l)
                    result(i, j) += A(i, j, k, l) * B(k, l);

    return result;
}

/**
 *  Puts in the given matrix zeros everywhere except in (\a row, \a col).
 */
template<typename Derived>
void
setUnitMatrix(size_t row, size_t col, Eigen::MatrixBase<Derived>& out)
{
    out.setZero();
    out(row, col) = 1.;
}

// Note: This could be factored by having a function that returnes the major index and another
// function that returnes the non-major index.
/**
 *  Matrix indices manipulation helper
 */
template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
struct Indices;

template<size_t _Dimension>
struct Indices<_Dimension, Eigen::ColMajor>
{
    static std::tuple<size_t, size_t> getNext(size_t row, size_t col)
    {
        ++row;
        if (row == _Dimension)
        {
            return std::make_tuple(0, col + 1);
        }
        return std::make_tuple(row, col);
    }

    static std::tuple<size_t, size_t> getUpperTriangleNext(size_t row, size_t col)
    {
        ++row;
        if (row > col)
        {
            return std::make_tuple(0, col + 1);
        }
        return std::make_tuple(row, col);
    }

    static bool arePastEnd(size_t /* row */, size_t col) { return col >= _Dimension; }
};

template<size_t _Dimension>
struct Indices<_Dimension, Eigen::RowMajor>
{
    static std::tuple<size_t, size_t> getNext(size_t row, size_t col)
    {
        ++col;
        if (col == _Dimension)
        {
            return std::make_tuple(row + 1, 0);
        }
        return std::make_tuple(row, col);
    }

    static std::tuple<size_t, size_t> getUpperTriangleNext(size_t row, size_t col)
    {
        ++col;
        if (col > row)
        {
            return std::make_tuple(row + 1, 0);
        }
        return std::make_tuple(row, col);
    }

    static bool arePastEnd(size_t row, size_t /* col */) { return row >= _Dimension; }
};

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
std::tuple<size_t, size_t> getNextIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::getNext(row, col);
}

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
std::tuple<size_t, size_t> getUpperTriangleNextIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::getUpperTriangleNext(row, col);
}

template<size_t _Dimension, size_t _StoragePolicy = Eigen::ColMajor>
bool arePastEndIndices(size_t row, size_t col) {
    return Indices<_Dimension, _StoragePolicy>::arePastEnd(row, col);
}

////////////////////////////////////////////////////////////////////////////////
// Support for accelerating calculations involving Jacobians of vector-valued
// shape functions
////////////////////////////////////////////////////////////////////////////////

// The vectorized shape functions are of the form
//      e_c phi_n
// and their Jacobians look like:
//      e_c \otimes grad phi_n
// where e_c is a canonical basis vector for R^D (the output space dimension)
// and grad phi_n is a vector in R^N (the input space dimension).
// This class provides a compact representation for these Jacobians which
// will also allow more efficient contraction operations.
template<int D, class GradType>
struct VectorizedShapeFunctionJacobian {
    static constexpr int N = GradType::RowsAtCompileTime;

    // Emulate part of Eigen's interface.
    // This also allows VectorizedShapeFunctionJacobian to
    // masquerade as a DxN matrix in metaprogramming type checks (e.g., isMatrixOfSize).
    static constexpr int RowsAtCompileTime = D;
    static constexpr int ColsAtCompileTime = N;
    using Scalar     = typename GradType::Scalar;
    using MatrixType = Eigen::Matrix<Scalar, D, N>;
    using ColVec     = Eigen::Matrix<Scalar, D, 1>;
    using Derived    = MatrixType;

    int c;
    GradType g;

    VectorizedShapeFunctionJacobian(int cc, Eigen::Ref<const GradType> gg)
        : c(cc), g(gg) { }

    MatrixType toMatrix() const {
        MatrixType result(MatrixType::Zero());
        result.row(c) = g.transpose();
        return result;
    }

    // Note: it doesn't seem possible to actually use this explicit cast operator
    // except by directly calling `.operator MatrixType()`--this is because Eigen's
    // converting constructor is preferred when issuing a
    // `static_cast<MatrixType()` or `MatrixType()`.
    explicit operator MatrixType() const { // Allow conversion to underlying matrix type when necessary.
        return toMatrix();
    }

    // Note: this method provides the same conversion interface as Eigen::DenseBase::matrix();
    // this allows generic code to call `.matrix()` on VSFJ or Eigen types.
    MatrixType matrix() const { return toMatrix(); }

    template<class Derived>
    friend auto operator*(const VectorizedShapeFunctionJacobian &A, const Eigen::MatrixBase<Derived> &B) {
        using ResultType = VectorizedShapeFunctionJacobian<D, Eigen::Matrix<Scalar, Derived::ColsAtCompileTime, 1>>;
        return ResultType(A.c, B.template cast<Scalar>().transpose() * A.g);
    }

    template<class Derived>
    friend auto operator*(const Eigen::MatrixBase<Derived> &A, const VectorizedShapeFunctionJacobian &B) {
        return A.col(B.c).template cast<Scalar>() * B.g.transpose();
    }

    template<typename Real2, class Enable = std::enable_if_t<std::is_arithmetic<Real2>::value>>
    friend VectorizedShapeFunctionJacobian operator*(const Real2 &s, const VectorizedShapeFunctionJacobian &B) {
        return VectorizedShapeFunctionJacobian(B.c, s * B.g);
    }

    template<typename Real2, class Enable = std::enable_if_t<std::is_arithmetic<Real2>::value>>
    friend VectorizedShapeFunctionJacobian operator*(const VectorizedShapeFunctionJacobian &B, const Real2 &s) {
        return VectorizedShapeFunctionJacobian(B.c, s * B.g);
    }

    template<class Derived>
    friend MatrixType operator+(const VectorizedShapeFunctionJacobian &A, const Eigen::MatrixBase<Derived> &B) {
        static_assert((RowsAtCompileTime == Derived::RowsAtCompileTime) &&
                      (ColsAtCompileTime == Derived::ColsAtCompileTime), "Size mismatch");
        MatrixType result(B);
        result.row(A.c) += A.g.transpose();
        return result;
    }

    template<class Derived>
    friend ColVec colCross(const VectorizedShapeFunctionJacobian &A, int j, const Eigen::MatrixBase<Derived> &v) {
        // g[j] * e_c.cross(v)
        ColVec result;
        result[ A.c         ] = 0.0;
        result[(A.c + 2) % D] =  A.g[j] * v[(A.c + 1) % D];
        result[(A.c + 1) % D] = -A.g[j] * v[(A.c + 2) % D];
        return result;
    }

    template<class Derived>
    friend MatrixType operator+(const Eigen::MatrixBase<Derived> &A, const VectorizedShapeFunctionJacobian &B) {
        return B + A;
    }
};

// A : (B.c otimes B.g)
template<class Derived, int D, class GradType>
typename Derived::Scalar doubleContract(const Eigen::MatrixBase<Derived> &A,
                      const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    return A.row(B.c).dot(B.g);
}

template<class Derived, int D, class GradType>
auto doubleContract(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                      const Eigen::MatrixBase<Derived> &B) { return doubleContract(B, A); }

template<class T>
struct IsVectorizedShapeFunctionJacobian { static constexpr bool value = false; };
template<int D, class GradType>
struct IsVectorizedShapeFunctionJacobian<VectorizedShapeFunctionJacobian<D, GradType>> {
    static constexpr bool value = true;
};

// Some operations that can be accelerated with VectorizedShapeFunctionJacobian types.
template<class AType, class BType, typename =
    std::enable_if_t<!IsVectorizedShapeFunctionJacobian<AType>::value ||
                     !IsVectorizedShapeFunctionJacobian<BType>::value, void>>
bool AtBKnownZero(const AType &, const BType &) { return false; }

template<int D, class GradType>
bool AtBKnownZero(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                  const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    return A.c != B.c;
}

template<class AType, class BType, typename =
    std::enable_if_t<!IsVectorizedShapeFunctionJacobian<AType>::value ||
                     !IsVectorizedShapeFunctionJacobian<BType>::value, void>>
auto computeAtB(const AType &A, const BType &B) {
    return A.transpose() * B;
}

template<int D, class GradType>
auto computeAtB(const VectorizedShapeFunctionJacobian<D, GradType> &A,
                const VectorizedShapeFunctionJacobian<D, GradType> &B) {
    using VSFJ = VectorizedShapeFunctionJacobian<D, GradType>;
    using Scalar = typename VSFJ::Scalar;
    using Result = Eigen::Matrix<Scalar, VSFJ::N, VSFJ::N>;

    if (A.c == B.c)
        return Result(A.g * B.g.transpose());
    return Result(Result::Zero());
}

template<class Derived1, class Derived2>
Eigen::Matrix<typename Derived1::Scalar, 3, 1>
colCross(const Eigen::MatrixBase<Derived1> &A, int j, const Eigen::MatrixBase<Derived2> &v) {
    static_assert((Derived1::RowsAtCompileTime == 3) && (Derived2::RowsAtCompileTime == 3) && (Derived2::ColsAtCompileTime == 1),
                  "Unexpected sizes for colCross");
    return A.col(j).cross(v);
}

template<int D, class GradType>
SMVType<VectorizedShapeFunctionJacobian<D, GradType>>
symmetrized(const VectorizedShapeFunctionJacobian<D, GradType> &A) {
    SMVType<VectorizedShapeFunctionJacobian<D, GradType>> result; // zero-initializes
    for (int i = 0; i < int(D); ++i)
        result(A.c, i) = ((i == A.c) ? 1.0 : 0.5) * A.g[i];
    return result;
}

// Note: C better be symmetric!
template<class Mat_>
Eigen::Matrix<typename Mat_::Scalar,
              Mat_::RowsAtCompileTime,
              Mat_::ColsAtCompileTime>
spdMatrixSqrt(const Mat_ &C) {
    constexpr static int N = Mat_::RowsAtCompileTime;
    static_assert((N == 2) || (N == 3), "Unexpected matrix size");
    using MNd = Eigen::Matrix<typename Mat_::Scalar, N, N>;
    return Eigen::SelfAdjointEigenSolver<MNd>(C).operatorSqrt();
}

// Compute the double contraction `C : e` for fourth order tensor `C` and matrix `e`.
// Assumes that C has been flattened with the same ordering as e's storage storage order!
template<class FlattenedTensorDerived, class Derived>
std::enable_if_t<(FlattenedTensorDerived::RowsAtCompileTime == FlattenedTensorDerived::ColsAtCompileTime)
                && (FlattenedTensorDerived::ColsAtCompileTime == (Derived::RowsAtCompileTime * Derived::ColsAtCompileTime)),
Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>>
applyFlattened4thOrderTensor(const Eigen::MatrixBase<FlattenedTensorDerived> &C, const Eigen::MatrixBase<Derived> &e) {
    using Scalar = typename Derived::Scalar;
    constexpr int M = Derived::RowsAtCompileTime,
                  N = Derived::ColsAtCompileTime;
    using FlatMatrix = Eigen::Matrix<Scalar, M * N, 1>;
    Eigen::Matrix<Scalar, M, N, Derived::Options> result;
    Eigen::Map<FlatMatrix>(result.data()) = C * Eigen::Map<const FlatMatrix>(e.derived().data()).eval();
    return result;
}

// Compute the double contraction `C : e` for fourth order tensor `C` and matrix `e`.
// Assumes that C has been flattened **in column major order**
template<class FlattenedTensorDerived, int D, class GradType>
std::enable_if_t<(FlattenedTensorDerived::RowsAtCompileTime == FlattenedTensorDerived::RowsAtCompileTime)
                && (FlattenedTensorDerived::ColsAtCompileTime == (D * GradType::RowsAtCompileTime)),
Eigen::Matrix<typename FlattenedTensorDerived::Scalar, D, GradType::RowsAtCompileTime>>
applyFlattened4thOrderTensor(const Eigen::MatrixBase<FlattenedTensorDerived> &C,
                             const VectorizedShapeFunctionJacobian<D, GradType> &e) {
    using Scalar = typename FlattenedTensorDerived::Scalar;
    constexpr int M = D,
                  N = GradType::RowsAtCompileTime;
    using FlatMatrix = Eigen::Matrix<Scalar, M * N, 1>;
    // "e" consists of a single nonzero row at index "e.c" with values "e.g"
    // We assume column major ordering, so the flattened version of "e" has
    // nonzero values at indices `e.c + D * i`.
    FlatMatrix flatResult = C.col(e.c) * e.g[0];
    for (int i = 1; i < N; ++i)
        flatResult += C.col(e.c + D * i) * e.g[i];
    return Eigen::Map<Eigen::Matrix<Scalar, M, N>>(flatResult.data());
}

#endif


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP
#define BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP

#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/fix.hpp>
#include <boost/hana/functional/flip.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/power.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/remove_at.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unpack.hpp>

#include <utility>

#include "matrix.hpp"

namespace cppcon {
    namespace hana = boost::hana;
    auto det = hana::fix([](auto det, auto&& m) -> decltype(auto) {
        auto matrix_minor = [=](auto&& m, auto i, auto j) -> decltype(auto) {
            return det(hana::unpack(
                hana::transform(
                    hana::remove_at(rows(std::forward<decltype(m)>(m)), i),
                    hana::partial(hana::flip(hana::remove_at), j)
                ),
                matrix
            ));
        };

        auto cofactor = [=](auto&& m, auto i, auto j) {
            return hana::power(hana::int_c<-1>, hana::plus(i, j)) *
                    matrix_minor(std::forward<decltype(m)>(m), i, j);
        };

        return hana::eval_if(m.size() == hana::size_c<1>,
            hana::always(m.at(hana::size_c<0>, hana::size_c<0>)),
            [=](auto _) {
                auto cofactors_1st_row = hana::unpack(_(hana::make_range)(hana::size_c<0>, m.ncolumns()),
                    hana::on(hana::make_tuple, hana::partial(cofactor, m, hana::size_c<0>))
                );
                return detail::tuple_scalar_product(hana::front(rows(m)), cofactors_1st_row);
            }
        );
    });
} // end namespace cppcon

#endif // !BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP


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

#include <nt2/linalg/functions/det.hpp>
#include <nt2/include/functions/getrf.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/globalprod.hpp>
#include <nt2/core/container/table/table.hpp>
#include <boost/core/ignore_unused.hpp>

namespace nt2{ namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( det_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                 type_t;
    typedef typename boost::dispatch::meta::as_floating<type_t>
                                          ::type  result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      BOOST_ASSERT_MSG( issquare(a0)
                      , "DET: Argument must be a square matrix"
                      );

      nt2::container::table<typename A0::value_type>  lu(a0);
      nt2::container::table<nt2_la_int>               ip;

      // Factorize A as L/U
      nt2_la_int  info = nt2::getrf(boost::proto::value(lu),boost::proto::value(ip));
      boost::ignore_unused(info);

      // DET(A) is the product of LU(A) diagonal by -1 at the power of
      // the number of non-permutations done in LU(A)
      std::size_t n     = nt2::numel(ip);
      result_type sign  = One<result_type>();

      // TODO: Parallelize this somehow ?
      for(std::size_t i = 1;i <= n;++i)
        sign  *= (ip(i) != nt2_la_int(i)) ? 1 : -1;

      return nt2::globalprod(nt2::diag_of(lu))*sign;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( det_, tag::cpu_
                            , (A0)
                            , (scalar_< unspecified_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>
                                          ::type  result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };
} }

#endif


#include "ros/ros.h"
#include "tr3_msgs/InverseIK.h"
#include <Eigen/Geometry>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

bool inverse_ik(tr3_msgs::InverseIK::Request &req, tr3_msgs::InverseIK::Response &res)
{
	robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
	robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
	ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

	robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
	kinematic_state->setToDefaultValues();
	const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("tr3_arm");

	const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();

	const geometry_msgs::Pose &m = req.pose;
	const Eigen::Isometry3d& end_effector_state = Eigen::Translation3d(m.position.x, m.position.y, m.position.z)
		* Eigen::Quaterniond(m.orientation.w, m.orientation.x, m.orientation.y, m.orientation.z);

	std::vector<double> joint_values;

	double timeout = 0.2;
	bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout);

	if (found_ik)
	{
		kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
		for (std::size_t i = 0; i < joint_names.size(); ++i)
		{
			res.state.name.push_back(joint_names[i]);
			res.state.position.push_back(joint_values[i]);
			res.state.effort.push_back(0);
			res.state.velocity.push_back(0);
		}
		ROS_INFO("Found solution");
		return true;
	}
	else
	{
		ROS_INFO("Could not find IK solution");
		return false;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "inverse_ik_server");
	ros::NodeHandle n;

	ros::ServiceServer service = n.advertiseService("inverse_ik", inverse_ik);
	ROS_INFO("Ready.");
	ros::spin();

	return 0;
}