/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "method.h"

#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/uuid/uuid.hpp>
#include <boost/variant/static_visitor.hpp>

#include "db/ast-inserter.h"
#include "db/query.h"
#include "method/equation-grammar.h"
#include "method/equation-lexer.h"
#include "method/helper.h"
#include "method/parser.h"
#include "method/printer.h"

namespace flint {
namespace method {

namespace {

class VariantPrinter : public boost::static_visitor<> {
public:
	explicit VariantPrinter(std::ostream *os)
		: os_(os)
	{}

	void operator()(const Compound &c) const {
		os_->put('(');
		*os_ << c.keyword;
		for (const auto &child : c.children) {
			os_->put(' ');
			boost::apply_visitor(*this, child);
		}
		os_->put(')');
	}

	void operator()(const std::string &s) const {
		if (s[0] != '%') {
			*os_ << s;
			return;
		}
		if (s == "%time") {
			*os_ << "(plus %time @dt)";
			return;
		}
		*os_ << s << "#0";
	}

	void operator()(int i) const {
		*os_ << i;
	}

	void operator()(const flint::lexer::Rational &r) const {
		*os_ << r.lexeme;
	}

	void operator()(const flint::lexer::Real &r) const {
		*os_ << r.lexeme;
	}

private:
	std::ostream *os_;
};

bool IsDifferential(const Expr &e, std::string &id)
{
	if (e.which() != kExprIsCompound)
		return false;
	const auto &c = boost::get<Compound>(e);
	if (c.keyword != "$differential")
		return false;
	if (c.children.size() != 1)
		return false;
	const auto &x = c.children.at(0);
	if (x.which() != kExprIsString)
		return false;
	id = boost::get<std::string>(x);
	return true;
}

bool IsMinus1(const Expr &e, Expr &arg)
{
	if (e.which() != kExprIsCompound)
		return false;
	const auto &c = boost::get<Compound>(e);
	if (c.keyword != "minus")
		return false;
	if (c.children.size() == 1) {
		arg = c.children[0];
		return true;
	}
	return false;
}

bool IsMinus2(const Expr &e, Expr &arg0, Expr &arg1)
{
	if (e.which() != kExprIsCompound)
		return false;
	const auto &c = boost::get<Compound>(e);
	if (c.keyword != "minus")
		return false;
	if (c.children.size() == 2) {
		arg0 = c.children[0];
		arg1 = c.children[1];
		return true;
	}
	return false;
}

Expr ChangeSign(const Expr &e, double sign)
{
	if (sign > 0)
		return e;
	Expr r0, r1;
	if (IsMinus1(e, r0))
		return r0;
	Compound m;
	m.keyword = "minus";
	if (IsMinus2(e, r0, r1)) { // swap the 1st argument with the 2nd
		m.children.push_back(r1);
		m.children.push_back(r0);
		return m;
	}
	m.children.push_back(e);
	return m;
}

bool DecomposeSdeRhsTerm(const Expr &term, std::string &id, Expr &factor, double sign)
{
	if (IsDifferential(term, id)) {
		factor = sign * 1.0;
		return true;
	}
	if (term.which() != kExprIsCompound)
		return false;
	const auto &c = boost::get<Compound>(term);
	if (c.keyword != "times")
		return false;
	if (IsDifferential(c.children.back(), id)) {
		if (c.children.size() == 2) {
			factor = ChangeSign(c.children.front(), sign);
			return true;
		}
		Compound p;
		p.keyword = "times";
		// copy children except the last one
		for (size_t i=0;i<c.children.size()-1;i++)
			p.children.push_back(c.children[i]);
		factor = ChangeSign(p, sign);
		return true;
	}
	if (IsDifferential(c.children.front(), id)) {
		if (c.children.size() == 2) {
			factor = ChangeSign(c.children.back(), sign);
			return true;
		}
		Compound p;
		p.keyword = "times";
		// copy children except the first one
		for (size_t i=1;i<c.children.size();i++)
			p.children.push_back(c.children[i]);
		factor = ChangeSign(p, sign);
		return true;
	}
	return false;
}

bool IsVoidDrift(Expr &drift)
{
	return drift.which() == kExprIsInteger && boost::get<int>(drift) == 0;
}

/*
 * precondition: drift = 0
 */
bool DecomposeSdeRhs(const Expr &rhs, Expr &drift, std::unordered_map<std::string, Expr> &diffusion,
					 double sign = 1.0)
{
	assert(rhs.which() == kExprIsCompound);
	std::string id;
	Expr factor;
	if (DecomposeSdeRhsTerm(rhs, id, factor, sign)) {
		if (id == "%time") {
			if (IsVoidDrift(drift)) {
				drift = factor;
				return true;
			} else {
				std::cerr << "more than one drift terms" << std::endl;
				return false;
			}
		} else if (diffusion.emplace(id, factor).second) {
			return true;
		} else {
			std::cerr << "more than one diffusion terms are unsupported: " << id << std::endl;
			return false;
		}
	}
	const auto &c = boost::get<Compound>(rhs);
	if (c.keyword == "plus") {
		for (const auto &e : c.children) {
			Expr d = 0;
			if (!DecomposeSdeRhs(e, d, diffusion, sign))
				return false;
			// check if there are more than one drift terms
			if (!IsVoidDrift(d)) {
				if (IsVoidDrift(drift)) {
					drift = d;
				} else {
					std::cerr << "more than one drift terms" << std::endl;
					return false;
				}
			}
		}
		return true;
	}
	if (c.keyword == "minus") {
		if (c.children.size() == 1) {
			Expr d = 0;
			if (!DecomposeSdeRhs(c.children[0], d, diffusion, -sign))
				return false;
			// check if there are more than one drift terms
			if (!IsVoidDrift(d)) {
				if (IsVoidDrift(drift)) {
					drift = d;
				} else {
					std::cerr << "more than one drift terms" << std::endl;
					return false;
				}
			}
		} else {
			assert(c.children.size() == 2);
			Expr d0 = 0;
			if (!DecomposeSdeRhs(c.children[0], d0, diffusion, sign))
				return false;
			// check if there are more than one drift terms
			if (!IsVoidDrift(d0)) {
				if (IsVoidDrift(drift)) {
					drift = d0;
				} else {
					std::cerr << "more than one drift terms" << std::endl;
					return false;
				}
			}
			Expr d1 = 0;
			if (!DecomposeSdeRhs(c.children[1], d1, diffusion, -sign))
				return false;
			// check if there are more than one drift terms
			if (!IsVoidDrift(d1)) {
				if (IsVoidDrift(drift)) {
					drift = d1;
				} else {
					std::cerr << "more than one drift terms" << std::endl;
					return false;
				}
			}
		}
		return true;
	}
	std::cerr << "either plus or minus expected, but got: " << c.keyword << std::endl;
	return false;
}

class Inserter : db::AstInserter {
public:
	explicit Inserter(sqlite3 *db)
		: db::AstInserter(db)
	{
	}

	bool PrintAndInsert(const boost::uuids::uuid &uuid,
						const Expr &lhs,
						const Expr &rhs)
	{
		std::ostringstream oss;
		if (lhs.which() == kExprIsString) { // LHS: x = ...
			const std::string &id(boost::get<std::string>(lhs));
			oss << id << "#0";
			std::string name = oss.str();
			oss.str("");
			boost::apply_visitor(VariantPrinter(&oss), rhs);
			std::string math = oss.str();
			return Insert(uuid, name.c_str(), math.c_str());
		}
		std::string id;
		if (IsDifferential(lhs, id)) { // LHS: dx = ...
			Expr drift = 0;
			std::unordered_map<std::string, Expr> diffusion;
			if (!DecomposeSdeRhs(rhs, drift, diffusion))
				return false;
			oss << id << "#0";
			std::string name = oss.str();
			oss.str("");
			for (const auto &d : diffusion) {
				(void)d;
				oss << "(plus ";
			}
			if (IsVoidDrift(drift)) {
				oss << id;
			} else {
				oss << "(plus " << id << " (times ";
				boost::apply_visitor(Printer(&oss), drift);
				oss << " @dt))";
			}
			for (const auto &d : diffusion) {
				oss << " (times ";
				boost::apply_visitor(Printer(&oss), d.second);
				oss << " " << d.first << "#1))";
			}
			std::string math = oss.str();
			return Insert(uuid, name.c_str(), math.c_str());
		}
		// LHS: dx/dt = ...
		assert(lhs.which() == kExprIsCompound);
		const Compound &c(boost::get<Compound>(lhs));
		assert(c.children.size() == 2);
		const Expr &e(c.children.at(1));
		assert(e.which() == kExprIsString);
		id = boost::get<std::string>(e);
		oss << id << "#0";
		std::string name = oss.str();
		oss.str("");
		oss << "(plus " << id << " (times @dt ";
		boost::apply_visitor(Printer(&oss), rhs);
		oss << "))";
		std::string math = oss.str();
		return Insert(uuid, name.c_str(), math.c_str());
	}

	bool PrintAndInsertWiener(const boost::uuids::uuid &uuid,
							  const Expr &e)
	{
		std::ostringstream oss;
		assert(e.which() == kExprIsString);
		const std::string &id(boost::get<std::string>(e));
		oss << id << "#1";
		std::string name = oss.str();
		if (!Insert(uuid, name.c_str(), "($gauss_variate 0 (root @dt))"))
			return false;
		oss.str("");
		oss << id << "#0";
		name = oss.str();
		oss.str("");
		oss << "(plus " << id << " " << id << "#1)";
		std::string math = oss.str();
		return Insert(uuid, name.c_str(), math.c_str());
	}
};

}

bool EulerMaruyama(sqlite3 *db, const char *input, sqlite3 *output)
{
	return Parse<2, EquationLexer, EquationGrammar, Inserter>(db, input, output);
}

}
}


#include "config.h"
#include "common.hpp"
#include "post_process/adminizer.hpp"
#include "logging/logger.hpp"

#include <boost/property_tree/ptree.hpp>

#include <mapnik/datasource_cache.hpp>
#include <mapnik/wkt/wkt_factory.hpp>
#include <mapnik/wkt/wkt_grammar.hpp>
#include <mapnik/wkt/wkt_grammar_impl.hpp>

#include <iostream>

using std::make_pair;
namespace pp = avecado::post_process;

