#ifndef CREATEDUALGRAPH_HPP
#define CREATEDUALGRAPH_HPP

#include <GraphLayoutLibrary_global.h>

#include <vector>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <GridBasedLayout/MyPlanarFaceTraversal.hpp>

#include <iostream>
#include <boost/graph/properties.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/ref.hpp>

#include <algorithm>

using namespace boost;

/**
 * A structure to maintain dual graph of a planar graph. Inherited from my_planar_face_traversal_visitor.
 */
template <typename InputGraph, typename OutputGraph, typename EdgeIndexMap>
struct dual_graph_visitor : public my_planar_face_traversal_visitor
{

    typedef typename graph_traits<OutputGraph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<InputGraph>::edge_descriptor edge_t;
    typedef typename std::vector<vertex_t> vertex_vector_t;
    typedef iterator_property_map< typename vertex_vector_t::iterator, EdgeIndexMap > edge_to_face_map_t;
    typedef typename graph_traits<OutputGraph>::edge_descriptor edge_descriptor_t;
    typedef typename property_map<OutputGraph, edge_index_t>::type e_index_t;
    typedef typename graph_traits<OutputGraph>::edges_size_type edge_count_t;
    typedef typename graph_traits<OutputGraph>::edge_iterator edge_iteratot_t;

   dual_graph_visitor(InputGraph& arg_g, OutputGraph& arg_dual_g, EdgeIndexMap arg_em) :
        g(arg_g),
        dual_g(arg_dual_g),
        em(arg_em),
        edge_to_face_vector(num_edges(g), graph_traits<OutputGraph>::null_vertex()),
        edge_to_face(edge_to_face_vector.begin(), em)
    {
    }

    void begin_face()
    {
        //add a vertex (for current face being traversed) to dualG
        current_face = add_vertex(dual_g);
    }

    template <typename Edge, typename adjFaces, typename dualEdges>
    void next_edge(Edge& e, adjFaces &adjecentFaces, dualEdges& dual) //traverse edges bounding current face
    {
        vertex_t existing_face = edge_to_face[e];
        if (existing_face == graph_traits<OutputGraph>::null_vertex()) //if edge is traversed first time
        {
            edge_to_face[e] = current_face;
        }
        else //if edge is traversed second time
        {
            //add edge between its 2 adjecent faces (in dualG)
            edge_descriptor_t newEdge = add_edge(existing_face, current_face, dual_g).first;

            //Initialize the interior edge index in dualG
            e_index_t e_index = get(edge_index, dual_g);
            edge_count_t edge_count = 0;
            edge_iteratot_t ei, ei_end;
            for(boost::tie(ei, ei_end) = edges(dual_g); ei != ei_end; ++ei)
                put(e_index, *ei, edge_count++);

            //remember its dual edge from g
            dual[get(edge_index, dual_g, newEdge)] = e;
        }

        //remember current face as an 'adjecent face' to vertices on current edge (from g)
        adjecentFaces[get(vertex_index, g, source(e, g))].push_back(current_face); //adjecentFaces[u] <= current_face;
        adjecentFaces[get(vertex_index, g, target(e, g))].push_back(current_face); //adjecentFaces[v] <= current_face;
    }

    InputGraph& g;
    OutputGraph& dual_g;
    EdgeIndexMap em;
    vertex_t current_face;
    vertex_vector_t edge_to_face_vector;
    edge_to_face_map_t edge_to_face;
};

template <typename InputGraph, typename OutputGraph, typename PlanarEmbedding, typename EdgeIndexMap, typename adjFaces, typename dualEdges>
void create_dual_graph(InputGraph& g, OutputGraph& dual_g, PlanarEmbedding embedding, EdgeIndexMap em, adjFaces &adjecentFaces, dualEdges& dualE)
{
    dual_graph_visitor<InputGraph, OutputGraph, EdgeIndexMap> visitor(g, dual_g, em);
    planar_face_traversal(g, embedding, visitor, em, adjecentFaces, dualE);
}

template <typename InputGraph, typename OutputGraph, typename PlanarEmbedding, typename adjFaces, typename dualEdges>
void create_dual_graph(InputGraph& g, OutputGraph& dual_g, PlanarEmbedding embedding, adjFaces &adjecentFaces, dualEdges& dualE)
{
    create_dual_graph(g, dual_g, embedding, get(edge_index,g), adjecentFaces, dualE);
}

#endif // CREATEDUALGRAPH_HPP


/*
Copyright (C) 2006 Pedro Felzenszwalb

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
*/

#ifndef DISJOINT_SET
#define DISJOINT_SET

// disjoint-set forests using union-by-rank and path compression (sort of).

typedef struct {
  int rank;
  int p;
  int size;
} uni_elt;

class universe {
public:
  universe(int elements);
  ~universe();
  int find(int x);
  void join(int x, int y);
  int size(int x) const { return elts[x].size; }
  int num_sets() const { return num; }

private:
  uni_elt *elts;
  int num;
};

universe::universe(int elements) {
  elts = new uni_elt[elements];
  num = elements;
  for (int i = 0; i < elements; i++) {
    elts[i].rank = 0;
    elts[i].size = 1;
    elts[i].p = i;
  }
}

universe::~universe() {
  delete [] elts;
}

int universe::find(int x) {
  int y = x;
  while (y != elts[y].p)
    y = elts[y].p;
  elts[x].p = y;
  return y;
}

void universe::join(int x, int y) {
  if (elts[x].rank > elts[y].rank) {
    elts[y].p = x;
    elts[x].size += elts[y].size;
  } else {
    elts[x].p = y;
    elts[y].size += elts[x].size;
    if (elts[x].rank == elts[y].rank)
      elts[y].rank++;
  }
  num--;
}

#endif

/*
Copyright (C) 2006 Pedro Felzenszwalb

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
*/

#ifndef SEGMENT_GRAPH
#define SEGMENT_GRAPH

#include <algorithm>
#include <cmath>
//#include "disjoint-set.h"

// threshold function
#define THRESHOLD(size, c) (c/size)

typedef struct {
  float w;
  int a, b;
} edge;

bool operator<(const edge &a, const edge &b) {
  return a.w < b.w;
}

/*
 * Segment a graph
 *
 * Returns a disjoint-set forest representing the segmentation.
 *
 * num_vertices: number of vertices in graph.
 * num_edges: number of edges in graph
 * edges: array of edges.
 * c: constant for treshold function.
 */
universe *segment_graph(int num_vertices, int num_edges, edge *edges,
			float c) {
  // sort edges by weight
  std::sort(edges, edges + num_edges);

  // make a disjoint-set forest
  universe *u = new universe(num_vertices);

  // init thresholds
  float *threshold = new float[num_vertices];
  for (int i = 0; i < num_vertices; i++)
    threshold[i] = THRESHOLD(1,c);

  // for each edge, in non-decreasing weight order...
  for (int i = 0; i < num_edges; i++) {
    edge *pedge = &edges[i];

    // components conected by this edge
    int a = u->find(pedge->a);
    int b = u->find(pedge->b);
    if (a != b) {
      if ((pedge->w <= threshold[a]) &&
	  (pedge->w <= threshold[b])) {
	u->join(a, b);
	a = u->find(a);
	threshold[a] = pedge->w + THRESHOLD(u->size(a), c);
      }
    }
  }

  // free up
  delete[] threshold;
  return u;
}

#endif

// mexopts = gptoolbox_mexopts('Static',false,'Debug',true);
// mex('segment_graph.cpp',mexopts{:});
#ifdef MEX
#  include <mex.h>
#  include <igl/C_STR.h>
#  include <igl/matlab/mexErrMsgTxt.h>
#  undef assert
#  define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#endif

#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/unique.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/requires_arg.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/MexStream.h>
#include <Eigen/Sparse>
void mexFunction(
 int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  using namespace igl::matlab;
  using namespace Eigen;
  using namespace std;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs>0,"Too few inputs");
  mexErrMsgTxt(mxIsSparse(prhs[0]),"Matrix should be sparse");
  const mxArray * mx_data = prhs[0];
  const int m = mxGetM(mx_data);
  const int n = mxGetN(mx_data);
  mexErrMsgTxt(n == mxGetM(prhs[0]), "Matrix should be square");
  assert(mxIsSparse(mx_data));
  assert(mxGetNumberOfDimensions(mx_data) == 2);
  // TODO: It should be possible to directly load the data into the sparse
  // matrix without going through the triplets
  // Copy data immediately
  double * pr = mxGetPr(mx_data);
  mwIndex * ir = mxGetIr(mx_data);
  mwIndex * jc = mxGetJc(mx_data);
  const int num_edges = mxGetNzmax(mx_data);
  edge * edges = new edge[num_edges];
  int k = 0;
  for(int j=0; j<n;j++)
  {
    // Iterate over inside
    while(k<(int)jc[j+1])
    {
      //cout<<ir[k]<<" "<<j<<" "<<pr[k]<<endl;
      assert((int)ir[k]<m);
      assert((int)j<n);
      edges[k].a = ir[k];
      edges[k].b = j;
      edges[k].w = pr[k];
      k++;
    }
  }

  // defaults
  int min_size = 0;
  // Threshold
  int c = sqrt((double)n);
  {
    int i = 1;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Threshold",name) == 0)
      {
        requires_arg(i,nrhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        validate_arg_double(i,nrhs,prhs,name);
        c = (double)*mxGetPr(prhs[++i]);
      }else if(strcmp("MinSize",name) == 0)
      {
        requires_arg(i,nrhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        validate_arg_double(i,nrhs,prhs,name);
        min_size = (int)((double)*mxGetPr(prhs[++i]));
      }
      i++;
    }
  }

  universe *u = segment_graph(n, num_edges, edges, c);

  // post process small components
  for (int i = 0; i < num_edges; i++) {
    int a = u->find(edges[i].a);
    int b = u->find(edges[i].b);
    if ((a != b) && ((u->size(a) < min_size) || (u->size(b) < min_size)))
      u->join(a, b);
  }

  switch(nlhs)
  {
    case 1:
    {
      plhs[0] = mxCreateDoubleMatrix(m,1, mxREAL);
      Eigen::VectorXi C(m);
      for(int i = 0;i<m;i++)
      {
        C(i) = u->find(i);
      }
      Eigen::VectorXi uC,I,J;
      igl::unique(C,uC,I,J);
      prepare_lhs_index(J,plhs);
    }
    default: break;
  }

  delete[] edges;
  delete u;
  std::cout.rdbuf(outbuf);
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing fmod capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    rounded toward zero.

    This is a convenient alias of @ref rem
  **/
  const boost::dispatch::functor<tag::fmod_> fmod = {};
} }
#endif

