/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>
#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CEllipsoid, CRenderizableDisplayList, mrpt::opengl)

/*---------------------------------------------------------------
							render
  ---------------------------------------------------------------*/
void CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
	MRPT_START

	const size_t dim = m_cov.cols();

	if (m_eigVal(0, 0) != 0.0 && m_eigVal(1, 1) != 0.0 &&
		(dim == 2 || m_eigVal(2, 2) != 0.0) && m_quantiles != 0.0)
	{
		glEnable(GL_BLEND);
		checkOpenGLError();
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
		checkOpenGLError();
		glLineWidth(m_lineWidth);
		checkOpenGLError();

		if (dim == 2)
		{
			glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			// ---------------------
			//     2D ellipse
			// ---------------------

			/* Equivalent MATLAB code:
			 *
			 * q=1;
			 * [vec val]=eig(C);
			 * M=(q*val*vec)';
			 * R=M*[x;y];
			 * xx=R(1,:);yy=R(2,:);
			 * plot(xx,yy), axis equal;
			 */

			double ang;
			unsigned int i;

			// Compute the new vectors for the ellipsoid:
			auto M = CMatrixDouble(m_eigVal.asEigen() * m_eigVec.transpose());
			M *= double(m_quantiles);

			glBegin(GL_LINE_LOOP);

			// Compute the points of the 2D ellipse:
			for (i = 0, ang = 0; i < m_2D_segments;
				 i++, ang += (M_2PI / m_2D_segments))
			{
				double ccos = cos(ang);
				double ssin = sin(ang);

				const float x = ccos * M(0, 0) + ssin * M(1, 0);
				const float y = ccos * M(0, 1) + ssin * M(1, 1);

				glVertex2f(x, y);
			}  // end for points on ellipse

			glEnd();

			// 2D: Save bounding box:
			const double max_radius =
				m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
			m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);

			glEnable(GL_LIGHTING);
		}
		else
		{
			// ---------------------
			//    3D ellipsoid
			// ---------------------
			GLfloat mat[16];

			//  A homogeneous transformation matrix, in this order:
			//
			//     0  4  8  12
			//     1  5  9  13
			//     2  6  10 14
			//     3  7  11 15
			//
			mat[3] = mat[7] = mat[11] = 0;
			mat[15] = 1;
			mat[12] = mat[13] = mat[14] = 0;

			mat[0] = m_eigVec(0, 0);
			mat[1] = m_eigVec(1, 0);
			mat[2] = m_eigVec(2, 0);  // New X-axis
			mat[4] = m_eigVec(0, 1);
			mat[5] = m_eigVec(1, 1);
			mat[6] = m_eigVec(2, 1);  // New X-axis
			mat[8] = m_eigVec(0, 2);
			mat[9] = m_eigVec(1, 2);
			mat[10] = m_eigVec(2, 2);  // New X-axis

			GLUquadricObj* obj = gluNewQuadric();
			checkOpenGLError();

			if (!m_drawSolid3D)
				glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			gluQuadricDrawStyle(obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);

			glPushMatrix();
			glMultMatrixf(mat);
			glScalef(
				m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
				m_eigVal(2, 2) * m_quantiles);

			gluSphere(obj, 1, m_3D_segments, m_3D_segments);
			checkOpenGLError();

			glPopMatrix();

			gluDeleteQuadric(obj);
			checkOpenGLError();

			// 3D: Save bounding box:
			const double max_radius =
				m_quantiles *
				std::max(
					m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
			m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);
		}

		glDisable(GL_BLEND);

		glEnable(GL_LIGHTING);
	}
	MRPT_END_WITH_CLEAN_UP(cout << "Covariance matrix leading to error is:"
								<< endl
								<< m_cov << endl;);
#endif
}

uint8_t CEllipsoid::serializeGetVersion() const { return 1; }
void CEllipsoid::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments
		<< (uint32_t)m_3D_segments << m_lineWidth;
}

void CEllipsoid::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			uint32_t i;
			readFromStreamRender(in);
			if (version == 0)
			{
				CMatrixF c;
				in >> c;
				m_cov = c.cast_double();
			}
			else
			{
				in >> m_cov;
			}

			in >> m_drawSolid3D >> m_quantiles;
			in >> i;
			m_2D_segments = i;
			in >> i;
			m_3D_segments = i;
			in >> m_lineWidth;

			// Update cov. matrix cache:
			setCovMatrix(m_cov);
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizableDisplayList::notifyChange();
}

bool quickSolveEqn(double a, double b_2, double c, double& t)
{
	double delta = square(b_2) - a * c;
	if (delta == 0)
		return (t = -b_2 / a) >= 0;
	else if (delta > 0)
	{
		delta = sqrt(delta);
		if ((t = (-b_2 - delta) / a) >= 0)
			return true;
		else
			return (t = (-b_2 + delta) / a) >= 0;
	}
	else
		return false;
}

bool CEllipsoid::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
	if (m_cov.rows() != 3) return false;
	TLine3D lin, lin2;
	createFromPoseX((o - this->m_pose).asTPose(), lin);
	lin.unitarize();  // By adding this line, distance from any point of the
	// line to its base is exactly equal to the "t".
	for (size_t i = 0; i < 3; i++)
	{
		lin2.pBase[i] = 0;
		lin2.director[i] = 0;
		for (size_t j = 0; j < 3; j++)
		{
			double vji = m_eigVec(j, i);
			lin2.pBase[i] += vji * lin.pBase[j];
			lin2.director[i] += vji * lin.director[j];
		}
	}
	double a = 0, b_2 = 0, c = -square(m_quantiles);
	for (size_t i = 0; i < 3; i++)
	{
		double ev = m_eigVal(i, i);
		a += square(lin2.director[i] / ev);
		b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
		c += square(lin2.pBase[i] / ev);
	}
	return quickSolveEqn(a, b_2, c, dist);
}

void CEllipsoid::setCovMatrix(
	const mrpt::math::CMatrixDouble& m, int resizeToSize)
{
	MRPT_START

	ASSERT_(m.cols() == m.rows());
	ASSERT_(
		m.rows() == 2 || m.rows() == 3 ||
		(resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));

	m_cov = m;
	if (resizeToSize > 0 && resizeToSize < (int)m.rows())
		m_cov.setSize(resizeToSize, resizeToSize);

	if (m_cov == m_prevComputedCov) return;  // Done.

	m_prevComputedCov = m_cov;

	CRenderizableDisplayList::notifyChange();

	// Handle the special case of an ellipsoid of volume = 0
	const double d = m_cov.det();
	if (d == 0 || d != d)  // Note: "d!=d" is a great test for invalid numbers,
	// don't remove!
	{
		// All zeros:
		m_eigVec.setZero(3, 3);
		m_eigVal.setZero(3, 3);
	}
	else
	{
		// Not null matrix: compute the eigen-vectors & values:
		std::vector<double> eigvals;
		if (m_cov.eig_symmetric(m_eigVec, eigvals))
		{
			// Do the scale at render to avoid recomputing the m_eigVal for
			// different m_quantiles
			m_eigVal.setDiagonal(eigvals);
			m_eigVal.array() = m_eigVal.array().sqrt().matrix();
		}
		else
		{
			m_eigVec.setZero(3, 3);
			m_eigVal.setZero(3, 3);
		}
	}

	MRPT_END
}

void CEllipsoid::setCovMatrix(
	const mrpt::math::CMatrixFloat& m, int resizeToSize)
{
	CRenderizableDisplayList::notifyChange();
	setCovMatrix(CMatrixDouble(m), resizeToSize);
}

/** Evaluates the bounding box of this object (including possible children) in
 * the coordinate frame of the object parent. */
void CEllipsoid::getBoundingBox(
	mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
	bb_min = m_bb_min;
	bb_max = m_bb_max;
}


#ifndef FLIGHTGOGGLESTRANSFORMS_H
#define FLIGHTGOGGLESTRANSFORMS_H

/**
 * @brief A set of functions that convert common right handed coordinate
 * systems used in robotics to the left hand system used by the FlightGoggles
 * backend (Unity 3D).
 *
 * @file transforms.hpp
 * @author Winter Guerra <winterg@mit.edu>
 * @date 2018-03-25
 */

////////////////////////////////
// TRANSFORMS
////////////////////////////////

// For transforms
#include <Eigen/Geometry>
#include <Eigen/Dense>

#define PRECISION 10
#define DONTALIGNCOLS 1

// define some eigen types and formats
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 7, 1> Vector7d;
typedef Eigen::Quaternion<double> Quaternionx;
typedef Eigen::Affine3d Transform3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix3d Matrix3;
Eigen::IOFormat CSV(PRECISION, DONTALIGNCOLS, ",", ",", "", "", "", "");

/**
 * @brief Converts right hand rule North East Down (NED) global poses to
 * left handed coordinates (as used by the Unity3D backend of FlightGoggles).
 *
 * @param NEDworld_T_object
 * @return Transform3
 */
Transform3 convertNEDGlobalPoseToGlobalUnityCoordinates(
    Transform3 NEDworld_T_object)
{
    // Switch axis to unity axis.
    Matrix4 unity_pose_T_NED_pose;
    // x->z, y->x, z->-y
    // clang-format off
    unity_pose_T_NED_pose <<    0, 1, 0,  0,
                                0, 0, -1, 0,
                                1, 0, 0,  0,
                                0, 0, 0,  1;
    // clang-format on

    Transform3 unity_pose;

    unity_pose.matrix() = unity_pose_T_NED_pose *
                          NEDworld_T_object.matrix() *
                          unity_pose_T_NED_pose.transpose();

    return unity_pose;
}

/**
 * @brief Converts right hand rule North East Down (NED) poses to left handed
 * rule coordinates (as used by Unity3D). Takes in an additional parameter
 * for the translational and rotational offset between the FlightGoggles world
 * and the global NED poses used by the robot.
 *
 * @deprecated - will be removed soon.
 *
 * @param NEDworld_T_object
 * @param unityWorld_T_NEDworld
 * @return Transform3
 */