namespace {

pp::izer_ptr mk_10x10_poly_izer() {
  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("datasource.type", "csv");
  // Square going to +/- 10
  conf.put("datasource.inline",
           "wkt|foo\n"
           "Polygon((-10.0 -10.0, -10.0 10.0, 10.0 10.0, 10.0 -10.0, -10.0 -10.0))|foo_value\n");
  return pp::create_adminizer(conf);
}

mapnik::feature_ptr mk_feat_wkt(const std::string &wkt) {
  mapnik::context_ptr ctx = std::make_shared<mapnik::context_type>();
  mapnik::feature_ptr feat = std::make_shared<mapnik::feature_impl>(ctx, 0);
  if (!mapnik::from_wkt(wkt, feat->paths())) {
    throw std::runtime_error((boost::format("Unable to parse WKT geometry from "
                                            "string \"%1%\"") % wkt).str());
  }
  return feat;
}

void assert_line_geom_equal(mapnik::feature_ptr &feat,
                            const std::initializer_list<double> &coords) {
  using mapnik::geometry_type;

  test::assert_equal<size_t>(feat->num_geometries(), 1,
                             "Feature should only have one geometry");
  const geometry_type &geom = feat->get_geometry(0);
  test::assert_equal<geometry_type::types>(geom.type(), geometry_type::LineString,
                                           "geometry should be LineString");
  test::assert_equal<size_t>(geom.size(), coords.size() / 2,
                             "should have the expected number of coordinates");

  auto itr = coords.begin();
  auto end = coords.end();
  auto expect_cmd = mapnik::SEG_MOVETO;
  mapnik::vertex_adapter path(geom);
  path.rewind(0);

  double expect_x = -1, expect_y = -1, actual_x = -1, actual_y = -1;

  while (itr != end) {
    expect_x = *itr++;
    if (itr == end) { break; }
    expect_y = *itr++;

    auto actual_cmd = path.vertex(&actual_x, &actual_y);

    test::assert_equal<unsigned int>(actual_cmd, expect_cmd, "command");
    test::assert_equal<double>(actual_x, expect_x, "x");
    test::assert_equal<double>(actual_y, expect_y, "y");

    expect_cmd = mapnik::SEG_LINETO;
  }
}

void assert_points_geom_equal(mapnik::feature_ptr &feat,
                              const std::initializer_list<std::pair<double, double> > &coords) {
  using mapnik::geometry_type;

  test::assert_equal<size_t>(feat->num_geometries(), 1,
                             "Feature should only have one geometry");
  const geometry_type &geom = feat->get_geometry(0);
  test::assert_equal<geometry_type::types>(geom.type(), geometry_type::Point,
                                           "geometry should be Point");
  test::assert_equal<size_t>(geom.size(), coords.size(),
                             "should have the expected number of coordinates");

  auto itr = coords.begin();
  auto end = coords.end();
  mapnik::vertex_adapter path(geom);
  path.rewind(0);

  double expect_x = -1, expect_y = -1, actual_x = -1, actual_y = -1;

  while (itr != end) {
    boost::tie(expect_x, expect_y) = *itr++;

    unsigned int actual_cmd = path.vertex(&actual_x, &actual_y);

    test::assert_equal<unsigned int>(actual_cmd, mapnik::SEG_MOVETO, "command");
    test::assert_equal<double>(actual_x, expect_x, "x");
    test::assert_equal<double>(actual_y, expect_y, "y");
  }
}

void assert_wkt_geom_equal(mapnik::feature_ptr &feat, const std::string &wkt) {
  using mapnik::geometry_type;
  using mapnik::geometry_container;

  geometry_container expected_paths;
  test::assert_equal<bool>(mapnik::from_wkt(wkt, expected_paths), true, "valid WKT");

  const geometry_container &actual_paths = feat->paths();

  const size_t num_paths = expected_paths.size();
  test::assert_equal<size_t>(actual_paths.size(), num_paths,
                             "same number of paths in geometry");

  // NOTE: this doesn't take account of potential re-ordering of the paths
  // within the geometry container, which would be valid. it's not clear
  // whether anything which processes the geometry would re-order them, so
  // until the test has a spurious failure because of re-ordering, let's
  // just assume the ordering remains the same.
  for (size_t i = 0; i < num_paths; ++i) {
    const geometry_type &actual_geom = actual_paths[i];
    const geometry_type &expected_geom = expected_paths[i];
    mapnik::vertex_adapter actual(actual_geom), expected(expected_geom);

    test::assert_equal<geometry_type::types>(actual.type(), expected.type(),
                                             "geometry types");
    test::assert_equal<size_t>(actual.size(), expected.size(),
                               "number of coordinates");

    // NOTE: this doesn't account for rotation of any polygon rings,
    // which means the test might give a false negative. however, until
    // we have an example of that, let's just assume it doesn't happen.
    actual.rewind(0);
    expected.rewind(0);
    while (true) {
      double actual_x = 0, actual_y = 0, expected_x = 0, expected_y = 0;
      unsigned int actual_cmd = actual.vertex(&actual_x, &actual_y);
      unsigned int expected_cmd = expected.vertex(&expected_x, &expected_y);

      test::assert_equal<unsigned int>(actual_cmd, expected_cmd, "command");
      if (actual_cmd == mapnik::SEG_END) {
        break;
      }

      test::assert_equal<double>(actual_x, expected_x, "x");
      test::assert_equal<double>(actual_y, expected_y, "y");
    }
  }
}

void assert_has_new_param(const mapnik::feature_ptr &feat) {
  test::assert_equal<bool>(feat->has_key("foo"), true,
                           "feature should have parameter key \"foo\" after adminizing");
  test::assert_equal<std::string>(feat->get("foo").to_string(), "foo_value",
                                  "feature should have parameter from adminizing polygon");
}

void assert_izer_include(const std::string &wkt) {
  pp::izer_ptr izer = mk_10x10_poly_izer();
  std::vector<mapnik::feature_ptr> features;
  features.push_back(mk_feat_wkt(wkt));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  test::assert_equal<size_t>(features.size(), 1, "should be only one feature");

  // being adminized should have added (or overwritten) the 'foo'
  // parameter from the admin polygon.
  assert_has_new_param(features[0]);

  // being adminized shouldn't have affected this geometry because it's
  // entirely within the admin polygon
  assert_wkt_geom_equal(features[0], wkt);
}

void assert_izer_exclude(const std::string &wkt) {
  pp::izer_ptr izer = mk_10x10_poly_izer();
  std::vector<mapnik::feature_ptr> features;
  features.push_back(mk_feat_wkt(wkt));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  test::assert_equal<size_t>(features.size(), 1, "should be only one feature");

  // since the geometry is outside the admin polygon, no parameter
  // should have been written.
  test::assert_equal<bool>(features[0]->has_key("foo"), false,
                           "feature should not have been affected by adminizer.");

  // being adminized shouldn't have affected this geometry because it's
  // entirely outside the admin polygon
  assert_wkt_geom_equal(features[0], wkt);
}

// test simple assignment of an object's parameter by inclusion
// in an area.
void test_point_simple_inclusion_param() {
  assert_izer_include("POINT(0 0)");
}

// test that an object outside of the polygon is not modified.
void test_point_simple_exclusion_param() {
  assert_izer_exclude("POINT(11 11)");
}

void test_multipoint_simple_inclusion_param() {
  assert_izer_include("MULTIPOINT((0 0))");
  assert_izer_include("MULTIPOINT((0 0),(1 1))");
}

// test that an object outside of the polygon is not modified.
void test_multipoint_simple_exclusion_param() {
  assert_izer_exclude("MULTIPOINT((11 11))");
  assert_izer_exclude("MULTIPOINT((11 11), (12 12))");
}

void test_line_simple_inclusion_param() {
  assert_izer_include("LINESTRING(0 0, 1 1, 2 0, 3 1, 4 0)");
}

void test_line_simple_exclusion_param() {
  assert_izer_exclude("LINESTRING(0 11, 11 11, 11 -11, 0 -11)");
}

void test_poly_simple_inclusion_param() {
  assert_izer_include("POLYGON((0 0, 1 0, 1 1, 0 1, 0 0))");
}

void test_poly_simple_exclusion_param() {
  assert_izer_exclude("POLYGON((20 0, 21 0, 21 1, 20 1, 20 0))");
}

void test_multipoly_simple_inclusion_param() {
  assert_izer_include("MULTIPOLYGON(((0 0, 1 0, 1 1, 0 1, 0 0)))");
  assert_izer_include("MULTIPOLYGON(((0 0, 1 0, 1 1, 0 1, 0 0)),((5 5, 6 5, 6 6, 5 6, 5 5)))");
}

void test_multipoly_simple_exclusion_param() {
  assert_izer_exclude("MULTIPOLYGON(((20 0, 21 0, 21 1, 20 1, 20 0)))");
  assert_izer_exclude("MULTIPOLYGON(((20 0, 21 0, 21 1, 20 1, 20 0)),((-20 0, -21 0, -21 1, -20 1, -20 0)))");
}

std::string intersection_param(pp::izer_ptr &izer, const std::string &wkt) {
  std::vector<mapnik::feature_ptr> features;
  features.push_back(mk_feat_wkt(wkt));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  test::assert_equal<size_t>(features.size(), 1, "should be only one feature");
  mapnik::feature_ptr feat = features[0];

  test::assert_equal<bool>(feat->has_key("foo"), true,
                           "feature should have parameter key \"foo\" after adminizing");

  return feat->get("foo").to_string();
}

void test_intersection_mode_first() {
  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 3 0, 3 3, 0 3, 0 0))|first_value\n"
           "POLYGON((1 1, 4 1, 4 4, 1 4, 1 1))|second_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::string adminized_param = intersection_param(izer, "POINT(2 2)");

  test::assert_equal<std::string>(adminized_param, "first_value",
                                  "when intersection mode is first, should have the "
                                  "first admin polygon's parameter");
}

void test_intersection_mode_collect() {
  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("collect", "true");
  conf.put("delimiter", "|");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 3 0, 3 3, 0 3, 0 0))|first_value\n"
           "POLYGON((1 1, 4 1, 4 4, 1 4, 1 1))|second_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::string adminized_param = intersection_param(izer, "POINT(2 2)");

  test::assert_equal<std::string>(adminized_param, "first_value|second_value",
                                  "when intersection mode is collect, should have "
                                  "all the admin polygons' parameters");
}

void test_intersection_mode_split() {
  using mapnik::feature_ptr;
  using mapnik::geometry_type;

  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("split", "true");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 3 0, 3 3, 0 3, 0 0))|first_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::vector<feature_ptr> features;
  features.push_back(mk_feat_wkt("LINESTRING(-1 2, 5 2)"));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  int segments[] = {-1, -1, -1};
  int num_segments = 0;

  for (size_t i = 0; i < features.size(); ++i) {
    feature_ptr feat = features[i];

    const size_t num_geoms = feat->num_geometries();
    for (size_t j = 0; j < num_geoms; ++j) {
      const geometry_type &geom = feat->get_geometry(j);
      test::assert_equal<geometry_type::types>(geom.type(), geometry_type::LineString,
                                               "geometry should be LineString");

      mapnik::vertex_adapter path(geom);
      path.rewind(0);
      double x = 0, y = 0;
      unsigned int cmd = 0;

      while ((cmd = path.vertex(&x, &y)) != mapnik::SEG_END) {
        // we're expecting segments running L->R starting at x = -1, 0 & 3
        if (cmd == mapnik::SEG_MOVETO) {
          if (std::abs(x + 1.0) < 1.0e-6) {
            segments[0] = i;

          } else if (std::abs(x) < 1.0e-6) {
            segments[1] = i;

          } else if (std::abs(x - 3.0) < 1.0e-6) {
            segments[2] = i;

          } else {
            throw std::runtime_error((boost::format("Unexpected x=%1% coordinate") % x).str());
          }

          ++num_segments;
        }
      }
    }
  }

  test::assert_equal<int>(num_segments, 3, "should be 3 segments");

  for (int i = 0; i < 3; ++i) {
    if (segments[i] == -1) {
      throw std::runtime_error((boost::format("Segment %1% not found") % i).str());
    }
  }

  // segments outside of the admin polygon should not be set
  test::assert_equal<bool>(features[segments[0]]->has_key("foo"), false,
                           "segment 0 outside of hit area should not have \"foo\" set");
  test::assert_equal<bool>(features[segments[2]]->has_key("foo"), false,
                           "segment 3 outside of hit area should not have \"foo\" set");

  // the segments inside of the admin polygons should have been adminized
  test::assert_equal<bool>(features[segments[1]]->has_key("foo"), true,
                           "segment 1 inside of hit area should have \"foo\" set");
  test::assert_equal<std::string>(features[segments[1]]->get("foo").to_string(),
                                  "first_value", "segment 1 should be adminized");
}