#include <boost/simd/function/scalar/rem.hpp>
#include <boost/simd/function/simd/fmod.hpp>

#endif


/* ---------------------------------------------------------------------------
 *
 * Autonomous Navigation and Perception Lab (ANPL),
 * Technion, Israel Institute of Technology,
 * Faculty of Aerospace Engineering,
 * Haifa, Israel, 32000
 * All Rights Reserved
 *
 * See LICENSE for the license information
 *
 * -------------------------------------------------------------------------- */

/**
 * @file: generate_actions_prm.cpp
 * @brief: Implementation of base class for planners
 * @author: Tal Regev
 * @author: Asaf Feniger
 *
 */
#include "planner/generate_actions/generate_actions_prm.h"

#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/SimpleSetup.h>

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

#include <diverse_short_paths/TestData.h>
#include <diverse_short_paths/Levenshtein.h>
#include <diverse_short_paths/Neighborhood.h>
#include <diverse_short_paths/Eppstein.h>
#include <diverse_short_paths/Frechet.h>
#include <diverse_short_paths/Hausdorff.h>
#include <diverse_short_paths/Voss.h>

using namespace ANPL;

namespace ob  = ompl::base;
namespace og  = ompl::geometric;

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

GenerateActionsPRM::GenerateActionsPRM(const Perception *const p_perception, const Goals *const p_goals) :
    GenerateActions(p_perception, p_goals)
{
	ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    m_min_edge_length = 2;
    m_max_edge_length = 10;
    m_max_vertices = 550;
    m_max_path_length = 1000;
    m_min_path_pairwise_distance = 1;

    m_time_candidate_paths = 20;
    m_time_edges_and_vertices = 3;

    m_num_candidate_paths = 10;
    m_low_boundary = -20;
    m_high_boundary = 10;
    m_range_box = 5;

    m_k_diverse_short_alg = EPPSTEIN;
    m_path_distance_measure_alg = LEVENSHTEIN;

    //this two parameters don't in use in original code (Without the xml loader)
    m_radius_factor = 0;
    m_avoid_method = AM::UNKNOWN;

    visualization_msgs::Marker::_scale_type vertices_scale;
    vertices_scale.x = 0.2;
    vertices_scale.y = 0.2;

    visualization_msgs::Marker::_color_type vertices_color;
    // points are red
    vertices_color.r = 1;
    vertices_color.g = 0;
    vertices_color.b = 0;
    vertices_color.a = 1.0;

    visualization_msgs::Marker::_scale_type edges_scale;
    edges_scale.x = 0.1;
    edges_scale.y = 0.1;

    visualization_msgs::Marker::_color_type edges_color;
    // lines are olive green
    edges_color.r = 85  / 255.0;
    edges_color.g = 107 / 255.0;
    edges_color.b = 47  / 255.0;
    edges_color.a = 1.0;

    visualization_msgs::Marker::_scale_type bounds_scale;
    bounds_scale.x = 0.1;
    bounds_scale.y = 0.1;

    visualization_msgs::Marker::_color_type bounds_color;
    // points are black
    bounds_color.r = 0;
    bounds_color.g = 0;
    bounds_color.b = 0;
    bounds_color.a = 1.0;

    visualization_msgs::Marker::_scale_type start_goal_scale;
    start_goal_scale.x = 0.5;
    start_goal_scale.y = 0.5;

    visualization_msgs::Marker::_color_type start_goal_color;
    // points are dark blue
    start_goal_color.r = 0;
    start_goal_color.g = 0;
    start_goal_color.b = 139 / 255.0;
    start_goal_color.a = 1.0;

    visualization_msgs::Marker::_scale_type candidate_paths_scale;
    candidate_paths_scale.x = 0.1;
    candidate_paths_scale.y = 0.1;

//    init("map",
//         vertices_scale,
//         vertices_color,
//         edges_scale,
//         edges_color,
//         bounds_scale,
//         bounds_color,
//         start_goal_scale,
//         start_goal_color,
//         candidate_paths_scale
//    );
}

GenerateActionsPRM::GenerateActionsPRM(const Perception *const p_perception, const Goals *const p_goals, const Visualization *const vis, ConfigLoader& loader) :
        GenerateActions(p_perception, p_goals, loader, vis),
        m_min_edge_length(loadValueAs(loader, m_min_edge_length)),
        m_max_edge_length(loadValueAs(loader, m_max_edge_length)),
        m_max_vertices(loadValueAs(loader, m_max_vertices)),
        m_max_path_length(loadValueAs(loader, m_max_path_length)),
        m_min_path_pairwise_distance(loadValueAs(loader, m_min_path_pairwise_distance)),
        m_num_candidate_paths(loadValueAs(loader, m_num_candidate_paths)),
        m_low_boundary(loadValueAs(loader, m_low_boundary)),
        m_high_boundary(loadValueAs(loader, m_high_boundary)),
        m_range_box(loadValueAs(loader, m_range_box)),
        m_radius_factor(loadValueAs(loader, m_radius_factor))
{
	ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    std::string ns = "GenerateActionsPRM";

    m_vis_vertices.ns       = ns;
    m_vis_edges.ns          = ns;
    m_vis_bounds.ns         = ns;
    m_vis_start_goal.ns     = ns;
    m_vis_vertices_text.ns  = ns;

    loadKDS(loader);
    loadPDM(loader);
    loadAM(loader);
}

void GenerateActionsPRM::setup(ob::PlannerTerminationConditionFn &conditionFn) {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    ob::StateSpacePtr space(new ob::SE2StateSpace());

    ob::RealVectorBounds bounds(2);
    bounds.setLow(m_low_boundary);
    bounds.setHigh(m_high_boundary);

    space->as<ob::SE2StateSpace>()->setBounds(bounds);

    // define a simple setup class
    og::SimpleSetup simple_setup(space);

    Waypoint pose = m_p_perception->getRobotLocation();
    ob::ScopedState<ob::SE2StateSpace> start(space);
    start->setX(pose.x());
    start->setY(pose.y());
    start->setYaw(pose.rotation().yaw());

    // create a goal state
    Waypoint nextGoal = m_p_goals->front().getWaypoint();
    ob::ScopedState<ob::SE2StateSpace> goal(space);
    goal->setX(nextGoal.x());
    goal->setY(nextGoal.y());
    goal->setYaw(nextGoal.rotation().yaw());

    // set the start and goal states
    simple_setup.setStartAndGoalStates(start, goal);

    m_planner = ob::PlannerPtr(new og::PRM(simple_setup.getSpaceInformation()));

    og::PRM::ConnectionFilter connection_filter = prmFilterStrategy();
    m_planner->as<og::PRM>()->setConnectionFilter(connection_filter);

    simple_setup.setPlanner(m_planner);

    conditionFn = [this]()
    {
        ob::PlannerData data(m_planner->getSpaceInformation());
        m_planner->getPlannerData(data);
        unsigned long int num_vertices = data.numVertices();
        bool to_stop =  num_vertices >= m_max_vertices;
        return to_stop;
    };

    // set state validity checking for this space
    simple_setup.setStateValidityChecker(boost::bind(&GenerateActionsPRM::isStateValidFCL, this, _1));
    double resolution = m_p_perception->getResolution();
    simple_setup.getSpaceInformation()->setStateValidityCheckingResolution(resolution);

    simple_setup.setup();
    ob::PlannerTerminationConditionFn stopConditionFn = [this]()
    {
        return true;
    };
    simple_setup.solve(stopConditionFn);
}

og::PRM::ConnectionFilter GenerateActionsPRM::prmFilterStrategy() {
	ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    og::PRM::ConnectionFilter connection_filter = [this](const og::PRM::Vertex &v1, const og::PRM::Vertex &v2) {
        //from: http://www.boost.org/doc/libs/1_38_0/libs/graph/doc/using_adjacency_list.html
        og::PRM::Graph g = m_planner->as<og::PRM>()->getRoadmap();
        boost::property_map<og::PRM::Graph, og::PRM::vertex_state_t>::type
                states = boost::get(og::PRM::vertex_state_t(), g);
        ob::State* state1 = states[v1];
        ob::State* state2 = states[v2];
        gtsam::Pose3 waypoint1(Conversion<gtsam::Pose2>::as(state1));
        gtsam::Pose3 waypoint2(Conversion<gtsam::Pose2>::as(state2));

        double range = waypoint1.range(waypoint2);

        Globals::string_stream << std::endl << "v1: "    << v1
                               << std::endl << "v2: "    << v2
                               << std::endl << "g[v1]: " << waypoint1.translation()
                               << std::endl << "g[v2]: " << waypoint2.translation()
                               << std::endl << "range: " << range;
        anplLogMessage(info, 2, Globals::string_stream, LogTag::generate_action);

        if (range <= m_max_edge_length && range >= m_min_edge_length) {
            return !m_p_perception->isObstacleAheadFCL(waypoint1, waypoint2);
        }
        return false;
    };
    return connection_filter;
}

bool GenerateActionsPRM::isStateValid(const ob::State *state) const {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    Waypoint waypoint(Conversion<gtsam::Pose2>::as(state));
    return !m_p_perception->isStateOccupied(waypoint);
}

bool GenerateActionsPRM::isStateValidFCL(const ob::State *state) const {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    Waypoint waypoint(Conversion<gtsam::Pose2>::as(state));
    Waypoint pose = m_p_perception->getRobotLocation();
    if (waypoint.x() == pose.x() && waypoint.y() == pose.y() && pose.rotation().yaw() == waypoint.rotation().yaw()) {
        return true;
    }
    return !m_p_perception->isStateOccupiedFCL(waypoint);
}