Transform3 convertNEDGlobalPoseToGlobalUnityCoordinates(
    Transform3 NEDworld_T_object, Transform3 unityWorld_T_NEDworld)
{
    // Switch axis to unity axis.
    Matrix4 unity_pose_T_NED_pose;
    // x->z, y->x, z->-y
    // clang-format off
    unity_pose_T_NED_pose <<    0, 1, 0,  0,
                                0, 0, -1, 0,
                                1, 0, 0,  0,
                                0, 0, 0,  1;
    // clang-format on

    Transform3 unity_pose;

    unity_pose.matrix() = unity_pose_T_NED_pose * unityWorld_T_NEDworld.matrix() *
                          NEDworld_T_object.matrix() *
                          unity_pose_T_NED_pose.transpose();

    return unity_pose;
}

/**
 * @brief Converts ROS global poses to NED poses. These poses are then converted to
 * Unity left handed coordinates. Assumes that ROS global coordinates for translation are
 * East north up (ENU) and robot local coordinates are North west up (NWU). E.g. the robot
 * local X axis is going out of the camera.
 *
 * @param ENUworld_T_object
 * @return Transform3
 */
Transform3 convertROSToNEDCoordinates(Transform3 ENUworld_T_object)
{
    // Switch ENU axis to NED axis.
    Matrix4 NED_T_ENU;
    // x->y, y->x, z->-z, w->w
    // clang-format off
    NED_T_ENU <<  1, 0, 0,  0,
                    0, -1, 0,  0,
                     0, 0, -1, 0,
                     0, 0, 0,  1;
    // clang-format on

    Transform3 NED_pose;
    NED_pose.matrix() = NED_T_ENU * ENUworld_T_object.matrix() * NED_T_ENU.transpose();

    // Rotate robot pose by -90deg about robot z axis since ROS
    // expects that "X" is forward in robot frame.
    // Quaternionx Y_front_to_X_front_quat(sqrt(0.5f),	0.0f, 0.0f, -sqrt(0.5f));

    // NED_pose = Y_front_to_X_front_quat * NED_pose;

    return NED_pose;
}

/**
 * @brief Converts ROS global poses to NED poses. These poses are then converted to
 * Unity left handed coordinates. Assumes that ROS global coordinates for translation are
 * East north up (ENU) and robot local coordinates are North west up (NWU). E.g. the robot
 * local X axis is going out of the camera.
 *
 * @param ENUworld_T_object
 * @return Transform3
 */
Transform3 convertEDNToNEDCoordinates(Transform3 EDNworld_T_object)
{
    // Switch ENU axis to NED axis.
    Matrix4 NED_T_EDN;
    // x->y, y->x, z->-z, w->w
    // clang-format off
    NED_T_EDN <<  0, 1, 0,  0,
                  1, 0, 0,  0,
                  0, 1, 0, 0,
                  0, 0, 0,  1;
    // clang-format on

    Transform3 NED_pose;
    NED_pose.matrix() = NED_T_EDN * EDNworld_T_object.matrix(); //EDN_pose_T_NED_pose * EDNworld_T_object.matrix() * EDN_pose_T_NED_pose.transpose();

    // Rotate robot pose by -90deg about robot z axis since ROS
    // expects that "X" is forward in robot frame.
    // Quaternionx Y_front_to_X_front_quat(sqrt(0.5f),	0.0f, 0.0f, -sqrt(0.5f));

    // NED_pose = Y_front_to_X_front_quat * NED_pose;

    return NED_pose;
}

Transform3 convertEDNGlobalPoseToGlobalUnityCoordinates(
        Transform3 EDNworld_T_object)
{
    // Switch axis to unity axis.
    Matrix4 ENU_T_NED;
    // x->z, y->x, z->-y
    // clang-format off
    ENU_T_NED <<    1, 0,  0,  0,
                    0, -1, 0, 0,
                    0, 0,  1,  0,
                    0, 0,  0,  1;
    // clang-format on

    Transform3 unity_pose;

    unity_pose.matrix() = ENU_T_NED *
                          EDNworld_T_object.matrix() *
                          ENU_T_NED.transpose();

    return unity_pose;
}

// Converts a given LCM drone pose and relative camera pose into a global Unity
// pose for the camera.
/* Move world_T_cam from drone (right handed) coordinate system to Unity (left
handed) coordinate system:
Input vector in unity coordinates goes through the following
transformations:
Unity coordinates -> Drone coordinates -> perform rotation -> unity
coordinates
*/
Transform3 convertCameraAndDronePoseToUnityCoordinates(
    Transform3 world_T_body, Transform3 body_T_cam,
    Transform3 unityWorld_T_NEDworld)
{

    Transform3 world_T_cam = world_T_body * body_T_cam;
    Matrix4 cam_T_unitycam;
    // clang-format off
    cam_T_unitycam <<   0, 1, 0, 0,
                        0, 0, 1, 0,
                        1, 0, 0, 0,
                        0, 0, 0, 1;
    // clang-format on

    Transform3 world_T_unitycam;
    world_T_unitycam.matrix() =
        world_T_cam *
        cam_T_unitycam; // Move from z aligned camera to x aligned camera

    // x->z, y->x, z->-y
    Matrix4
        unity_from_drone; /**< Coordinate change from drone coordinate system to
                            unity coordinate system */
    // clang-format off
    unity_from_drone << 0, 1,  0, 0,
                        0, 0, -1, 0,
                        1, 0,  0, 0,
                        0, 0,  0, 1;
    // clang-format on

    Transform3 unityWorld_T_unitycam_unity; /**< Transformation from camera to
                                                world in unity coordinates */
    unityWorld_T_unitycam_unity.matrix() =
        unity_from_drone * unityWorld_T_NEDworld.matrix() *
        world_T_unitycam.matrix() * unity_from_drone.transpose();

    return unityWorld_T_unitycam_unity;
}

#endif


#include "software/geom/util.h"

#include <algorithm>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <cassert>
#include <cmath>
#include <iostream>
#include <limits>
#include <tuple>

#include "software/logger/logger.h"
#include "software/new_geom/angle.h"
#include "software/new_geom/rectangle.h"
#include "software/new_geom/segment.h"
#include "software/new_geom/triangle.h"
#include "software/new_geom/util/collinear.h"
#include "software/new_geom/util/distance.h"
#include "software/new_geom/util/intersection.h"
#include "software/new_geom/util/intersects.h"

bool isDegenerate(const Segment &segment)
{
    return distanceSquared(segment.getSegStart(), segment.getEnd()) < EPS2;
}

double length(const Segment &segment)
{
    return distance(segment.getSegStart(), segment.getEnd());
}

double lengthSquared(const Segment &segment)
{
    return distanceSquared(segment.getSegStart(), segment.getEnd());
}

bool collinear(const Segment &segment1, const Segment &segment2)
{
    // Two segments are collinear if all Points are collinear
    if (collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getSegStart()) &&
        collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getEnd()))
    {
        return true;
    }
    return false;
}

std::vector<Point> lineCircleIntersect(const Point &centre, double radius,
                                       const Point &segA, const Point &segB)
{
    std::vector<Point> ans;

    // take care of 0 length segments too much error here
    if ((segB - segA).lengthSquared() < EPS)
    {
        return ans;
    }

    double lenseg = (segB - segA).dot(centre - segA) / (segB - segA).length();
    Point C       = segA + lenseg * (segB - segA).normalize();

    // if C outside circle no intersections
    if ((C - centre).lengthSquared() > radius * radius + EPS)
    {
        return ans;
    }

    // if C on circle perimeter return the only intersection
    if ((C - centre).lengthSquared() < radius * radius + EPS &&
        (C - centre).lengthSquared() > radius * radius - EPS)
    {
        ans.push_back(C);
        return ans;
    }
    // first possible intersection
    double lensegb = radius * radius - (C - centre).lengthSquared();

    ans.push_back(C - (lensegb * (segB - segA).normalize()));
    ans.push_back(C + lensegb * (segB - segA).normalize());

    return ans;
}

Point closestPointOnSeg(const Point &p, const Segment &segment)
{
    return closestPointOnSeg(p, segment.getSegStart(), segment.getEnd());
}
Point closestPointOnSeg(const Point &centre, const Point &segA, const Point &segB)
{
    // if one of the end-points is extremely close to the centre point
    // then return 0.0
    if ((segB - centre).lengthSquared() < EPS2)
    {
        return segB;
    }

    if ((segA - centre).lengthSquared() < EPS2)
    {
        return segA;
    }

    // take care of 0 length segments
    if ((segB - segA).lengthSquared() < EPS2)
    {
        return segA;
    }

    // find point C
    // which is the projection onto the line
    double lenseg = (segB - segA).dot(centre - segA) / (segB - segA).length();
    Point C       = segA + lenseg * (segB - segA).normalize();

    // check if C is in the line seg range
    double AC     = (segA - C).lengthSquared();
    double BC     = (segB - C).lengthSquared();
    double AB     = (segA - segB).lengthSquared();
    bool in_range = AC <= AB && BC <= AB;

    // if so return C
    if (in_range)
    {
        return C;
    }
    double lenA = (centre - segA).length();
    double lenB = (centre - segB).length();

    // otherwise return closest end of line-seg
    if (lenA < lenB)
    {
        return segA;
    }
    return segB;
}

bool uniqueLineIntersects(const Point &a, const Point &b, const Point &c, const Point &d)
{
    return std::abs((d - c).cross(b - a)) > EPS;
}

Point calcBlockCone(const Vector &a, const Vector &b, const double &radius)
{
    if (a.length() < EPS || b.length() < EPS)
    {
    }
    // unit vector and bisector
    Vector au = a / a.length();
    Vector c  = au + b / b.length();
    // use similar triangle
    return Point(c * (radius / std::fabs(au.cross(c))));
}

Point calcBlockCone(const Point &a, const Point &b, const Point &p, const double &radius)
{
    return p + (calcBlockCone(a - p, b - p, radius)).toVector();
}

double offsetToLine(Point x0, Point x1, Point p)
{
    Vector n;

    // get normal to line
    n = (x1 - x0).perpendicular().normalize();

    return fabs(n.dot(p - x0));
}

Angle acuteVertexAngle(Vector v1, Vector v2)
{
    return v1.orientation().minDiff(v2.orientation());
}

Angle acuteVertexAngle(Point p1, Point p2, Point p3)
{
    return acuteVertexAngle(p1 - p2, p3 - p2);
}

