/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan */

#include "geometric_shapes/shape_operations.h"

#include <cstdio>
#include <cmath>
#include <algorithm>
#include <set>
#include <float.h>

#include <console_bridge/console.h>

#include <Eigen/Geometry>

#include <geometric_shapes/shape_to_marker.h>
#include <geometric_shapes/shape_extents.h>
#include <geometric_shapes/solid_primitive_dims.h>

namespace shapes
{
Shape* constructShapeFromMsg(const shape_msgs::Plane& shape_msg)
{
  return new Plane(shape_msg.coef[0], shape_msg.coef[1], shape_msg.coef[2], shape_msg.coef[3]);
}

Shape* constructShapeFromMsg(const shape_msgs::Mesh& shape_msg)
{
  if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
  {
    CONSOLE_BRIDGE_logWarn("Mesh definition is empty");
    return nullptr;
  }
  else
  {
    EigenSTL::vector_Vector3d vertices(shape_msg.vertices.size());
    std::vector<unsigned int> triangles(shape_msg.triangles.size() * 3);
    for (unsigned int i = 0; i < shape_msg.vertices.size(); ++i)
      vertices[i] = Eigen::Vector3d(shape_msg.vertices[i].x, shape_msg.vertices[i].y, shape_msg.vertices[i].z);
    for (unsigned int i = 0; i < shape_msg.triangles.size(); ++i)
    {
      unsigned int i3 = i * 3;
      triangles[i3++] = shape_msg.triangles[i].vertex_indices[0];
      triangles[i3++] = shape_msg.triangles[i].vertex_indices[1];
      triangles[i3] = shape_msg.triangles[i].vertex_indices[2];
    }
    return createMeshFromVertices(vertices, triangles);
  }
}

Shape* constructShapeFromMsg(const shape_msgs::SolidPrimitive& shape_msg)
{
  Shape* shape = nullptr;
  if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE)
  {
    if (shape_msg.dimensions.size() >=
        geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value)
      shape = new Sphere(shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS]);
  }
  else if (shape_msg.type == shape_msgs::SolidPrimitive::BOX)
  {
    if (shape_msg.dimensions.size() >= geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value)
      shape = new Box(shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X],
                      shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y],
                      shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
  }
  else if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER)
  {
    if (shape_msg.dimensions.size() >=
        geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value)
      shape = new Cylinder(shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS],
                           shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]);
  }
  else if (shape_msg.type == shape_msgs::SolidPrimitive::CONE)
  {
    if (shape_msg.dimensions.size() >=
        geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CONE>::value)
      shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS],
                       shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]);
  }
  if (shape == nullptr)
    CONSOLE_BRIDGE_logError("Unable to construct shape corresponding to shape_msg of type %d", (int)shape_msg.type);

  return shape;
}

namespace
{
class ShapeVisitorAlloc : public boost::static_visitor<Shape*>
{
public:
  Shape* operator()(const shape_msgs::Plane& shape_msg) const
  {
    return constructShapeFromMsg(shape_msg);
  }

  Shape* operator()(const shape_msgs::Mesh& shape_msg) const
  {
    return constructShapeFromMsg(shape_msg);
  }

  Shape* operator()(const shape_msgs::SolidPrimitive& shape_msg) const
  {
    return constructShapeFromMsg(shape_msg);
  }
};
}  // namespace

Shape* constructShapeFromMsg(const ShapeMsg& shape_msg)
{
  return boost::apply_visitor(ShapeVisitorAlloc(), shape_msg);
}

namespace
{
class ShapeVisitorMarker : public boost::static_visitor<void>
{
public:
  ShapeVisitorMarker(visualization_msgs::Marker* marker, bool use_mesh_triangle_list)
    : boost::static_visitor<void>(), use_mesh_triangle_list_(use_mesh_triangle_list), marker_(marker)
  {
  }

  void operator()(const shape_msgs::Plane& /* shape_msg */) const
  {
    throw std::runtime_error("No visual markers can be constructed for planes");
  }

  void operator()(const shape_msgs::Mesh& shape_msg) const
  {
    geometric_shapes::constructMarkerFromShape(shape_msg, *marker_, use_mesh_triangle_list_);
  }

  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
  {
    geometric_shapes::constructMarkerFromShape(shape_msg, *marker_);
  }

private:
  bool use_mesh_triangle_list_;
  visualization_msgs::Marker* marker_;
};
}  // namespace

bool constructMarkerFromShape(const Shape* shape, visualization_msgs::Marker& marker, bool use_mesh_triangle_list)
{
  ShapeMsg shape_msg;
  if (constructMsgFromShape(shape, shape_msg))
  {
    bool ok = false;
    try
    {
      boost::apply_visitor(ShapeVisitorMarker(&marker, use_mesh_triangle_list), shape_msg);
      ok = true;
    }
    catch (std::runtime_error& ex)
    {
      CONSOLE_BRIDGE_logError("%s", ex.what());
    }
    if (ok)
      return true;
  }
  return false;
}

namespace
{
class ShapeVisitorComputeExtents : public boost::static_visitor<Eigen::Vector3d>
{
public:
  Eigen::Vector3d operator()(const shape_msgs::Plane& /* shape_msg */) const
  {
    Eigen::Vector3d e(0.0, 0.0, 0.0);
    return e;
  }

  Eigen::Vector3d operator()(const shape_msgs::Mesh& shape_msg) const
  {
    double x_extent, y_extent, z_extent;
    geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
    Eigen::Vector3d e(x_extent, y_extent, z_extent);
    return e;
  }

  Eigen::Vector3d operator()(const shape_msgs::SolidPrimitive& shape_msg) const
  {
    double x_extent, y_extent, z_extent;
    geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
    Eigen::Vector3d e(x_extent, y_extent, z_extent);
    return e;
  }
};
}  // namespace

Eigen::Vector3d computeShapeExtents(const ShapeMsg& shape_msg)
{
  return boost::apply_visitor(ShapeVisitorComputeExtents(), shape_msg);
}

Eigen::Vector3d computeShapeExtents(const Shape* shape)
{
  if (shape->type == SPHERE)
  {
    double d = static_cast<const Sphere*>(shape)->radius * 2.0;
    return Eigen::Vector3d(d, d, d);
  }
  else if (shape->type == BOX)
  {
    const double* sz = static_cast<const Box*>(shape)->size;
    return Eigen::Vector3d(sz[0], sz[1], sz[2]);
  }
  else if (shape->type == CYLINDER)
  {
    double d = static_cast<const Cylinder*>(shape)->radius * 2.0;
    return Eigen::Vector3d(d, d, static_cast<const Cylinder*>(shape)->length);
  }
  else if (shape->type == CONE)
  {
    double d = static_cast<const Cone*>(shape)->radius * 2.0;
    return Eigen::Vector3d(d, d, static_cast<const Cone*>(shape)->length);
  }
  else if (shape->type == MESH)
  {
    const Mesh* mesh = static_cast<const Mesh*>(shape);
    if (mesh->vertex_count > 1)
    {
      std::vector<double> vmin(3, std::numeric_limits<double>::max());
      std::vector<double> vmax(3, -std::numeric_limits<double>::max());
      for (unsigned int i = 0; i < mesh->vertex_count; ++i)
      {
        unsigned int i3 = i * 3;
        for (unsigned int k = 0; k < 3; ++k)
        {
          unsigned int i3k = i3 + k;
          if (mesh->vertices[i3k] > vmax[k])
            vmax[k] = mesh->vertices[i3k];
          if (mesh->vertices[i3k] < vmin[k])
            vmin[k] = mesh->vertices[i3k];
        }
      }
      return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]);
    }
    else
      return Eigen::Vector3d(0.0, 0.0, 0.0);
  }
  else
    return Eigen::Vector3d(0.0, 0.0, 0.0);
}

void computeShapeBoundingSphere(const Shape* shape, Eigen::Vector3d& center, double& radius)
{
  center.x() = 0.0;
  center.y() = 0.0;
  center.z() = 0.0;
  radius = 0.0;

  if (shape->type == SPHERE)
  {
    radius = static_cast<const Sphere*>(shape)->radius;
  }
  else if (shape->type == BOX)
  {
    const double* sz = static_cast<const Box*>(shape)->size;
    double half_width = sz[0] * 0.5;
    double half_height = sz[1] * 0.5;
    double half_depth = sz[2] * 0.5;
    radius = std::sqrt(half_width * half_width + half_height * half_height + half_depth * half_depth);
  }
  else if (shape->type == CYLINDER)
  {
    double cyl_radius = static_cast<const Cylinder*>(shape)->radius;
    double half_len = static_cast<const Cylinder*>(shape)->length * 0.5;
    radius = std::sqrt(cyl_radius * cyl_radius + half_len * half_len);
  }
  else if (shape->type == CONE)
  {
    double cone_radius = static_cast<const Cone*>(shape)->radius;
    double cone_height = static_cast<const Cone*>(shape)->length;

    if (cone_height > cone_radius)
    {
      // center of sphere is intersection of perpendicular bisectors:
      double z = (cone_height - (cone_radius * cone_radius / cone_height)) * 0.5;
      center.z() = z - (cone_height * 0.5);
      radius = cone_height - z;
    }
    else
    {
      // short cone.  Bounding sphere touches base only.
      center.z() = -(cone_height * 0.5);
      radius = cone_radius;
    }
  }
  else if (shape->type == MESH)
  {
    const Mesh* mesh = static_cast<const Mesh*>(shape);
    if (mesh->vertex_count > 1)
    {
      double mx = std::numeric_limits<double>::max();
      Eigen::Vector3d min(mx, mx, mx);
      Eigen::Vector3d max(-mx, -mx, -mx);
      unsigned int cnt = mesh->vertex_count * 3;
      for (unsigned int i = 0; i < cnt; i += 3)
      {
        Eigen::Vector3d v(mesh->vertices[i + 0], mesh->vertices[i + 1], mesh->vertices[i + 2]);
        min = min.cwiseMin(v);
        max = max.cwiseMax(v);
      }

      center = (min + max) * 0.5;
      radius = (max - min).norm() * 0.5;
    }
  }
}

bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg)
{
  if (shape->type == SPHERE)
  {
    shape_msgs::SolidPrimitive s;
    s.type = shape_msgs::SolidPrimitive::SPHERE;
    s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
    s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = static_cast<const Sphere*>(shape)->radius;
    shape_msg = s;
  }
  else if (shape->type == BOX)
  {
    shape_msgs::SolidPrimitive s;
    s.type = shape_msgs::SolidPrimitive::BOX;
    const double* sz = static_cast<const Box*>(shape)->size;
    s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
    s.dimensions[shape_msgs::SolidPrimitive::BOX_X] = sz[0];
    s.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = sz[1];
    s.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = sz[2];
    shape_msg = s;
  }
  else if (shape->type == CYLINDER)
  {
    shape_msgs::SolidPrimitive s;
    s.type = shape_msgs::SolidPrimitive::CYLINDER;
    s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
    s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = static_cast<const Cylinder*>(shape)->radius;
    s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = static_cast<const Cylinder*>(shape)->length;
    shape_msg = s;
  }
  else if (shape->type == CONE)
  {
    shape_msgs::SolidPrimitive s;
    s.type = shape_msgs::SolidPrimitive::CONE;
    s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CONE>::value);
    s.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] = static_cast<const Cone*>(shape)->radius;
    s.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT] = static_cast<const Cone*>(shape)->length;
    shape_msg = s;
  }
  else if (shape->type == PLANE)
  {
    shape_msgs::Plane s;
    const Plane* p = static_cast<const Plane*>(shape);
    s.coef[0] = p->a;
    s.coef[1] = p->b;
    s.coef[2] = p->c;
    s.coef[3] = p->d;
    shape_msg = s;
  }
  else if (shape->type == MESH)
  {
    shape_msgs::Mesh s;
    const Mesh* mesh = static_cast<const Mesh*>(shape);
    s.vertices.resize(mesh->vertex_count);
    s.triangles.resize(mesh->triangle_count);

    for (unsigned int i = 0; i < mesh->vertex_count; ++i)
    {
      unsigned int i3 = i * 3;
      s.vertices[i].x = mesh->vertices[i3];
      s.vertices[i].y = mesh->vertices[i3 + 1];
      s.vertices[i].z = mesh->vertices[i3 + 2];
    }

    for (unsigned int i = 0; i < s.triangles.size(); ++i)
    {
      unsigned int i3 = i * 3;
      s.triangles[i].vertex_indices[0] = mesh->triangles[i3];
      s.triangles[i].vertex_indices[1] = mesh->triangles[i3 + 1];
      s.triangles[i].vertex_indices[2] = mesh->triangles[i3 + 2];
    }
    shape_msg = s;
  }
  else
  {
    CONSOLE_BRIDGE_logError("Unable to construct shape message for shape of type %d", (int)shape->type);
    return false;
  }

  return true;
}

void saveAsText(const Shape* shape, std::ostream& out)
{
  if (shape->type == SPHERE)
  {
    out << Sphere::STRING_NAME << std::endl;
    out << static_cast<const Sphere*>(shape)->radius << std::endl;
  }
  else if (shape->type == BOX)
  {
    out << Box::STRING_NAME << std::endl;
    const double* sz = static_cast<const Box*>(shape)->size;
    out << sz[0] << " " << sz[1] << " " << sz[2] << std::endl;
  }
  else if (shape->type == CYLINDER)
  {
    out << Cylinder::STRING_NAME << std::endl;
    out << static_cast<const Cylinder*>(shape)->radius << " " << static_cast<const Cylinder*>(shape)->length
        << std::endl;
  }
  else if (shape->type == CONE)
  {
    out << Cone::STRING_NAME << std::endl;
    out << static_cast<const Cone*>(shape)->radius << " " << static_cast<const Cone*>(shape)->length << std::endl;
  }
  else if (shape->type == PLANE)
  {
    out << Plane::STRING_NAME << std::endl;
    const Plane* p = static_cast<const Plane*>(shape);
    out << p->a << " " << p->b << " " << p->c << " " << p->d << std::endl;
  }
  else if (shape->type == MESH)
  {
    out << Mesh::STRING_NAME << std::endl;
    const Mesh* mesh = static_cast<const Mesh*>(shape);
    out << mesh->vertex_count << " " << mesh->triangle_count << std::endl;

    for (unsigned int i = 0; i < mesh->vertex_count; ++i)
    {
      unsigned int i3 = i * 3;
      out << mesh->vertices[i3] << " " << mesh->vertices[i3 + 1] << " " << mesh->vertices[i3 + 2] << std::endl;
    }

    for (unsigned int i = 0; i < mesh->triangle_count; ++i)
    {
      unsigned int i3 = i * 3;
      out << mesh->triangles[i3] << " " << mesh->triangles[i3 + 1] << " " << mesh->triangles[i3 + 2] << std::endl;
    }
  }
  else
  {
    CONSOLE_BRIDGE_logError("Unable to save shape of type %d", (int)shape->type);
  }
}

Shape* constructShapeFromText(std::istream& in)
{
  Shape* result = nullptr;
  if (in.good() && !in.eof())
  {
    std::string type;
    in >> type;
    if (in.good() && !in.eof())
    {
      if (type == Sphere::STRING_NAME)
      {
        double radius;
        in >> radius;
        result = new Sphere(radius);
      }
      else if (type == Box::STRING_NAME)
      {
        double x, y, z;
        in >> x >> y >> z;
        result = new Box(x, y, z);
      }
      else if (type == Cylinder::STRING_NAME)
      {
        double r, l;
        in >> r >> l;
        result = new Cylinder(r, l);
      }
      else if (type == Cone::STRING_NAME)
      {
        double r, l;
        in >> r >> l;
        result = new Cone(r, l);
      }
      else if (type == Plane::STRING_NAME)
      {
        double a, b, c, d;
        in >> a >> b >> c >> d;
        result = new Plane(a, b, c, d);
      }
      else if (type == Mesh::STRING_NAME)
      {
        unsigned int v, t;
        in >> v >> t;
        Mesh* m = new Mesh(v, t);
        result = m;
        for (unsigned int i = 0; i < m->vertex_count; ++i)
        {
          unsigned int i3 = i * 3;
          in >> m->vertices[i3] >> m->vertices[i3 + 1] >> m->vertices[i3 + 2];
        }
        for (unsigned int i = 0; i < m->triangle_count; ++i)
        {
          unsigned int i3 = i * 3;
          in >> m->triangles[i3] >> m->triangles[i3 + 1] >> m->triangles[i3 + 2];
        }
        m->computeTriangleNormals();
        m->computeVertexNormals();
      }
      else
        CONSOLE_BRIDGE_logError("Unknown shape type: '%s'", type.c_str());
    }
  }
  return result;
}

const std::string& shapeStringName(const Shape* shape)
{
  static const std::string unknown = "unknown";
  if (shape)
    switch (shape->type)
    {
      case SPHERE:
        return Sphere::STRING_NAME;
      case CYLINDER:
        return Cylinder::STRING_NAME;
      case CONE:
        return Cone::STRING_NAME;
      case BOX:
        return Box::STRING_NAME;
      case PLANE:
        return Plane::STRING_NAME;
      case MESH:
        return Mesh::STRING_NAME;
      case OCTREE:
        return OcTree::STRING_NAME;
      default:
        return unknown;
    }
  else
  {
    static const std::string empty;
    return empty;
  }
}
}  // namespace shapes


#ifndef FILTERING_HPP
#define FILTERING_HPP

#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

typedef boost::numeric::ublas::vector<double> RealVector;
typedef boost::numeric::ublas::matrix<double> RealMatrix;

class EditorDefault;

enum FilterStates {
	IDLE,         // Filter is awaiting the first observation
	INITIALIZING, // Filter has processed one or more observations, but state estimate is not reliable
	RUNNING,      // Filter state estimate is reliable
	COASTING,     // Filter has not received a recent observation, but the predicted state should be usable
	RESETING,     // Filter coast interval has been exceed and it will reinitialize on the next observation
};

class FilterBase {
public:
	FilterBase(unsigned n0 = 1);

	void restart(double t0, RealVector Z0);

	double getGoodnessOfFit(void);

	double getBiasOfFit(void);

	unsigned getN0(void);

	FilterStates getState(void);

	void setEditor(EditorDefault* editor);

	void setState(FilterStates state);

	virtual double getTime(void) = 0;

	virtual RealVector getState(double t) = 0;

	virtual bool add(double t, double y) = 0;

protected:
	double t0;
	double t;
	RealVector Z;
	unsigned n0;
	FilterStates state;
	EditorDefault* editor;

};

class EditorDefault {
public:
	EditorDefault(FilterBase* filter, std::size_t editingWindow = 25);

	void reset(void);

	void updateResiduals(double e);

	boost::tuples::tuple<double, double> getResidualStatitics(void);

	bool isGoodObservation(double t, double y, double e);

protected:
	FilterBase*     filter;
	unsigned        n;
	RealVector  E;
};

FilterBase::FilterBase(unsigned n0) :
	state(IDLE) {
	this->n0 = n0;
	this->editor = new EditorDefault(this);
}