ActionsPaths GenerateActionsPRM::generateActions() {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    const bool is_vis_edges_and_vertices    = true;
    unsigned int id_vis                     = m_id_start + 1;

    m_vis_array.markers.push_back(m_vis_clear_all);
    m_marker_array_pub.publish(m_vis_array);
    m_vis_array.markers.clear();

    // Setup the planner: m_planner member and stop condition (maximum number of vertices)
    ob::PlannerTerminationConditionFn conditionFn;
    setup(conditionFn);

    setMarkerTime(m_vis_start_goal);
    setMarkerTime(m_vis_vertices);
    setMarkerTime(m_vis_edges);
    setMarkerTime(m_vis_bounds);

    ob::PlannerData data(m_planner->getSpaceInformation());
    m_planner->getPlannerData(data);
    m_vis_start_goal.points.push_back(getPointFromVertex<gtsam::Pose2>(data.getVertex(data.getStartIndex(0))));
    m_vis_start_goal.points.push_back(getPointFromVertex<gtsam::Pose2>(data.getVertex(data.getGoalIndex(0))));
    setMarkerBounds(m_vis_bounds, m_low_boundary, m_high_boundary);

    m_vis_array.markers.push_back(m_vis_start_goal);
    m_vis_array.markers.push_back(m_vis_bounds);
    m_marker_array_pub.publish(m_vis_array);
    m_vis_array.markers.clear();

    m_planner->as<og::PRM>()->growRoadmap(conditionFn);
    m_planner->getPlannerData(data);

    Globals::string_stream << "########## OMPL planning ###########";
    anplLogMessage(info, 1, Globals::string_stream, LogTag::generate_action);

    Globals::string_stream << "data.numVertices(): " << data.numVertices();
    anplLogMessage(info, 1, Globals::string_stream, LogTag::generate_action);

    if(is_vis_edges_and_vertices) {
        for (unsigned int v = 0; v < data.numVertices(); ++v, ++id_vis) {
            geometry_msgs::Point point_v = getPointFromVertex<gtsam::Pose2>(data.getVertex(v));
            std::vector<unsigned int> edgeList;
            data.getEdges(v, edgeList);
            for (unsigned int v_edge : edgeList) {
                geometry_msgs::Point point_e = getPointFromVertex<gtsam::Pose2>(data.getVertex(v_edge));
                m_vis_edges.points.push_back(point_v);
                m_vis_edges.points.push_back(point_e);
            }
            m_vis_vertices.points.push_back(point_v);

            setMarkerTime(m_vis_vertices_text);
            m_vis_vertices_text.id = id_vis;
            m_vis_vertices_text.pose.position = point_v;
            m_vis_vertices_text.pose.position.z = 1;
            m_vis_vertices_text.text = std::to_string(v);
            m_vis_array.markers.push_back(m_vis_vertices_text);
        }

        m_vis_array.markers.push_back(m_vis_vertices);
        m_vis_array.markers.push_back(m_vis_edges);
        m_marker_array_pub.publish(m_vis_array);
        m_vis_array.markers.clear();
    }

    std::stringstream string_stream;
    data.printGraphML(string_stream);

    //from: https://bitbucket.org/caleb_voss/diverse_short_paths
    //from: http://calebvoss.com/wp-content/uploads/voss-2015-diverse_short_paths.pdf
    TestData *testData = new TestData(string_stream, m_num_candidate_paths, m_max_path_length, m_min_path_pairwise_distance);
    PathDistanceMeasure *distanceMeasure = nullptr;
    switch(m_path_distance_measure_alg) {
        case LEVENSHTEIN:
            distanceMeasure = new Levenshtein();
            break;
        case FRECHET:
            distanceMeasure = new Frechet();
            break;
        case HAUSDORFF:
            distanceMeasure = new Hausdorff();
    }

    Neighborhood::AvoidMethod avoidMethod;
    switch(m_avoid_method) {
        case AM::CSPACE:
            avoidMethod = Neighborhood::CSPACE;
            break;
        case AM::GRAPH:
            avoidMethod = Neighborhood::GRAPH;
            break;
        case AM::UNKNOWN:
            avoidMethod = Neighborhood::UNKNOWN;
    }
    KDiverseShort *kDiverseShort;
    switch(m_k_diverse_short_alg) {
        case EPPSTEIN:
            kDiverseShort = new Eppstein(testData, distanceMeasure);
            break;
        case VOSS:
            kDiverseShort = new Voss(testData, distanceMeasure, m_radius_factor, avoidMethod);
    }

    PathsVertices paths_vertices;
    kDiverseShort->timedRun();
    kDiverseShort->print(Globals::string_stream);
    anplLogMessage(info, 1, Globals::string_stream, LogTag::generate_action);
    kDiverseShort->saveNodes(paths_vertices);

    delete kDiverseShort;
    delete distanceMeasure;
    delete testData;

    //actions for all paths generate from actions generator;
    ActionsPaths paths_actions;
    //waypoints for all paths in robot perspective
    std::vector<PathPose3> paths_waypoints;

    for (int path_number = 0; path_number < paths_vertices.size(); ++path_number) {
        auto &path_vertices = paths_vertices[path_number];
        PathPose3 path_waypoints;
        PathPose3 path_actions;
        setWaypointsPathFromVerticesPath<gtsam::Pose2>(data, path_vertices, data.getGoalIndex(0), path_waypoints);
        setActionsPathFromWaypointsPath(path_waypoints, path_actions);
        paths_waypoints.push_back(path_waypoints);
        paths_actions.push_back(path_actions);
    }

    drawActionsPaths(paths_actions);

    return paths_actions;
}

void GenerateActionsPRM::loadPDM(ConfigLoader& loader) {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    std::string $ = loader.getValue<std::string>(nameOf(m_path_distance_measure_alg));

    if (strcasecmp($.c_str(), "f") == 0 || strcasecmp($.c_str(), "frechet") == 0) {
        m_path_distance_measure_alg = PDM::FRECHET;
        return;
    }
    if (strcasecmp($.c_str(), "l") == 0 || strcasecmp($.c_str(), "levenshtein") == 0) {
        m_path_distance_measure_alg = PDM::LEVENSHTEIN;
        return;
}
    if (strcasecmp($.c_str(), "h") == 0 || strcasecmp($.c_str(), "hausdorff") == 0) {
        m_path_distance_measure_alg = PDM::HAUSDORFF;
        return;
    }

    throw std::invalid_argument(std::string("loading error: unknown path distance measure algorithm: ") + $);
}

void GenerateActionsPRM::loadKDS(ConfigLoader& loader) {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    std::string $ = loader.getValue<std::string>(nameOf(m_k_diverse_short_alg));

    if (strcasecmp($.c_str(), "e") == 0 || strcasecmp($.c_str(), "eppstein") == 0) {
        m_k_diverse_short_alg = KDS::EPPSTEIN;
        return;
    }
    if (strcasecmp($.c_str(), "v") == 0 || strcasecmp($.c_str(), "voss") == 0) {
        m_k_diverse_short_alg = KDS::VOSS;
        return;
    }

    throw std::invalid_argument(std::string("loading error: unknown k diverse short algorithm: ") + $);
}

void GenerateActionsPRM::loadAM(ConfigLoader& loader) {
    ADD_FIRST_LEVEL_INFO_LOG(LogTag::generate_action);

    std::string $ = loader.getValue<std::string>(nameOf(m_avoid_method));

    if (strcasecmp($.c_str(), "c") == 0 || strcasecmp($.c_str(), "cspace") == 0) {
        m_avoid_method = AM::CSPACE;
        return;
    }
    if (strcasecmp($.c_str(), "g") == 0 || strcasecmp($.c_str(), "graph") == 0) {
        m_avoid_method = AM::GRAPH;
        return;
    }

    m_avoid_method = AM::UNKNOWN;
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_relative_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/five_point_relative_pose.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing the relative pose from 5 feature
// correspondences. The feature correspondences should be normalized
// by the focal length with the principal point at (0, 0).
class RelativePoseEstimator
    : public Estimator<FeatureCorrespondence, RelativePose> {
 public:
  RelativePoseEstimator() {}

  // 5 correspondences are needed to determine an essential matrix and thus a
  // relative pose..
  double SampleSize() const { return 5; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<RelativePose>* relative_poses) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    image1_points.reserve(correspondences.size());
    image2_points.reserve(correspondences.size());
    for (int i = 0; i < correspondences.size(); i++) {
      image1_points.emplace_back(correspondences[i].feature1.point_);
      image2_points.emplace_back(correspondences[i].feature2.point_);
    }

    std::vector<Matrix3d> essential_matrices;
    if (!FivePointRelativePose(
            image1_points, image2_points, &essential_matrices)) {
      return false;
    }

    relative_poses->reserve(essential_matrices.size() * 4);
    for (const Eigen::Matrix3d& essential_matrix : essential_matrices) {
      RelativePose relative_pose;
      relative_pose.essential_matrix = essential_matrix;

      // The best relative pose decomposition should have at least 4
      // triangulated points in front of the camera. This is because one point
      // may be at infinity.
      const int num_points_in_front_of_cameras =
          GetBestPoseFromEssentialMatrix(essential_matrix,
                                         correspondences,
                                         &relative_pose.rotation,
                                         &relative_pose.position);
      if (num_points_in_front_of_cameras >= 4) {
        relative_poses->push_back(relative_pose);
      }
    }
    return relative_poses->size() > 0;
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& correspondence,
               const RelativePose& relative_pose) const {
    if (IsTriangulatedPointInFrontOfCameras(
            correspondence, relative_pose.rotation, relative_pose.position)) {
      return SquaredSampsonDistance(relative_pose.essential_matrix,
                                    correspondence.feature1.point_,
                                    correspondence.feature2.point_);
    }
    return std::numeric_limits<double>::max();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(RelativePoseEstimator);
};

}  // namespace

bool EstimateRelativePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    RelativePose* relative_pose,
    RansacSummary* ransac_summary) {
  RelativePoseEstimator relative_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<RelativePoseEstimator> > ransac =
      CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, relative_pose_estimator);
  // Estimate the relative pose.
  return ransac->Estimate(
      normalized_correspondences, relative_pose, ransac_summary);
}

}  // namespace theia


//
// Created by przemek on 27.03.2020.
//

#include <AST/expression/and_logic_expr.h>
#include <AST/expression/base_math_expr.h>
#include <boost/test/unit_test.hpp>
#include <error/exeception.h>

using namespace vecc;
using namespace vecc::ast;

using std::make_unique;
using std::move;
using std::unique_ptr;

BOOST_AUTO_TEST_SUITE(AST_Test_Suite)

BOOST_AUTO_TEST_SUITE(And_Logic_Expr_Test_Suite)