bool pointInFrontVector(Point offset, Vector direction, Point p)
{
    // compare angle different
    Angle a1   = direction.orientation();
    Angle a2   = (p - offset).orientation();
    Angle diff = (a1 - a2).clamp();
    return diff < Angle::quarter() && diff > -Angle::quarter();
}

std::pair<Point, Point> getCircleTangentPoints(const Point &start, const Circle &circle,
                                               double buffer)
{
    // If the point is already inside the circe arccos won't work so just return
    // the perp points
    if (circle.contains(start))
    {
        double perpDist = std::sqrt(circle.getRadius() * circle.getRadius() -
                                    (circle.getOrigin() - start).lengthSquared());
        Point p1 =
            start +
            (circle.getOrigin() - start).perpendicular().normalize(perpDist + buffer);
        Point p2 =
            start -
            ((circle.getOrigin() - start).perpendicular().normalize(perpDist + buffer));
        return std::make_pair(p1, p2);
    }
    else
    {
        double radiusAngle =
            std::acos(circle.getRadius() / (start - circle.getOrigin()).length());
        Point p1 = circle.getOrigin() + (start - circle.getOrigin())
                                            .rotate(Angle::fromRadians(radiusAngle))
                                            .normalize(circle.getRadius() + buffer);
        Point p2 = circle.getOrigin() + (start - circle.getOrigin())
                                            .rotate(-Angle::fromRadians(radiusAngle))
                                            .normalize(circle.getRadius() + buffer);
        return std::make_pair(p1, p2);
    }
}

std::pair<Ray, Ray> getCircleTangentRaysWithReferenceOrigin(const Point reference,
                                                            const Circle circle)
{
    auto [tangent_point1, tangent_point2] = getCircleTangentPoints(reference, circle, 0);

    return std::make_pair(Ray(reference, (tangent_point1 - reference)),
                          Ray(reference, (tangent_point2 - reference)));
}

Point getPointsMean(const std::vector<Point> &points)
{
    Point average = Point(0, 0);
    for (unsigned int i = 0; i < points.size(); i++)
    {
        average += points[i].toVector();
    }

    Vector averageVector = average.toVector();

    averageVector /= static_cast<double>(points.size());
    return Point(averageVector);
}

std::optional<Segment> segmentEnclosedBetweenRays(Segment segment, Ray ray1, Ray ray2)
{
    // Create rays located at the extremes of the segment, that point in the direction
    // outwards are parallel to the segment
    const Ray extremes1 =
        Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
    const Ray extremes2 =
        Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));

    const std::optional<Point> extreme_intersect11 = intersection(extremes1, ray1);
    const std::optional<Point> extreme_intersect12 = intersection(extremes2, ray1);
    const std::optional<Point> extreme_intersect21 = intersection(extremes1, ray2);
    const std::optional<Point> extreme_intersect22 = intersection(extremes2, ray2);

    // Check for the cases that the rays intersect the same segment projection
    if ((extreme_intersect11.has_value() && extreme_intersect21.has_value()) ||
        (extreme_intersect12.has_value() && extreme_intersect22.has_value()))
    {
        return std::nullopt;
    }
    else
    {
        // Since we know that both rays aren't passing through the same side of the
        // segment at this point, then as long as they both only intersect 1 point the
        // segment must be enclosed between them
        if ((extreme_intersect11.has_value() != extreme_intersect12.has_value()) &&
            (extreme_intersect21.has_value() != extreme_intersect22.has_value()))
        {
            return std::make_optional(segment);
        }
        // Covers the case where a single ray passes by both sides of the segment
        else
        {
            return std::nullopt;
        }
    }
}
std::optional<Segment> getIntersectingSegment(Ray ray1, Ray ray2, Segment segment)
{
    // Check if the segment is enclosed between the rays
    if (segmentEnclosedBetweenRays(segment, ray1, ray2))
    {
        return segment;
    }

    // Calculate intersections of each individual ray and the segment
    std::vector<Point> intersection1 = intersection(ray1, segment);
    std::vector<Point> intersection2 = intersection(ray2, segment);

    std::optional<Point> intersect11;
    std::optional<Point> intersect12;
    std::optional<Point> intersect21;
    std::optional<Point> intersect22;

    if (!intersection1.empty())
    {
        intersect11 = intersection1[0];

        if (intersection1.size() > 1)
        {
            intersect12 = intersection1[1];
        }
    }

    if (!intersection2.empty())
    {
        intersect21 = intersection2[0];

        if (intersection2.size() > 1)
        {
            intersect22 = intersection2[1];
        }
    }

    // Check if there are any real intersections
    if (!intersect11.has_value() && !intersect21.has_value())
    {
        return std::nullopt;
    }
    // Check if one of the rays is overlapping the segment. If this is the case, return
    // the segment (If a ray intersects a ray more than one time it must be overlapping)
    else if ((intersect11.has_value() && intersect12.has_value()) ||
             (intersect21.has_value() && intersect22.has_value()))
    {
        return segment;
    }
    // If there is only one intersection point for each ray combine the intersections into
    // a segment
    else if ((intersect11.has_value() && !intersect12.has_value()) &&
             (intersect21.has_value() && !intersect22.has_value()))
    {
        return std::make_optional(Segment(intersect11.value(), intersect21.value()));
    }
    // If only one ray intersects the segment return the segment between the intersection
    // and the segment extreme Point (intersection11 is real, intersection22 is not)
    else if (intersect11.has_value() && !intersect21.has_value())
    {
        const Ray extremes1 =
            Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
        const Ray extremes2 =
            Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));
        ;

        std::optional<Point> extreme_intersect1 = intersection(extremes1, ray2);
        std::optional<Point> extreme_intersect2 = intersection(extremes2, ray2);

        if (extreme_intersect1.has_value())
        {
            return std::make_optional(Segment(intersect11.value(), segment.getEnd()));
        }
        else if (extreme_intersect2.has_value())
        {
            return std::make_optional(
                Segment(intersect11.value(), segment.getSegStart()));
        }
    }
    // If only one ray intersects the segment return the segment between the intersection
    // and the segment extreme (intersection11 is real, intersection22 is not)
    else if (intersect21.has_value() && !intersect11.has_value())
    {
        const Ray extremes1 =
            Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
        const Ray extremes2 =
            Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));
        ;

        std::optional<Point> extreme_intersect1 = intersection(extremes1, ray1);
        std::optional<Point> extreme_intersect2 = intersection(extremes2, ray1);

        if (extreme_intersect1.has_value())
        {
            return std::make_optional(Segment(intersect21.value(), segment.getEnd()));
        }
        else if (extreme_intersect2.has_value())
        {
            return std::make_optional(
                Segment(intersect21.value(), segment.getSegStart()));
        }
    }
    // All cases have been checked, return std::nullopt
    return std::nullopt;
}

std::optional<Segment> mergeOverlappingParallelSegments(Segment segment1,
                                                        Segment segment2)
{
    std::optional<Segment> redundant_segment =
        mergeFullyOverlappingSegments(segment1, segment2);

    // If the segments are not parallel, then return std::nullopt. (The segments are
    // parallel of all points are collinear)
    if (!collinear(segment1, segment2))
    {
        return std::nullopt;
    }
    // Check the case where one segment is completely contained in the other
    else if (redundant_segment.has_value())
    {
        return redundant_segment;
    }
    // Check if the beginning of segment2 lays inside segment1
    else if (segment1.contains(segment2.getSegStart()))
    {
        // If segment2.getSegStart() lays in segment1, then the combined segment is
        // segment2,getEnd() and the point furthest from segment2.getEnd()
        return (segment1.getSegStart() - segment2.getEnd()).lengthSquared() >
                       (segment1.getEnd() - segment2.getEnd()).lengthSquared()
                   ? Segment(segment1.getSegStart(), segment2.getEnd())
                   : Segment(segment1.getEnd(), segment2.getEnd());
    }
    // Now check if the end of segment2 lays inside segment1
    else if (segment1.contains(segment2.getEnd()))
    {
        // If segment2.getSegStart() lays in segment1, then the combined segment is
        // segment2,getEnd() and the point furtherst from segmen2.getEnd()
        return (segment1.getSegStart() - segment2.getSegStart()).lengthSquared() >
                       (segment1.getEnd() - segment2.getSegStart()).lengthSquared()
                   ? Segment(segment1.getSegStart(), segment2.getSegStart())
                   : Segment(segment1.getEnd(), segment2.getSegStart());
    }
    return std::nullopt;
}

std::optional<Segment> mergeFullyOverlappingSegments(Segment segment1, Segment segment2)
{
    // If the segments are not parallel, then return std::nullopt. (The segments are
    // parallel if all points are collinear)
    if (!collinear(segment1, segment2))
    {
        return std::nullopt;
    }

    Segment largest_segment, smallest_segment;
    // Grab the largest segment
    if (segment1.toVector().lengthSquared() > segment2.toVector().lengthSquared())
    {
        largest_segment  = segment1;
        smallest_segment = segment2;
    }
    else
    {
        largest_segment  = segment2;
        smallest_segment = segment1;
    }

    // The segment is redundant if both points of the smallest segment are contained in
    // the largest segment
    if (largest_segment.contains(smallest_segment.getSegStart()) &&
        largest_segment.contains(smallest_segment.getEnd()))
    {
        return std::make_optional(largest_segment);
    }
    else
    {
        return std::nullopt;
    }
}