void test_intersection_mode_split_first() {
  using mapnik::feature_ptr;
  using mapnik::geometry_type;

  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("split", "true");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 3 0, 3 3, 0 3, 0 0))|first_value\n"
           "POLYGON((1 1, 4 1, 4 4, 1 4, 1 1))|second_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::vector<feature_ptr> features;
  features.push_back(mk_feat_wkt("LINESTRING(-1 2, 5 2)"));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  int segments[] = {-1, -1, -1, -1};
  int num_segments = 0;

  for (size_t i = 0; i < features.size(); ++i) {
    feature_ptr feat = features[i];

    const size_t num_geoms = feat->num_geometries();
    for (size_t j = 0; j < num_geoms; ++j) {
      const geometry_type &geom = feat->get_geometry(j);
      test::assert_equal<geometry_type::types>(geom.type(), geometry_type::LineString,
                                               "geometry should be LineString");

      mapnik::vertex_adapter path(geom);
      path.rewind(0);
      double x = 0, y = 0;
      unsigned int cmd = 0;

      while ((cmd = path.vertex(&x, &y)) != mapnik::SEG_END) {
        // we're expecting segments running L->R starting at x = -1, 0, 3 & 4
        if (cmd == mapnik::SEG_MOVETO) {
          if (std::abs(x + 1.0) < 1.0e-6) {
            segments[0] = i;

          } else if (std::abs(x) < 1.0e-6) {
            segments[1] = i;

          } else if (std::abs(x - 3.0) < 1.0e-6) {
            segments[2] = i;

          } else if (std::abs(x - 4.0) < 1.0e-6) {
            segments[3] = i;

          } else {
            throw std::runtime_error((boost::format("Unexpected x=%1% coordinate") % x).str());
          }

          ++num_segments;
        }
      }
    }
  }

  test::assert_equal<int>(num_segments, 4, "should be 4 segments");

  for (int i = 0; i < 4; ++i) {
    if (segments[i] == -1) {
      throw std::runtime_error((boost::format("Segment %1% not found") % i).str());
    }
  }

  // the segments outside of the admin polygons should not be affected
  test::assert_equal<bool>(features[segments[0]]->has_key("foo"), false,
                           "segment 0 outside of hit area should not have \"foo\" set");
  test::assert_equal<bool>(features[segments[3]]->has_key("foo"), false,
                           "segment 3 outside of hit area should not have \"foo\" set");

  // the segments inside of the admin polygons should have been adminized
  test::assert_equal<bool>(features[segments[1]]->has_key("foo"), true,
                           "segment 1 inside of hit area should have \"foo\" set");
  test::assert_equal<bool>(features[segments[2]]->has_key("foo"), true,
                           "segment 2 inside of hit area should have \"foo\" set");

  // and they should be set to the right values...
  test::assert_equal<std::string>(features[segments[1]]->get("foo").to_string(),
                                  "first_value", "segment 1 should have first value");
  test::assert_equal<std::string>(features[segments[2]]->get("foo").to_string(),
                                  "second_value", "segment 2 should have second value");
}

void test_intersection_mode_split_collect() {
  using mapnik::feature_ptr;
  using mapnik::geometry_type;

  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("split", "true");
  conf.put("collect", "true");
  conf.put("delimiter", "|");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 3 0, 3 3, 0 3, 0 0))|first_value\n"
           "POLYGON((1 1, 4 1, 4 4, 1 4, 1 1))|second_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::vector<feature_ptr> features;
  features.push_back(mk_feat_wkt("LINESTRING(-1 2, 5 2)"));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  int segments[] = {-1, -1, -1, -1, -1};
  int num_segments = 0;

  for (size_t i = 0; i < features.size(); ++i) {
    feature_ptr feat = features[i];

    const size_t num_geoms = feat->num_geometries();
    for (size_t j = 0; j < num_geoms; ++j) {
      const geometry_type &geom = feat->get_geometry(j);
      test::assert_equal<geometry_type::types>(geom.type(), geometry_type::LineString,
                                               "geometry should be LineString");

      mapnik::vertex_adapter path(geom);
      path.rewind(0);
      double x = 0, y = 0;
      unsigned int cmd = 0;

      while ((cmd = path.vertex(&x, &y)) != mapnik::SEG_END) {
        // we're expecting segments running L->R starting at x = -1, 0, 1, 3 & 4
        if (cmd == mapnik::SEG_MOVETO) {
          if (std::abs(x + 1.0) < 1.0e-6) {
            segments[0] = i;

          } else if (std::abs(x) < 1.0e-6) {
            segments[1] = i;

          } else if (std::abs(x - 1.0) < 1.0e-6) {
            segments[2] = i;

          } else if (std::abs(x - 3.0) < 1.0e-6) {
            segments[3] = i;

          } else if (std::abs(x - 4.0) < 1.0e-6) {
            segments[4] = i;

          } else {
            throw std::runtime_error((boost::format("Unexpected x=%1% coordinate") % x).str());
          }

          ++num_segments;
        }
      }
    }
  }

  test::assert_equal<int>(num_segments, 5, "should be 5 segments");

  for (int i = 0; i < 5; ++i) {
    if (segments[i] == -1) {
      throw std::runtime_error((boost::format("Segment %1% not found") % i).str());
    }
  }

  // the segments outside of the admin polygons should not be affected
  test::assert_equal<bool>(features[segments[0]]->has_key("foo"), false,
                           "segment 0 outside of hit area should not have \"foo\" set");
  test::assert_equal<bool>(features[segments[4]]->has_key("foo"), false,
                           "segment 3 outside of hit area should not have \"foo\" set");

  // the segments inside of the admin polygons should have been adminized
  test::assert_equal<bool>(features[segments[1]]->has_key("foo"), true,
                           "segment 1 inside of hit area should have \"foo\" set");
  test::assert_equal<bool>(features[segments[2]]->has_key("foo"), true,
                           "segment 2 inside of hit area should have \"foo\" set");
  test::assert_equal<bool>(features[segments[3]]->has_key("foo"), true,
                           "segment 3 inside of hit area should have \"foo\" set");

  // and they should be set to the right values...
  test::assert_equal<std::string>(features[segments[1]]->get("foo").to_string(),
                                  "first_value", "segment 1 should have first value");
  test::assert_equal<std::string>(features[segments[2]]->get("foo").to_string(),
                                  "first_value|second_value",
                                  "segment 2 should have both values");
  test::assert_equal<std::string>(features[segments[3]]->get("foo").to_string(),
                                  "second_value", "segment 3 should have second value");
}

// test that a polygon with a hole in it, but still intersecting the
// adminizer box gets included.
void test_poly_inner_inclusion_param() {
  assert_izer_include(
    "POLYGON("
    "  (-10 -20, 30 -20, 30 20, -10 20, -10 -20),"
    "  ( -1 -11, 21 -11, 21 11,  -1 11,  -1 -11)"
    ")");
}

// test that a polygon with a hole in it, where the hole means the
// adminizer polygon is completely outside the polygon, is not
// included.
void test_poly_inner_exclusion_param() {
  /** NOTE: this currently does not work due to a bug upstream which
   ** is being worked on: https://github.com/boostorg/geometry/pull/159
   ** and, until we pull in a fixed version of boost::geometry, this
   ** test will fail. **/
  // assert_izer_exclude(
  //   "POLYGON("
  //   "  (-20 -20, 20 -20, 20 20, -20 20, -20 -20),"
  //   "  (-11 -11, 11 -11, 11 11, -11 11, -11 -11)"
  //   ")");
}

// test that when the admin areas are disjoint, but still cover the
// input features, that they still get matched.
void test_disjoint_admin_polygons() {
  using mapnik::feature_ptr;
  using mapnik::geometry_type;

  pt::ptree conf;
  conf.put("param_name", "foo");
  conf.put("split", "true");
  conf.put("collect", "true");
  conf.put("delimiter", "|");
  conf.put("datasource.type", "csv");
  conf.put("datasource.inline",
           "wkt|foo\n"
           "POLYGON((0 0, 2 0, 2 2, 0 2, 0 0))|first_value\n"
           "POLYGON((7 7, 9 7, 9 9, 7 9, 7 7))|second_value\n");
  pp::izer_ptr izer = pp::create_adminizer(conf);

  std::vector<feature_ptr> features;
  features.push_back(mk_feat_wkt("POINT(1 1)"));
  features.push_back(mk_feat_wkt("POINT(8 8)"));

  izer->process(features, test::make_map("test/empty_map_file.xml", 256, 0, 0, 0));

  test::assert_equal<size_t>(features.size(), 2, "should still be 2 features");
  for (size_t i = 0; i < 2; ++i) {
    test::assert_equal<size_t>(features[i]->num_geometries(), 1,
                               "feature should just have one point");
    const geometry_type &geom = features[i]->get_geometry(0);
    test::assert_equal<geometry_type::types>(geom.type(), geometry_type::Point,
                                             "geometry should still be a point");
  }

  // the points should now have the parameter set
  test::assert_equal<bool>(features[0]->has_key("foo"), true,
                           "features[0] should have \"foo\" set");
  test::assert_equal<bool>(features[1]->has_key("foo"), true,
                           "features[1] should have \"foo\" set");

  // and they should be set to the right values...
  test::assert_equal<std::string>(features[0]->get("foo").to_string(),
                                  "first_value",
                                  "first feature should have first value");
  test::assert_equal<std::string>(features[1]->get("foo").to_string(),
                                  "second_value",
                                  "second feature should have second value");
}

} // anonymous namespace

int main() {
  int tests_failed = 0;

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

  // set up mapnik's datasources, as we'll be using them in the
  // tests.
  mapnik::datasource_cache::instance().register_datasources(MAPNIK_DEFAULT_INPUT_PLUGIN_DIR);

#define RUN_TEST(x) { tests_failed += test::run(#x, &(x)); }
  RUN_TEST(test_point_simple_inclusion_param);
  RUN_TEST(test_point_simple_exclusion_param);

  // mapbox/mapnik-vector-tile#62 prevents multipoints from working properly through the entire toolchain
  RUN_TEST(test_multipoint_simple_inclusion_param);
  RUN_TEST(test_multipoint_simple_exclusion_param);

  RUN_TEST(test_line_simple_inclusion_param);
  RUN_TEST(test_line_simple_exclusion_param);

  RUN_TEST(test_poly_simple_inclusion_param);
  RUN_TEST(test_poly_simple_exclusion_param);

  RUN_TEST(test_multipoly_simple_inclusion_param);
  RUN_TEST(test_multipoly_simple_exclusion_param);

  RUN_TEST(test_intersection_mode_first);
  RUN_TEST(test_intersection_mode_collect);
  RUN_TEST(test_intersection_mode_split);
  RUN_TEST(test_intersection_mode_split_first);
  RUN_TEST(test_intersection_mode_split_collect);

  RUN_TEST(test_poly_inner_inclusion_param);
  RUN_TEST(test_poly_inner_exclusion_param);

  RUN_TEST(test_disjoint_admin_polygons);

  std::cout << " >> Tests failed: " << tests_failed << std::endl << std::endl;

  return (tests_failed > 0) ? 1 : 0;
}


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cstdio>

namespace gazebo
{
	class BoxInfoPublisher : public ModelPlugin
	{
	public:
		void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
		{
			if(!ros::isInitialized())
			{
				printf("ROS is NOT initialized. Not using plugin\n");
				return;
			}

			//box_info_pub = nh.advertise<visualization_msgs::Marker>("/gazebo/

			this->model = _parent;

			marker_msg.header.frame_id = "/map";
			marker_msg.header.stamp = ros::Time::now();
			marker_msg.ns = "gazebo_box_info";
			marker_msg.id = 0;
			marker_msg.type = visualization_msgs::Marker::CUBE;
			marker_msg.action = visualization_msgs::Marker::ADD;

			math::Pose world_pose = this->model->GetWorldPose();
			marker_msg.pose.position.x = world_pose.pos.x;
			marker_msg.pose.position.y = world_pose.pos.y;
			marker_msg.pose.position.z = world_pose.pos.z;
			marker_msg.pose.orientation.x = world_pose.rot.x;
			marker_msg.pose.orientation.y = world_pose.rot.y;
			marker_msg.pose.orientation.z = world_pose.rot.z;
			marker_msg.pose.orientation.w = world_pose.rot.w;

			physics::Link_V links = this->model->GetLinks();
			for(physics::Link_V::const_iterator link = links.begin();
				link != links.end();
				link++)
			{

				physics::Collision_V collisions = (*link)->GetCollisions();
				for(physics::Collision_V::const_iterator collision = collisions.begin();
					collision != collisions.end();
					collision++)
				{
					physics::ShapePtr shape((*collision)->GetShape());

					if(shape->HasType(physics::Base::BOX_SHAPE))
					{
						physics::BoxShape *box = static_cast<physics::BoxShape*>(shape.get());
						math::Vector3 size = box->GetSize();

						marker_msg.scale.x = size.x;
						marker_msg.scale.y = size.y;
						marker_msg.scale.z = size.z;
					}
				}
			}

			marker_msg.color.a = 0.5;
			marker_msg.color.g = 1.0;

			// create a latching publisher
			box_info_pub = nh.advertise<visualization_msgs::Marker>("/gazebo/" + this->model->GetName() + "/info", 1, true);

			box_info_pub.publish(marker_msg);

			this->updateConnection = event::Events::ConnectWorldUpdateBegin(
				boost::bind(&BoxInfoPublisher::OnUpdate, this, _1));
		}