BOOST_AUTO_TEST_CASE(GivenMathExpr_ValueIsCorrect) {
  Variable var = Variable({1, 2, 3});

  AndLogicExpr expr(make_unique<BaseMathExpr>(var));

  BOOST_CHECK_EQUAL(expr.calculate(), var);
}

BOOST_AUTO_TEST_CASE(GivenTrueAndFalse_ValueIsCorrect) {
  Variable trueVar  = Variable({1, 2, 3});
  Variable falseVar = Variable({0, 0});

  BOOST_REQUIRE_EQUAL(true, trueVar);
  BOOST_REQUIRE_EQUAL(false, falseVar);

  AndLogicExpr expr(make_unique<BaseMathExpr>(trueVar));
  expr.addOperand(make_unique<BaseMathExpr>(falseVar));

  BOOST_CHECK_EQUAL(false, expr.calculate());
}

BOOST_AUTO_TEST_CASE(GivenFalseAndFalse_ValueIsCorrect) {
  Variable falseVar1 = Variable({0, 0, 0});
  Variable falseVar2 = Variable({0, 0});

  BOOST_REQUIRE_EQUAL(false, falseVar1);
  BOOST_REQUIRE_EQUAL(false, falseVar2);

  AndLogicExpr expr(make_unique<BaseMathExpr>(falseVar1));
  expr.addOperand(make_unique<BaseMathExpr>(falseVar2));

  BOOST_CHECK_EQUAL(false, expr.calculate());
}

BOOST_AUTO_TEST_CASE(GivenTrueAndTrue_ValueIsCorrect) {
  Variable trueVar1 = Variable({1, 1, 0});
  Variable trueVar2 = Variable({1, 0});

  BOOST_REQUIRE_EQUAL(true, trueVar1);
  BOOST_REQUIRE_EQUAL(true, trueVar2);

  AndLogicExpr expr(make_unique<BaseMathExpr>(trueVar1));
  expr.addOperand(make_unique<BaseMathExpr>(trueVar2));

  BOOST_CHECK_EQUAL(true, expr.calculate());
}

BOOST_AUTO_TEST_CASE(GivenThreeVals_ValueIsCorrect) {
  Variable trueVar1  = Variable({5, 1, 0});
  Variable trueVar2  = Variable({1, -9});
  Variable falseVar1 = Variable({0, 0});

  BOOST_REQUIRE_EQUAL(true, trueVar1);
  BOOST_REQUIRE_EQUAL(true, trueVar2);
  BOOST_REQUIRE_EQUAL(false, falseVar1);

  AndLogicExpr expr1(make_unique<BaseMathExpr>(trueVar1));
  expr1.addOperand(make_unique<BaseMathExpr>(trueVar2));
  expr1.addOperand(make_unique<BaseMathExpr>(falseVar1));

  AndLogicExpr expr2(make_unique<BaseMathExpr>(trueVar2));
  expr2.addOperand(make_unique<BaseMathExpr>(falseVar1));
  expr2.addOperand(make_unique<BaseMathExpr>(trueVar1));

  AndLogicExpr expr3(make_unique<BaseMathExpr>(falseVar1));
  expr3.addOperand(make_unique<BaseMathExpr>(trueVar2));
  expr3.addOperand(make_unique<BaseMathExpr>(trueVar1));

  BOOST_CHECK_EQUAL(false, expr1.calculate());
  BOOST_CHECK_EQUAL(false, expr2.calculate());
  BOOST_CHECK_EQUAL(false, expr3.calculate());
}

BOOST_AUTO_TEST_CASE(GivenFiveValues_ValueIsCorrect) {
  Variable trueVar1  = Variable({-1, 1, 0});
  Variable trueVar2  = Variable({1, 9});
  Variable trueVar3  = Variable({0, -2});
  Variable trueVar4  = Variable({1, 7});
  Variable falseVar1 = Variable({0, 0});

  BOOST_REQUIRE_EQUAL(true, trueVar1);
  BOOST_REQUIRE_EQUAL(true, trueVar2);
  BOOST_REQUIRE_EQUAL(true, trueVar3);
  BOOST_REQUIRE_EQUAL(true, trueVar4);
  BOOST_REQUIRE_EQUAL(false, falseVar1);

  AndLogicExpr expr(make_unique<BaseMathExpr>(trueVar1));
  expr.addOperand(make_unique<BaseMathExpr>(trueVar2));
  expr.addOperand(make_unique<BaseMathExpr>(trueVar3));
  expr.addOperand(make_unique<BaseMathExpr>(trueVar4));
  expr.addOperand(make_unique<BaseMathExpr>(falseVar1));

  BOOST_CHECK_EQUAL(false, expr.calculate());
}

BOOST_AUTO_TEST_CASE(GivenVal_ToStringCorrect) {
  Variable var = Variable({1, 2});

  AndLogicExpr expr(make_unique<BaseMathExpr>(var));

  BOOST_CHECK_EQUAL(expr.toString(), "vec(1, 2)");
}