std::vector<Segment> getEmptySpaceWithinParentSegment(std::vector<Segment> segments,
                                                      Segment parent_segment)
{
    // Make sure the starting point of all segments is closer to the start of the
    // reference segment to simplify the evaluation
    for (auto &unordered_seg : segments)
    {
        if ((parent_segment.getSegStart() - unordered_seg.getSegStart()).length() >
            (parent_segment.getSegStart() - unordered_seg.getEnd()).length())
        {
            // We need to flip the start/end of the segment
            Segment temp = unordered_seg;
            unordered_seg.setSegStart(temp.getEnd());
            unordered_seg.setEnd(temp.getSegStart());
        }
    }

    // Now we must sort the segments so that we can iterate through them in order to
    // generate open angles sort using a lambda expression
    // We sort the segments based on how close their 'start' point is to the 'start'
    // of the reference Segment
    std::sort(segments.begin(), segments.end(), [parent_segment](Segment &a, Segment &b) {
        return (parent_segment.getSegStart() - a.getSegStart()).length() <
               (parent_segment.getSegStart() - b.getSegStart()).length();
    });

    // Now we need to find the largest open segment/angle
    std::vector<Segment> open_segs;

    // The first Angle is between the reference Segment and the first obstacle Segment
    // After this one, ever open angle is between segment(i).end and
    // segment(i+1).start
    open_segs.push_back(
        Segment(parent_segment.getSegStart(), segments.front().getSegStart()));

    // The 'open' Segment in the space between consecutive 'blocking' Segments
    for (std::vector<Segment>::const_iterator it = segments.begin();
         it != segments.end() - 1; it++)
    {
        open_segs.push_back(Segment(it->getEnd(), (it + 1)->getSegStart()));
    }

    // Lastly, the final open angle is between obstacles.end().getEnd() and
    // reference_segment.getEnd()
    open_segs.push_back(Segment(segments.back().getEnd(), parent_segment.getEnd()));

    // Remove all zero length open Segments
    for (std::vector<Segment>::const_iterator it = open_segs.begin();
         it != open_segs.end();)
    {
        if (it->length() < EPS)
        {
            open_segs.erase(it);
        }
        else
        {
            it++;
        }
    }

    return open_segs;
}

std::vector<Segment> projectCirclesOntoSegment(Segment segment,
                                               std::vector<Circle> circles, Point origin)
{
    // Loop through all obstacles to create their projected Segment
    std::vector<Segment> obstacle_segment_projections = {};

    for (Circle circle : circles)
    {
        // If the reference is inside an obstacle there is no open direction
        if (circle.contains(origin))
        {
            obstacle_segment_projections.push_back(segment);
            return obstacle_segment_projections;
        }

        // Get the tangent rays from the reference point to the obstacle
        auto [ray1, ray2] = getCircleTangentRaysWithReferenceOrigin(origin, circle);

        // Project the tangent Rays to obtain a 'blocked' segment on the reference
        // Segment
        std::optional<Segment> intersect_segment =
            getIntersectingSegment(ray1, ray2, segment);

        if (intersect_segment.has_value())
        {
            obstacle_segment_projections.push_back(intersect_segment.value());
        }
    }
    return obstacle_segment_projections;
}

std::vector<Segment> combineToParallelSegments(std::vector<Segment> segments,
                                               Vector direction)
{
    std::vector<Segment> projected_segments = {};

    // Project all Segments onto the direction Vector
    for (Segment segment : segments)
    {
        // The projection of the Segment without including the original Segment location
        Vector raw_projection = segment.toVector().project(direction);

        // Only count projections that have a non-zero magnitude
        if (raw_projection.lengthSquared() > EPS)
        {
            projected_segments.push_back(
                Segment(segment.getSegStart(), segment.getSegStart() + raw_projection));
        }
    }
    std::vector<Segment> unique_segments;

    unsigned int j = 0;
    // Loop through all segments and combine segments
    // to reduce the vector to the smallest number of independent (not overlapping)
    // segments
    while (projected_segments.size() > 0)
    {
        std::optional<Segment> temp_segment;
        unique_segments.push_back(projected_segments[0]);
        projected_segments.erase(projected_segments.begin());

        for (unsigned int i = 0; i < projected_segments.size(); i++)
        {
            temp_segment = mergeOverlappingParallelSegments(unique_segments[j],
                                                            projected_segments[i]);

            if (temp_segment.has_value())
            {
                unique_segments[j] = temp_segment.value();
                // Remove segments[i] from the list as it is not unique
                projected_segments.erase(projected_segments.begin() + i);
                i--;
            }
        }
        j++;
    }

    return unique_segments;
}


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

// These are the headers included by the Boost.TR1 implementation,
// by including these directly we probe for problems with cyclic
// dependencies when the TR1 headers are in the include search path.

#ifdef TEST_STD_HEADERS
#include <cmath>
#else
#include <boost/tr1/cmath.hpp>
#endif

#include "verify_return.hpp"