void FilterBase::restart(double t0, RealVector Z0) {
	this->t0 = t0;
	this->t = t0;
	this->Z = Z0;
}

double FilterBase::getGoodnessOfFit(void) {
	return boost::tuples::get<1>(this->editor->getResidualStatitics());
}

double FilterBase::getBiasOfFit(void) {
	return boost::tuples::get<0>(this->editor->getResidualStatitics());
}

unsigned FilterBase::getN0(void) {
	return n0;
}

FilterStates FilterBase::getState(void) {
	return state;
}

void FilterBase::setEditor(EditorDefault* editor) {
	this->editor = editor;
}

void FilterBase::setState(FilterStates state) {
	this->state = state;
}

EditorDefault::EditorDefault(FilterBase* filter, std::size_t editingWindow) :
	filter(filter) {
	this->n = 0;
	this->E = RealVector(editingWindow);
}

void EditorDefault::reset() {
	this->n = 0;
	this->E *= 0.0;
}

void EditorDefault::updateResiduals(double e) {
	if (this->E.size() > 0) {
		this->E(this->n % this->E.size()) = e;
	}
	this->n++;
}

boost::tuples::tuple<double, double> EditorDefault::getResidualStatitics(void) {
	//accumulator_set<double, stats<tag::variance> > acc;
	//for_each(E.begin(), E.end(), boost::bind<void>(boost::ref(acc), _1) );
	return boost::tuples::tuple<double, double>(0.0, 0.0); // mean(acc), variance(acc));
}

bool EditorDefault::isGoodObservation(double t, double y, double e) {
	if (this->n >= this->filter->getN0()) {
		this->filter->setState(RUNNING);
	}
	else {
		this->filter->setState(INITIALIZING);
	}
	return false;
}

#endif


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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2013-2014.
// Modifications copyright (c) 2013-2014, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP

#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/algorithms/point_on_surface.hpp>

#include <boost/geometry/algorithms/detail/disjoint/linear_linear.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace disjoint
{

template<typename Geometry>
struct check_each_ring_for_within
{
    bool has_within;
    Geometry const& m_geometry;

    inline check_each_ring_for_within(Geometry const& g)
        : has_within(false)
        , m_geometry(g)
    {}

    template <typename Range>
    inline void apply(Range const& range)
    {
        if ( geometry::within(geometry::return_point_on_surface(range), m_geometry) )
        {
            has_within = true;
        }
    }
};

template <typename FirstGeometry, typename SecondGeometry>
inline bool rings_containing(FirstGeometry const& geometry1,
                                 SecondGeometry const& geometry2)
{
    check_each_ring_for_within<FirstGeometry> checker(geometry1);
    geometry::detail::for_each_range(geometry2, checker);
    return checker.has_within;
}

template <typename Geometry1, typename Geometry2>
struct general_areal
{
    static inline
    bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2)
    {
        if ( ! disjoint_linear<Geometry1, Geometry2>::apply(geometry1, geometry2) )
        {
            return false;
        }

        // If there is no intersection of segments, they might located
        // inside each other

        // We check that using a point on the surface, and see if that is inside
        // the other geometry. And vice versa.

        if ( rings_containing(geometry1, geometry2)
          || rings_containing(geometry2, geometry1) )
        {
            return false;
        }

        return true;
    }
};

}} // namespace detail::disjoint
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Areal1, typename Areal2>
struct disjoint<Areal1, Areal2, 2, areal_tag, areal_tag, false>
    : detail::disjoint::general_areal<Areal1, Areal2>
{};

template <typename Areal, typename Box>
struct disjoint<Areal, Box, 2, areal_tag, box_tag, false>
    : detail::disjoint::general_areal<Areal, Box>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISJOINT_AREAL_AREAL_HPP


#include <iostream>
#include <type_traits>
#include <boost/hana/traits.hpp>
#include "pred_expr.h"
namespace hana = boost::hana;
namespace traits = hana::traits;
struct Pod{

};
  constexpr auto pc = hana::traits::is_pod & hana::traits::is_class;
  constexpr auto pf = hana::traits::is_signed & hana::traits::is_fundamental;
template<class T, traits::Constraint<decltype(pc), T> = nullptr>
void f(T)
{
  std::cout<<2<<std::endl;
}
template<class T, traits::Constraint<decltype(pf), T> = nullptr>
void f(T)
{
  std::cout<<1<<std::endl;
}
template<class T, bool = traits::constraint(pc)>
void f2(T)
{
  std::cout<<3<<std::endl;
}
struct CA{};
int main()
{
  Pod pod;
  f(pod);
  auto i = 100;
  f(i);
  f2(pod);
  return 0;
  //hana::traits::S s;
  //constexpr auto tb = s&s;
  //constexpr auto r = hana::traits::b & hana::traits::b;
  constexpr auto t = hana::traits::is_pod & hana::traits::is_class;
  constexpr auto o = hana::traits::is_pod | hana::traits::is_fundamental;
  constexpr auto xor_ = hana::traits::is_class ^ hana::traits::is_fundamental;
  constexpr auto notclass_ = !hana::traits::is_class ;
  static_assert(hana::traits::is_pod(hana::type_c<Pod>), "");
  static_assert(t(hana::type_c<Pod>), "");
  static_assert(o(hana::type_c<int>), "");
  static_assert(xor_(hana::type_c<int>), "");
  static_assert(xor_(hana::type_c<CA>), "");
  static_assert(notclass_(hana::type_c<int>), "");
  //constexpr auto tb = hana::traits::isvoid & hana::traits::isvoid;
  std::cout<<traits::AllPAst<decltype(hana::traits::is_pod),decltype(hana::traits::is_class) >::value<<std::endl;
  //std::cout<<traits::isTraits<decltype(hana::traits::is_pod)><<std::endl;
  std::cout<<traits::isTraits<const hana::traits::detail::hana_trait<std::is_pod>><<std::endl;
  //static_assert(traits::isTraits<decltype(hana::traits::is_pod)>,"");
}


#include <ros/ros.h>
#include "ros_utils.hpp"
#include "kinematics.hpp"
#include "estimators.hpp"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <casadi/casadi.hpp>

#include <mhe_estimator/ArticulatedAngles.h>
#include <mhe_estimator/CanData.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Twist.h"
#include "ackermann_msgs/AckermannDrive.h"
#include "std_srvs/Trigger.h"
#include "std_srvs/Empty.h"

//--------------------------| Moving Horizen Window Length |---------------------------//
const long unsigned int N_mhe = 60;
//-------------------------------------------------------------------------------------//

int main(int argc, char **argv)
{
    using namespace mhe_estimator;
    using namespace ast;
    using namespace ast::ros;
    ::ros::init(argc, argv, "mhe_estimator_node");
    NodeHandle nh;

    /*----------------| Get Params |----------------*/
    CarParams carParams;
    MheParams mheParams;
    ParamsIn(carParams, mheParams, nh);
    /*---------------------------------------------*/

    /*------------| Global Var and Obj |-----------*/
    ::ros::Time lastPerceptionTime;
    const long unsigned int N_mhePlus1 = N_mhe + 1;
    boost::array<Vec4, N_mhePlus1> q4wLoc;        //x y theta beta window
    boost::array<Vec2, N_mhe> control2w;           //control2 window

    boost::array<Vec4, N_mhePlus1> q4wCov;        //state cov window
    boost::array<Vec2, N_mhe> control2wCov;        //control cov window

    boost::array<Vec5, N_mhePlus1> q5wLocTrailer; //x y theta beta window triler =1
    boost::array<Vec2, N_mhe> control2wTrailer;    //control2 window triler =1

    boost::array<Vec5, N_mhePlus1> q5wCovTrailer; //state cov window trailer1
    boost::array<Vec2, N_mhe> control2wCovTrailer; //control cov window trailer1

    Vec4 qLoc;                 //triler == 0
    Vec2 controls;              //triler == 0
    Vec4 qEstFirstSample;       //triler == 0

    Vec4 qCov;               //cov triler == 0
    Vec4 qCovMhe;
    Vec2 controlsCov;           //cov triler == 0

    Vec5 qLocTrailer;         //triler == 1
    Vec2 controlsTrailer;        //triler == 1
    Vec5 qEstFirstSampleTrailer; //triler == 1

    Vec5 qCovTrailer;
    Vec5 qCovMheTrailer;         //Cov triler == 1
    Vec2 controlsCovTrailer;     //cov triler == 1

    Vec3 q;
    Vec4 qTrailerLoc;
    Vec3 qCarEst;
    Vec4 qCarEstMhe;
    Vec2 ctrlEstMhe;
    Vec5 qMheTrailer;
    Vec2 ctrMheTrailer;
    Vec4 qOneTrailerEst;
    int sampleNo;
    /*---------------------------------------------*/

    /*----------------| Get Casadi Solver  |----------------*/
    casadi::Function solverCar;
    casadi::Function solverTrailer;
    ROS_INFO_STREAM("Creating CasADi Solvers");
    mheSetupCar(solverCar, carParams, mheParams,N_mhe);
    mheSetupTrailer(solverTrailer, carParams, mheParams,N_mhe);
    ROS_INFO_STREAM("Car like solver: "<< solverCar <<"");
    ROS_INFO_STREAM("single wagon like solver: "<< solverTrailer <<"");

    casadi::DM argx0 = casadi::SX::zeros((4*(N_mhe + 1))+(2*N_mhe));  // 4 =  n_state    2 = n_control   //triler = 0
    casadi::DM argx0Trailer = casadi::SX::zeros((5*(N_mhe + 1))+(2*N_mhe));  // 4 =  n_state    2 = n_control  //triler =1
    /*------------------------------------------------------*/

    /*----------------| ROS Subscribers and Publishers  |-----------------------------------------*/

    auto canDataIn = nh.Input<CanData>("/processed_can_data");
    boost::shared_ptr<CanData> canData(new CanData());

    auto perceptionPoseCamIn = nh.Input<geometry_msgs::PoseStamped>("/pose_estimator/charger_pose/location_cam");
    auto perceptionPoseGpsIn = nh.Input<geometry_msgs::PoseStamped>("/pose_estimator/charger_pose/location_gps");
    boost::shared_ptr<geometry_msgs::PoseStamped> perceptionPose(new geometry_msgs::PoseStamped());

    auto perceptionTwistOut = nh.Output<geometry_msgs::Twist>("mhe_node/perception/twist");
    boost::shared_ptr<geometry_msgs::Twist> perceptionTwistData(new geometry_msgs::Twist());

    auto perceptionTwistMheOut = nh.Output<geometry_msgs::Twist>("mhe_node/mhe_estimated/twist");
    geometry_msgs::Twist perceptionTwistMheData;

    auto perceptionTwistWeightedOut = nh.Output<geometry_msgs::Twist>("mhe_node/weighted_estimated/twist");
    geometry_msgs::Twist perceptionTwistWeightedData;

    auto articulatedAnglesOut = nh.Output<ArticulatedAngles>("mhe_node/can_data/articulated_angles");
    ArticulatedAngles articulatedAnglesData;

    auto articulatedAnglesMheOut = nh.Output<ArticulatedAngles>("mhe_node/mhe_estimated/articulated_angles");
    ArticulatedAngles articulatedAnglesMheData;

    auto articulatedAnglesWeightedOut = nh.Output<ArticulatedAngles>("mhe_node/weighted_estimated/articulated_angles");
    ArticulatedAngles articulatedAnglesWeightedData;

    auto poseWithCovarianceIn = nh.Input<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/perception_data/pose_with_covariance");
    //geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceData;

    auto poseWithCovarianceMheOut = nh.Output<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/mhe_estimated/pose_with_covariance");
    geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceMheData;

    auto poseWithCovarianceWeightedOut = nh.Output<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/weighted_estimated/pose_with_covariance");
    geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceWeightedData;

    auto ackermannDriveOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/can_data/ackermann_drive");
    ackermann_msgs::AckermannDrive ackermannDriveData;

    auto ackermannDriveMheOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/mhe_estimated/ackermann_drive");
    ackermann_msgs::AckermannDrive ackermannDriveMheData;

    //auto ackermannDriveWeightedOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/weighted_estimated/ackermann_drive");
    //ackermann_msgs::AckermannDrive ackermannDriveWeightedData;
    /*------------------------------------------------------------------------------------------*/

    ROS_INFO_STREAM("Estimator loop rate: "<< mheParams.loopRate <<"");
    Real t = 0;
    bool firstPoseMeasured = false;
    ::ros::Rate loop_rate(mheParams.loopRate);
    while(::ros::ok())
    {

        auto now = ::ros::Time::now();

        *canData = canDataIn();

        ackermannDriveData.speed = canData->tachoVelocity;
        ackermannDriveData.steering_angle = canData->steeringAngle;
        auto& poseWithCovarianceData = poseWithCovarianceIn();

        if (!mheParams.covarianceFromTopicStamp)
        {
            if(mheParams.perceptionGPS)
            {
                *perceptionPose = perceptionPoseGpsIn();
            }else
            {
                *perceptionPose = perceptionPoseCamIn();
            }
        } else
        {
            perceptionPose->pose = poseWithCovarianceData.pose.pose;
            perceptionPose->header.stamp = poseWithCovarianceData.header.stamp;
        }

        auto perceptionTh = tf::getYaw(perceptionPose->pose.orientation);
        ::ros::Duration timeDiff = perceptionPose->header.stamp - lastPerceptionTime;
        lastPerceptionTime = perceptionPose->header.stamp;
        bool isPerceptionPoseFresh = timeDiff.toSec() >= 0.01;

        perceptionTwistData->linear.x = perceptionPose->pose.position.x;
        perceptionTwistData->linear.y = perceptionPose->pose.position.y;
        perceptionTwistData->angular.z = perceptionTh;
        articulatedAnglesData.trailer1 = canData->beta1;
        if(perceptionPose->pose.position.x != 0 && perceptionPose->pose.position.y != 0)
        {
            if (carParams.TrailerNumber == 0)
            {

                //Th base on estimator
                qLoc[1] = perceptionPose->pose.position.x - carParams.L*cos(perceptionTh);
                qLoc[2] = perceptionPose->pose.position.y - carParams.L*sin(perceptionTh);
                qLoc[3] = canData->steeringAngle;

                if(mheParams.mheActive)
                {
                    controlsCov[0] = 0;
                    controlsCov[1] = 1/mheParams.noiseVarianceLinearVel;
                    if (!mheParams.covarianceFromTopicStamp)
                    {
                        qCov[0] = 1/mheParams.noiseVarianceTh;
                        qCov[1] = 1/mheParams.noiseVariancePos;
                        qCov[2] = 1/mheParams.noiseVariancePos;
                    }else
                    {
                        qCov[0] = poseWithCovarianceData.pose.covariance.elems[35];
                        qCov[1] = poseWithCovarianceData.pose.covariance.elems[0];
                        qCov[2] = poseWithCovarianceData.pose.covariance.elems[7];
                    }

                    qCov[3] = 1/mheParams.noiseVariancesteering;

                    qLoc[0] = continuousAngle(perceptionTh, qCarEstMhe[0]);
                    controls[0] = 0;  //no measured value for dbeta
                    controls[1] = canData->tachoVelocity; //uCar.longitudinalVelocity + noiseArray[4];

                    std::rotate(q4wLoc.begin(), q4wLoc.begin()+1, q4wLoc.end());
                    std::rotate(control2w.begin(), control2w.begin()+1, control2w.end());
                    std::rotate(q4wCov.begin(), q4wCov.begin()+1, q4wCov.end());
                    std::rotate(control2wCov.begin(), control2wCov.begin()+1, control2wCov.end());

                    ::ros::Duration sampleDiff = ::ros::Time::now() - perceptionPose->header.stamp;
                    int sampleNumber = floor(sampleDiff.toSec()/(1.0/mheParams.loopRate));
                    if (sampleNumber > N_mhe)
                    {
                        ROS_WARN_STREAM("WARNING: localization delay = "<< sampleNumber<<" sample" );
                    }
                    if(sampleNumber >=0  && sampleNumber < N_mhe)
                    {
                        if(!firstPoseMeasured)
                        {
                          ROS_INFO_STREAM("First config "<<qLoc[0] <<" "<< qLoc[1]<<" "<< qLoc[2]<<" "<<qLoc[3]<<" ");
                          for(size_t i = 0; i < q4wCov.size(); ++i)
                          {
                            q4wCov[i] = qCov;
                            q4wLoc[i] = qLoc;
                          }
                          qEstFirstSample = qLoc;

                          for(size_t i = 0; i < control2w.size(); ++i)
                          {
                              controls[0] = 0.0;
                              controls[1] = 0.0;
                              control2w[i] = controls;
                              control2wCov[i] = controlsCov;
                          }
                          sampleNo = sampleNumber;
                          ROS_INFO_STREAM("feeding at: N_mhe - "<< sampleNo << " index");
                          ROS_INFO("First pose received. Starting.");
                          firstPoseMeasured = true;
                        }

                        q4wCov[N_mhe] = {0.0, 0.0, 0.0, 0.0};
                        //-----   canData to window -------//
                        q4wCov[N_mhe][3] = qCov[3];
                        q4wLoc[N_mhe][3] = qLoc[3];

                        control2w[N_mhe-1] = controls;
                        control2wCov[N_mhe-1] = controlsCov;
                        //---------------------------------//

                        // ---------perception to window -------//
                        q4wCov[N_mhe - sampleNo][0] = qCov[0];
                        q4wCov[N_mhe - sampleNo][1] = qCov[1];
                        q4wCov[N_mhe - sampleNo][2] = qCov[2];

                        q4wLoc[N_mhe - sampleNo][0] = qLoc[0];
                        q4wLoc[N_mhe - sampleNo][1] = qLoc[1];
                        q4wLoc[N_mhe - sampleNo][2] = qLoc[2];
                        //---------------------------------//

                    }

                    q4wLoc[0] = qEstFirstSample;
                    auto qCovF = qCov;
                    qCovF[0] = qCov[0]*5;
                    qCovF[1] = qCov[1]*5;
                    qCovF[2] = qCov[2]*5;
                    qCovF[3] = qCov[3]*5;
                    q4wCov[0] = qCovF;

                    estimateMhe(argx0, q4wLoc, control2w, q4wCov, control2wCov, carParams, mheParams, solverCar);
                    std::vector<double> resx = std::vector<double>(argx0);
                    qCarEstMhe[0] = resx[(4*(N_mhe+1))-4]; //theta    return estimated 4 = n_states
                    qCarEstMhe[1] = resx[(4*(N_mhe+1))-3]; //x
                    qCarEstMhe[2] = resx[(4*(N_mhe+1))-2]; //y
                    qCarEstMhe[3] = resx[(4*(N_mhe+1))-1]; //beta0

                    qEstFirstSample[0] = resx[0]; //theta return estimated 4 = n_states
                    qEstFirstSample[1] = resx[1]; //x
                    qEstFirstSample[2] = resx[2]; //y
                    qEstFirstSample[3] = resx[3]; //beta0

                    ctrlEstMhe[0] = resx[(4*(N_mhe+1))+(4*N_mhe)-4]; //dbeta
                    ctrlEstMhe[1] = resx[(4*(N_mhe+1))+(4*N_mhe)-3]; //u2

                    qCovMhe[0] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-4];//theta
                    qCovMhe[1] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-3];//x
                    qCovMhe[2] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-2];//y
                    qCovMhe[3] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-1];//beta0

                    poseWithCovarianceMheData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qCarEstMhe[0]);
                    poseWithCovarianceMheData.pose.pose.position.x = qCarEstMhe[1] + carParams.L*cos(qCarEstMhe[0]);
                    poseWithCovarianceMheData.pose.pose.position.y = qCarEstMhe[2] + carParams.L*sin(qCarEstMhe[0]);

                    poseWithCovarianceMheData.pose.covariance.elems[0] = qCovMhe[1];
                    poseWithCovarianceMheData.pose.covariance.elems[7] = qCovMhe[2];
                    poseWithCovarianceMheData.pose.covariance.elems[35] = qCovMhe[0];
                    poseWithCovarianceMheOut(poseWithCovarianceMheData);

                    ackermannDriveMheData.steering_angle = qCarEstMhe[3];
                    ackermannDriveMheData.speed = ctrlEstMhe[1];
                    ackermannDriveMheData.steering_angle_velocity = ctrlEstMhe[0];
                    ackermannDriveMheOut(ackermannDriveMheData);

                    perceptionTwistMheData.angular.z = qCarEstMhe[0];
                    perceptionTwistMheData.linear.x = qCarEstMhe[1] + carParams.L*cos(qCarEstMhe[0]);
                    perceptionTwistMheData.linear.y = qCarEstMhe[2] + carParams.L*sin(qCarEstMhe[0]);
                    perceptionTwistMheOut(perceptionTwistMheData);

                    perceptionTwistOut(perceptionTwistData);
                    articulatedAnglesOut(articulatedAnglesData);
                    ackermannDriveOut(ackermannDriveData);

                }
                if(mheParams.WeightedActive)
                {
                    qLoc[0] = continuousAngle(perceptionTh, qCarEst[0]);
                    q = {qLoc[0],qLoc[1],qLoc[2]};
                    auto simFunc = [&](const Vec3& q, Vec3& dq, const double t)
                    {
                        //if(!carParams.moveGuidancePoint)
                            dq = RDCarKinematicsGPRear(carParams, q, ackermannDriveData);
                        //else
                        //  dq = RDCarKinematicsGPFront(carParams, q, ackermannDriveData);
                    };
                    Vec3 qCarPred = qCarEst;
                    boost::numeric::odeint::integrate(simFunc, qCarPred, 0.0, 1.0/(Real)mheParams.loopRate, 1.0/(Real)mheParams.loopRate);

                    if(!isPerceptionPoseFresh)
                    {
                        qCarEst = qCarPred;
                    }
                    else
                    {
                        //ROS_INFO("fresh gps pose");
                        estimateEst(qCarEst, q, qCarPred ,mheParams);
                    }
                    poseWithCovarianceWeightedData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qCarEst[0]);
                    poseWithCovarianceWeightedData.pose.pose.position.x = qCarEst[1] + carParams.L*cos(qCarEst[0]);
                    poseWithCovarianceWeightedData.pose.pose.position.y = qCarEst[2] + carParams.L*sin(qCarEst[0]);
                    poseWithCovarianceWeightedData.header.stamp = ::ros::Time::now();
                    poseWithCovarianceWeightedOut(poseWithCovarianceWeightedData);

                    perceptionTwistWeightedData.angular.z = qCarEst[0];
                    perceptionTwistWeightedData.linear.x = qCarEst[1] + carParams.L*cos(qCarEst[0]);
                    perceptionTwistWeightedData.linear.y = qCarEst[2] + carParams.L*sin(qCarEst[0]);
                    perceptionTwistWeightedOut(perceptionTwistWeightedData);
                }

            }else if (carParams.TrailerNumber == 1)
            {

                qLocTrailer[0] = canData->beta1;
                //Th base on estimator is different
                qLocTrailer[2] = perceptionPose->pose.position.x - carParams.L*cos(perceptionTh);
                qLocTrailer[3] = perceptionPose->pose.position.y - carParams.L*sin(perceptionTh);
                qLocTrailer[4] = canData->steeringAngle;

                if(mheParams.mheActive)
                {
                    controlsCovTrailer[0] = 0;
                    controlsCovTrailer[1] = 1/mheParams.noiseVarianceLinearVel;

                    qCovTrailer[0] = 1/mheParams.noiseVarianceTrailer1;
                    if (!mheParams.covarianceFromTopicStamp)
                    {
                        qCovTrailer[1] = 1/mheParams.noiseVarianceTh;
                        qCovTrailer[2] = 1/mheParams.noiseVariancePos;
                        qCovTrailer[3] = 1/mheParams.noiseVariancePos;
                    }else
                    {
                        qCovTrailer[1] = poseWithCovarianceData.pose.covariance.elems[35];
                        qCovTrailer[2] = poseWithCovarianceData.pose.covariance.elems[0];
                        qCovTrailer[3] = poseWithCovarianceData.pose.covariance.elems[7];
                    }

                    qCovTrailer[4] = 1/mheParams.noiseVariancesteering;

                    qLocTrailer[1] = continuousAngle(perceptionTh, qMheTrailer[1]);
                    controlsTrailer[0] = 0; //no measured value for dbeta
                    controlsTrailer[1] = canData->tachoVelocity; //uCar.longitudinalVelocity + noiseArray[4];

                    std::rotate(q5wLocTrailer.begin(), q5wLocTrailer.begin()+1, q5wLocTrailer.end());
                    std::rotate(control2wTrailer.begin(), control2wTrailer.begin()+1, control2wTrailer.end());
                    std::rotate(q5wCovTrailer.begin(), q5wCovTrailer.begin()+1, q5wCovTrailer.end());
                    std::rotate(control2wCovTrailer.begin(), control2wCovTrailer.begin()+1, control2wCovTrailer.end());

                    ::ros::Duration sampleDiff = ::ros::Time::now() - perceptionPose->header.stamp;
                    int sampleNumber = floor(sampleDiff.toSec()/(1.0/mheParams.loopRate));
                    if (sampleNumber > N_mhe)
                    {
                        ROS_WARN_STREAM("WARNING: localization delay = "<< sampleNumber<<" sample" );
                    }
                    if(sampleNumber >=0  && sampleNumber < N_mhe)
                    {
                        if(!firstPoseMeasured)
                        {
                          ROS_INFO_STREAM("First config "<<qLocTrailer[0] <<" "<< qLocTrailer[1]<<" "<< qLocTrailer[2]<<" "<<qLocTrailer[3]<<" "<<qLocTrailer[4]);

                          for(size_t i = 0; i < q5wCovTrailer.size(); ++i)
                          {
                            q5wCovTrailer[i] = qCovTrailer;
                            q5wLocTrailer[i] = qLocTrailer;
                          }

                          qEstFirstSampleTrailer = qLocTrailer;

                          for(size_t i = 0; i < control2wTrailer.size(); ++i)
                          {
                            controlsTrailer[0] = 0.0;
                            controlsTrailer[1] = 0.0;
                            control2wTrailer[i] = controlsTrailer;
                            control2wCovTrailer[i] = controlsCovTrailer;
                          }
                          sampleNo = sampleNumber;
                          ROS_INFO_STREAM("feeding at: N_mhe - "<< sampleNo << " index");
                          ROS_INFO("First pose received. Starting.");
                          firstPoseMeasured = true;
                        }

                        q5wCovTrailer[N_mhe] = {0.0, 0.0, 0.0, 0.0, 0.0};

                        //-----   canData to window -------//
                        q5wCovTrailer[N_mhe][0] = 1/mheParams.noiseVarianceTrailer1;
                        q5wCovTrailer[N_mhe][4] = 1/mheParams.noiseVariancesteering;

                        q5wLocTrailer[N_mhe][0] = qLocTrailer[0];
                        q5wLocTrailer[N_mhe][4] = qLocTrailer[4];

                        control2wTrailer[N_mhe-1] = controlsTrailer;
                        control2wCovTrailer[N_mhe-1] = controlsCovTrailer;
                        //---------------------------------//

                        // ---------perception to window -------//
                        q5wCovTrailer[N_mhe -sampleNo][1] = qCovTrailer[1];
                        q5wCovTrailer[N_mhe -sampleNo][2] = qCovTrailer[2];
                        q5wCovTrailer[N_mhe -sampleNo][3] = qCovTrailer[3];

                        q5wLocTrailer[N_mhe -sampleNo][1] = qLocTrailer[1];
                        q5wLocTrailer[N_mhe -sampleNo][2] = qLocTrailer[2];
                        q5wLocTrailer[N_mhe -sampleNo][3] = qLocTrailer[3];
                        //--------------------------------------------//

                    }

                    q5wLocTrailer[0] = qEstFirstSampleTrailer;
                    auto qCovTrailerF = qCovTrailer;
                    qCovTrailerF[0] = 5/mheParams.noiseVarianceTrailer1;

                    qCovTrailerF[1] = qCovTrailer[1]*5;
                    qCovTrailerF[2] = qCovTrailer[2]*5;
                    qCovTrailerF[3] = qCovTrailer[3]*5;

                    qCovTrailerF[4] = 5/mheParams.noiseVariancesteering;
                    q5wCovTrailer[0] = qCovTrailerF;

                    control2wTrailer[N_mhe-1] = controlsTrailer;
                    control2wCovTrailer[N_mhe-1] = controlsCovTrailer;

                    estimateMheTrailer(argx0Trailer, q5wLocTrailer, control2wTrailer, q5wCovTrailer, control2wCovTrailer, carParams, mheParams,solverTrailer);

                    std::vector<double> resxTrailer = std::vector<double>(argx0Trailer);
                    qMheTrailer[0] = resxTrailer[(5*(N_mhe+1))-5]; //beta1                return estimated 4 = n_states
                    qMheTrailer[1] = resxTrailer[(5*(N_mhe+1))-4]; //theta
                    qMheTrailer[2] = resxTrailer[(5*(N_mhe+1))-3]; //x
                    qMheTrailer[3] = resxTrailer[(5*(N_mhe+1))-2]; //y
                    qMheTrailer[4] = resxTrailer[(5*(N_mhe+1))-1]; //beta0

                    qEstFirstSampleTrailer[0] = resxTrailer[0]; //beta1                return estimated 4 = n_states
                    qEstFirstSampleTrailer[1] = resxTrailer[1]; //theta
                    qEstFirstSampleTrailer[2] = resxTrailer[2]; //x
                    qEstFirstSampleTrailer[3] = resxTrailer[3]; //y
                    qEstFirstSampleTrailer[4] = resxTrailer[4]; //beta0

                    ctrMheTrailer[0] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)-5]; //dbeta
                    ctrMheTrailer[1] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)-4]; //u2

                    qCovMheTrailer[0] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-5]; //beta1
                    qCovMheTrailer[1] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-4]; //theta
                    qCovMheTrailer[2] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-3]; //x
                    qCovMheTrailer[3] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-2]; //y
                    qCovMheTrailer[4] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-1]; //beta0

                    poseWithCovarianceMheData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qMheTrailer[1]);
                    poseWithCovarianceMheData.pose.pose.position.x = qMheTrailer[2] + carParams.L*cos(qMheTrailer[1]);
                    poseWithCovarianceMheData.pose.pose.position.y = qMheTrailer[3] + carParams.L*sin(qMheTrailer[1]);

                    poseWithCovarianceMheData.pose.covariance.elems[0] = qCovMheTrailer[2];
                    poseWithCovarianceMheData.pose.covariance.elems[7] = qCovMheTrailer[3];
                    poseWithCovarianceMheData.pose.covariance.elems[35] = qCovMheTrailer[1];
                    poseWithCovarianceMheOut(poseWithCovarianceMheData);

                    ackermannDriveMheData.steering_angle = qMheTrailer[4];
                    ackermannDriveMheData.speed = ctrMheTrailer[1];
                    ackermannDriveMheData.steering_angle_velocity = ctrMheTrailer[0];
                    ackermannDriveMheOut(ackermannDriveMheData);

                    perceptionTwistMheData.angular.z = qMheTrailer[1];
                    perceptionTwistMheData.linear.x = qMheTrailer[2] + carParams.L*cos(qMheTrailer[1]);
                    perceptionTwistMheData.linear.y = qMheTrailer[3] + carParams.L*sin(qMheTrailer[1]);
                    perceptionTwistMheOut(perceptionTwistMheData);

                    articulatedAnglesMheData.trailer1 = qMheTrailer[0];
                    articulatedAnglesMheOut(articulatedAnglesMheData);

                    perceptionTwistOut(perceptionTwistData);
                    articulatedAnglesOut(articulatedAnglesData);
                    ackermannDriveOut(ackermannDriveData);

                }
                if(mheParams.WeightedActive)
                {

                    qLocTrailer[1] = continuousAngle(perceptionTh, qOneTrailerEst[1]);
                    qTrailerLoc = {qLocTrailer[0],qLocTrailer[1],qLocTrailer[2],qLocTrailer[3] };
                    auto simFunc = [&](const Vec4& qTrailer, Vec4& dq, const double t)
                    {
                        if(!carParams.moveGuidancePoint)
                            dq = OneTrailerKinematicsGPRear(carParams, qTrailer, ackermannDriveData);
                        else
                            dq = OneTrailerKinematicsGPFront(carParams, qTrailer, ackermannDriveData);
                    };

                    Vec4 qOneTrailerPred = qOneTrailerEst;
                    boost::numeric::odeint::integrate(simFunc, qOneTrailerPred, 0.0, 1.0/(Real)mheParams.loopRate, 1.0/(Real)mheParams.loopRate);

                    if(!isPerceptionPoseFresh)
                    {
                        qOneTrailerEst = qOneTrailerPred;
                    }
                    else
                    {
                        ROS_INFO("fresh gps pose");
                        estimateEstTrailer(qOneTrailerEst, qTrailerLoc, qOneTrailerPred, mheParams);
                        //betaEst = estParams.weightBeta*betaPred + (1-estParams.weightBeta)*(beta);
                    }
                    poseWithCovarianceWeightedData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.pose.pose.position.x = qOneTrailerEst[2]  + carParams.L*cos(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.pose.pose.position.y = qOneTrailerEst[3]  + carParams.L*sin(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.header.stamp = ::ros::Time::now();
                    poseWithCovarianceWeightedOut(poseWithCovarianceWeightedData);

                    articulatedAnglesWeightedData.trailer1 = qOneTrailerEst[0];
                    articulatedAnglesWeightedOut(articulatedAnglesWeightedData);

                    perceptionTwistWeightedData.angular.z = qOneTrailerEst[1];
                    perceptionTwistWeightedData.linear.x = qOneTrailerEst[2] + carParams.L*cos(qOneTrailerEst[1]);
                    perceptionTwistWeightedData.linear.y = qOneTrailerEst[3] + carParams.L*sin(qOneTrailerEst[1]);
                    perceptionTwistWeightedOut(perceptionTwistWeightedData);

                }

            }else
            {
                ROS_ERROR(" Trailer Number out of range, accepted values: 0, 1");
            }

        }

        auto loopTime =  ::ros::Time::now() - now;
        ROS_INFO_STREAM("loop time" << loopTime.toSec());

        t += 1/((Real) mheParams.loopRate);
        ::ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}