BOOST_AUTO_TEST_CASE(GivenThreeVals_ToStringCorrect) {
  Variable trueVar1  = Variable({5, 1, 0});
  Variable falseVar1 = Variable({0, 0, 0});
  Variable falseVar2 = Variable({0, 0});

  AndLogicExpr expr1(make_unique<BaseMathExpr>(trueVar1));
  expr1.addOperand(make_unique<BaseMathExpr>(falseVar1));
  expr1.addOperand(make_unique<BaseMathExpr>(falseVar2));

  BOOST_CHECK_EQUAL(expr1.toString(),
                    "(vec(5, 1, 0) and vec(0, 0, 0) and vec(0, 0))");
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/*
 * hartman_skrekovski_color.hpp
 * Author: Aven Bross
 *
 * Implementation of Hartman-Skrekovski path 3-choosing algorithm.
 */

#ifndef __HARTMAN_SKREKOVSKI_COLOR_HPP
#define __HARTMAN_SKREKOVSKI_COLOR_HPP

// STL headers
#include <vector>
#include <utility>
#include <algorithm>

// Basic graph headers
#include <boost/graph/graph_traits.hpp>

// Local project headers
#include "disjoint_set.hpp"
#include "incidence_list_helpers.hpp"

namespace {
    // The three states a vertex may have
    const int INTERIOR_MARK = 0;

    /*
     * set_face_location
     *
     * inputs: a vertex v, the new face location for v, a read-write-able vertex
     *     property map storing the face location of each vertex, a disjoint set
     *     structure to store face location sets.
     *
     * outputs: sets the face location of v to the given face location, if the
     *     given location doesn't exist, that is, the integer new_face_location
     *     is negative, a new face location is created in the disjoint set
     *     structure.
     */
    template<typename vertex_t, typename face_location_map_t>
    inline int set_face_location(
            vertex_t v, int new_face_location,
            face_location_map_t & face_location_map,
            disjoint_set_t & face_location_sets
        )
    {
        if(!face_location_sets.exists(new_face_location)) {
            face_location_map[v] = face_location_sets.make_next();
        }
        else {
            face_location_map[v] = face_location_sets.find(new_face_location);
        }
        return face_location_map[v];
    }

    /*
     * hartman_skrekovski_color
     *
     * assumptions: There is a cycle C=v_0v_1...v_k in the weakly triangulated
     *     being colored. Suppose x, y, and p are in C such that p is between x
     *     and y clockwise around C. Suppose all vertices other than x, y, and p
     *     are uncolored. Suppose vertices x, y, and p have lists of size 1 or
     *     more, other vertices in C have lists of size 2 or more, and vertices
     *     in the subgraph bounded by C have lists of size 3 or more.
     *     Additionally, if p is colored some color c, assume vertices between x
     *     and p clockwise, including x, don't have the color c in their lists.
     *
     * inputs: A weakly triangulated planar graph with vertex indices
     *     (predefined boost property), a valid augmented planar embedding of
     *     the graph (modeling the boost AugmentedEmbedding concept defined in
     *     documentation), a read-write-able vertex property map assigning each
     *     vertex an integer to track face location, a disjoint set structure to
     *     store face location sets, a read-write-able vertex property map
     *     assigning each vertex an integer to track state, a read-able vertex
     *     property map assigning a pair of iterators from its augmented
     *     embedding list to track the range of valid neighbors, a read-write-
     *     able vertex property map to which the coloring will be assigned, the
     *     vertices x, y, and p on the outer cycle of the current subgraph, and
     *     the integers for the current face location marks.
     *
     * outputs: The coloring vertex property will contain a valid path list-
     *     coloring of the subgraph bounded by C such that x, y, and p each
     *     receive at most one same color neighbor. Moreover, if x=y=p then
     *     x,y,p receive no same color neighbors.
     */

    template<
            typename graph_t, typename augmented_embedding_t,
            typename face_location_map_t, typename neighbor_range_map_t,
            typename color_list_map_t, typename vertex_t
                = typename boost::graph_traits<graph_t>::vertex_descriptor
        >
    void hartman_skrekovski_color_recursive(
            const graph_t & graph,
            const augmented_embedding_t & augmented_embedding,
            face_location_map_t & face_location_map,
            disjoint_set_t & face_location_sets,
            neighbor_range_map_t & neighbor_range_map,
            color_list_map_t & color_list_map,
            vertex_t x, vertex_t y, vertex_t p,
            int before_p, int before_y, int before_x
        )
    {
        // If p isn't colored yet we are starting a new path
        if(color_list_map[p].size() > 1) {
            // Color p the first color in its list
            color_list_map[p].erase(
                    ++color_list_map[p].begin(),
                    color_list_map[p].end()
                );
        }

        // Grab p's the color from p's singleton list
        auto p_color = color_list_map[p].front();

        // Track the vertices that will become x and y in the first cycle
        vertex_t new_x = x, new_y = y;

        // Iterate through p's adjacency list
        auto neighbor_iter = neighbor_range_map[p].first;
        do {
            // Wrap adjacency list
            if(neighbor_iter == augmented_embedding[p].end())
                neighbor_iter = augmented_embedding[p].begin();

            // Grab neighbor n, and the iterator to p in n's adjacency list
            vertex_t n = neighbor_iter -> vertex;
            auto back_iter = neighbor_iter -> iterator;
            int n_location = face_location_map[n];

            // Look for p_color in n's color list
            auto color_iter = std::find(
                    color_list_map[n].begin(),
                    color_list_map[n].end(), p_color
                );

            // The case n is not in C
            if(face_location_map[n] == INTERIOR_MARK) {
                // Note that n is between x and p on the first cycle
                before_p = set_face_location(
                        n, before_p, face_location_map, face_location_sets
                    );

                // Initialize n's neighbor range to start at the edge pn
                initialize_neighbor_range(
                        n, back_iter, neighbor_range_map, augmented_embedding
                    );

                // Remove the edge pn from n's neighbor range
                remove_first_neighbor(
                        n, neighbor_range_map, augmented_embedding
                    );

                // Remove the path color from n's list
                if(color_iter != color_list_map[n].end()) {
                    color_list_map[n].erase(color_iter);
                    color_iter = color_list_map[n].end();
                }
            }
            // The case n is immediately counterclockwise to p in C
            else if(neighbor_iter == neighbor_range_map[p].first) {
                // If p has a single neighbor the current subgraph is K_2
                if(neighbor_iter == neighbor_range_map[p].second) {
                    // If is neither x nor y, ensure it doesn't receive p_color
                    if(n != x && n != y) {
                        // Remove the path color from n's list
                        if(color_iter != color_list_map[n].end()) {
                            color_list_map[n].erase(color_iter);
                            color_iter = color_list_map[n].end();
                        }
                    }

                    // Color n with any color remaining in its list
                    if(color_list_map[n].size() > 1) {
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }

                    break;
                }
                // If n is y and y has the path color, swap p and y so the
                //     path runs clockwise
                else if(n == y && color_iter != color_list_map[n].end()) {
                    if(color_list_map[n].size() > 1) {
                        // Remove all colors other than the path color
                        color_list_map[n].erase(
                                color_list_map[n].begin(),
                                color_iter
                            );
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }

                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            y, p, y,
                            -1, -1, before_y
                        );

                    break;
                }
                else {
                    // If we are removing x, setup new_x as n
                    if(x == p) {
                        new_x = n;

                        // Note that n is between x and p on the first cycle
                        before_p = set_face_location(
                                n, before_p, face_location_map,
                                face_location_sets
                            );
                    }

                    // Remove path color from n's list
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // Remove the edge pn from n's adjacency list
                    remove_last_neighbor(
                            n, neighbor_range_map, augmented_embedding
                        );
                }
            }
            else {
                // Divide the neighbor range of n at the edge pn
                auto n_ranges = split_neighbor_range(
                        n, back_iter, neighbor_range_map, augmented_embedding
                    );

                // p has been removed from the first cycle so we may remove
                //     those neighbors from p's neighbor range
                neighbor_range_map[p].first = neighbor_iter;

                // The case we are removing y
                if(p == y) {
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // If p=x=y then we are removing a single vertex path
                    if(p == x) {
                        // Color the subgraph bounded by the first cycle
                        neighbor_range_map[n] = n_ranges.second;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                new_x, n, new_x,
                                -1, before_p, before_y
                            );

                        // If the edge pn is a chord, we must color the rest
                        if(back_iter != n_ranges.first.first) {
                            // Color the subgraph bounded by the second cycle
                            neighbor_range_map[n] = n_ranges.first;
                            hartman_skrekovski_color_recursive(
                                    graph, augmented_embedding,
                                    face_location_map, face_location_sets,
                                    neighbor_range_map, color_list_map,
                                    n, p, p,
                                    -1, -1, before_y
                                );
                        }

                        break;
                    }
                    // The case the edge pn is in the cycle, i.e. not a chord
                    else if(neighbor_iter == neighbor_range_map[p].second) {
                        // Color the subgraph bounded by the first cycle
                        neighbor_range_map[n] = n_ranges.second;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                new_x, n, new_x,
                                -1, before_p, before_x
                            );

                        break;
                    }
                    // Otherwise we condition on the location of n as usual

                    // Remember n will be our new y vertex for the first cycle
                    new_y = n;
                }

                // The case n is in C[p,y]
                if(face_location_sets.compare(n_location, before_y) || n == y) {
                    vertex_t new_p = n;

                    // The case n needs to be colored
                    if(color_iter != color_list_map[n].end()
                        && color_list_map[n].size() > 1)
                    {
                        // Remove all colors other than the path color
                        color_list_map[n].erase(
                                color_list_map[n].begin(),
                                color_iter
                            );
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }
                    // The case n will not be added to the path
                    else if(color_list_map[n].size() > 1
                        || color_list_map[n].front() != p_color)
                    {
                        // Start new path at the new x on the first cycle
                        new_p = new_x;

                        // Combine the before_p and before_y segments
                        before_y = face_location_sets.take_union(
                                before_p, before_y
                            );

                        // Note before_p is gone
                        before_p = -1;
                    }

                    /*
                     * This is the most complicated case.
                     * There are two sub-cases to consider:
                     *
                     * Case 1: p_color in L[n]
                     *   In this case |L[n]|=1. In the first call we will
                     *   continue coloring the path from n. In the second
                     *   call, we will immediately find the edge pn and,
                     *   since it the first edge and in the cycle, we will
                     *   immediately remove the two vertex path np. Thus
                     *   no vertex in the path will recieve a new same color
                     *   neighbor from the second call.
                     *
                     * Case 2: p_color is not in L[n]
                     *   In this case the first call will start a new path
                     *   from the vertex new_x. By removing p_color from all
                     *   neighbors of vertices in the path, we ensure no
                     *   vertices in the path, including n, will recieve any
                     *   new same color neighbors in the first call. In the
                     *   second call we will be continuing the path from p.
                     *   Since p_color is not in L[n] and x=y=n, the vertex
                     *   n will not recieve any new same color neighbors in
                     *   the second call.

                     */

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            new_x, new_y, new_p,
                            before_p, before_y, before_x
                        );

                    // If the edge pn is a chord, handle the rest
                    if(back_iter != n_ranges.first.first) {
                        // Color the subgraph bounded by the second cycle
                        neighbor_range_map[n] = n_ranges.first;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                n, n, p,
                                -1, before_y, -1
                            );
                    }
                }
                // The case n is in C[y,x], n != y
                else if(face_location_sets.compare(n_location, before_x)) {
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            new_x, n, new_x,
                            -1, before_p, before_x
                        );

                    // Color the subgraph bounded by the second cycle
                    neighbor_range_map[n] = n_ranges.first;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            n, y, p,
                            -1, before_y, before_x
                        );
                }
                // The case n is in C[x,p]
                else {
                    // Color the subgraph bounded b the second cycle
                    neighbor_range_map[n] = n_ranges.first;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            x, y, p,
                            before_p, before_y, before_x
                        );

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            n, n, n,
                            -1, before_p, -1
                        );
                }

                break;
            }
        } while(neighbor_iter++ != neighbor_range_map[p].second);
    }
}

/*
 * hartman_skrekovski_color
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predefined
 *     boost property), a valid augmented planar embedding of the graph
 *     (modeling the boost AugmentedEmbedding concept defined in documentation),
 *     a read-able vertex property map assigning a range of colors to each
 *     vertex (each vertex must recieve a range of at least 3 colors if
 *     interior, and at least two colors if on the outer face), a read-write-
 *     able vertex property map to which the coloring will be assigned, and
 *     finally a pair of iterators for the list of vertices on the outer face of
 *     the graph in clockwise order.
 *
 * outputs: The coloring will be a valid assignment of colors from the input
 *     color lists such that each color class induces a disjoint union of paths.
 */

template<
        typename graph_t,
        typename augmented_embedding_t,
        typename color_list_map_t,
        typename neighbor_range_map_t,
        typename face_location_map_t,
        typename face_iterator_t
    >
void hartman_skrekovski_color(
        const graph_t & graph,
        const augmented_embedding_t & augmented_embedding,
        face_iterator_t face_begin, face_iterator_t face_end,
        neighbor_range_map_t & neighbor_range_map,
        face_location_map_t & face_location_map,
        color_list_map_t & color_list_map
    )
{
    // Type definitions
    typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;

    // Setup face location sets (we will have only the region before_y)
    disjoint_set_t face_location_sets;

    // Make a set for 0, the location given to interior vertices
    face_location_sets.make_next();

    // Intitially all vertices will be
    int before_y = -1;

    // Initialize vertices on outer face
    for(auto face_iter = face_begin; face_iter != face_end; ++face_iter)
    {
        auto next = face_iter;
        if(++next == face_end) next = face_begin;

        vertex_t l = *face_iter;
        vertex_t v = *next;

        before_y = set_face_location(
                v, before_y, face_location_map, face_location_sets
            );

        auto back_iter = find_neighbor_iterator(
                v, l, augmented_embedding, graph
            );
        initialize_neighbor_range(
                v, back_iter, neighbor_range_map, augmented_embedding
            );
    }

    vertex_t x = *face_begin, y = *(--face_end);

    // Construct the path choosing from given color lists
    hartman_skrekovski_color_recursive(
            graph, augmented_embedding,
            face_location_map, face_location_sets,
            neighbor_range_map, color_list_map,
            x, y, x,
            -1, before_y, -1
        );
}

/*
 * A wrapper that automatically construct fast property maps for the
 * face_location_map and neighbor_range_map, but requires that graph_t is some
 * definition of boost::adjacency_list.
 */

template<
        typename graph_t,
        typename augmented_embedding_t,
        typename color_list_map_t,
        typename face_iterator_t
    >
void hartman_skrekovski_color(
        const graph_t & graph,
        const augmented_embedding_t & augmented_embedding,
        face_iterator_t face_begin, face_iterator_t face_end,
        color_list_map_t & color_list_map
    )
{
    // Vertex property map to store vertex marks
    typedef boost::iterator_property_map<
            std::vector<int>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > integer_property_map_t;

    // Vertex property map type for the neighbor ranges of planar_embedding_t
    typedef typename boost::property_traits<augmented_embedding_t>::value_type
            ::const_iterator embedding_iterator_t;
    typedef typename std::vector<
            std::pair<embedding_iterator_t, embedding_iterator_t>
        > neighbor_range_storage_t;
    typedef boost::iterator_property_map<
            typename neighbor_range_storage_t::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > neighbor_range_map_t;

    // Construct a vertex property for neighbor ranges
    neighbor_range_storage_t neighbor_range_storage(boost::num_vertices(graph));
    neighbor_range_map_t neighbor_range_map(
            neighbor_range_storage.begin(),
            boost::get(boost::vertex_index, graph)
        );

    // Construct a vertex property map to store face location marks
    std::vector<int> face_location_storage(boost::num_vertices(graph));
    integer_property_map_t face_location_map(
            face_location_storage.begin(),
            boost::get(boost::vertex_index, graph)
        );

    // Call Hartman-Skrekovski with the given cycle and
    hartman_skrekovski_color(
            graph,
            augmented_embedding,
            face_begin, face_end,
            neighbor_range_map,
            face_location_map,
            color_list_map
        );
}