int main(int argc, const char* [])
{
   if(argc > 500)
   {
      float f = 0;
      double d = 0;
      long double ld = 0;
      unsigned ui = 0;
      int i = 0;
      long int li = 0;
      bool b = false;
      const char* nan_str = "";
#ifdef BOOST_HAS_LONG_LONG
      long long lli = 0;
#endif
      verify_return_type((std::tr1::acosh)(d), d);
      verify_return_type((std::tr1::acosh)(f), f);
      verify_return_type((std::tr1::acosh)(ld), ld);
      verify_return_type((std::tr1::acoshf)(f), f);
      verify_return_type((std::tr1::acoshl)(ld), ld);
      verify_return_type((std::tr1::asinh)(d), d);
      verify_return_type((std::tr1::asinh)(f), f);
      verify_return_type((std::tr1::asinh)(ld), ld);
      verify_return_type((std::tr1::asinhf)(f), f);
      verify_return_type((std::tr1::asinhl)(ld), ld);
      verify_return_type((std::tr1::atanh)(d), d);
      verify_return_type((std::tr1::atanh)(f), f);
      verify_return_type((std::tr1::atanh)(ld), ld);
      verify_return_type((std::tr1::atanhf)(f), f);
      verify_return_type((std::tr1::atanhl)(ld), ld);
      verify_return_type((std::tr1::cbrt)(d), d);
      verify_return_type((std::tr1::cbrt)(f), f);
      verify_return_type((std::tr1::cbrt)(ld), ld);
      verify_return_type((std::tr1::cbrtf)(f), f);
      verify_return_type((std::tr1::cbrtl)(ld), ld);
      verify_return_type((std::tr1::copysign)(d, d), d);
      verify_return_type((std::tr1::copysign)(f, f), f);
      verify_return_type((std::tr1::copysign)(ld, ld), ld);
      verify_return_type((std::tr1::copysignf)(f, f), f);
      verify_return_type((std::tr1::copysignl)(ld, ld), ld);
      verify_return_type((std::tr1::erf)(d), d);
      verify_return_type((std::tr1::erf)(f), f);
      verify_return_type((std::tr1::erf)(ld), ld);
      verify_return_type((std::tr1::erff)(f), f);
      verify_return_type((std::tr1::erfl)(ld), ld);
      verify_return_type((std::tr1::erfc)(d), d);
      verify_return_type((std::tr1::erfc)(f), f);
      verify_return_type((std::tr1::erfc)(ld), ld);
      verify_return_type((std::tr1::erfcf)(f), f);
      verify_return_type((std::tr1::erfcl)(ld), ld);
#if 0
      verify_return_type((std::tr1::exp2)(d), d);
      verify_return_type((std::tr1::exp2)(f), f);
      verify_return_type((std::tr1::exp2)(ld), ld);
      verify_return_type((std::tr1::exp2f)(f), f);
      verify_return_type((std::tr1::exp2l)(ld), ld);
#endif
      verify_return_type((std::tr1::expm1)(d), d);
      verify_return_type((std::tr1::expm1)(f), f);
      verify_return_type((std::tr1::expm1)(ld), ld);
      verify_return_type((std::tr1::expm1f)(f), f);
      verify_return_type((std::tr1::expm1l)(ld), ld);
#if 0
      verify_return_type((std::tr1::fdim)(d, d), d);
      verify_return_type((std::tr1::fdim)(f, f), f);
      verify_return_type((std::tr1::fdim)(ld, ld), ld);
      verify_return_type((std::tr1::fdimf)(f, f), f);
      verify_return_type((std::tr1::fdiml)(ld, ld), ld);
      verify_return_type((std::tr1::fma)(d, d, d), d);
      verify_return_type((std::tr1::fma)(f, f, f), f);
      verify_return_type((std::tr1::fma)(ld, ld, ld), ld);
      verify_return_type((std::tr1::fmaf)(f, f, f), f);
      verify_return_type((std::tr1::fmal)(ld, ld, ld), ld);
#endif
      verify_return_type((std::tr1::fmax)(d, d), d);
      verify_return_type((std::tr1::fmax)(f, f), f);
      verify_return_type((std::tr1::fmax)(ld, ld), ld);
      verify_return_type((std::tr1::fmaxf)(f, f), f);
      verify_return_type((std::tr1::fmaxl)(ld, ld), ld);
      verify_return_type((std::tr1::fmin)(d, d), d);
      verify_return_type((std::tr1::fmin)(f, f), f);
      verify_return_type((std::tr1::fmin)(ld, ld), ld);
      verify_return_type((std::tr1::fminf)(f, f), f);
      verify_return_type((std::tr1::fminl)(ld, ld), ld);
      verify_return_type((std::tr1::hypot)(d, d), d);
      verify_return_type((std::tr1::hypot)(f, f), f);
      verify_return_type((std::tr1::hypot)(ld, ld), ld);
      verify_return_type((std::tr1::hypotf)(f, f), f);
      verify_return_type((std::tr1::hypotl)(ld, ld), ld);
#if 0
      verify_return_type((std::tr1::ilogb)(d), d);
      verify_return_type((std::tr1::ilogb)(f), f);
      verify_return_type((std::tr1::ilogb)(ld), ld);
      verify_return_type((std::tr1::ilogbf)(f), f);
      verify_return_type((std::tr1::ilogbl)(ld), ld);
#endif
      verify_return_type((std::tr1::lgamma)(d), d);
      verify_return_type((std::tr1::lgamma)(f), f);
      verify_return_type((std::tr1::lgamma)(ld), ld);
      verify_return_type((std::tr1::lgammaf)(f), f);
      verify_return_type((std::tr1::lgammal)(ld), ld);
#if 0
      verify_return_type((std::tr1::llrint)(d), d);
      verify_return_type((std::tr1::llrint)(f), f);
      verify_return_type((std::tr1::llrint)(ld), ld);
      verify_return_type((std::tr1::llrintf)(f), f);
      verify_return_type((std::tr1::llrintl)(ld), ld);
#endif
#ifdef BOOST_HAS_LONG_LONG
      verify_return_type((std::tr1::llround)(d), lli);
      verify_return_type((std::tr1::llround)(f), lli);
      verify_return_type((std::tr1::llround)(ld), lli);
      verify_return_type((std::tr1::llroundf)(f), lli);
      verify_return_type((std::tr1::llroundl)(ld), lli);
#endif
      verify_return_type((std::tr1::log1p)(d), d);
      verify_return_type((std::tr1::log1p)(f), f);
      verify_return_type((std::tr1::log1p)(ld), ld);
      verify_return_type((std::tr1::log1pf)(f), f);
      verify_return_type((std::tr1::log1pl)(ld), ld);
#if 0
      verify_return_type((std::tr1::log2)(d), d);
      verify_return_type((std::tr1::log2)(f), f);
      verify_return_type((std::tr1::log2)(ld), ld);
      verify_return_type((std::tr1::log2f)(f), f);
      verify_return_type((std::tr1::log2l)(ld), ld);
      verify_return_type((std::tr1::logb)(d), d);
      verify_return_type((std::tr1::logb)(f), f);
      verify_return_type((std::tr1::logb)(ld), ld);
      verify_return_type((std::tr1::logbf)(f), f);
      verify_return_type((std::tr1::logbl)(ld), ld);
      verify_return_type((std::tr1::lrint)(d), d);
      verify_return_type((std::tr1::lrint)(f), f);
      verify_return_type((std::tr1::lrint)(ld), ld);
      verify_return_type((std::tr1::lrintf)(f), f);
      verify_return_type((std::tr1::lrintl)(ld), ld);
#endif
      verify_return_type((std::tr1::lround)(d), li);
      verify_return_type((std::tr1::lround)(f), li);
      verify_return_type((std::tr1::lround)(ld), li);
      verify_return_type((std::tr1::lroundf)(f), li);
      verify_return_type((std::tr1::lroundl)(ld), li);
#if 0
      verify_return_type((std::tr1::nan)(nan_str), d);
      verify_return_type((std::tr1::nanf)(nan_str), f);
      verify_return_type((std::tr1::nanl)(nan_str), ld);
      verify_return_type((std::tr1::nearbyint)(d), d);
      verify_return_type((std::tr1::nearbyint)(f), f);
      verify_return_type((std::tr1::nearbyint)(ld), ld);
      verify_return_type((std::tr1::nearbyintf)(f), f);
      verify_return_type((std::tr1::nearbyintl)(ld), ld);
#endif
      verify_return_type((std::tr1::nextafter)(d, d), d);
      verify_return_type((std::tr1::nextafter)(f, f), f);
      verify_return_type((std::tr1::nextafter)(ld, ld), ld);
      verify_return_type((std::tr1::nextafterf)(f, f), f);
      verify_return_type((std::tr1::nextafterl)(ld, ld), ld);
      verify_return_type((std::tr1::nexttoward)(d, ld), d);
      verify_return_type((std::tr1::nexttoward)(f, ld), f);
      verify_return_type((std::tr1::nexttoward)(ld, ld), ld);
      verify_return_type((std::tr1::nexttowardf)(f, ld), f);
      verify_return_type((std::tr1::nexttowardl)(ld, ld), ld);
#if 0
      verify_return_type((std::tr1::remainder)(d, d), d);
      verify_return_type((std::tr1::remainder)(f, f), f);
      verify_return_type((std::tr1::remainder)(ld, ld), ld);
      verify_return_type((std::tr1::remainderf)(f, f), f);
      verify_return_type((std::tr1::remainderl)(ld, ld), ld);
      verify_return_type((std::tr1::remquo)(d, d, &i), d);
      verify_return_type((std::tr1::remquo)(f, f, &i), f);
      verify_return_type((std::tr1::remquo)(ld, ld, &i), ld);
      verify_return_type((std::tr1::remquof)(f, f, &i), f);
      verify_return_type((std::tr1::remquol)(ld, ld, &i), ld);
      verify_return_type((std::tr1::rint)(d), d);
      verify_return_type((std::tr1::rint)(f), f);
      verify_return_type((std::tr1::rint)(ld), ld);
      verify_return_type((std::tr1::rintf)(f), f);
      verify_return_type((std::tr1::rintl)(ld), ld);
#endif
      verify_return_type((std::tr1::round)(d), d);
      verify_return_type((std::tr1::round)(f), f);
      verify_return_type((std::tr1::round)(ld), ld);
      verify_return_type((std::tr1::roundf)(f), f);
      verify_return_type((std::tr1::roundl)(ld), ld);
#if 0
      verify_return_type((std::tr1::scalbln)(d, li), d);
      verify_return_type((std::tr1::scalbln)(f, li), f);
      verify_return_type((std::tr1::scalbln)(ld, li), ld);
      verify_return_type((std::tr1::scalblnf)(f, li), f);
      verify_return_type((std::tr1::scalblnl)(ld, li), ld);
      verify_return_type((std::tr1::scalbn)(d, i), d);
      verify_return_type((std::tr1::scalbn)(f, i), f);
      verify_return_type((std::tr1::scalbn)(ld, i), ld);
      verify_return_type((std::tr1::scalbnf)(f, i), f);
      verify_return_type((std::tr1::scalbnl)(ld, i), ld);
#endif
      verify_return_type((std::tr1::tgamma)(d), d);
      verify_return_type((std::tr1::tgamma)(f), f);
      verify_return_type((std::tr1::tgamma)(ld), ld);
      verify_return_type((std::tr1::tgammaf)(f), f);
      verify_return_type((std::tr1::tgammal)(ld), ld);
      verify_return_type((std::tr1::trunc)(d), d);
      verify_return_type((std::tr1::trunc)(f), f);
      verify_return_type((std::tr1::trunc)(ld), ld);
      verify_return_type((std::tr1::truncf)(f), f);
      verify_return_type((std::tr1::truncl)(ld), ld);
      verify_return_type((std::tr1::signbit)(d), b);
      verify_return_type((std::tr1::signbit)(f), b);
      verify_return_type((std::tr1::signbit)(ld), b);
      verify_return_type((std::tr1::fpclassify)(d), i);
      verify_return_type((std::tr1::fpclassify)(f), i);
      verify_return_type((std::tr1::fpclassify)(ld), i);
      verify_return_type((std::tr1::isfinite)(d), b);
      verify_return_type((std::tr1::isfinite)(f), b);
      verify_return_type((std::tr1::isfinite)(ld), b);
      verify_return_type((std::tr1::isinf)(d), b);
      verify_return_type((std::tr1::isinf)(f), b);
      verify_return_type((std::tr1::isinf)(ld), b);
      verify_return_type((std::tr1::isnan)(d), b);
      verify_return_type((std::tr1::isnan)(f), b);
      verify_return_type((std::tr1::isnan)(ld), b);
      verify_return_type((std::tr1::isnormal)(d), b);
      verify_return_type((std::tr1::isnormal)(f), b);
      verify_return_type((std::tr1::isnormal)(ld), b);
#if 0
      verify_return_type((std::tr1::isgreater)(d, d), b);
      verify_return_type((std::tr1::isgreater)(f, f), b);
      verify_return_type((std::tr1::isgreater)(ld, ld), b);
      verify_return_type((std::tr1::isgreaterequal)(d, d), b);
      verify_return_type((std::tr1::isgreaterequal)(f, f), b);
      verify_return_type((std::tr1::isgreaterequal)(ld, ld), b);
      verify_return_type((std::tr1::isless)(d, d), b);
      verify_return_type((std::tr1::isless)(f, f), b);
      verify_return_type((std::tr1::isless)(ld, ld), b);
      verify_return_type((std::tr1::islessequal)(d, d), b);
      verify_return_type((std::tr1::islessequal)(f, f), b);
      verify_return_type((std::tr1::islessequal)(ld, ld), b);
      verify_return_type((std::tr1::islessgreater)(d, d), b);
      verify_return_type((std::tr1::islessgreater)(f, f), b);
      verify_return_type((std::tr1::islessgreater)(ld, ld), b);
      verify_return_type((std::tr1::isunordered)(d, d), b);
      verify_return_type((std::tr1::isunordered)(f, f), b);
      verify_return_type((std::tr1::isunordered)(ld, ld), b);
#endif

      // [5.2.1.1] associated Laguerre polynomials:
      verify_return_type((std::tr1::assoc_laguerre)(ui, ui, d), d);
      verify_return_type((std::tr1::assoc_laguerre)(ui, ui, f), f);
      verify_return_type((std::tr1::assoc_laguerre)(ui, ui, ld), ld);
      verify_return_type((std::tr1::assoc_laguerref)(ui, ui, f), f);
      verify_return_type((std::tr1::assoc_laguerrel)(ui, ui, ld), ld);
      // [5.2.1.2] associated Legendre functions:
      verify_return_type((std::tr1::assoc_legendre)(ui, ui, d), d);
      verify_return_type((std::tr1::assoc_legendre)(ui, ui, f), f);
      verify_return_type((std::tr1::assoc_legendre)(ui, ui, ld), ld);
      verify_return_type((std::tr1::assoc_legendref)(ui, ui, f), f);
      verify_return_type((std::tr1::assoc_legendrel)(ui, ui, ld), ld);
      // [5.2.1.3] beta function:
      verify_return_type((std::tr1::beta)(d, d), d);
      verify_return_type((std::tr1::beta)(f, f), f);
      verify_return_type((std::tr1::beta)(ld, ld), ld);
      verify_return_type((std::tr1::betaf)(f, f), f);
      verify_return_type((std::tr1::betal)(ld, ld), ld);
      // [5.2.1.4] (complete) elliptic integral of the first kind:
      verify_return_type((std::tr1::comp_ellint_1)(d), d);
      verify_return_type((std::tr1::comp_ellint_1)(f), f);
      verify_return_type((std::tr1::comp_ellint_1)(ld), ld);
      verify_return_type((std::tr1::comp_ellint_1f)(f), f);
      verify_return_type((std::tr1::comp_ellint_1l)(ld), ld);
      // [5.2.1.5] (complete) elliptic integral of the second kind:
      verify_return_type((std::tr1::comp_ellint_2)(d), d);
      verify_return_type((std::tr1::comp_ellint_2)(f), f);
      verify_return_type((std::tr1::comp_ellint_2)(ld), ld);
      verify_return_type((std::tr1::comp_ellint_2f)(f), f);
      verify_return_type((std::tr1::comp_ellint_2l)(ld), ld);
      // [5.2.1.6] (complete) elliptic integral of the third kind:
      verify_return_type((std::tr1::comp_ellint_3)(d, d), d);
      verify_return_type((std::tr1::comp_ellint_3)(f, f), f);
      verify_return_type((std::tr1::comp_ellint_3)(ld, ld), ld);
      verify_return_type((std::tr1::comp_ellint_3f)(f, f), f);
      verify_return_type((std::tr1::comp_ellint_3l)(ld, ld), ld);
#if 0
      // [5.2.1.7] confluent hypergeometric functions:
      verify_return_type((std::tr1::conf_hyperg)(d, d, d), d);
      verify_return_type((std::tr1::conf_hyperg)(f, f, f), f);
      verify_return_type((std::tr1::conf_hyperg)(ld, ld, ld), ld);
      verify_return_type((std::tr1::conf_hypergf)(f, f, f), f);
      verify_return_type((std::tr1::conf_hypergl)(ld, ld, ld), ld);
#endif
      // [5.2.1.8] regular modified cylindrical Bessel functions:
      verify_return_type((std::tr1::cyl_bessel_i)(d, d), d);
      verify_return_type((std::tr1::cyl_bessel_i)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_i)(ld, ld), ld);
      verify_return_type((std::tr1::cyl_bessel_if)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_il)(ld, ld), ld);
      // [5.2.1.9] cylindrical Bessel functions (of the first kind):
      verify_return_type((std::tr1::cyl_bessel_j)(d, d), d);
      verify_return_type((std::tr1::cyl_bessel_j)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_j)(ld, ld), ld);
      verify_return_type((std::tr1::cyl_bessel_jf)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_jl)(ld, ld), ld);
      // [5.2.1.10] irregular modified cylindrical Bessel functions:
      verify_return_type((std::tr1::cyl_bessel_k)(d, d), d);
      verify_return_type((std::tr1::cyl_bessel_k)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_k)(ld, ld), ld);
      verify_return_type((std::tr1::cyl_bessel_kf)(f, f), f);
      verify_return_type((std::tr1::cyl_bessel_kl)(ld, ld), ld);
      // [5.2.1.11] cylindrical Neumann functions;
      // cylindrical Bessel functions (of the second kind):
      verify_return_type((std::tr1::cyl_neumann)(d, d), d);
      verify_return_type((std::tr1::cyl_neumann)(f, f), f);
      verify_return_type((std::tr1::cyl_neumann)(ld, ld), ld);
      verify_return_type((std::tr1::cyl_neumannf)(f, f), f);
      verify_return_type((std::tr1::cyl_neumannl)(ld, ld), ld);
      // [5.2.1.12] (incomplete) elliptic integral of the first kind:
      verify_return_type((std::tr1::ellint_1)(d, d), d);
      verify_return_type((std::tr1::ellint_1)(f, f), f);
      verify_return_type((std::tr1::ellint_1)(ld, ld), ld);
      verify_return_type((std::tr1::ellint_1f)(f, f), f);
      verify_return_type((std::tr1::ellint_1l)(ld, ld), ld);
      // [5.2.1.13] (incomplete) elliptic integral of the second kind:
      verify_return_type((std::tr1::ellint_2)(d, d), d);
      verify_return_type((std::tr1::ellint_2)(f, f), f);
      verify_return_type((std::tr1::ellint_2)(ld, ld), ld);
      verify_return_type((std::tr1::ellint_2f)(f, f), f);
      verify_return_type((std::tr1::ellint_2l)(ld, ld), ld);
      // [5.2.1.14] (incomplete) elliptic integral of the third kind:
      verify_return_type((std::tr1::ellint_3)(d, d, d), d);
      verify_return_type((std::tr1::ellint_3)(f, f, f), f);
      verify_return_type((std::tr1::ellint_3)(ld, ld, ld), ld);
      verify_return_type((std::tr1::ellint_3f)(f, f, f), f);
      verify_return_type((std::tr1::ellint_3l)(ld, ld, ld), ld);
      // [5.2.1.15] exponential integral:
      verify_return_type((std::tr1::expint)(d), d);
      verify_return_type((std::tr1::expint)(f), f);
      verify_return_type((std::tr1::expint)(ld), ld);
      verify_return_type((std::tr1::expintf)(f), f);
      verify_return_type((std::tr1::expintl)(ld), ld);
      // [5.2.1.16] Hermite polynomials:
      verify_return_type((std::tr1::hermite)(ui, d), d);
      verify_return_type((std::tr1::hermite)(ui, f), f);
      verify_return_type((std::tr1::hermite)(ui, ld), ld);
      verify_return_type((std::tr1::hermitef)(ui, f), f);
      verify_return_type((std::tr1::hermitel)(ui, ld), ld);
      // [5.2.1.17] hypergeometric functions:
#if 0
      verify_return_type((std::tr1::hyperg)(d, d, d, d), d);
      verify_return_type((std::tr1::hyperg)(f, f, f, f), f);
      verify_return_type((std::tr1::hyperg)(ld, ld, ld, ld), ld);
      verify_return_type((std::tr1::hypergf)(f, f, f, f), f);
      verify_return_type((std::tr1::hypergl)(ld, ld, ld, ld), ld);
#endif
      // [5.2.1.18] Laguerre polynomials:
      verify_return_type((std::tr1::laguerre)(ui, d), d);
      verify_return_type((std::tr1::laguerre)(ui, f), f);
      verify_return_type((std::tr1::laguerre)(ui, ld), ld);
      verify_return_type((std::tr1::laguerref)(ui, f), f);
      verify_return_type((std::tr1::laguerrel)(ui, ld), ld);
      // [5.2.1.19] Legendre polynomials:
      verify_return_type((std::tr1::legendre)(ui, d), d);
      verify_return_type((std::tr1::legendre)(ui, f), f);
      verify_return_type((std::tr1::legendre)(ui, ld), ld);
      verify_return_type((std::tr1::legendref)(ui, f), f);
      verify_return_type((std::tr1::legendrel)(ui, ld), ld);
      // [5.2.1.20] Riemann zeta function:
      verify_return_type((std::tr1::riemann_zeta)(d), d);
      verify_return_type((std::tr1::riemann_zeta)(f), f);
      verify_return_type((std::tr1::riemann_zeta)(ld), ld);
      verify_return_type((std::tr1::riemann_zetaf)(f), f);
      verify_return_type((std::tr1::riemann_zetal)(ld), ld);
      // [5.2.1.21] spherical Bessel functions (of the first kind):
      verify_return_type((std::tr1::sph_bessel)(ui, d), d);
      verify_return_type((std::tr1::sph_bessel)(ui, f), f);
      verify_return_type((std::tr1::sph_bessel)(ui, ld), ld);
      verify_return_type((std::tr1::sph_besself)(ui, f), f);
      verify_return_type((std::tr1::sph_bessell)(ui, ld), ld);
      // [5.2.1.22] spherical associated Legendre functions:
      verify_return_type((std::tr1::sph_legendre)(ui, ui, d), d);
      verify_return_type((std::tr1::sph_legendre)(ui, ui, f), f);
      verify_return_type((std::tr1::sph_legendre)(ui, ui, ld), ld);
      verify_return_type((std::tr1::sph_legendref)(ui, ui, f), f);
      verify_return_type((std::tr1::sph_legendrel)(ui, ui, ld), ld);
      // [5.2.1.23] spherical Neumann functions;
      // spherical Bessel functions (of the second kind):
      verify_return_type((std::tr1::sph_neumann)(ui, d), d);
      verify_return_type((std::tr1::sph_neumann)(ui, f), f);
      verify_return_type((std::tr1::sph_neumann)(ui, ld), ld);
      verify_return_type((std::tr1::sph_neumannf)(ui, f), f);
      verify_return_type((std::tr1::sph_neumannl)(ui, ld), ld);
   }
   return 0;
}


//ROS + OpenCv
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <iostream>
#include <fstream>
#include <signal.h>
#include <sstream>
#include <list>
#include <vector>
#include <cmath>

// Videoray
#include "videoray/Status.h"
#include "videoray/UHRIComm.h"
#include "videoray/Throttle.h"

// darknet_ros_msgs
#include "videoray/BoundingBox.h"
#include "videoray/BoundingBoxes.h"

double depth_ref = 0;
double speed_ref = 0;
double heading_ref = 0;
int VideoRay_Speed = 0;
double sharpturn_speed = 0;
double roll_ = 0;
double pitch_ = 0;
double yaw_ = 0;
double depth_err = 0;
double speed_err = 0;
double heading_err = 0;
double heading = 0;
double heading_weight = 0.5;
double speed_weight = 0.5;
double K_heading = 1;
double K_speed = 10;
double K_depth = 50;

float previous_err = 0;
float integral = 0;
float derivative = 0;
float output;
float Kp=0.01;
float Ki=0;
float Kd=0.01;
float des_speed=0;

static const std::string OPENCV_WINDOW = "Image window";
int imageWidth = 720;
int imageHeight = 480;
int maxa;
int xPosCenter;
int  yPosCenter;
bool detectedPlastic=false;
float errx;
float erry;
float prevx;
float prevy;
int width;
std::list<double> area;
cv::Mat imgOriginal; //Input image
cv::RNG rng(12345);
float heading_port, heading_star, speed_port, speed_star, speed_vert; //speed_vert is a new addition

videoray::Throttle throttle_;
ros::Subscriber boxes;
ros::Publisher throttle_pub;
image_transport::Publisher image_pub_;
image_transport::Subscriber image_sub_;

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  //Subscribe
  //Publishers
  //ros::Publisher videoray_status_pub_;

public:
  ImageConverter(): it_(nh_)
  {

    // Subscribe to input video feed and publish output video feed
    boxes = nh_.subscribe("/darknet_ros/bounding_boxes", 1, &ImageConverter::boxesCallback,this);
    image_sub_ = it_.subscribe("/camera/rgb/image_raw", 1,&ImageConverter::image, this);
    image_pub_ = it_.advertise("/image_converter/output_video", 1);
    throttle_pub = nh_.advertise<videoray::Throttle>("/videoray/throttle", 1);
  }

  ~ImageConverter()
  {
    throttle_.PortInput =  0;
    throttle_.StarInput =  0;
    throttle_.VertInput = 0;
    throttle_pub.publish(throttle_);
    cv::destroyWindow(OPENCV_WINDOW);
  }

  void boxesCallback(const videoray::BoundingBoxes& msg){
    std::vector<videoray::BoundingBox> vec = msg.bounding_boxes;
    double maxa=0;
    int index=0;
    errx=0;
    erry=0;
    prevx=0;
    prevy=0;
    xPosCenter=0;
    yPosCenter=0;
    width=0;
    //std::cout<< "ENTER BOX CALLBACK" << speed_star << std::endl;
    for(int i =0; i < vec.size(); i++){
      if (msg.bounding_boxes[i].Class == "plastic") {
        detectedPlastic = true;
      }
      else {
        throttle_.PortInput =  0;
        throttle_.StarInput =  0;
        throttle_.VertInput = 0;
        throttle_pub.publish(throttle_);
        detectedPlastic= false;
      }

      if(detectedPlastic == true){
          double a = (msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin) * (msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin);
          area.push_back(a);
          if(a > maxa){
            maxa=a;
            index=i;
       }
      }
    }
    xPosCenter = msg.bounding_boxes[index].xmin + (msg.bounding_boxes[index].xmax - msg.bounding_boxes[index].xmin) * 0.5;
    yPosCenter = msg.bounding_boxes[index].ymin + (msg.bounding_boxes[index].ymax - msg.bounding_boxes[index].ymin) * 0.5;

    width=msg.bounding_boxes[index].xmax-msg.bounding_boxes[index].xmin;
    if(width>450){
	throttle_.PortInput =  0;
        throttle_.StarInput =  0;
        throttle_.VertInput = 0;
        throttle_pub.publish(throttle_);
    }
    else{
	    std::cout<< "width " << width << std::endl;
	    errx = xPosCenter - imageWidth/2;
	    erry = yPosCenter - imageHeight/2; //added
	    prevx=errx;
	    prevy=erry;
	    if(errx<0){
	    	speed_port=(errx)/imageWidth*40-15*abs((errx-prevx)/imageWidth);
	    	speed_star=-(errx)/imageWidth*40+15*abs((errx-prevx)/imageWidth);
	    	speed_vert=(erry)/imageHeight*40+15*abs((erry-prevy)/imageHeight);
	    }
	    else{
	    	speed_port=(errx)/imageWidth*40+15*abs((errx-prevx)/imageWidth);
	    	speed_star=-(errx)/imageWidth*40-15*abs((errx-prevx)/imageWidth);
	    	speed_vert=(erry)/imageHeight*40+15*abs((erry-prevy)/imageHeight);
	    }
	    std::cout<< "errx " << errx << "prevx" << prevx<< std::endl;
	    std::cout<< "speed star " << speed_star << std::endl;
	    std::cout<< "speed port " << speed_port << std::endl;
	    std::cout<< "speed vert " << speed_vert << std::endl; //added

	    throttle_.PortInput=speed_port;
	    throttle_.StarInput=speed_star;
	    throttle_.VertInput= speed_vert; //was originally set to 0
	    throttle_pub.publish(throttle_);
    }
  }

  void image(const sensor_msgs::ImageConstPtr& msg){
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    imgOriginal = cv_ptr->image;
    cv::circle( imgOriginal, cv::Point(xPosCenter,yPosCenter), 4, cv::Scalar(0,0,255), -1 );
    cv::namedWindow("imgOriginal", CV_WINDOW_NORMAL);
    cv::imshow("imgOriginal", imgOriginal);
    cv::waitKey(3);
  }

};

void mySigintHandler(int sig){
 throttle_.PortInput =  0;
 throttle_.StarInput =  0;
 throttle_.VertInput = 0;
 throttle_pub.publish(throttle_);
 ros::shutdown();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  signal(SIGINT, mySigintHandler);
  ros::spin();
return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_RNG_HPP

#include <boost/random/student_t_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    student_t_rng(double nu,
                  double mu,
                  double sigma,
                  RNG& rng) {
      using boost::variate_generator;
      using boost::random::student_t_distribution;

      static const char* function("student_t_rng");

      check_positive_finite(function, "Degrees of freedom parameter", nu);
      check_finite(function, "Location parameter", mu);
      check_positive_finite(function, "Scale parameter", sigma);

      variate_generator<RNG&, student_t_distribution<> >
        rng_unit_student_t(rng, student_t_distribution<>(nu));
      return mu + sigma * rng_unit_student_t();
    }

  }
}
#endif


/**
 * @file  maximumprinciple.cc
 * @brief NPDE homework "MaximumPrinciple" code
 * @author Oliver Rietmann
 * @date 25.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "maximumprinciple.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <vector>

namespace MaximumPrinciple {

/**
 * @brief Assembly on a tensor product mesh
 *
 * Compute the global Galerkin matrix from the local
 * element matrix.
 *
 * @param M Number of interior vertices in x and y direction.
 * @param B_K Local element matrix.
 * @return Global Galerkin matrix of size M^2 times M^2.
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<double> assemble(int M, const Eigen::Matrix3d &B_K) {
  int M2 = M * M;
  Eigen::SparseMatrix<double> A(M2, M2);
  //====================
  // Your code goes here
  //====================
  return A;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<double> computeGalerkinMatrix(int M, double c) {
  Eigen::Matrix3d B_K;
  //====================
  // Your code goes here
  //====================
  return assemble(M, B_K);
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_4 */
Eigen::SparseMatrix<double> computeGalerkinMatrixTR(int M, double c) {
  Eigen::Matrix3d B_K;
  //====================
  // Your code goes here
  //====================
  return assemble(M, B_K);
}
/* SAM_LISTING_END_4 */

}  // namespace MaximumPrinciple


#include <OmplWrappers/MyOptimizationObjective.h>

// Eigen
#include <Eigen/Dense>
using Eigen::VectorXd;

// OMPL
#include <ompl/base/State.h>
#include <ompl/base/Cost.h>
#include <ompl/base/samplers/InformedStateSampler.h>

// Internal Libraries
#include <Dimt/Params.h>

// stdlib
#include <exception>

//
// Return the cost of the state at this point
//
// @param s State to get the cost for
// @return Cost of the state
//
ompl::base::Cost ompl::base::MyOptimizationObjective::stateCost(const ompl::base::State *s) const
{
    if (sampler_ == nullptr or opt_ == nullptr)
    {
        throw std::runtime_error("An informed sampler with optimization objective "
                                 "must be provided or set.");
    }
    return opt_->stateCost(s);
}

//
// Return the cost of moving from s1 to s2
//
// @param s1 Start state
// @param s2 Goal state
// @return Cost of going from s1 to s2
//
ompl::base::Cost ompl::base::MyOptimizationObjective::motionCost(const ompl::base::State *s1,
                                                                 const ompl::base::State *s2) const
{
    if (sampler_ == nullptr)
    {
        throw std::runtime_error("An informed sampler with optimization objective "
                                 "must be provided or set.");
    }
    return opt_->motionCost(s1, s2);
}

//
// Function to get the informed sampler pointer
//
// @param probDefn Problem definition pointer (OMPL)
// @param maxNumberCalls Maximum number of sampling calls
// @return Infromed sampler
//
ompl::base::InformedSamplerPtr ompl::base::MyOptimizationObjective::allocInformedStateSampler(
    const ProblemDefinitionPtr& probDefn, unsigned int maxNumberCalls) const
{
    if (sampler_ == nullptr)
    {
        throw std::runtime_error("An informed sampler with optimization objective "
                                 "must be provided or set.");
    }
    return sampler_;
}

namespace ompl
{
    namespace base
    {
        Cost GeometricObjective::stateCost(const State *s) const
        {
            Eigen::VectorXd stateVec(param.dimensions);
            get_eigen_vector(s, stateVec);
            return Cost((startVec_ - stateVec).norm() + (goalVec_ - stateVec).norm());
        }

        Cost GeometricObjective::motionCost(const State *s1, const State *s2) const
        {
            Eigen::VectorXd s1Vec, s2Vec;
            get_eigen_vector(s1, s1Vec);
            get_eigen_vector(s2, s2Vec);
            return Cost((s1Vec - s2Vec).norm());
        }

        Cost GeometricObjective::combineCosts(Cost c1, Cost c2) const
        {
            return Cost(c1.value() + c2.value());
        }

        Cost DimtObjective::stateCost(const State *s) const
        {
            return Cost(dimt_->getMinTime(startState_, s) +
                        dimt_->getMinTime(s, goalState_));
        }

        Cost DimtObjective::motionCost(const State *s1, const State *s2) const
        {
            return Cost(dimt_->getMinTime(s1, s2));
        }

        Cost DimtObjective::getCostIfSmallerThan(const State* s1, const State *s2, Cost thresholdCost) const
        {
            return Cost(dimt_->getMinTimeIfSmallerThan(s1, s2, thresholdCost.value()));
        }
    }  // namespace base
}  // namespace ompl


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <iterator>
#include <numeric>
#include <memory>
#include <chrono>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "TestSet.hpp"
#include "VNS.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*							INITIAL SOLUTIONS FOR VNS                            */
/*********************************************************************************/

// Initial Feasible Solution
void VNS::initialGreedy(
	vector <bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &fi,
	const vector<double> &dj,
	ublas::matrix<double> &cij,
	const double &sum_dj)
{
	unsigned int r_index = 0;

	// Closes Facilitys till Demand can be fullfilled
	while (canUpdateXij(bi, incumbent_solution, sum_dj))
	{
		r_index = select_randomly(incumbent_solution);
		incumbent_solution[r_index] = 0;
	}
	incumbent_solution[r_index] = 1;

	if (!updateXij(m_cij, m_dj, m_bi, incumbent_solution, m_update_xij_mode, m_best_fx))
	{
		m_best_fx = DBL_MAX;
		fill(incumbent_solution.begin(), incumbent_solution.end(), 1);
	}

	m_fx = m_best_fx;
}

// Inital Solution from RVNS
void VNS::initialRVNS(
	vector <bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &dj,
	ublas::matrix<double> &cij)
{
	unsigned int  k = 0, r_index = 0;

	// Closes Facilitys till Demand can be fullfilled
	while (canUpdateXij(bi, incumbent_solution, m_sum_dj))
	{
		r_index = select_randomly(incumbent_solution);
		incumbent_solution[r_index] = 0;
	}
	incumbent_solution[r_index] = 1;

	if (!updateXij(m_cij, m_dj, m_bi, incumbent_solution, m_update_xij_mode, m_fx))
	{
		m_fx = DBL_MAX;
		fill(incumbent_solution.begin(), incumbent_solution.end(), 1);
	}

	if (m_rMax > 0)
	{
		for (size_t i = 0; i != m_rMax; ++i)
		{
			// For all N_k Structures
			m_k = m_kMin - 1;

			while (m_k < m_kMax)
			{
				// Shake incumbent Solution
				m_perturbed_solution = shaking(incumbent_solution, bi, dj, cij, m_k, m_fx, m_shaking_mode);
				// Move or not
				neighborhoodChange(incumbent_solution, m_perturbed_solution, m_k, m_fx, m_best_fx, m_kMin);
			}
		}

		m_k = m_kMin - 1;
		m_fx = m_best_fx;
	}
}


#include <boost/test/unit_test.hpp>
#include "test_utils.hpp"

#include <moreorg/utils/CoalitionStructureGeneration.hpp>

using namespace multiagent::utils;

void updateCostMap(std::map<Coalition, double>& costMap, const std::string& desc, double cost)
{
    Coalition c;
    for(size_t i = 0; i < desc.size(); ++i)
    {
        std::string name = desc.substr(i,1);
        c.push_back(name);
    }
    std::sort(c.begin(), c.end());
    costMap[c] = cost;
}

double coalitionValueFunction(const Coalition& coalition)
{
//    return coalition.size();

    Coalition c = coalition;
    std::sort(c.begin(), c.end());

    std::map<Coalition, double> costMap;
    updateCostMap(costMap, "a", 125.0);
    updateCostMap(costMap, "b", 50.0);
    updateCostMap(costMap, "c", 75.0);
    updateCostMap(costMap, "d", 150);
    for(char i = 'e'; i < 'z'; ++i)
    {
        updateCostMap(costMap, std::string(1, i), 100);
    }

    updateCostMap(costMap, "ab", 176);
    updateCostMap(costMap, "ac", 150);
    updateCostMap(costMap, "ad", 100);
    updateCostMap(costMap, "bc", 150);
    updateCostMap(costMap, "bd", 200);
    updateCostMap(costMap, "cd", 125);
    updateCostMap(costMap, "de", 425);

    updateCostMap(costMap, "abc", 200);
    updateCostMap(costMap, "abd", 150);
    updateCostMap(costMap, "acd", 220);
    updateCostMap(costMap, "bcd", 150);

    updateCostMap(costMap, "abcd", 325);
    updateCostMap(costMap, "efghijklm", 1400);
    updateCostMap(costMap, "opqr", 5400);
    updateCostMap(costMap, "stuvwxyz", 15400);

   // std::map<Coalition, double>::const_iterator cit = costMap.begin();
   // for(; cit != costMap.end(); ++cit)
   // {
   //     Coalition c = cit->first;
   //     std::string s;
   //     for(size_t i = 0; i < c.size(); ++i)
   //     {
   //         s += c[i];
   //     }
   //     BOOST_TEST_MESSAGE("Return value: " << cit->second << " for " << s);
   // }

    std::map<Coalition, double>::const_iterator cit = costMap.find(coalition);
    if(cit != costMap.end())
    {
        return cit->second;
    } else {
        double value = coalition.size()*coalition.size();
        return value;
    }
}

double coalitionStructureValueFunction(const CoalitionStructure& c)
{
    double value = 0.0;
    for(int i = 0; i < c.size(); ++i)
    {
        value += coalitionValueFunction( c[i] );
    }
    return value;
}

BOOST_AUTO_TEST_CASE(it_should_compute_csg)
{
    AgentList agents;
    char numberOfAgents = 3;
    for(char a = 'a'; a < 'a' + numberOfAgents; ++a)
    {
        std::stringstream ss;
        ss << a;
        agents.push_back(ss.str());
    }
    CoalitionStructureGeneration csg(agents, coalitionValueFunction, coalitionStructureValueFunction);
    BOOST_TEST_MESSAGE("Preparation done: " << csg.toString());
    csg.anytimeSearch(1.0);
    while(!csg.anytimeSearchCompleted())
    {
        sleep(1);
        BOOST_TEST_MESSAGE("Intermediate result: " << CoalitionStructureGeneration::toString( csg.currentBestSolution()) << ", value: " << coalitionStructureValueFunction(csg.currentBestSolution()) << ", quality: " << csg.currentBestSolutionQuality() );
    }
    BOOST_TEST_MESSAGE("Final result: " << CoalitionStructureGeneration::toString(csg.currentBestSolution()) << ", value: " << coalitionStructureValueFunction(csg.currentBestSolution()) );

    csg.reset();
    BOOST_TEST_MESSAGE("Reset done: " << csg.toString());
    CoalitionStructure cs = csg.findBest(1.0);
    BOOST_TEST_MESSAGE("Found best: " << CoalitionStructureGeneration::toString(cs) << ", value: " << coalitionStructureValueFunction(cs));
}

BOOST_AUTO_TEST_CASE(it_should_compute_csg_anytime)
{
    AgentList agents;
    char numberOfAgents = 8;
    for(char a = 'a'; a < 'a' + numberOfAgents; ++a)
    {
        std::stringstream ss;
        ss << a;
        agents.push_back(ss.str());
    }
    CoalitionStructureGeneration csg(agents, coalitionValueFunction, coalitionStructureValueFunction);
    BOOST_TEST_MESSAGE("Preparation done: " << csg.toString());
    csg.anytimeSearch(1.0);
    while(!csg.anytimeSearchCompleted())
    {
        sleep(1);
        BOOST_TEST_MESSAGE("Intermediate result: " << CoalitionStructureGeneration::toString( csg.currentBestSolution()) << ", value: " << coalitionStructureValueFunction(csg.currentBestSolution()) << ", quality: " << csg.currentBestSolutionQuality() );
        BOOST_TEST_MESSAGE("      " << csg.getStatistics().toString());
    }
    BOOST_TEST_MESSAGE("Final result: " << CoalitionStructureGeneration::toString(csg.currentBestSolution()) << ", value: " << coalitionStructureValueFunction(csg.currentBestSolution()) );
}


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

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

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

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

#include <boost/make_shared.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <qle/instruments/impliedbondspread.hpp>

using namespace QuantLib;

namespace QuantExt {

namespace {

class PriceError {
public:
    PriceError(const Bond& engine, SimpleQuote& spread, Real targetValue, bool isCleanPrice);
    Real operator()(Real spread) const;

private:
    const Bond& bond_;
    SimpleQuote& spread_;
    Real targetValue_;
    bool isCleanPrice_;
};

PriceError::PriceError(const Bond& bond, SimpleQuote& spread, Real targetValue, bool isCleanPrice)
    : bond_(bond), spread_(spread), targetValue_(targetValue), isCleanPrice_(isCleanPrice) {}

Real PriceError::operator()(Real spread) const {
    spread_.setValue(spread);
    Real guessValue = isCleanPrice_ ? bond_.cleanPrice() : bond_.dirtyPrice();
    return guessValue - targetValue_;
}

} // namespace

namespace detail {

Real ImpliedBondSpreadHelper::calculate(const boost::shared_ptr<Bond>& bond,
                                        const boost::shared_ptr<PricingEngine>& engine,
                                        const boost::shared_ptr<SimpleQuote>& spreadQuote, Real targetValue,
                                        bool isCleanPrice, Real accuracy, Natural maxEvaluations, Real minSpread,
                                        Real maxSpread) {

    Bond clonedBond = *bond;
    clonedBond.setPricingEngine(engine);
    clonedBond.recalculate();
    spreadQuote->setValue(0.005);

    PriceError f(clonedBond, *spreadQuote, targetValue, isCleanPrice);
    Brent solver;
    solver.setMaxEvaluations(maxEvaluations);
    Real guess = (minSpread + maxSpread) / 2.0;
    Real result = solver.solve(f, accuracy, guess, minSpread, maxSpread);
    return result;
}

} // namespace detail

} // namespace QuantExt


//==============================================================================
//         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/evstat.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/include/vector_tie.hpp>
#include <nt2/include/constants/euler.hpp>

#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( evstat, NT2_REAL_TYPES)
{
  using nt2::evstat;
  using nt2::tag::evstat_;
  using nt2::sqr;

  T rmean = T(5.696083774616789e-02);
  T rvar = T(1.028083791780141e+01);
  std::cout << nt2::Euler<T>() << std::endl;
  std::cout << nt2::Euler<T>()-T(5.772156649015323e-01)<< std::endl;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<evstat_(T, T)>::type)
                  , (std::pair<T,T>)
                  );

  T mean;
  T var;
  T m = T(1.5);
  T s = T(2.5);
  {
    evstat(m, s, mean, var);
    NT2_TEST_ULP_EQUAL(var, rvar, 2);
    NT2_TEST_ULP_EQUAL(mean, rmean, 2);
  }

  {
    mean = evstat(m, s, var);
    NT2_TEST_ULP_EQUAL(var, rvar, 2);
    NT2_TEST_ULP_EQUAL(mean, rmean, 2);
  }

  {
    boost::fusion::vector_tie(mean,var) = evstat(m, s);
    NT2_TEST_ULP_EQUAL(var, rvar, 2);
    NT2_TEST_ULP_EQUAL(mean, rmean, 2);
  }

  {
    std::pair<T,T> p;

    p = evstat(m, s);
    NT2_TEST_ULP_EQUAL(p.first, rmean, 2);
    NT2_TEST_ULP_EQUAL(p.second, rvar, 2);
  }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MAXFLINT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MAXFLINT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Maxflint generic tag

     Represents the Maxflint constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Maxflint , double, 1
                                , 0x4b800000, 0x4340000000000000ll
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Maxflint, Site> dispatching_Maxflint(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Maxflint, Site>();
   }
   template<class... Args>
   struct impl_Maxflint;
  }
  /*!
    Generates the least integer value which is exactly
    representable in floating point numbers and
    equal to its integral successor.

    All floating numbers greater than Maxflint are integral.

    @par Semantic:

    @code
    T r = Maxflint<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 9007199254740992.0
    else if T is float
      r = 16777216.0f
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Maxflint, Maxflint)
} }

#include <boost/simd/constant/common.hpp>

#endif