#include <opencv2/opencv.hpp>
#include <boost/algorithm/string.hpp>

#include "segments.hpp"

using namespace std;
using namespace cv;

bool compareSquaresBySize(Rectangle a, Rectangle b) {
    return a.width * a.height > b.width * b.height;
}

bool compareSquaresById(Rectangle a, Rectangle b) {
    return a.id < b.id;
}

int* horizontalSegments(Mat& src) {
    int* seg = (int*) calloc(src.cols, sizeof(int));

    for(int i = 0; i < src.cols; i++) {
        for(int k = 0; k < src.rows; k++) {
            uchar pixel = src.data[src.cols * k + i];

            if(pixel)
                seg[i]++;
        }
    }

    return seg;
}

Mat drawHorizontalSegments(int* seg, int rows, int cols) {
    Mat segImage = Mat(rows, cols, CV_8U, Scalar(0));

    for(int i = 0; i < cols; i++) {
        if(seg[i] > 0)
            line(segImage, Point(i, rows - 1), Point(i, rows - 1 - seg[i]), Scalar(255), 1);
    }

    return segImage;
}

int* verticalSegments(Mat& src) {
    int* seg = (int*) calloc(src.rows, sizeof(int));

    for(int i = 0; i < src.rows; i++) {
        for(int k = 0; k < src.cols; k++) {
            uchar pixel = src.data[src.cols * i + k];

            if(pixel)
                seg[i]++;
        }
    }

    return seg;
}

Mat drawVerticalSegments(int* seg, int rows, int cols) {
    Mat segImage = Mat(rows, cols, CV_8U, Scalar(0));

    for(int i = 0; i < rows; i++) {
        if(seg[i] > 0)
            line(segImage, Point(0, i), Point(seg[i], i), Scalar(255), 1);
    }

    return segImage;
}

vector<pair<int, int> > createSegmentPairs(int* seg, int segSize) {
    int top = 0, bottom = 0;
    bool in = false;

    vector<pair<int, int> > pairs;

    for(int i = 0; i < segSize; i++) {
        if(seg[i])
            in = true;
        else
            in = false;

        if(in) {
            if(!top) {
                top = i;
                bottom = i;
            } else {
                bottom = i;
            }

            // corner case
            if(i == segSize - 1) {
                pairs.push_back(make_pair(top, bottom));
                top = bottom = 0;
            }
        } else {
            if(top && bottom) {
                pairs.push_back(make_pair(top, bottom));
                top = bottom = 0;
            }
        }
    }

    return pairs;
}

vector<pair<int, int> > filterVerticalPairs(vector<pair<int, int> > verticalPairs) {
    vector<pair<int, int> > new_pairs;

    int biggest = 0, index = 0;

    for(int i = 0; i < verticalPairs.size(); i++) {
        int current = verticalPairs[i].second - verticalPairs[i].first;

        if(current > biggest) {
            index = i;
            biggest = current;
        }
    }

    new_pairs.push_back(make_pair(verticalPairs[index].first, verticalPairs[index].second));

    return new_pairs;
}

vector<pair<int, int> > filterHorizontalPairs(vector<pair<int, int> > horizontalPairs, int segSize) {
    const int MAGIC_DIFF = 10;
    const int MAGIC_SIZE = 14;

    vector<pair<int, int> > new_pairs;

    int* seg = (int*) calloc(segSize, sizeof(int));

    for(int i = 0; i < horizontalPairs.size(); i++) {
        int size = horizontalPairs[i].second - horizontalPairs[i].first;

        // Too small
        if(size < MAGIC_SIZE) {
            // Check right side
            if(i < horizontalPairs.size() - 1 && horizontalPairs.size() > 1) {
                if(horizontalPairs[i + 1].first - horizontalPairs[i].second < MAGIC_DIFF) {
                    for(int k = horizontalPairs[i].first; k <= horizontalPairs[i + 1].second; k++)
                        seg[k] = 1;
                }
            }

            // Check left side
            if(i > 0 && horizontalPairs.size() > 1) {
                if(horizontalPairs[i].first - horizontalPairs[i - 1].second < MAGIC_DIFF) {
                    for(int k = horizontalPairs[i - 1].first; k <= horizontalPairs[i].second; k++)
                        seg[k] = 1;
                }
            }
        // This one is large enough
        } else {
            for(int k = horizontalPairs[i].first; k <= horizontalPairs[i].second; k++)
                seg[k] = 1;
        }
    }

    return createSegmentPairs(seg, segSize);
}

vector<pair<int, int> > splitLarge(vector<pair<int, int> > horizontalSegments) {
    const int MAGIC_SIZE = 50;

    vector<pair<int, int> > new_pairs;

    for(int i = 0; i < horizontalSegments.size(); i++) {
        int size = horizontalSegments[i].second - horizontalSegments[i].first;

        if(size > MAGIC_SIZE) {
            int f1 = horizontalSegments[i].first;
            int s1 = (int)((horizontalSegments[i].second - horizontalSegments[i].first) / 2) + horizontalSegments[i].first;
            int f2 = (int)((horizontalSegments[i].second - horizontalSegments[i].first) / 2) + horizontalSegments[i].first + 2;
            int s2 = horizontalSegments[i].second;

            new_pairs.push_back(make_pair(f1, s1));
            new_pairs.push_back(make_pair(f2, s2));
        } else {
            new_pairs.push_back(horizontalSegments[i]);
        }
    }

    return new_pairs;
}

vector<Rectangle> getRectangles(vector<pair<int, int> > verticalPairs, vector<pair<int, int> > horizontalPairs) {
    vector<Rectangle> squares;
    int id = 0;

    for(vector<pair<int, int> >::iterator itV = verticalPairs.begin(); itV != verticalPairs.end(); itV++) {
        for(vector<pair<int, int> >::iterator itH = horizontalPairs.begin(); itH != horizontalPairs.end(); itH++) {
            Rectangle r = { id++, itH->first, itV->first, itH->second - itH->first, itV->second - itV->first };
            squares.push_back(r);
        }
    }

    return squares;
}

vector<Rectangle> shrinkRectangles(Mat& image, vector<Rectangle> squares) {
    vector<Rectangle> new_squares;

    for(int i = 0; i < squares.size(); i++) {
        int top = -1, bottom = 0, left = 9999, right = -1;
        Mat tmp2 = image(Rect(squares[i].x, squares[i].y, squares[i].width, squares[i].height));
        Mat tmp = tmp2.clone();

        for(int y = 0; y < tmp.rows; y++) {
            for(int x = 0; x < tmp.cols; x++) {
                int pixel = tmp.data[x + y * tmp.cols];

                if(pixel) {
                    tmp.data[x + y * tmp.cols] = 127;
                    if(top == -1) top = y; // store the lowest value
                    if(left > x) left = x;
                    bottom = y; // y will be always incremented, we just store the highest value
                    if(right < x) right = x;
                }
            }
        }

        top -= 1;
        left -= 1;
        bottom += 1;
        right += 1;

        Rectangle r = { squares[i].id, squares[i].x + left, squares[i].y + top, right - left, bottom - top };
        new_squares.push_back(r);
        tmp.release();
        tmp2.release();
    }

    return new_squares;
}

vector<Rectangle> takeRectangles(vector<Rectangle> squares, int number) {
    vector<Rectangle> new_squares;

    sort(squares.begin(), squares.end(), compareSquaresBySize);
    int min = MIN(number, (int)squares.size());

    for(int i = 0; i < min; i++) {
        new_squares.push_back(squares[i]);
    }

    sort(new_squares.begin(), new_squares.end(), compareSquaresById);

    return new_squares;
}

void drawRectangles(Mat& image, vector<Rectangle> squares) {
    for(int i = 0; i < squares.size(); i++)
        rectangle(image, Point(squares[i].x, squares[i].y), Point(squares[i].x + squares[i].width, squares[i].y + squares[i].height), Scalar(255));
}

void saveRectangles(Mat& image, vector<Rectangle> squares, string output_folder, string code, map<string, int>& counter) {
    for(int i = 0; i < squares.size(); i++) {
        Mat tmp = image(Rect(squares[i].x, squares[i].y, squares[i].width, squares[i].height));
        Mat tmp2 = tmp.clone();

        string letter(1, code[i]);

        string sub = letter;
        boost::replace_all(sub, "@", "at");
        boost::replace_all(sub, "%", "pct");
        boost::replace_all(sub, "&", "and");

        string filename = output_folder + sub + "/" + to_string(counter[letter]) + ".png";
        counter[letter]++;

        resize(tmp2, tmp2, Size(32, 48));

        imwrite(filename, tmp2);

        tmp.release();
        tmp2.release();
    }
}

void drawSegmentRectangles(Mat& image, vector<pair<int, int> > verticalPairs, vector<pair<int, int> > horizontalPairs) {
    for(vector<pair<int, int> >::iterator itV = verticalPairs.begin(); itV != verticalPairs.end(); itV++) {
        for(vector<pair<int, int> >::iterator itH = horizontalPairs.begin(); itH != horizontalPairs.end(); itH++) {
            rectangle(image, Point(itH->first, itV->first), Point(itH->second, itV->second), Scalar(255));
        }
    }
}


//==================================================================================================
/*!
  @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_SIMD_FMA_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_FMA_INCLUDED

#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/fma.hpp>

#if defined(BOOST_HW_SIMD_X86)

#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA3_VERSION
// #    include <boost/simd/arch/x86/fma3/simd/function/fma.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA4_VERSION
// #    include <boost/simd/arch/x86/fma4/simd/function/fma.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_XOP_VERSION
// #    include <boost/simd/arch/x86/xop/simd/function/fma.hpp>
#  endif

#endif

#if defined(BOOST_HW_SIMD_PPC)

#  if BOOST_HW_SIMD_PPC >= BOOST_HW_SIMD_PPC_VMX_VERSION
// #    include <boost/simd/arch/power/vmx/simd/function/fma.hpp>
#  endif

#endif

#endif


/*
 *  tutte.cpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#include "chinese_remainder.hpp"
#include "graph_type.hpp"
#include "parse_graph.hpp"
#include "transfer.hpp"
#include "tree_decomposition/heuristics.hpp"
#include "tree_decomposition/tree_decomposition.hpp"
#include "tutte.hpp"
#include "utility/gmp.hpp"
#include "utility/polynomial_two.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm/equal.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>

#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

const char manifesto[] =
  "tutte - computes the Tutte Polynomial\n"
  "Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.\n\n"
  "Distributed under the terms of the Modified BSD License.\n"
  "The full license is in the file COPYING, distributed as part of\n"
  "this software.\n";

/*
 * code to parse user-supplied elimination order
 */

template<class OutputIterator>
void parse_elimination_order(std::string const& s, OutputIterator out)
{
  boost::tokenizer<> tok(s);
  for (auto& c : tok)
    *out++ = boost::lexical_cast<unsigned>(c);
}

/*
 *  validate the ordering by checking if it's a permutation of [0..num_vertices)
 */
template<class Range, class Graph>
bool validate_elimination_order(Range range, Graph const& g)
{
  using namespace boost;
  return range::equal(sort(range), irange((size_t) 0, num_vertices(g)));
}

/*
 *  The algorithm to run, dependent on the weight type
 */

template<typename T>
using algo = tutte<polynomial_two<T>>;

int main (int argc, char *argv[])
{
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "Produce help message")
    ("input-file", po::value<std::string>(), "Read the graph from a file.")
    // tree decomposition options
    ("degree", "Use greedy degree algorithm [default].")
    ("fill-in", "Use greedy fill-in algorithm.")
    ("local-degree", "Use 'local' greedy degree algorithm.")
    ("local-fill-in", "Use 'local' greedy fill-in algorithm.")
    ("elimination-order", po::value<std::string>(), "Specify a vertex elimination order.")
    ("print-tree", "Print tree decomposition.")
    ("tree-only", "Print tree decomposition and exit.")
    // tutte options
    ("flow,f", "Compute the flow polynomial")
    ("chromatic,c", "Compute the chromatic polynomial")
    ("Q,Q", po::value<int32_t>(), "Fix Q value, to be used with v")
    ("v,v", po::value<int32_t>(), "Fix v value, to be used with Q")
    ("chinese-remainder", "Use the chinese remainder trick.")
    ;

  po::variables_map vm;

  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
  } catch (po::error& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cerr << manifesto << "\n" << desc << "\n";
    return 0;
  }

  int check = 0;
  check += vm.count("degree");
  check += vm.count("fill-in");
  check += vm.count("local-degree");
  check += vm.count("local-fill-in");
  check += vm.count("elimination-order");

  if (check > 1) {
    std::cerr <<
      "error: please specify at most one between degree, fill-in,"
      "local-degree, local-fill-in and elimination-order\n";
    return 1;
  }

  graph_type g;
  try {
    std::string s;
    if (vm.count("input-file")) {
      std::string filename = vm["input-file"].as<std::string>();
      std::ifstream input(filename.c_str(), std::ios_base::in);
      if (not input.is_open()) {
        std::cerr << "error: file " << filename << " not found\n";
        return 1;
      }
      input >> s;
    } else {
      std::cin >> s;
    }
    g = parse_graph(s);
  } catch (std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  std::cerr << "Graph with " << num_vertices(g) << " vertices and "
            << num_edges(g) << " edges.\n";

  // check for connectedness
  auto component = boost::make_vector_property_map<int>(
    get(boost::vertex_index, g));
  auto num = connected_components(g, component);
  if (num > 1) {
    std::cerr << "The input graph is not connected. A connected input is required\n";
    return 1;
  }

  std::vector<unsigned int> order(num_vertices(g));

  if (vm.count("fill-in")) {
    heuristics::greedy_fillin_order(g, order.begin());
  } else if (vm.count("local-degree")) {
    heuristics::greedy_local_degree_order(g, order.begin());
  } else if (vm.count("local-fill-in")) {
    heuristics::greedy_local_fillin_order(g, order.begin());
  } else if (vm.count("elimination-order")) {
    // parse the std::string
    std::string s = vm["elimination-order"].as<std::string>();
    parse_elimination_order(s, order.begin());
    bool valid = validate_elimination_order(order, g);
    if (not valid) {
      std::cerr << "error: elimination order not valid\n";
      return 1;
    }
    std::cerr << "Vertex ordering: " << s << "\n";
  } else {
    heuristics::greedy_degree_order(g, order.begin());
  }

  auto td = tree_decomposition::build_tree_decomposition(order, g);

  if (vm.count("print-tree") or vm.count("tree-only")) {
    std::cerr << "Elimination order: ";
    for (auto x : order)
      std::cerr << x << " ";
    std::cerr << "\n";

    std::cerr << "Tree decomposition: " << td << "\n"
              << "Tree decomposition width: "
              << max_bag_size(td) - 1 << "\n";
  }

  if (vm.count("tree-only"))
    return 0;

  if (vm.count("Q") && vm.count("v")) {
    std::cerr << "Running with fixed values of Q and v\n";
    auto Q = vm["Q"].as<int32_t>();
    auto v = vm["v"].as<int32_t>();
    chinese_remainder::chinese_remainder<tutte>(td, Q, v);
  } else {
    auto Q = polynomial_two<int>::Q();
    auto v = polynomial_two<int>::v();

    if (vm.count("flow")) {
      v = -Q;
    } else if (vm.count("chromatic")) {
      v = -1;
    }

    if (vm.count("chinese-remainder")) {
      chinese_remainder::chinese_remainder<algo>(td, Q, v);
    } else {
      using gmp::mpz_int;
      auto result = transfer::transfer(algo<mpz_int>(Q, v), td);
      std::cout << result << "\n";
    }
  }
}