#endif


/*
 * MyPIDControllerAgent.cpp
 *
 *  Created on: 26 dic 2015
 *      Author: andrea
 */
#include <cmath>

#include <agents/MyPIDControllerAgent.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/log/trivial.hpp>
#include <events/MyIMUSample.h>
#include <events/MyBaroSample.h>
#include <events/MyOutMotors.h>
#include <events/MyYPRError.h>
#include <events/MyRCSample.h>
#include <events/MyMotorsDisarmed.h>
#include <events/MyMotorsArmed.h>
#include <events/MyBaroSample.h>
#include <iostream>
#include <syslog.h>

RangeFloat MyPIDControllerAgent::TARGET_RANGES[] = {RangeFloat(-62.0f, 62.0f), // roll
		                                            RangeFloat(-62.0f, 62.0f), // pitch
		                                            RangeFloat(-125.0f, 125.0f), // yaw
		                                            RangeFloat(1000.0f, 2000.0f) // thrust
};
ValueFloat MyPIDControllerAgent::TARGET_VALUES[] = {ValueFloat(0.0f, MyPIDControllerAgent::TARGET_RANGES[ROLL_POS]),
													ValueFloat(0.0f, MyPIDControllerAgent::TARGET_RANGES[PITCH_POS]),
													ValueFloat(0.0f, MyPIDControllerAgent::TARGET_RANGES[YAW_POS]),
													ValueFloat(0.0f, MyPIDControllerAgent::TARGET_RANGES[THRUST_POS])
};
RangeFloat MyPIDControllerAgent::INTEGRAL_RANGE = RangeFloat(-200.0f, 200.0f);

using namespace std;

MyPIDControllerAgent::MyPIDControllerAgent(boost::shared_ptr<MyEventBus> bus,
		vector<MyEvent::EventType> acceptedEventTypes) :
		MyAgent(bus, acceptedEventTypes), initialized(false), armed(false), yawCurr(0.0f), pitchCurr(0.0f), rollCurr(0.0f), yawErr(1.0f, 4, 10, 3, INTEGRAL_RANGE), pitchErr(
				1.0f, 4, 10, 3, INTEGRAL_RANGE), rollErr(1.0f, 4, 10, 3, INTEGRAL_RANGE) {
	keRoll = 8.55f;      // local tests: 1.587f
	keIRoll = 0.0073f; //0.0353f; //= 1.5186f;    // local tests: 0.0186f
	keDRoll = 90.0125f; //112.0186f; //= 62.00f;     // local tests: 52.00f

	kePitch = 8.55f;     // local tests: 1.587f
	keIPitch = 0.0073f; //0.0353f; //= 1.5186f;   // local tests: 0.0186f
	keDPitch = 90.0125f; //112.0186f; //= 62.00f;    // local tests: 52.00f

	keYaw = 1.687f;       // local tests:  1.687f
	keIYaw = 0.0f; //= 0.056f;      // local tests:  0.156f
	keDYaw = 0.0f; //= 10.000f;     // local tests:  65.00f

}

MyPIDControllerAgent::~MyPIDControllerAgent() {
}
void MyPIDControllerAgent::initialize() {
	initialized = true;
}

void MyPIDControllerAgent::calcErr(boost::math::quaternion<float> q) {
	float real = q.real();
	float x = q.R_component_2();
	float y = q.R_component_3();
	float z = q.R_component_4();

	yawCurr = atan2(2.0f * (real * z + x * y), 1.0f - 2.0f * (y * y + z * z))
			* 57.295779513f;
	pitchCurr = asin(2.0f * real * y - 2.0 * z * x) * 57.295779513f;
	rollCurr = atan2(2.0f * (real * x + y * z), 1.0f - 2.0f * (x * x + y * y))
			* 57.295779513f;

//	yawCurr = uint16_t(std::rint(0.8f*float(yawCurr) + 0.2f*float(_yawCurr)));
//	pitchCurr = uint16_t(std::rint(0.8f*float(pitchCurr) + 0.2f*float(_pitchCurr)));
//	rollCurr = uint16_t(std::rint(0.8f*float(rollCurr) + 0.2f*float(_rollCurr)));

	// considero errore limitato a 10 deg. more less for yaw an 60deg for pitch and roll
	yawErr.push(std::min(10.0f, std::max(-10.0f, yawCurr - MyPIDControllerAgent::TARGET_VALUES[YAW_POS].getValue())));
	pitchErr.push(std::min(60.0f, std::max(-60.0f, pitchCurr - MyPIDControllerAgent::TARGET_VALUES[PITCH_POS].getValue())));
	rollErr.push(std::min(60.0f, std::max(-60.0f, rollCurr - MyPIDControllerAgent::TARGET_VALUES[ROLL_POS].getValue())));
}
void MyPIDControllerAgent::calcCorrection() {
//	float eRoll = std::min(10.0f, std::max(-10.0f, rollErr.getMean()));
	float eRoll = rollErr.getMean();
	float eIRoll = rollErr.getIntegral();
	float eDRoll = rollErr.getDerivate();

//	float ePitch = std::min(10.0f, std::max(-10.0f, pitchErr.getMean()));
	float ePitch = pitchErr.getMean();
	float eIPitch = pitchErr.getIntegral();
	float eDPitch = pitchErr.getDerivate();

	float eYaw = yawErr.getMean();
	float eIYaw = yawErr.getIntegral();
	float eDYaw = yawErr.getDerivate();

	// integrale limitato
//	float corrRoll = keRoll * eRoll + min(50.0f, max(-50.0f, keIRoll*eIRoll)) + keDRoll * eDRoll;
//	float corrPitch = kePitch * ePitch + min(50.0f, max(-50.0f, keIPitch*eIPitch)) + keDPitch * eDPitch;
//	float corrYaw = keYaw * eYaw + min(10.0f, max(-10.0f, keIYaw*eIYaw)) + keDYaw * eDYaw;

	float corrRoll = keRoll * eRoll + keIRoll*eIRoll + keDRoll * eDRoll;
	float corrPitch = kePitch * ePitch + keIPitch*eIPitch + keDPitch * eDPitch;
	float corrYaw = keYaw * eYaw + min(10.0f, max(-10.0f, keIYaw*eIYaw)) + keDYaw * eDYaw;

	int32_t front = std::lrint(std::max(double(1000000), std::min(double(2000000), double(MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].getValue()) + corrPitch - corrYaw)*1000.0));
	int32_t rear = std::lrint(std::max(double(1000000), std::min(double(2000000), double(MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].getValue()) - corrPitch - corrYaw)*1000.0));
	int32_t left = std::lrint(std::max(double(1000000), std::min(double(2000000), double(MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].getValue()) - corrRoll + corrYaw)*1000.0));
	int32_t right = std::lrint(std::max(double(1000000), std::min(double(2000000), double(MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].getValue()) + corrRoll + corrYaw)*1000.0));

//    syslog(LOG_INFO, "mydrone Thrust: %f", double(MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].getValue()));

//	syslog(LOG_INFO, "errRoll: %5.5f, eRoll:%5.5f, eIRoll: %5.5f, eDRoll: %5.5f, totCorrRoll:%5.5f. left: %d, right: %d \n",
//			eRoll, keRoll * eRoll, keIRoll*eIRoll, keDRoll * eDRoll, corrRoll, left, right);

	// level motors on [1000000, 2000000]
//	int32_t minMotors = 9999999;
//	if(front < minMotors) {
//		minMotors = front;
//	}
//	if(rear < minMotors) {
//		minMotors = rear;
//	}
//	if(left < minMotors) {
//		minMotors = left;
//	}
//	if(right < minMotors) {
//		minMotors = right;
//	}
//	int32_t diff = 0;
//	if(minMotors < 1000000) {
//		diff = 1000000 - minMotors;
//	}
//	front = std::min(2000000, front + diff);
//	rear = std::min(2000000, rear + diff);
//	left = std::min(2000000, left + diff);
//	right = std::min(2000000, right + diff);
//
//	int32_t maxMotors = 0;
//	if(front > maxMotors) {
//		maxMotors = front;
//	}
//	if(rear > maxMotors) {
//		maxMotors = rear;
//	}
//	if(left > maxMotors) {
//		maxMotors = left;
//	}
//	if(right > maxMotors) {
//		maxMotors = right;
//	}
//	diff = 0;
//	if(maxMotors > 2000000) {
//		diff = maxMotors - 2000000;
//	}
//	front = std::max(1000000, front - diff);
//	rear = std::max(1000000, rear - diff);
//	left = std::max(1000000, left - diff);
//	right = std::max(1000000, right - diff);

	// printf("%6.3f, %6.3f, %6.3f, %d, %6.3f, %d, %d \n", ePitch, eIPitch, eDPitch, front, corrPitch, pitchCurr, MyPIDControllerAgent::TARGET_VALUES[PITCH_POS].getValue());

	{ // out error event
		boost::shared_ptr<MyYPRError> evOut(boost::make_shared<MyYPRError>(this->getUuid(), yawCurr, pitchCurr, rollCurr, MyPIDControllerAgent::TARGET_VALUES[YAW_POS].getValue(), MyPIDControllerAgent::TARGET_VALUES[PITCH_POS].getValue(), MyPIDControllerAgent::TARGET_VALUES[ROLL_POS].getValue(), keRoll*eRoll, keIRoll*eIRoll, keDRoll*eDRoll,
				kePitch*ePitch, keIPitch*eIPitch, keDPitch*eDPitch,
				keYaw*eYaw, keIYaw*eIYaw, keDYaw*eDYaw));
		m_signal(evOut);
	}
	{// out state motors
		boost::shared_ptr<MyOutMotors> evOut(boost::make_shared<MyOutMotors>(this->getUuid(), front, rear, left, right));
		m_signal(evOut);
	}
}
void MyPIDControllerAgent::processEvent(boost::shared_ptr<MyEvent> event) {
	if (!initialized) {
		initialize();
	}
	if (this->getState() == MyAgentState::Active) {
		if(event->getType() == MyEvent::EventType::MotorsDisarmed) {
			syslog(LOG_INFO, "Motors Disarmed");
			this->armed = false;
		} else
		if(event->getType() == MyEvent::EventType::MotorsArmed) {
			syslog(LOG_INFO, "Motors Armed");
			this->armed = true;
		} else
		if (event->getType() == MyEvent::EventType::IMUSample && this->armed) {
			boost::shared_ptr<MyIMUSample> imuSample =
					boost::static_pointer_cast<MyIMUSample>(event);

			boost::math::quaternion<float> q = imuSample->getQuaternion();
			calcErr(q);
			calcCorrection();
		} else if(event->getType() == MyEvent::EventType::RCSample && this->armed) {
			boost::shared_ptr<MyRCSample> rcSample =
					boost::static_pointer_cast<MyRCSample>(event);
			MyPIDControllerAgent::TARGET_VALUES[ROLL_POS].setPercentValue((*rcSample).getRollPercent());
			MyPIDControllerAgent::TARGET_VALUES[PITCH_POS].setPercentValue((*rcSample).getPitchPercent());
			MyPIDControllerAgent::TARGET_VALUES[YAW_POS].setPercentValue((*rcSample).getYawPercent());
			MyPIDControllerAgent::TARGET_VALUES[THRUST_POS].setPercentValue((*rcSample).getThrustPercent());
		} else if(event->getType() == MyEvent::EventType::BaroSample) {
			boost::shared_ptr<MyBaroSample> baroSample =
					boost::static_pointer_cast<MyBaroSample>(event);
			// syslog(LOG_INFO, "BaroSample: press=%d, alt=%5.5f, temp=%5.5f, seeLevelPress=%d", baroSample->getPressure(), baroSample->getAltitude(), baroSample->getTemperature(), baroSample->getSeeLevelPressure());
		}
        else {
			// skip events
		}
	}
}


/** nonfinite_serialization_archives.cpp
*
* Copyright (c) 2011 Paul A. Bristow
*
* Distributed under the Boost Software License, Version 1.0.
* (See accompanying file LICENSE_1_0.txt
* or copy at http://www.boost.org/LICENSE_1_0.txt)
*
* This very simple program illustrates how to use the
* `boost/math/nonfinite_num_facets.hpp' to obtain C99
* representation of infinity and NaN.
* From the original Floating Point  Utilities contribution by Johan Rade.
* Floating Point Utility library has been accepted into Boost,
* but the utilities are incorporated into Boost.Math library.
*
\file

\brief A simple example of using non_finite_num facet for
C99 standard output of infinity and NaN in serialization archives.

\detail This example shows how to create a C99 non-finite locale,
and imbue input and output streams with the non_finite_num put and get facets.
This allow output and input of infinity and NaN in a Standard portable way,
This permits 'loop-back' of output back into input (and portably across different system too).
This is particularly useful when used with Boost.Seralization so that non-finite NaNs and infinity
values in text and xml archives can be handled correctly and portably.

*/

#ifdef _MSC_VER
#   pragma warning(disable : 4127) // conditional expression is constant.
#endif

#include <boost/archive/text_oarchive.hpp>
using boost::archive::text_oarchive;
#include <boost/archive/codecvt_null.hpp>
using boost::archive::codecvt_null;
using boost::archive::no_codecvt;

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
using boost::math::nonfinite_num_get;
using boost::math::nonfinite_num_put;

#include <iostream>
using std::cout;
using std::endl;
using std::cerr;

#include <iomanip>
using std::setw;
using std::left;
using std::right;
using std::internal;

#include <string>
using std::string;

#include <sstream>
using std::istringstream;

#include <fstream>
using std::ofstream;

#include <limits>
using std::numeric_limits;

#include <locale>
using std::locale;

/*
Use with serialization archives.

It is important that the same locale is used
when an archive is saved and when it is loaded.
Otherwise, loading the archive may fail.

By default, archives are saved and loaded with a classic C locale with a
`boost::archive::codecvt_null` facet added.
Normally you do not have to worry about that.
The constructors for the archive classes, as a side-effect,
imbue the stream with such a locale.

However, if you want to use the facets `nonfinite_num_put` and `nonfinite_num_get`
with archives,`then you have to manage the locale manually.

That is done by calling the archive constructor with the flag `boost::archive::no_codecvt`.
Then the archive constructor will not imbue the stream with a new locale.

The following code shows how to use `nonfinite_num_put` with a `text_oarchive`:

*/

int main()
{

  if((std::numeric_limits<double>::has_infinity == false) || (std::numeric_limits<double>::infinity() == 0))
  {
    std::cout << "Infinity not supported on this platform." << std::endl;
    return 0;
  }

  if((std::numeric_limits<double>::has_quiet_NaN == false) || (std::numeric_limits<double>::quiet_NaN() == 0))
  {
    std::cout << "NaN not supported on this platform." << std::endl;
    return 0;
  }

  locale default_locale(locale::classic(), new boost::archive::codecvt_null<char>);
  // codecvt_null so the archive constructor will not imbue the stream with a new locale.

  locale my_locale(default_locale, new nonfinite_num_put<char>);
  // Add nonfinite_num_put facet to locale.

  // Use a temporary folder /.temps (which contains "boost-no-inspect" so that it will not be inspected, and made 'hidden' too).
  ofstream ofs("./.temps/test.txt");
  ofs.imbue(my_locale);

  boost::archive::text_oarchive oa(ofs, no_codecvt);

  double x = numeric_limits<double>::infinity();
  oa & x;

} // int main()

/* The same method works with nonfinite_num_get  and text_iarchive.

If you use the trap_infinity and trap_nan flags with a serialization archive,
then you must set the exception mask of the stream.
Serialization archives do not check the stream state.

*/