		void OnUpdate(const common::UpdateInfo& /*_info*/)
		{
			// publish data here
			marker_msg.header.stamp = ros::Time::now();
			box_info_pub.publish(marker_msg);
		}

	private:
		physics::ModelPtr model;

		event::ConnectionPtr updateConnection;

		ros::NodeHandle nh;
		ros::Publisher box_info_pub;

		visualization_msgs::Marker marker_msg;

	};

		GZ_REGISTER_MODEL_PLUGIN(BoxInfoPublisher)
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/sort.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/enumerate.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/table.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <boost/simd/sdk/simd/io.hpp>

NT2_TEST_CASE_TPL ( sort, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  using boost::simd::sort;
  using boost::dispatch::meta::as_;
  using boost::simd::tag::sort_;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T> cT;
  typedef native<cT,ext_t> vcT;
  typedef typename boost::dispatch::meta::call<sort_(vcT)>::type r_t;

  vcT a = boost::simd::enumerate<vcT>(cT(10,5));
  NT2_TEST_EQUAL(sort(a), a);
  a = boost::simd::enumerate<vcT>(cT(10,5), cT(-1, -1));
  int N = vcT::static_size-1;
  vcT b = boost::simd::enumerate<vcT>(cT(10-N, 5-N), cT(1, 1));
  NT2_TEST_EQUAL(sort(a), b);
 }


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(
        unzip(list(list(1, '2', 3.3), list('4', 5.5, 6)))
        ==
        list(list(1, '4'), list('2', 5.5), list(3.3, 6))
    );

    BOOST_HANA_CONSTEXPR_ASSERT(
        unzip(list(list(1, '2', 3.3), list('4', 5.5, 6, "ignored")))
        ==
        list(list(1, '4'), list('2', 5.5), list(3.3, 6))
    );
    //! [main]
}


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

#ifndef OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_
#define OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_

#include <common/types.hpp>
#include <optimization/visibility_control.hpp>
#include <optimization/utils.hpp>
#include <helper_functions/crtp.hpp>
#include <Eigen/Core>
#include <vector>
#include <cstddef>
#include <memory>
#include <tuple>
#include <utility>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace common
{
namespace optimization
{
/// CRTP base class for a mathematical expression.
/// \tparam Derived Implementation class. Must implement `score_(..)`, `jacobian_()` and
/// `hessian(..)`.
/// \tparam DomainValueT Parameter type.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
class OPTIMIZATION_PUBLIC Expression : public common::helper_functions::crtp<Derived>
{
public:
  static constexpr auto NumJacobianCols = NumJacobianColsT;
  static constexpr auto NumVars = NumVarsT;
  using DomainValue = DomainValueT;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
  using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
  using JacobianRef = Eigen::Ref<Jacobian>;
  using HessianRef = Eigen::Ref<Hessian>;

  /// Get the result of an expression for a given parameter value.
  /// \param x Parameter value
  /// \return Evaluated score
  Value operator()(const DomainValue & x)
  {
    return this->impl().score_(x);
  }

  /// Get the jacobian at a given parameter value.
  /// \param x Parameter value.
  /// \param out Evaluated jacobian matrix.
  void jacobian(const DomainValue & x, JacobianRef out)
  {
    this->impl().jacobian_(x, out);
  }

  /// Get the hessian at a given parameter value.
  /// \param x Parameter value.{
  /// \param out Evaluated hessian matrix.
  void hessian(const DomainValue & x, HessianRef out)
  {
    this->impl().hessian_(x, out);
  }

  /// Pre-compute and cache the score/jacobian/hessian values. Which terms are to be computed is
  /// specified with the `ComputeMode` argument. By default, this function does nothing. You can
  /// implement and hide this function in your implementation to allow for some caching behavior.
  /// Though it is encouraged to use `CachedExpression` which already handles a big portion of the
  /// cache state management.
  /// \param x Parameter value.
  /// \param mode Computation mode. Which terms (score, jacobian, hessian) are to be computed
  /// can be set within this element.
  void evaluate(const DomainValue & x, const ComputeMode & mode)
  {
    (void)x;
    (void)mode;
  }
};

/// Expression class for cases when pre-evaluating elements or computing elements together
/// may be more efficient. This class implements the necessary boilerplate to manage the cache
/// state book-keeping.
/// This class implements `score_(..)`, `jacobian_(..)` and `hessian_(..)`; hence the
/// implementation must only implement `evaluate_(..)`
/// \tparam Derived CRTP implementation class. This class should implement `evaluate_()`.
/// \tparam DomainValueT Parameter type.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
/// \tparam ComparatorT Comparator function for `DomainValueT`
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
  typename ComparatorT>
class CachedExpression : public
  Expression<CachedExpression<Derived, DomainValueT,
    NumJacobianColsT, NumVarsT, ComparatorT>,
    DomainValueT, NumJacobianColsT, NumVarsT>
{
public:
  static constexpr auto NumJacobianCols = NumJacobianColsT;
  static constexpr auto NumVars = NumVarsT;
  using DomainValue = DomainValueT;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
  using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
  using JacobianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumJacobianCols>>;
  using HessianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumVars>>;

  explicit CachedExpression(const ComparatorT & comparator = ComparatorT())
  : m_score{0.0}, m_jacobian(Jacobian::Zero()), m_hessian{Hessian::Zero()},
    m_cache_state(comparator) {}

  Value score_(const DomainValue & x)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
      evaluate(x, ComputeMode{}.set_score());
    }
    return m_score;
  }

  void jacobian_(const DomainValue & x, JacobianRef out)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::JACOBIAN)) {
      evaluate(x, ComputeMode{}.set_jacobian());
    }
    out = m_jacobian;
  }

  void hessian_(const DomainValue & x, HessianRef out)
  {
    if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
      evaluate(x, ComputeMode{}.set_hessian());
    }
    out = m_hessian;
  }

  void evaluate(const DomainValue & x, const ComputeMode & mode)
  {
    if (!mode.score() && !mode.jacobian() && !mode.hessian()) {
      return;
    }
    m_cache_state.update(x, mode);
    // Call the implementation method.
    static_cast<Derived *>(this)->evaluate_(x, mode);
  }

protected:
  void set_score(Value score)
  {
    m_score = score;
  }

  void set_jacobian(const JacobianRef & jacobian)
  {
    m_jacobian = jacobian;
  }

  void set_hessian(const HessianRef & hessian)
  {
    m_hessian = hessian;
  }

private:
  Value m_score;
  Jacobian m_jacobian;
  Hessian m_hessian;
  CacheStateMachine<DomainValue, ComparatorT> m_cache_state;
};

/// A generalized representation of an optimization problem. Constrained
/// implementation classes must implement the following methods:
/// score_opt_(x);
/// jacobian_opt_(x, out);
/// hessian_opt_(x, out);
/// \tparam Derived Implementation class
/// \tparam DomainValueT The type for parameter to be optimized for.
/// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
/// \tparam NumVarsT Number of variables.
/// \tparam ObjectiveT Objective implementation class.
/// \tparam EqualityConstraintsT A tuple of equality constraint expressions.
/// \tparam InequalityConstraintsT A tuple of inequality constraint expressions.
template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
  typename ObjectiveT, typename EqualityConstraintsT, typename InequalityConstraintsT>
class OPTIMIZATION_PUBLIC OptimizationProblem;

// Definition of OptimizationProblem.
// Template specialization is used for type deduction and forwarding constraint parameter packs
// to the tuples. This way "std::tuple" is enforced to be used on the constrained
// OptimizationProblem implementations.
template<typename Derived,
  typename ... EqualityConstraints,
  typename ... InequalityConstraints,
  typename ObjectiveT, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