#pragma once

#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/Splines>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <local_planner_types.hpp>
#include <unordered_map>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <turtlebot_msgs/GoalPose.h>
#include <turtlebot_msgs/Trajectory.h>
#include <queue>
#include <stdlib.h>
#include <std_msgs/Bool.h>
#include <tf/transform_listener.h>
#include <urdf/model.h>

typedef std::vector<std::vector<double>> Graph;
typedef Eigen::Spline<double, 2> Spline1d;
typedef Eigen::SplineFitting<Spline1d> Spline1dFitting;

namespace Turtlebot
{

class LocalPlanner
{

public:
    LocalPlanner(ros::NodeHandle &nh, ros::NodeHandle &pnh);
    ~LocalPlanner();

    void planPath();
    bool have_goal = false;

private:
    //callbacks
    void costmapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
    void goalPoseCallback(const turtlebot_msgs::GoalPose::ConstPtr &msg);
    void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);

    //setup methods
    void getParams(ros::NodeHandle &pnh);
    void setupCollision();

    //methods

    //expansion
    void initializePlanner();
    void clearFrontier();
    void openNode(const GraphNode &node);
    void closeNode(const GraphNode &node);
    const bool checkForGoal(const GraphNode &node);
    void expandFrontier(const GraphNode &current_node);
    const std::vector<GraphNode> getNeighbors(const GraphNode &current_node);

    //sampling
    const std::vector<double> calcPossibleVelocities(const GraphNode &current_node) const;
    void calcPossibleYawRates();
    const tf::Transform calcNodeTransform(const Point<double> &pt, const double &heading) const;

    //cost functions
    const double calcG(const Point<double> &pt, const GraphNode &parent_node) const;
    const double calcH(const Point<double> &pt, const Point<double> &cur_pt, const double &heading, const double &velocity, const Spline1d &spline);
    const Spline1d calcSpline(const Point<double> &pt, const double &heading) const;
    const double calcTargetHeading(const Point<double> &pt, const double &velocity, const Spline1d &spline);
    const bool checkVelH(const Point<double> &pt, const double &heading, const double &velocity);
    const double calcDistanceBetween(const Point<double> &lhs, const Point<double> &rhs) const;

    //collision
    const bool checkForCollision(const tf::Transform &transform) const;
    const bool checkPointForCollision(const tf::Point &pt) const;

    //helper functions
    const int &calcGridLocation(const Point<double> &pt) const;

    //output functions
    void reconstructTrajectory();
    const std::vector<GraphNode> reverseTrajectory(const std::vector<GraphNode> &reverse_traj);
    void pubTrajectory(const std::vector<GraphNode> &traj);
    void pubHealth(const bool &health);

    //pubsub
    ros::Subscriber m_costmap_sub;
    ros::Subscriber m_goal_pose_sub;
    ros::Subscriber m_pose_sub;
    ros::Subscriber m_odom_sub;
    ros::Publisher m_path_pub;
    ros::Publisher m_trajectory_pub;
    ros::Publisher m_goal_pub;
    ros::Publisher m_health_pub;

    //members

    //frontier and nodes
    std::priority_queue<GraphNode, std::vector<GraphNode>, GraphNode::CheaperCost> m_frontier;
    std::vector<GraphNode> m_open_nodes;
    std::vector<GraphNode> m_closed_nodes;
    std::unordered_map<int, GraphNode> m_nodes;
    int m_node_id = 0;

    //collision
    std::vector<tf::Point> m_collision_pts;

    //tf listener
    const tf::TransformListener listener;

    //c-space stuff and constricting parameters
    std::vector<double> m_possible_yaw_rates;
    double m_local_costmap_res;
    int m_local_costmap_height;
    int m_local_costmap_width;
    double m_collision_buffer_distance;
    double m_time_step_ms;
    double m_velocity_res;
    double m_yaw_rate_res;
    double m_max_yaw_rate;
    double m_spline_order;
    double m_goal_pos_tolerance;
    double m_goal_heading_tolerance;
    double m_goal_speed_tolerance;
    double m_timeout_ms;
    int m_num_retries;
    double m_radius;
    double m_angular_col_res;
    double m_radial_col_res;

    //callback refs
    nav_msgs::OccupancyGrid::ConstPtr m_local_costmap;
    turtlebot_msgs::GoalPose m_goal_pose;
    nav_msgs::OccupancyGrid::ConstPtr m_map;
    geometry_msgs::Twist m_turtlebot_velocity;
    geometry_msgs::PoseWithCovarianceStamped::ConstPtr m_pose;

    //flags
    bool m_have_costmap = false;
    bool m_have_pose = false;
    bool m_have_odom = false;
    int m_retry_it = 0;
};

}


// Boost.Units

#include <iostream>

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/io.hpp>

int main()
{
    boost::units::quantity<boost::units::si::mass> w = 20.6 * boost::units::si::kilograms + 10.2 * boost::units::si::kilograms;
    std::cout << w << std::endl;
}


#ifndef ASLAM_BACKEND_MATRIX_BASIC_HPP
#define ASLAM_BACKEND_MATRIX_BASIC_HPP

#include <Eigen/Core>
#include <aslam/backend/DesignVariable.hpp>
#include "MatrixExpression.hpp"
#include "MatrixExpressionNode.hpp"

namespace aslam {
namespace backend {

class MatrixBasic : public MatrixExpressionNode, public DesignVariable {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  MatrixBasic(const Eigen::Matrix3d & A);
  MatrixBasic(const Eigen::Matrix3d & A, bool add_dv);
  MatrixBasic(const Eigen::Matrix3d & A, const Eigen::Matrix3i & UpdatePattern);

  ~MatrixBasic() override;

  /// \brief Revert the last state update.
  void revertUpdateImplementation() override;

  /// \brief Update the design variable.
  void updateImplementation(const double * dp, int size) override;

  /// \brief the size of an update step
  int minimalDimensionsImplementation() const override;

  MatrixExpression toExpression();
 private:
  Eigen::Matrix3d evaluateImplementation() const override;
  void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const override;
  void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

  /// Returns the content of the design variable
  void getParametersImplementation(Eigen::MatrixXd& value) const override;

  /// Sets the content of the design variable
  void setParametersImplementation(const Eigen::MatrixXd& value) override;

  Eigen::Matrix3d _A;
  Eigen::Matrix3d _p_A;
  Eigen::Matrix<double, 9, Eigen::Dynamic> _B;  //9xN (N<=9) update matrix derived from updatePattern
};

}  // namespace backend
}  // namespace aslam

#endif /* ASLAM_BACKEND_MATRIX_BASIC_HPP */


#include <tuple>
#include <Eigen/Core>
#include <npe.h>

npe_function(intlonglong)
npe_arg(b, dense_int, dense_longlong)
npe_arg(a, dense_int, dense_longlong)
npe_doc(R"(Add two matrices of the same type)")
npe_begin_code()

    npe_Matrix_a ret1 = a;

    return npe::move(ret1);

npe_end_code()

npe_function(intlong)
npe_arg(b, dense_int, dense_long)
npe_arg(a, dense_int, dense_long)
npe_doc(R"(Add two matrices of the same type)")
npe_begin_code()

    npe_Matrix_a ret1 = a;

    return npe::move(ret1);

npe_end_code()

npe_function(longlonglong)
npe_arg(b, dense_longlong, dense_long)
npe_arg(a, dense_longlong, dense_long)
npe_doc(R"(Add two matrices of the same type)")
npe_begin_code()

    npe_Matrix_a ret1 = a;

    return npe::move(ret1);

npe_end_code()