/*
@copyright Louis Dionne 2015
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/constant.hpp>
#include <boost/hana/foldable.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/compose.hpp>
#include <boost/hana/functional/fix.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/functional/placeholder.hpp>
#include <boost/hana/functor.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/iterable.hpp>
#include <boost/hana/sequence.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/monoid.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/ring.hpp>
#include <boost/hana/tuple.hpp>

#include <utility>

#include "matrix.hpp"

namespace cppcon {
    namespace detail {
        auto remove_at = [](auto n, auto xs) {
            using namespace boost::hana;
            using L = datatype_t<decltype(xs)>;
            auto with_indices = zip(xs, to<L>(range(size_t<0>, length(xs))));
            auto removed = filter(with_indices, compose(n != _, last));
            return transform(removed, head);
        };
    }

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

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

        return eval_if(m.size() == size_t<1>,
            always(m.at(size_t<0>, size_t<0>)),
            [=](auto _) {
                auto cofactors_1st_row = unpack(_(range)(size_t<0>, m.ncolumns()),
                    on(make<Tuple>, partial(cofactor, m, size_t<0>))
                );
                return detail::tuple_scalar_product(head(rows(m)), cofactors_1st_row);
            }
        );
    });
} // end namespace cppcon

#endif // !BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_DET_HPP


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

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

int main(void) {
    vector<int> arr(5);
    // 一番最後に端数の小さなものにする
    int lastMod = 100000;
    int lastIndex = -1;
    for(int i = 0 ; i < arr.size() ; ++i) {
        cin >> arr[i];

        int mod = arr[i] % 10;
        if (mod && lastMod > mod) {
            lastMod = mod;
            lastIndex = i;
        }
    }
    int sum = 0;
    for(int i = 0 ; i < arr.size() ; ++i) {
        if(lastIndex == i) {
            sum += arr[i];
        } else {
            int mod = arr[i] % 10;
            if (mod) {
                arr[i] += (10 - mod);

            }
            sum += arr[i];
        }
    }
    cout << sum << endl;
    return 0;
}


#include "common/common.hpp"

#include "geodb/hilbert.hpp"
#include "geodb/str.hpp"
#include "geodb/irwi/tree.hpp"
#include "geodb/irwi/tree_internal.hpp"
#include "geodb/irwi/bulk_load_quickload.hpp"

#include <boost/program_options.hpp>
#include <fmt/format.h>
#include <fmt/ostream.h>

#include <iostream>
#include <limits>

namespace po = boost::program_options;

using namespace geodb;

using std::cout;
using std::cerr;
using std::endl;
using std::flush;

static std::string algorithm;
static u32 seed;
static u32 num_points;
static bool skewed;
static bool heuristic;

static constexpr size_t leaf_size = 16;

struct vec2 {
    double x = 0, y = 0;

    vec2() = default;
    vec2(double x, double y): x(x), y(y) {}
};

void to_json(json& j, const vec2& vec) {
    j = {
        {"x", vec.x},
        {"y", vec.y}
    };
}

std::ostream& operator<<(std::ostream& o, const vec2& v) {
    return o << "(" << v.x << ", " << v.y << ")";
}

struct box2 {
    vec2 min, max;

    box2() = default;
    box2(vec2 min, vec2 max)
        : min(min), max(max)
    {
        geodb_assert(min.x <= max.x, "invalid x coordiantes");
        geodb_assert(min.y <= max.y, "invalid y coordiantes");
    }

    box2 extend(const vec2& point) const {
        return box2{
            vec2{std::min(min.x, point.x), std::min(min.y, point.y)},
            vec2{std::max(max.x, point.x), std::max(max.y, point.y)},
        };
    }

    double size() const {
        return (max.x - min.x) * (max.y - min.y);
    }
};

void to_json(json&j , const box2& box) {
    j = {
        {"min", box.min},
        {"max", box.max},
    };
}

struct leaf {
    box2 mbb;
    std::vector<vec2> points;
};

void to_json(json& j, const leaf& l) {
    j = {
        {"mbb", l.mbb},
        {"points", l.points},
    };
}

std::ostream& operator<<(std::ostream& o, const box2& b) {
    return o << "{" << b.min << ", " << b.max << "}";
}

void parse_options(int argc, char** argv) {
    po::options_description options("Options");
    options.add_options()
            ("points,p", po::value(&num_points)->default_value(1000),
             "The number of generated points.")
            ("skewed,s", po::bool_switch(&skewed)->default_value(false),
             "If false, generates uniformly distributed points in [0, 1]x[0, 1}. "
             "If true, the point set will be skewed instead.")
            ("seed", po::value(&seed),
             "The seed used by the random number generator. Defaults to a truly random seed.")
            ("algorithm", po::value(&algorithm),
             "The leaf packing algorithm. Either \"hilbert\", \"str\", \"quickload\" or \"obo\".")
            ("heuristic", po::bool_switch(&heuristic),
             "Enable the leaf size heuristic (hilbert only).")
            ("help,h", "Show this message.");

    po::variables_map vm;
    try {
        po::command_line_parser p(argc, argv);
        p.options(options);
        po::store(p.run(), vm);

        if (vm.count("help")) {
            fmt::print(cerr, "Usage: {0}\n"
                             "\n"
                             "Visualizes the bulk loading algorithm for a generated set of points in 2d.\n"
                             "The real algorithm works in 3d and is implemented in the loader tool.\n"
                             "\n"
                             "{1}",
                       argv[0], options);
            throw exit_main(0);
        }

        if (!vm.count("seed")) {
            seed = std::random_device{}();
        }

        po::notify(vm);
    } catch (const po::error& e) {
        fmt::print(cerr, "Failed to parse arguments: {}.\n", e.what());
        throw exit_main(1);
    }
}

/// Maps a point in [0, 1]^2 to its hilbert index.
u64 index(const vec2& p) {
    geodb_assert(p.x >= 0 && p.x <= 1, "invalid x coordinate");
    geodb_assert(p.y >= 0 && p.y <= 1, "invalid y coordinate");

    static constexpr u32 dimension = 2;
    static constexpr u32 precision = 16;
    static constexpr u32 max_coordiante = (1 << precision) - 1;

    using curve = hilbert_curve<dimension, precision>;

    auto map_coord = [](double value) {
        return curve::coordinate_t(u32(value * max_coordiante));
    };

    curve::point_t point{map_coord(p.x), map_coord(p.y)};
    return curve::hilbert_index(point);
}

/// Sorts the points by their hilbert index.
void sort_hilbert(std::vector<vec2>& points) {
    std::sort(points.begin(), points.end(), [](const vec2& a, const vec2& b) {
        return index(a) < index(b);
    });
}

/// Returns a random value in [0, 1].
double get_random() {
    static std::mt19937 rng(seed);
    static std::uniform_real_distribution<double> dist(0, std::nextafter(1.0, std::numeric_limits<double>::max()));
    return dist(rng);
}

/// Generates uniformly distributed points in [0, 1]^2.
std::vector<vec2> create_points(size_t count) {
    std::vector<vec2> points(count);
    for (auto& p : points) {
        p = vec2(get_random(), get_random());
    }
    return points;
}

/// Generates a skewed set of points in [0, 1]^2.
std::vector<vec2> create_skewed_points(size_t count) {
    vec2 center_a(0.3, 0.7);
    vec2 center_b(0.5, 0.2);
    vec2 center_c(0.7, 0.5);

    double radius_a = 0.30;
    double radius_b = 0.15;
    double radius_c = 0.20;

    std::vector<vec2> points(count);

    // A point on the circle determined by a random radius
    // and a random arc.
    // Thus, the density of points will be hightest at the center
    // and will become lower with increasing distance.
    auto gen_circle = [&](const vec2& center, double radius) {
        double v = get_random() * 2 * M_PI;
        double r = get_random() * radius;

        vec2 p;
        p.x = r * std::cos(v) + center.x;
        p.y = r * std::sin(v) + center.y;
        return p;
    };

    for (auto& p : points) {
        double which = get_random();
        if (which < 0.5) {
            p = gen_circle(center_a, radius_a);
        } else if (which < 0.75) {
            p = gen_circle(center_b, radius_b);
        } else {
            p = gen_circle(center_c, radius_c);
        }
    }
    return points;
}

/// Get the bounding box for a set of points.
/// There must be at least one point.
template<typename Iter>
box2 get_bounding_box(Iter first, Iter last) {
    geodb_assert(first != last, "cannot create the bounding box for an empty range");

    box2 box(*first, *first);
    ++first;
    for (; first != last; ++first) {
        box = box.extend(*first);
    }
    return box;
}

std::vector<leaf> create_hilbert_leaves(std::vector<vec2>& points) {
    geodb_assert(leaf_size > 1, "invalid leaf size");

    sort_hilbert(points);

    std::vector<leaf> leaves;

    auto pos = points.begin();
    auto end = points.end();
    while (pos != end) {
        size_t count = std::min(leaf_size, size_t(end - pos));

        leaf l;
        l.mbb = get_bounding_box(pos, pos + count);
        l.points.assign(pos, pos + count);
        leaves.push_back(std::move(l));

        pos += count;
    }
    return leaves;
}

std::vector<leaf> create_hilbert_leaves_heuristic(std::vector<vec2>& points) {
    geodb_assert(leaf_size > 1, "invalid leaf size");

    sort_hilbert(points);

    static constexpr double max_grow = 1.2;

    std::vector<leaf> leaves;

    auto pos = points.begin();
    auto end = points.end();
    while (pos != end) {
        size_t count = std::min(leaf_size / 2, size_t(end - pos));

        leaf l;

        // Take the first "count" points.
        l.points.assign(pos, pos + count);
        l.mbb = get_bounding_box(l.points.begin(), l.points.end());
        pos += count;

        // Take points while the box does not grow too large.
        double max_size = l.mbb.size() * max_grow;
        while (pos != end && l.points.size() < leaf_size) {
            const vec2& p = *pos;

            box2 new_mbb = l.mbb.extend(p);
            if (new_mbb.size() > max_size) {
                break;
            }

            l.points.push_back(p);
            l.mbb = new_mbb;
            ++pos;
        }

        leaves.push_back(std::move(l));
    }

    return leaves;
}

std::vector<leaf> create_str_leaves(std::vector<vec2>& points) {
    geodb_assert(leaf_size > 1, "invalid leaf size");

    auto cmp_x = [](const vec2& a, const vec2& b) {
        return a.x < b.x;
    };
    auto cmp_y = [](const vec2& a, const vec2& b) {
        return a.y < b.y;
    };
    sort_tile_recursive(points, leaf_size, cmp_x, cmp_y);

    std::vector<leaf> leaves;

    auto pos = points.begin();
    auto end = points.end();
    while (pos != end) {
        size_t count = std::min(leaf_size, size_t(end - pos));

        leaf l;
        l.mbb = get_bounding_box(pos, pos + count);
        l.points.assign(pos, pos + count);
        leaves.push_back(std::move(l));

        pos += count;
    }
    return leaves;
}

struct vec2_accessor {
    trajectory_id_type get_id(const vec2& e) const {
        (void) e;
        return 0;
    }

    bounding_box get_mbb(const vec2& e) const {
        return bounding_box(vector3(e.x, e.y, 0),
                            vector3(e.x, e.y, 1));
    }

    constexpr u64 get_total_count(const vec2& e) const {
        (void) e;
        return 1;
    }

    std::array<label_count, 1> get_label_counts(const vec2& e) const {
        (void) e;
        return { label_count(0, 1) };
    }
};

std::vector<leaf> create_quickload_leaves(const std::vector<vec2>& points) {
    geodb_assert(leaf_size > 1, "invalid leaf size");

    // quickload implementation currently requires a file stream.
    tpie::file_stream<vec2> entries;
    entries.open();
    for (const auto& point : points) {
        entries.write(point);
    }

    std::vector<leaf> leaves;

    // 16 leaves in memory.
    quick_load_pass<vec2, vec2_accessor,
            block_size, leaf_size, leaf_size
    > pass(16, 16, vec2_accessor(), 1.0);
    pass.run(entries, [&](gsl::span<const vec2> entries) {
        leaf l;
        l.mbb = get_bounding_box(entries.begin(), entries.end());
        l.points.assign(entries.begin(), entries.end());
        leaves.push_back(std::move(l));
    });

    return leaves;
}

std::vector<leaf> create_obo_leaves(const std::vector<vec2>& points) {
    geodb_assert(leaf_size > 1, "invalid leaf size");

    using internal = geodb::tree_internal<leaf_size, leaf_size>;
    using tree_t = geodb::tree<internal, 1>;

    tree_t tree(internal(), 1.0);
    for (const auto& p : points) {
        tree_entry e;
        e.trajectory_id = 1;
        e.unit_index = 1;
        e.unit.label = 1;
        e.unit.start = vector3(p.x, p.y, 0);
        e.unit.end = vector3(p.x, p.y, 1);
        tree.insert(e);
    }

    struct leaf_visitor {
        std::vector<leaf> leaves;

        void operator()(tree_t::cursor& c) {
            if (c.is_leaf()) {
                leaf l;
                for (size_t i = 0; i < c.size(); ++i) {
                    tree_entry e = c.value(i);
                    l.points.push_back({e.unit.start.x(), e.unit.start.y()});
                }
                l.mbb = get_bounding_box(l.points.begin(), l.points.end());
                leaves.push_back(std::move(l));
            } else {
                for (size_t i = 0; i < c.size(); ++i) {
                    c.move_child(i);
                    (*this)(c);
                    c.move_parent();
                }
            }
        }
    } visitor;
    auto cursor = tree.root();
    visitor(cursor);
    return std::move(visitor.leaves);
}

int main(int argc, char** argv) {
    return tpie_main([&]{
        parse_options(argc, argv);

        // Generate a set of random points.
        std::vector<vec2> points = skewed
                ? create_skewed_points(num_points)
                : create_points(num_points);

        // Iterate over the sorted points and create leaf nodes.
        std::vector<leaf> leaves = [&] {
            if (algorithm == "hilbert") {
                return heuristic
                        ? create_hilbert_leaves_heuristic(points)
                        : create_hilbert_leaves(points);
            } else if (algorithm == "str") {
                return create_str_leaves(points);
            } else if (algorithm == "quickload") {
                return create_quickload_leaves(points);
            } else if (algorithm == "obo") {
                return create_obo_leaves(points);
            } else {
                fmt::print(cerr, "Invalid algorithm: {}.\n", algorithm);
                throw exit_main(1);
            }
        }();

        json result = {
            {"leaves", leaves},
            {"seed", seed},
        };

        cout << result.dump(4) << endl;
        return 0;
    });
}