class OPTIMIZATION_PUBLIC OptimizationProblem<Derived, DomainValueT, NumJacobianColsT, NumVarsT,
    ObjectiveT,
    std::tuple<EqualityConstraints...>,
    std::tuple<InequalityConstraints...>>
  : public Expression<Derived, DomainValueT, NumJacobianColsT, NumVarsT>
{
public:
  using EqualityConstraintsT = std::tuple<EqualityConstraints...>;
  using InequalityConstraintsT = std::tuple<InequalityConstraints...>;
  using Value = autoware::common::types::float64_t;
  using Jacobian = Eigen::Matrix<Value, NumVarsT, NumJacobianColsT>;
  using Hessian = Eigen::Matrix<Value, NumVarsT, NumVarsT>;
  using JacobianRef = Eigen::Ref<Jacobian>;
  using HessianRef = Eigen::Ref<Hessian>;
  static constexpr bool8_t is_unconstrained{(std::tuple_size<EqualityConstraintsT>::value == 0U) &&
    (std::tuple_size<InequalityConstraintsT>::value == 0U)};

  OptimizationProblem(
    ObjectiveT && objective,
    EqualityConstraintsT && eq_constraints,
    InequalityConstraintsT && ineq_constraints
  )
  : m_objective(std::forward<ObjectiveT>(objective)),
    m_equality_constraints(std::forward<EqualityConstraintsT>(eq_constraints)),
    m_inequality_constraints(std::forward<InequalityConstraintsT>(ineq_constraints))
  {
  }

  template<typename ... Args, typename Dummy = void, typename = std::enable_if_t<
      is_unconstrained, Dummy>>
  OptimizationProblem(
    Args && ... args
  )
  : m_objective(std::forward<Args>(args)...)
  {
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, Value> score_(const DomainValueT & x)
  {
    return m_objective(x);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, Value> score_(const DomainValueT & x)
  {
    return opt_impl()->score_opt_(x);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
  {
    m_objective.jacobian(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
  {
    opt_impl()->jacobian_opt_(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> hessian_(const DomainValueT & x, HessianRef out)
  {
    m_objective.hessian(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> hessian_(const DomainValueT & x, HessianRef out)
  {
    opt_impl()->hessian_opt_(x, out);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<enabled, void> evaluate(
    const DomainValueT & x,
    const ComputeMode & mode)
  {
    m_objective.evaluate(x, mode);
  }

  template<bool8_t enabled = is_unconstrained>
  typename std::enable_if_t<!enabled, void> evaluate(
    const DomainValueT & x,
    const ComputeMode & mode)
  {
    opt_impl()->evaluate_(x, mode);
  }

protected:
  ObjectiveT & objective()
  {
    return m_objective;
  }

  EqualityConstraintsT & equality_constraints()
  {
    return m_equality_constraints;
  }

  InequalityConstraintsT & inequality_constraints()
  {
    return m_inequality_constraints;
  }

private:
  Derived * opt_impl()
  {
    return static_cast<Derived *>(this);
  }

  ObjectiveT m_objective;
  EqualityConstraintsT m_equality_constraints;
  InequalityConstraintsT m_inequality_constraints;
};

/// Convenience class for unconstrained optimization problems. This is a thin wrapper
/// around the objective expression.
/// \tparam ObjectiveT Objective of the optimization problem.
/// \tparam DomainValueT Parameter type.
/// \tparam NumVarsT Number of variables.
template<typename ObjectiveT, typename DomainValueT, int NumVarsT>
class UnconstrainedOptimizationProblem : public
  OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
    DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>
{
  using OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
    DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>::OptimizationProblem;
};

}  // namespace optimization
}  // namespace common
}  // namespace autoware

#endif  // OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_


#pragma once

#include <array>
#include <tuple>
#include <vector>
#include <boost/optional.hpp>
#include <cgo/base/types.hpp>

namespace cgo {
namespace base {

class State {
public:
   State();
   State(const State& state);
   State(const Board& board);
   ~State();

   const Marker& getMarker(const Position& position) const;

   const Board& getBoard() const;
   std::tuple< int, int > getScores() const;
   int getScore(Marker marker) const;
   std::vector< Position > getLiberties(Marker marker);
   std::vector< Successor > getSuccessors(Marker marker,
    boost::optional< Predecessor > predecessor) const;
   bool isActionValid(const Action& action,
    boost::optional< Predecessor > predecessor) const;

   State& operator=(const State& rhs);

   static struct InvalidMarker {} _invalidMarker;
   static struct InvalidPosition {} _invalidPosition;

   static State applyAction(const State& sourceState, const Action& action);

   static void validateMarker(const Marker& marker);
   static void validatePlayerMarker(const Marker& marker);
   static void validatePosition(const Position& position);

   static void printBoard(const State& state);
   static void printBoard(const Board& board);

   static unsigned int getIndex(const Position& position);
   static Position getPosition(unsigned int index);

private:
   std::vector< Position > calculateLiberties(Marker marker);
   std::vector< std::vector< Position > > groupTerritory() const;

   static std::vector< Position > getAdjacentPositions(const Position& position);
   static std::vector< Position > getSurroundingPositions(const Position& position);
   static void capturePosition(Board& board, Marker marker,
    const Position& position);
   static void collectPositions(const Board& board, Marker marker,
    const Position& position, std::vector< Position >& accumulator,
    std::array< bool, BOARD_DIMENSION * BOARD_DIMENSION >& collected);
   static bool isCaptured(const Board& board,
    const std::vector< Position > positionGroup);
   static void capturePositionGroup(Board& board,
    const std::vector< Position > positionGroup);
   static std::vector< Position > getSurroundingPositionGroup(const Board& board,
    const std::vector< Position > positionGroup, Marker marker);

   static void printHeader();
   static void printBreak();

   Board _board;
   boost::optional< std::vector< Position > > _liberties[2];
};

} // namespace base
} // namespace cgo


#include <boost/compute/functional/integer.hpp>


#ifndef _drc_LidarUtils_hpp_
#define _drc_LidarUtils_hpp_

#include <vector>
#include <Eigen/Geometry>

namespace drc {
class LidarUtils {
public:
  static bool
  interpolateScan(const std::vector<float>& iRanges,
                  const double iTheta0, const double iThetaStep,
                  const Eigen::Isometry3d& iPose0,
                  const Eigen::Isometry3d& iPose1,
                  std::vector<Eigen::Vector3f>& oPoints);
};
}

#endif


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

#ifndef __H__OBJ_PARSER_RENDER_STATE__H__
#define __H__OBJ_PARSER_RENDER_STATE__H__

#include <boost/fusion/adapted.hpp>

#include <string>
#include <cstdint>

//------------------------------------------------------------------------------------------

enum class OBJSubdivision
{
    None = 0,                     ///< No technique specified (technique not in use)
    Parametric,                   ///< Constant parametric subdivision using one resolution parameter
    ParametricA,                  ///< Constant parametric subdivision using separate resolution parameters for U and V
    ParametricB,                  ///< Constant parametric subdivision using one resolution parameter for both U and V
    Spatial,                      ///< Constant spatial subdivison
    Curvature,                    ///< Curvature-dependent subdivision

};

/**
 * \struct OBJCurveTechnique
 * \brief Specifies free-form curve subdivision technique.
 */
struct OBJCurveTechnique
{
    OBJCurveTechnique()
        : technique(OBJSubdivision::None),
          res(0.0f),
          maxLength(0.0f),
          maxDistance(0.0f),
          maxAngle(0.0f)
    {

    }

    OBJSubdivision technique;     ///< Subdivision type. For curves this may be: Parametric, Spatial, or Curvature. If None, then this technique has not been specified.

    float res;                    ///< Resolution used with Parametric subdivision. 0.0 if not used.

    float maxLength;              ///< Maximum length of line segments with Spatial subdivision. 0.0 if not used.
    float maxDistance;            ///< Maximum distance between line segments and curve in Curvature subdivision. 0.0 if not used.
    float maxAngle;               ///< Angle between tangent vectors at line segment ends in Curvature subdivision. 0.0 if not used.
};

/**
 * \struct OBJSurfaceTechnique
 * \brief Specifies free-form surface subdivision technique.
 */
struct OBJSurfaceTechnique
{
    OBJSurfaceTechnique()
        : technique(OBJSubdivision::None),
          resU(0.0f),
          resV(0.0f),
          maxLength(0.0f),
          maxDistance(0.0f),
          maxAngle(0.0f)
    {

    }

    OBJSubdivision technique;     ///< Subdivision type. For surfaces this may be: ParametricA, ParametricB, Spatial, or Curvature. If None, then this technique has not been specified.

    float resU;                   ///< Resolution parameter for the U direction with Parametric subdivision. If technique is ParametricB, then resU == resV
    float resV;                   ///< Resolution parameter for the V direction with Parametric subdivision. If technique is ParametricB, then resU == resV

    float maxLength;              ///< Maximum length of line segments with Spatial subdivision. 0.0 if not used.
    float maxDistance;            ///< Maximum distance between line segments and curve in Curvature subdivision. 0.0 if not used.
    float maxAngle;               ///< Angle between tangent vectors at line segment ends in Curvature subdivision. 0.0 if not used.
};

/**
 * \struct OBJRenderState
 * \brief Struct of optional state settings.
 *
 * The settings specified in this struct are on a per-face or free-form basis.
 * Most commonly used of these is the material name. The rest have highly varying
 * levels of support among OBJ writers, other readers, and end-use implementations.
 *
 * Multiple faces/free-forms may reference the same state.
 */
struct OBJRenderState
{
    OBJRenderState()
        : smoothing(0), lod(0), bevelInterp(false), colorInterp(false), dissolveInterp(false)
    {

    }

    //--------------------------------------------------------------------

    uint32_t smoothing;                      ///< Smoothing group number. Default/no smoothing group is 0.
    uint32_t lod;                            ///< Level-of-detail displayed. Off is 0. Otherwise, range of [1,100]

    bool bevelInterp;                        ///< Sets bevel interpolation on/off. Default off. Polygons only.
    bool colorInterp;                        ///< Sets color interpolation on/off. Default off. Polygons only.
    bool dissolveInterp;                     ///< Sets dissolve interpolation on/off. Default off. Polygons only.

    std::string material;                    ///< Specifies the material to use. Empty means no material. Once set, material can only be changed.
    std::string textureMap;                  ///< Specifies the texture map to use. Empty or "off" means no map specified.
    std::string shadowObj;                   ///< Specifies the shadow object filename. Empty means no object specified.
    std::string traceObj;                    ///< Specifies the ray tracing object filename. Empty means no object specified.

    OBJCurveTechnique curveTechnique;        ///< Specifies the curve approximation technique. Free-forms only.
    OBJSurfaceTechnique surfaceTechnique;    ///< Specified the surface approximation technique. Free-froms only.
};

//------------------------------------------------------------------------------------------

#endif


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/27/problem27.hpp"

BOOST_AUTO_TEST_SUITE( Problem27 )

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem27::solve();
        BOOST_CHECK_EQUAL(res, -59231);
    }

BOOST_AUTO_TEST_SUITE_END()


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

/*
 * File:   Utils.hpp
 * Author: Pedro Gandola <pedrogandola@smart.mit.edu>
 *
 * Created on June 12, 2013, 4:59 PM
 */
#pragma once

#include <ctime>
#include <vector>
#include <list>
#include <string>
#include <utility>
#include <sstream>
#include <boost/shared_ptr.hpp>
#include <boost/random.hpp>
#include <boost/nondet_random.hpp>
#include "boost/generator_iterator.hpp"
#include <stdexcept>
#include "boost/lexical_cast.hpp"

namespace sim_mob {

    class Utils {
    public:

        /**
         * Generates a new float value.
         * @param min minimum limit.
         * @param max maximum limit.
         * @return the generated value.
         */
        static float generateFloat(float min, float max);

        static double uRandom();
        static double nRandom(double mean, double stddev);

        /**
         * Generates a new integer value.
         * @param min limit.
         * @param max limit.
         * @return the generated value.
         */
        static int generateInt(int min, int max);

        /**
         * Convert argc/argv into a vector of strings representing each argument.
         */
        static std::vector<std::string> parseArgs(int argc, char* argv[]);

        /**
         * Merges log files.
         */
        static void printAndDeleteLogFiles(const std::list<std::string>& logFileNames,std::string outputFileName="out.txt");

        //Helper for XML parsing. Source value looks like this: "3000 : 6000", spaces optional.
        //\todo This is mostly in the wrong place; our whole "util" directory needs some reorganization.
        static std::pair<double, double> parseScaleMinmax(const std::string& src);

        /**
         * Restricts a value to be within a specified range.
         * @param value to clamp
         * @param min The minimum value.
         * @param max The maximum value.
         * @return If value is less than min, min will be returned,
         * If value is greater than max, max will be returned, otherwise returns
         * value.
         */
        template<typename T>
        static T clamp(const T& value, const T& min, const T& max) {
            return (value < min) ? min : (value > max) ? max : value;
        }

        /**
         * Converts the given data to string.
         *
         * If the type is not primitive type then
         * it must implement the operator:
         *
         * friend ostream& operator<<(std::ostream& strm, const MyType& data);
         *
         * @param data to convert to string.
         * @return string with data.
         */
        template<typename T>
        static std::string toStr(const T& data) {
            std::ostringstream stream;
            stream << data;
            return stream.str();
        }

        /**
         * Converts meter to feet.
         * @param meter value to convert.
         * @return value in feet.
         */
        static double toFeet(const double meter);

        /**
         * Converts feet to meter.
         * @param feet value to convert.
         * @return value in meter.
         */
        static double toMeter(const double feet);
        typedef boost::shared_ptr<boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > > ND_PTR;
        template<typename T>
        static ND_PTR initDistribution(std::pair<T , T > range){

            //normal distribution
            boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
            boost::normal_distribution<> nd(range.first, range.second);
            boost::shared_ptr<boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > > distributionPtr;
            distributionPtr.reset(new boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > (rng, nd));
            return distributionPtr;
        }
        /**
         * Converts the given value (in centimeters) to meters.
         * @param cmValue
         * @return value in meters
         */
        template<typename T> static T cmToMeter(const T& cmValue){
            return (T)(cmValue/(T)100);
        }

        /**
         * Converts given string to defined type.
         * This function throws exception in case of error during the conversion.
         * @param value to convert.
         * @return value in the defined type if the conversion is valid
         * otherwise an exception is thrown.
         */
        template<typename T> static T cast(const std::string& value) {
            try {
                return boost::lexical_cast<T>(value);
            } catch (boost::bad_lexical_cast&) {
                throw std::runtime_error("Cannot convert <" + value + ">");
            }
        }

        /**
         * Converts given string to defined type.
         * This function does not throws exception in the case of error during the conversion.
         * It returns the given errorValue instead.
         * @param value to convert.
         * @param errorValue value to return in the case of error.
         * @return value in the defined type or the errorValue if conversion has failed.
         */
        template<typename T> static T cast(const std::string& value, const T& errorValue) {
            try {
                return boost::lexical_cast<T>(value);
            } catch (boost::bad_lexical_cast&) {
            }
            return errorValue;
        }

        /**
         *  convert text string to double array
         *  @param str text string like "1 2 3" or "1,2,3"
         *  @array store extracted numbers from text
         */
        static void convertStringToArray(std::string& str,std::vector<double>& array);

        static double urandom();
        static int brandom(double p);
        /// extract id string from aimsun id
        static std::string getNumberFromAimsunId(std::string &aimsunid);

        /**
         * convert longitude,latitude into x,y coordinate in WGS84
         * @param x hold longitude as input and also store final x coordinate after conversion
         * @param y hold latitude as input and also store final y coordinate after conversion
         */
        static void convertWGS84_ToUTM(double& x, double& y);
    };

    /**
     * This class measures the running time of something.
     */
    class StopWatch {
    public:
        StopWatch();

        /**
         * Starts the watch.
         */
        void start();

        /**
         * Stops the watch.
         */
        void stop();

        /**
         * Gets the time taken between the last Start-Stop call.
         * @return time in seconds or -1 if the watch is running.
         */
        double getTime() const;

    private:
        time_t now;
        time_t end;
        volatile bool running;
    };
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/43/problem43.hpp"

BOOST_AUTO_TEST_SUITE( Problem43 )

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem43::solve();
        BOOST_CHECK_EQUAL(res, 16695334890);
    }

BOOST_AUTO_TEST_SUITE_END()


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include "mader_ros.hpp"

#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <nav_msgs/Path.h>

#include <decomp_ros_msgs/PolyhedronArray.h>
#include <decomp_ros_utils/data_ros_utils.h>  //For DecompROS::polyhedron_array_to_ros
#include <decomp_geometry/polyhedron.h>       //For hyperplane
#include <Eigen/Geometry>

#include <jsk_rviz_plugins/OverlayText.h>

typedef MADER_timers::Timer MyTimer;

// this object is created in the mader_ros_node
MaderRos::MaderRos(ros::NodeHandle nh1, ros::NodeHandle nh2, ros::NodeHandle nh3) : nh1_(nh1), nh2_(nh2), nh3_(nh3)
{
  mu::safeGetParam(nh1_, "use_ff", par_.use_ff);
  mu::safeGetParam(nh1_, "visual", par_.visual);
  mu::safeGetParam(nh1_, "color_type", par_.color_type);
  mu::safeGetParam(nh1_, "n_agents", par_.n_agents);

  mu::safeGetParam(nh1_, "dc", par_.dc);
  mu::safeGetParam(nh1_, "goal_radius", par_.goal_radius);
  mu::safeGetParam(nh1_, "drone_radius", par_.drone_radius);

  mu::safeGetParam(nh1_, "Ra", par_.Ra);

  mu::safeGetParam(nh1_, "w_max", par_.w_max);
  mu::safeGetParam(nh1_, "alpha_filter_dyaw", par_.alpha_filter_dyaw);

  mu::safeGetParam(nh1_, "x_min", par_.x_min);
  mu::safeGetParam(nh1_, "x_max", par_.x_max);

  mu::safeGetParam(nh1_, "y_min", par_.y_min);
  mu::safeGetParam(nh1_, "y_max", par_.y_max);

  mu::safeGetParam(nh1_, "z_min", par_.z_min);
  mu::safeGetParam(nh1_, "z_max", par_.z_max);

  std::vector<double> v_max_tmp;
  std::vector<double> a_max_tmp;
  std::vector<double> j_max_tmp;

  mu::safeGetParam(nh1_, "v_max", v_max_tmp);
  mu::safeGetParam(nh1_, "a_max", a_max_tmp);
  mu::safeGetParam(nh1_, "j_max", j_max_tmp);

  par_.v_max << v_max_tmp[0], v_max_tmp[1], v_max_tmp[2];
  par_.a_max << a_max_tmp[0], a_max_tmp[1], a_max_tmp[2];
  par_.j_max << j_max_tmp[0], j_max_tmp[1], j_max_tmp[2];

  mu::safeGetParam(nh1_, "factor_alloc", par_.factor_alloc);
  mu::safeGetParam(nh1_, "factor_alloc_close", par_.factor_alloc_close);
  mu::safeGetParam(nh1_, "dist_factor_alloc_close", par_.dist_factor_alloc_close);

  mu::safeGetParam(nh1_, "factor_alpha", par_.factor_alpha);

  mu::safeGetParam(nh1_, "num_pol", par_.num_pol);
  mu::safeGetParam(nh1_, "deg_pol", par_.deg_pol);
  mu::safeGetParam(nh1_, "weight", par_.weight);
  mu::safeGetParam(nh1_, "epsilon_tol_constraints", par_.epsilon_tol_constraints);
  mu::safeGetParam(nh1_, "xtol_rel", par_.xtol_rel);
  mu::safeGetParam(nh1_, "ftol_rel", par_.ftol_rel);
  mu::safeGetParam(nh1_, "solver", par_.solver);

  mu::safeGetParam(nh1_, "upper_bound_runtime_snlopt", par_.upper_bound_runtime_snlopt);
  mu::safeGetParam(nh1_, "lower_bound_runtime_snlopt", par_.lower_bound_runtime_snlopt);
  mu::safeGetParam(nh1_, "kappa", par_.kappa);
  mu::safeGetParam(nh1_, "mu", par_.mu);

  mu::safeGetParam(nh1_, "a_star_samp_x", par_.a_star_samp_x);
  mu::safeGetParam(nh1_, "a_star_samp_y", par_.a_star_samp_y);
  mu::safeGetParam(nh1_, "a_star_samp_z", par_.a_star_samp_z);
  mu::safeGetParam(nh1_, "a_star_fraction_voxel_size", par_.a_star_fraction_voxel_size);
  mu::safeGetParam(nh1_, "allow_infeasible_guess", par_.allow_infeasible_guess);

  mu::safeGetParam(nh1_, "a_star_bias", par_.a_star_bias);

  mu::safeGetParam(nh1_, "basis", par_.basis);

  mu::safeGetParam(nh1_, "res_plot_traj", par_.res_plot_traj);

  mu::safeGetParam(nh1_, "alpha", par_.alpha);
  mu::safeGetParam(nh1_, "beta", par_.beta);
  mu::safeGetParam(nh1_, "gamma", par_.gamma);

  mu::safeGetParam(nh1_, "alpha_shrink", par_.alpha_shrink);

  mu::safeGetParam(nh1_, "fov_horiz_deg", par_.fov_horiz_deg);
  mu::safeGetParam(nh1_, "fov_vert_deg", par_.fov_vert_deg);
  mu::safeGetParam(nh1_, "fov_depth", par_.fov_depth);

  std::cout << "Parameters obtained" << std::endl;

  // CHECK parameters
  std::cout << bold << "Parameters obtained, checking them..." << reset << std::endl;

  verify((par_.gamma > 0), "Not satisfied: (par_.gamma > 0)");
  verify((par_.beta >= 0 || par_.alpha >= 0), "Not satisfied: (par_.beta >= 0 || par_.alpha >= 0)");
  // verify((par_.a_max.z() <= 9.81), "par_.a_max.z() >= 9.81, the drone will flip");
  verify((par_.factor_alloc >= 1.0), "Not satisfied: (par_.factor_alloc >= 1.0)");
  verify((par_.kappa > 0 || par_.mu > 0), "Not satisfied: (par_.kappa > 0 || par_.mu > 0)");
  verify(((par_.kappa + par_.mu) <= 1), "Not satisfied: ((par_.kappa + par_.mu) <= 1)");
  verify((par_.a_star_fraction_voxel_size >= 0.0 || par_.a_star_fraction_voxel_size <= 1.0), "a_star_fraction_voxel_"
                                                                                "size must be in [0,1]");

  verify((par_.epsilon_tol_constraints < 0.02), "The tolerance on the constraints is too big -->  there will be "
                                                "jumps in accel/vel");
  std::cout << bold << "Parameters checked" << reset << std::endl;
  /////////////////////

  mader_ptr_ = std::unique_ptr<Mader>(new Mader(par_));

  // Publishers
  pub_goal_ = nh1_.advertise<snapstack_msgs::Goal>("goal", 1);
  pub_setpoint_ = nh1_.advertise<visualization_msgs::Marker>("setpoint", 1);
  pub_point_G_ = nh1_.advertise<geometry_msgs::PointStamped>("point_G", 1);
  pub_point_G_term_ = nh1_.advertise<geometry_msgs::PointStamped>("point_G_term", 1);
  pub_point_A_ = nh1_.advertise<visualization_msgs::Marker>("point_A", 1);
  pub_actual_traj_ = nh1_.advertise<visualization_msgs::Marker>("actual_traj", 1);
  poly_safe_pub_ = nh1_.advertise<decomp_ros_msgs::PolyhedronArray>("poly_safe", 1, true);
  pub_text_ = nh1_.advertise<jsk_rviz_plugins::OverlayText>("text", 1);
  pub_traj_safe_colored_ = nh1_.advertise<visualization_msgs::MarkerArray>("traj_safe_colored", 1);
  pub_traj_ = nh1_.advertise<mader_msgs::DynTraj>("/trajs", 1, true);  // The last boolean is latched or not
  pub_text_ = nh1_.advertise<jsk_rviz_plugins::OverlayText>("text", 1);
  pub_fov_ = nh1_.advertise<visualization_msgs::Marker>("fov", 1);
  pub_obstacles_ = nh1_.advertise<visualization_msgs::Marker>("obstacles", 1);

  // Subscribers
  sub_term_goal_ = nh1_.subscribe("term_goal", 1, &MaderRos::terminalGoalCB, this);
  // sub_mode_ = nh1_.subscribe("mode", 1, &MaderRos::modeCB, this);
  sub_whoplans_ = nh1_.subscribe("who_plans", 1, &MaderRos::whoPlansCB, this);
  sub_state_ = nh1_.subscribe("state", 1, &MaderRos::stateCB, this);
  sub_traj_ = nh1_.subscribe("/trajs", 20, &MaderRos::trajCB, this);  // The number is the queue size

  // Timers
  pubCBTimer_ = nh2_.createTimer(ros::Duration(par_.dc), &MaderRos::pubCB, this);
  replanCBTimer_ = nh3_.createTimer(ros::Duration(par_.dc), &MaderRos::replanCB, this);

  // For now stop all these subscribers/timers until we receive GO
  // // sub_state_.shutdown();
  // pubCBTimer_.stop();
  // replanCBTimer_.stop();
  sub_state_.shutdown();
  sub_term_goal_.shutdown();
  pubCBTimer_.stop();
  replanCBTimer_.stop();

  // Rviz_Visual_Tools
  visual_tools_.reset(new rvt::RvizVisualTools("world", "/rviz_visual_tools"));
  visual_tools_->loadMarkerPub();  // create publisher before waitin
  ros::Duration(0.5).sleep();
  visual_tools_->deleteAllMarkers();
  visual_tools_->enableBatchPublishing();

  // Markers
  setpoint_ = mu::getMarkerSphere(0.35, mu::orange_trans);
  E_ = mu::getMarkerSphere(0.35, mu::red_normal);
  A_ = mu::getMarkerSphere(0.35, mu::red_normal);

  // If you want another thread for the replanCB: replanCBTimer_ = nh_.createTimer(ros::Duration(par_.dc),
  // &MaderRos::replanCB, this);

  name_drone_ = ros::this_node::getNamespace();  // Return also the slashes (2 in Kinetic, 1 in Melodic)
  name_drone_.erase(std::remove(name_drone_.begin(), name_drone_.end(), '/'), name_drone_.end());  // Remove the slashes

  std::string id = name_drone_;
  id.erase(0, 2);  // Erase SQ or HX i.e. SQ12 --> 12  HX8621 --> 8621 # TODO Hard-coded for this this convention
  id_ = std::stoi(id);

  timer_stop_.Reset();

  clearMarkerActualTraj();

  ////// to avoid having to click on the GUI (TODO)
  // mader_msgs::Mode tmp;
  // tmp.mode = 1;
  // modeCB(tmp);
  //// ros::Duration(1.0).sleep();  // TODO
  //// bool success_service_call = system("rosservice call /change_mode 'mode: 1'");
  ////

  ROS_INFO("Planner initialized");
}

MaderRos::~MaderRos()
{
  sub_state_.shutdown();
  pubCBTimer_.stop();
  replanCBTimer_.stop();
}

void MaderRos::pubObstacles(mt::Edges edges_obstacles)
{
  pub_obstacles_.publish(mu::edges2Marker(edges_obstacles, mu::color(mu::red_normal)));

  return;
}

void MaderRos::trajCB(const mader_msgs::DynTraj& msg)
{
  if (msg.id == id_)
  {  // This is my own trajectory
    return;
  }

  Eigen::Vector3d W_pos(msg.pos.x, msg.pos.y, msg.pos.z);  // position in world frame
  double dist = (state_.pos - W_pos).norm();

  bool can_use_its_info;

  // if (par_.impose_fov == false)
  // {  // same as 360 deg of FOV
  //   can_use_its_info = dist <= par_.R_local_map;
  // }
  // else
  // {  // impose_fov==true

  //   Eigen::Vector3d B_pos = W_T_B_.inverse() * W_pos;  // position of the obstacle in body frame
  //   // check if it's inside the field of view.
  //   can_use_its_info =
  //       B_pos.x() < par_.fov_depth &&                                                       //////////////////////
  //       fabs(atan2(B_pos.y(), B_pos.x())) < ((par_.fov_horiz_deg * M_PI / 180.0) / 2.0) &&  //////////////////////
  //       fabs(atan2(B_pos.z(), B_pos.x())) < ((par_.fov_vert_deg * M_PI / 180.0) / 2.0);     ///////////////////////

  //   // std::cout << "inFOV= " << can_use_its_info << std::endl;
  // }

  can_use_its_info = (dist <= 4 * par_.Ra);  // See explanation of 4*Ra in Mader::updateTrajObstacles

  if (can_use_its_info == false)
  {
    return;
  }

  mt::dynTraj tmp;
  tmp.function.push_back(msg.function[0]);
  tmp.function.push_back(msg.function[1]);
  tmp.function.push_back(msg.function[2]);

  tmp.bbox << msg.bbox[0], msg.bbox[1], msg.bbox[2];

  tmp.id = msg.id;

  tmp.is_agent = msg.is_agent;

  if (msg.is_agent)
  {
    tmp.pwp = mu::pwpMsg2Pwp(msg.pwp);
  }

  tmp.time_received = ros::Time::now().toSec();

  mader_ptr_->updateTrajObstacles(tmp);
}

// This trajectory contains all the future trajectory (current_pos --> A --> final_point_of_traj), because it's the
// composition of pwp
void MaderRos::publishOwnTraj(const mt::PieceWisePol& pwp)
{
  std::vector<std::string> s;  // mu::pieceWisePol2String(pwp); The rest of the agents will use the pwp field, not the
                               // string
  s.push_back("");
  s.push_back("");
  s.push_back("");

  mader_msgs::DynTraj msg;
  msg.function = s;
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.pos.x = state_.pos.x();
  msg.pos.y = state_.pos.y();
  msg.pos.z = state_.pos.z();
  msg.id = id_;

  msg.is_agent = true;

  msg.pwp = mu::pwp2PwpMsg(pwp);

  // std::cout<<"msg.pwp.times[0]= "<<msg.pwp.times[0]

  pub_traj_.publish(msg);
}

void MaderRos::replanCB(const ros::TimerEvent& e)
{
  if (ros::ok() && published_initial_position_ == true)
  {
    mt::Edges edges_obstacles;
    std::vector<mt::state> X_safe;

    std::vector<Hyperplane3D> planes;
    mt::PieceWisePol pwp;

    bool replanned = mader_ptr_->replan(edges_obstacles, X_safe, planes, num_of_LPs_run_, num_of_QCQPs_run_, pwp);

    if (par_.visual)
    {
      // Delete markers to publish stuff
      visual_tools_->deleteAllMarkers();
      visual_tools_->enableBatchPublishing();

      pubObstacles(edges_obstacles);
      pubTraj(X_safe);
      publishPlanes(planes);
      publishText();
    }

    if (replanned)
    {
      publishOwnTraj(pwp);
      pwp_last_ = pwp;
    }
    else
    {
      int time_ms = int(ros::Time::now().toSec() * 1000);

      if (timer_stop_.ElapsedMs() > 500.0)  // publish every half a second. TODO set as param
      {
        publishOwnTraj(pwp_last_);  // This is needed because is drone DRONE1 stops, it needs to keep publishing his
                                    // last planned trajectory, so that other drones can avoid it (even if DRONE1 was
                                    // very far from the other drones with it last successfully planned a trajectory).
                                    // Note that these trajectories are time-indexed, and the last position is taken if
                                    // t>times.back(). See eval() function in the pwp struct
        timer_stop_.Reset();
      }
    }

    // std::cout << "[Callback] Leaving replanCB" << std::endl;
  }
}

void MaderRos::publishText()
{
  jsk_rviz_plugins::OverlayText text;
  text.width = 600;
  text.height = 133;
  text.left = 1600;
  text.top = 10;
  text.text_size = 17;
  text.line_width = 2;
  text.font = "DejaVu Sans Mono";
  text.text = "Num of LPs run= " + std::to_string(num_of_LPs_run_) + "\n" +  ///////////////////
              "Num of QCQPs run= " + std::to_string(num_of_QCQPs_run_);

  text.fg_color = mu::color(mu::teal_normal);
  text.bg_color = mu::color(mu::black_trans);

  pub_text_.publish(text);
}

void MaderRos::publishPlanes(std::vector<Hyperplane3D>& planes)
{
  //  int num_of_intervals = planes.size() / poly_safe.size();

  auto color = visual_tools_->getRandColor();

  int i = 0;
  for (auto plane_i : planes)
  {
    if ((i % par_.num_pol) == 0)  // planes for a new obstacle --> new color
    {
      color = visual_tools_->getRandColor();  // rviz_visual_tools::TRANSLUCENT_LIGHT;  //
    }
    Eigen::Isometry3d pose;
    pose.translation() = plane_i.p_;

    // Calculate the rotation matrix from the original normal z_0 = (0,0,1) to new normal n = (A,B,C)
    Eigen::Vector3d z_0 = Eigen::Vector3d::UnitZ();
    Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(z_0, plane_i.n_);
    pose.linear() = q.toRotationMatrix();

    double height = 0.001;  // very thin
    double x_width = 2;     // very thin
    double y_width = 2;     // very thin
    visual_tools_->publishCuboid(pose, x_width, y_width, height, color);
    i++;

    /*    double d_i = -plane_i.n_.dot(plane_i.p_);
        std::cout << bold << "Publishing plane, d_i= " << d_i << reset << std::endl;
        visual_tools_->publishABCDPlane(plane_i.n_.x(), plane_i.n_.y(), plane_i.n_.z(), d_i, rvt::MAGENTA, 2, 2);*/
  }
  visual_tools_->trigger();
}

void MaderRos::publishPoly(const vec_E<Polyhedron<3>>& poly)
{
  // std::cout << "Going to publish= " << (poly[0].hyperplanes())[0].n_ << std::endl;
  decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(poly);
  poly_msg.header.frame_id = world_name_;

  poly_safe_pub_.publish(poly_msg);
}

void MaderRos::whoPlansCB(const mader_msgs::WhoPlans& msg)
{
  if (msg.value != msg.MADER)
  {  // PANTHER does nothing
    sub_state_.shutdown();
    sub_term_goal_.shutdown();
    pubCBTimer_.stop();
    replanCBTimer_.stop();
    mader_ptr_->resetInitialization();
    std::cout << on_blue << "**************MADER STOPPED" << reset << std::endl;
  }
  else
  {  // PANTHER is the one who plans now (this happens when the take-off is finished)
    sub_term_goal_ = nh1_.subscribe("term_goal", 1, &MaderRos::terminalGoalCB, this);  // TODO: duplicated from above
    sub_state_ = nh1_.subscribe("state", 1, &MaderRos::stateCB, this);                 // TODO: duplicated from above
    pubCBTimer_.start();
    replanCBTimer_.start();
    std::cout << on_blue << "**************MADER STARTED" << reset << std::endl;
  }
}

void MaderRos::stateCB(const snapstack_msgs::State& msg)
{
  mt::state state_tmp;
  state_tmp.setPos(msg.pos.x, msg.pos.y, msg.pos.z);
  state_tmp.setVel(msg.vel.x, msg.vel.y, msg.vel.z);
  state_tmp.setAccel(0.0, 0.0, 0.0);
  // std::cout << bold << red << "MSG_QUAT= " << msg.quat << reset << std::endl;
  double roll, pitch, yaw;
  mu::quaternion2Euler(msg.quat, roll, pitch, yaw);
  state_tmp.setYaw(yaw);
  state_ = state_tmp;
  // std::cout << bold << red << "STATE_YAW= " << state_.yaw << reset << std::endl;

  // std::cout << "Updating state to" << std::endl;
  // state_tmp.print();

  mader_ptr_->updateState(state_tmp);

  W_T_B_ = Eigen::Translation3d(msg.pos.x, msg.pos.y, msg.pos.z) *
           Eigen::Quaterniond(msg.quat.w, msg.quat.x, msg.quat.y, msg.quat.z);

  if (published_initial_position_ == false)
  {
    pwp_last_ = mu::createPwpFromStaticPosition(state_);
    publishOwnTraj(pwp_last_);
    published_initial_position_ = true;
  }
  if (mader_ptr_->IsTranslating() == true && par_.visual)
  {
    pubActualTraj();
  }

  // publishFOV();
}

// void MaderRos::modeCB(const mader_msgs::Mode& msg)
// {
//   // mader_ptr_->changeMode(msg.mode);

//   if (msg.mode != msg.GO)
//   {  // MADER DOES NOTHING
//     // sub_state_.shutdown();
//     pubCBTimer_.stop();
//     replanCBTimer_.stop();
//     // std::cout << on_blue << "**************stopping replanCBTimer" << reset << std::endl;
//     mader_ptr_->resetInitialization();
//   }
//   else
//   {  // The mode changed to GO (the mode changes to go when takeoff is finished)
//     // sub_state_ = nh_.subscribe("state", 1, &MaderRos::stateCB, this);  // TODO duplicated from above
//     // std::cout << on_blue << "**************starting replanCBTimer" << reset << std::endl;
//     pubCBTimer_.start();
//     replanCBTimer_.start();
//   }
// }

void MaderRos::pubCB(const ros::TimerEvent& e)
{
  mt::state next_goal;
  if (mader_ptr_->getNextGoal(next_goal))
  {
    snapstack_msgs::Goal quadGoal;

    quadGoal.p = mu::eigen2point(next_goal.pos); //Kota changed it from eigen2rosvector July 26, 2021

    //printf("terminal goal x %f \n", next_goal.pos.x());
    //printf("terminal goal y %f \n", next_goal.pos.y());
    //printf("terminal goal z %f \n", next_goal.pos.z());

    quadGoal.p = mu::eigen2point(next_goal.pos);  // Kota changed it from eigen2rosvector July 26, 2021

    // printf("terminal goal x %f \n", next_goal.pos.x());
    // printf("terminal goal y %f \n", next_goal.pos.y());
    // printf("terminal goal z %f \n", next_goal.pos.z());

    quadGoal.v = mu::eigen2rosvector(next_goal.vel);
    quadGoal.a = mu::eigen2rosvector(next_goal.accel);
    quadGoal.j = mu::eigen2rosvector(next_goal.jerk);

    //quadGoal.dyaw = next_goal.dyaw;
    //quadGoal.yaw = next_goal.yaw;

    // quadGoal.dyaw = next_goal.dyaw;
    // quadGoal.yaw = next_goal.yaw;

    quadGoal.dpsi = next_goal.dyaw;
    quadGoal.psi = next_goal.yaw;

    quadGoal.header.stamp = ros::Time::now();
    quadGoal.header.frame_id = world_name_;

    quadGoal.power = true;  // kota added July 27, 2021

    pub_goal_.publish(quadGoal);

    setpoint_.header.stamp = ros::Time::now();
    setpoint_.pose.position.x = quadGoal.p.x;
    setpoint_.pose.position.y = quadGoal.p.y;
    setpoint_.pose.position.z = quadGoal.p.z;

    pub_setpoint_.publish(setpoint_);
  }
}

void MaderRos::clearMarkerArray(visualization_msgs::MarkerArray* tmp, ros::Publisher* publisher)
{
  if ((*tmp).markers.size() == 0)
  {
    return;
  }
  int id_begin = (*tmp).markers[0].id;

  for (int i = 0; i < (*tmp).markers.size(); i++)
  {
    visualization_msgs::Marker m;
    m.header.frame_id = "world";
    m.header.stamp = ros::Time::now();
    m.type = visualization_msgs::Marker::ARROW;
    m.action = visualization_msgs::Marker::DELETE;
    m.id = i + id_begin;
    m.scale.x = 0.15;
    m.scale.y = 0;
    m.scale.z = 0;
    (*tmp).markers[i] = m;
  }

  (*publisher).publish(*tmp);
  (*tmp).markers.clear();
}

void MaderRos::pubTraj(const std::vector<mt::state>& data)
{
  // Trajectory
  nav_msgs::Path traj;
  traj.poses.clear();
  traj.header.stamp = ros::Time::now();
  traj.header.frame_id = world_name_;

  geometry_msgs::PoseStamped temp_path;

  int increm = (int)std::max(data.size() / par_.res_plot_traj, 1.0);  // this is to speed up rviz

  for (int i = 0; i < data.size(); i = i + increm)
  {
    temp_path.pose.position.x = data[i].pos(0);
    temp_path.pose.position.y = data[i].pos(1);
    temp_path.pose.position.z = data[i].pos(2);
    temp_path.pose.orientation.w = 1;
    temp_path.pose.orientation.x = 0;
    temp_path.pose.orientation.y = 0;
    temp_path.pose.orientation.z = 0;
    traj.poses.push_back(temp_path);
  }

  pub_traj_safe_.publish(traj);
  clearMarkerArray(&traj_safe_colored_, &pub_traj_safe_colored_);

  double scale = 0.15;

  traj_safe_colored_ = mu::trajectory2ColoredMarkerArray(data, par_.v_max.maxCoeff(), increm, name_drone_, scale,
                                                         par_.color_type, id_, par_.n_agents);
  pub_traj_safe_colored_.publish(traj_safe_colored_);
}

void MaderRos::pubActualTraj()
{
  static geometry_msgs::Point p_last = mu::pointOrigin();

  mt::state current_state;
  mader_ptr_->getState(current_state);
  Eigen::Vector3d act_pos = current_state.pos;

  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::ADD;
  m.id = actual_trajID_;  // % 3000;  // Start the id again after ___ points published (if not RVIZ goes very slow)
  m.ns = "ActualTraj_" + name_drone_;
  actual_trajID_++;
  // m.color = mu::getColorJet(current_state.vel.norm(), 0, par_.v_max.maxCoeff());  // mu::color(mu::red_normal);

  if (par_.color_type == "vel")
  {
    m.color = mu::getColorJet(current_state.vel.norm(), 0, par_.v_max.maxCoeff());  // note that par_.v_max is per axis!
  }
  else
  {
    m.color = mu::getColorJet(id_, 0, par_.n_agents);  // note that par_.v_max is per axis!
  }

  m.scale.x = 0.15;
  m.scale.y = 0.0001;
  m.scale.z = 0.0001;
  m.header.stamp = ros::Time::now();
  m.header.frame_id = world_name_;

  // pose is actually not used in the marker, but if not RVIZ complains about the quaternion
  m.pose.position = mu::pointOrigin();
  m.pose.orientation.x = 0.0;
  m.pose.orientation.y = 0.0;
  m.pose.orientation.z = 0.0;
  m.pose.orientation.w = 1.0;

  geometry_msgs::Point p;
  p = mu::eigen2point(act_pos);
  m.points.push_back(p_last);
  m.points.push_back(p);
  p_last = p;

  if (m.id == 0)
  {
    return;
  }

  pub_actual_traj_.publish(m);
}

void MaderRos::clearMarkerActualTraj()
{
  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::DELETEALL;
  m.id = 0;
  m.scale.x = 0.02;
  m.scale.y = 0.04;
  m.scale.z = 1;
  pub_actual_traj_.publish(m);
  actual_trajID_ = 0;
}

void MaderRos::clearMarkerColoredTraj()
{
  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::DELETEALL;
  m.id = 0;
  m.scale.x = 1;
  m.scale.y = 1;
  m.scale.z = 1;
  pub_actual_traj_.publish(m);
}

void MaderRos::pubState(const mt::state& data, const ros::Publisher pub)
{
  geometry_msgs::PointStamped p;
  p.header.frame_id = world_name_;
  p.point = mu::eigen2point(data.pos);
  pub.publish(p);
}

void MaderRos::terminalGoalCB(const geometry_msgs::PoseStamped& msg)
{
  mt::state G_term;
  double z;
  if (fabs(msg.pose.position.z) < 1e-5)  // This happens when you click in RVIZ (msg.z is 0.0)
  {
    z = 1.0;
  }
  else  // This happens when you publish by yourself the goal (should always be above the ground)
  {
    z = msg.pose.position.z;
  }

  G_term.setPos(msg.pose.position.x, msg.pose.position.y, z);
  mader_ptr_->setTerminalGoal(G_term);

  mt::state G;  // projected goal
  mader_ptr_->getG(G);

  pubState(G_term, pub_point_G_term_);
  pubState(G, pub_point_G_);

  clearMarkerActualTraj();
}

void MaderRos::publishFOV()
{
  visualization_msgs::Marker marker_fov;
  marker_fov.header.frame_id = name_drone_;
  marker_fov.header.stamp = ros::Time::now();
  marker_fov.ns = "marker_fov";
  marker_fov.id = 0;
  marker_fov.type = marker_fov.LINE_LIST;
  marker_fov.action = marker_fov.ADD;
  marker_fov.pose = mu::identityGeometryMsgsPose();

  double delta_y = par_.fov_depth * fabs(tan((par_.fov_horiz_deg * M_PI / 180) / 2.0));
  double delta_z = par_.fov_depth * fabs(tan((par_.fov_vert_deg * M_PI / 180) / 2.0));

  geometry_msgs::Point v0 = mu::eigen2point(Eigen::Vector3d(0.0, 0.0, 0.0));
  geometry_msgs::Point v1 = mu::eigen2point(Eigen::Vector3d(par_.fov_depth, delta_y, -delta_z));
  geometry_msgs::Point v2 = mu::eigen2point(Eigen::Vector3d(par_.fov_depth, -delta_y, -delta_z));
  geometry_msgs::Point v3 = mu::eigen2point(Eigen::Vector3d(par_.fov_depth, -delta_y, delta_z));
  geometry_msgs::Point v4 = mu::eigen2point(Eigen::Vector3d(par_.fov_depth, delta_y, delta_z));

  marker_fov.points.clear();

  // Line
  marker_fov.points.push_back(v0);
  marker_fov.points.push_back(v1);

  // Line
  marker_fov.points.push_back(v0);
  marker_fov.points.push_back(v2);

  // Line
  marker_fov.points.push_back(v0);
  marker_fov.points.push_back(v3);

  // Line
  marker_fov.points.push_back(v0);
  marker_fov.points.push_back(v4);

  // Line
  marker_fov.points.push_back(v1);
  marker_fov.points.push_back(v2);

  // Line
  marker_fov.points.push_back(v2);
  marker_fov.points.push_back(v3);

  // Line
  marker_fov.points.push_back(v3);
  marker_fov.points.push_back(v4);

  // Line
  marker_fov.points.push_back(v4);
  marker_fov.points.push_back(v1);

  marker_fov.scale.x = 0.03;
  marker_fov.scale.y = 0.00001;
  marker_fov.scale.z = 0.00001;
  marker_fov.color.a = 1.0;
  marker_fov.color.r = 0.0;
  marker_fov.color.g = 1.0;
  marker_fov.color.b = 0.0;

  pub_fov_.publish(marker_fov);

  ////
  /// https://github.com/PickNikRobotics/rviz_visual_tools/blob/80212659be877f221cf23528b4e4887eaf0c08a4/src/rviz_visual_tools.cpp#L957

  return;
}

void MaderRos::verify(bool cond, std::string info_if_false)
{
  if (cond == false)
  {
    std::cout << termcolor::bold << termcolor::red << info_if_false << termcolor::reset << std::endl;
    std::cout << termcolor::red << "Aborting" << termcolor::reset << std::endl;
    abort();
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testCameraSet.cpp
 *  @brief  Unit tests for testCameraSet Class
 *  @author Frank Dellaert
 *  @date   Feb 19, 2015
 */

#include <gtsam/geometry/PinholeSet.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
#include <gtsam/geometry/CalibratedCamera.h>
TEST(PinholeSet, Stereo) {
  typedef vector<Point2> ZZ;
  PinholeSet<CalibratedCamera> set;
  CalibratedCamera camera;
  set.push_back(camera);
  set.push_back(camera);
  //  set.print("set: ");
  Point3 p(0, 0, 1);
  EXPECT_LONGS_EQUAL(6, traits<CalibratedCamera>::dimension);

  // Check measurements
  Point2 expected(0, 0);
  ZZ z = set.project2(p);
  EXPECT(assert_equal(expected, z[0]));
  EXPECT(assert_equal(expected, z[1]));

  // Calculate expected derivatives using Pinhole
  Matrix43 actualE;
  Matrix F1;
  {
    Matrix23 E1;
    camera.project2(p, F1, E1);
    actualE << E1, E1;
  }

  // Check computed derivatives
  PinholeSet<CalibratedCamera>::FBlocks Fs;
  Matrix E;
  set.project2(p, Fs, E);
  LONGS_EQUAL(2, Fs.size());
  EXPECT(assert_equal(F1, Fs[0]));
  EXPECT(assert_equal(F1, Fs[1]));
  EXPECT(assert_equal(actualE, E));

  // Instantiate triangulateSafe
  // TODO triangulation does not work yet for CalibratedCamera
  // PinholeSet<CalibratedCamera>::Result actual = set.triangulateSafe(z);
}

/* ************************************************************************* */
// Cal3Bundler test
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
TEST(PinholeSet, Pinhole) {
  typedef PinholeCamera<Cal3Bundler> Camera;
  typedef vector<Point2> ZZ;
  PinholeSet<Camera> set;
  Camera camera;
  set.push_back(camera);
  set.push_back(camera);
  Point3 p(0, 0, 1);
  EXPECT(assert_equal(set, set));
  PinholeSet<Camera> set2 = set;
  set2.push_back(camera);
  EXPECT(!set.equals(set2));

  // Check measurements
  Point2 expected(0,0);
  ZZ z = set.project2(p);
  EXPECT(assert_equal(expected, z[0]));
  EXPECT(assert_equal(expected, z[1]));

  // Calculate expected derivatives using Pinhole
  Matrix actualE;
  Matrix F1;
  {
    Matrix23 E1;
    camera.project2(p, F1, E1);
    actualE.resize(4, 3);
    actualE << E1, E1;
  }

  // Check computed derivatives
  {
    PinholeSet<Camera>::FBlocks Fs;
    Matrix E;
    set.project2(p, Fs, E);
    EXPECT(assert_equal(actualE, E));
    LONGS_EQUAL(2, Fs.size());
    EXPECT(assert_equal(F1, Fs[0]));
    EXPECT(assert_equal(F1, Fs[1]));
  }

  // Check errors
  ZZ measured;
  measured.push_back(Point2(1, 2));
  measured.push_back(Point2(3, 4));
  Vector4 expectedV;

  // reprojectionError
  expectedV << -1, -2, -3, -4;
  Vector actualV = set.reprojectionError(p, measured);
  EXPECT(assert_equal(expectedV, actualV));

  // reprojectionErrorAtInfinity
  Unit3 pointAtInfinity(0, 0, 1000);
  {
    Matrix22 E1;
    camera.project2(pointAtInfinity, F1, E1);
    actualE.resize(4, 2);
    actualE << E1, E1;
  }
  EXPECT(
      assert_equal(pointAtInfinity,
          camera.backprojectPointAtInfinity(Point2(0,0))));
  {
    PinholeSet<Camera>::FBlocks Fs;
    Matrix E;
    actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
    EXPECT(assert_equal(actualE, E));
    LONGS_EQUAL(2, Fs.size());
    EXPECT(assert_equal(F1, Fs[0]));
    EXPECT(assert_equal(F1, Fs[1]));
  }
  EXPECT(assert_equal(expectedV, actualV));

  // Instantiate triangulateSafe
  TriangulationParameters params;
  TriangulationResult actual = set.triangulateSafe(z, params);
  CHECK(actual.degenerate());
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */