/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2017 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#include <mapnik/geometry/interior.hpp>
#include <mapnik/geometry_envelope.hpp>
#include <mapnik/box2d.hpp>
#include <mapnik/geometry_centroid.hpp>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/optional.hpp>
#pragma GCC diagnostic pop

namespace mapnik { namespace geometry {

// Interior algorithm is realized as a modification of Polylabel algorithm
// from https://github.com/mapbox/polylabel.
// The modification aims to improve visual output by prefering
// placements closer to centroid.

namespace detail {

// get squared distance from a point to a segment
template <class T>
T segment_dist_sq(const point<T>& p,
                  const point<T>& a,
                  const point<T>& b)
{
    auto x = a.x;
    auto y = a.y;
    auto dx = b.x - x;
    auto dy = b.y - y;

    if (dx != 0 || dy != 0) {

        auto t = ((p.x - x) * dx + (p.y - y) * dy) / (dx * dx + dy * dy);

        if (t > 1) {
            x = b.x;
            y = b.y;

        } else if (t > 0) {
            x += dx * t;
            y += dy * t;
        }
    }

    dx = p.x - x;
    dy = p.y - y;

    return dx * dx + dy * dy;
}

template <class T>
void point_to_ring_dist(point<T> const& point, linear_ring<T> const& ring,
                        bool & inside, double & min_dist_sq)
{
    for (std::size_t i = 0, len = ring.size(), j = len - 1; i < len; j = i++)
    {
        const auto& a = ring[i];
        const auto& b = ring[j];

        if ((a.y > point.y) != (b.y > point.y) &&
            (point.x < (b.x - a.x) * (point.y - a.y) / (b.y - a.y) + a.x)) inside = !inside;

        min_dist_sq = std::min(min_dist_sq, segment_dist_sq(point, a, b));
    }
}

// signed distance from point to polygon outline (negative if point is outside)
template <class T>
double point_to_polygon_dist(const point<T>& point, const polygon<T>& polygon)
{
    bool inside = false;
    double min_dist_sq = std::numeric_limits<double>::infinity();

    point_to_ring_dist(point, polygon.exterior_ring, inside, min_dist_sq);

    for (const auto& ring : polygon.interior_rings)
    {
        point_to_ring_dist(point, ring, inside, min_dist_sq);
    }

    return (inside ? 1 : -1) * std::sqrt(min_dist_sq);
}

template <class T>
struct fitness_functor
{
    fitness_functor(point<T> const& centroid, point<T> const& polygon_size)
        : centroid(centroid),
          max_size(std::max(polygon_size.x, polygon_size.y))
        {}

    T operator()(point<T> const& cell_center, T distance_polygon) const
    {
        if (distance_polygon <= 0)
        {
            return distance_polygon;
        }
        point<T> d(cell_center.x - centroid.x, cell_center.y - centroid.y);
        double distance_centroid = std::sqrt(d.x * d.x + d.y * d.y);
        return distance_polygon * (1 - distance_centroid / max_size);
    }

    point<T> centroid;
    T max_size;
};

template <class T>
struct cell
{
    template <class FitnessFunc>
    cell(const point<T>& c_, T h_,
         const polygon<T>& polygon,
         const FitnessFunc& ff)
        : c(c_),
          h(h_),
          d(point_to_polygon_dist(c, polygon)),
          fitness(ff(c, d)),
          max_fitness(ff(c, d + h * std::sqrt(2)))
        {}

    point<T> c; // cell center
    T h; // half the cell size
    T d; // distance from cell center to polygon
    T fitness; // fitness of the cell center
    T max_fitness; // a "potential" of the cell calculated from max distance to polygon within the cell
};

template <class T>
point<T> polylabel(polygon<T> const& polygon, box2d<T> const& bbox , T precision = 1)
{
    const point<T> size { bbox.width(), bbox.height() };

    const T cell_size = std::min(size.x, size.y);
    T h = cell_size / 2;

    // a priority queue of cells in order of their "potential" (max distance to polygon)
    auto compare_func = [] (const cell<T>& a, const cell<T>& b)
    {
        return a.max_fitness < b.max_fitness;
    };
    using Queue = std::priority_queue<cell<T>, std::vector<cell<T>>, decltype(compare_func)>;
    Queue queue(compare_func);

    if (cell_size == 0)
    {
        return { bbox.minx(), bbox.miny() };
    }

    point<T> centroid;
    if (!mapnik::geometry::centroid(polygon, centroid))
    {
        auto center = bbox.center();
        return { center.x, center.y };
    }

    fitness_functor<T> fitness_func(centroid, size);

    // cover polygon with initial cells
    for (T x = bbox.minx(); x < bbox.maxx(); x += cell_size)
    {
        for (T y = bbox.miny(); y < bbox.maxy(); y += cell_size)
        {
            queue.push(cell<T>({x + h, y + h}, h, polygon, fitness_func));
        }
    }

    // take centroid as the first best guess
    auto best_cell = cell<T>(centroid, 0, polygon, fitness_func);

    while (!queue.empty())
    {
        // pick the most promising cell from the queue
        auto current_cell = queue.top();
        queue.pop();

        // update the best cell if we found a better one
        if (current_cell.fitness > best_cell.fitness)
        {
            best_cell = current_cell;
        }

        // do not drill down further if there's no chance of a better solution
        if (current_cell.max_fitness - best_cell.fitness <= precision) continue;

        // split the cell into four cells
        h = current_cell.h / 2;
        queue.push(cell<T>({current_cell.c.x - h, current_cell.c.y - h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x + h, current_cell.c.y - h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x - h, current_cell.c.y + h}, h, polygon, fitness_func));
        queue.push(cell<T>({current_cell.c.x + h, current_cell.c.y + h}, h, polygon, fitness_func));
    }

    return best_cell.c;
}

} // namespace detail

template <class T>
bool interior(polygon<T> const& polygon, double scale_factor, point<T> & pt)
{
    if (polygon.exterior_ring.empty())
    {
        return false;
    }

    const box2d<T> bbox = envelope(polygon.exterior_ring);

    // Let the precision be 1% of the polygon size to be independent to map scale.
    double precision = (std::max(bbox.width(), bbox.height()) / 100.0) * scale_factor;

    pt = detail::polylabel(polygon, bbox, precision);
    return true;
}

template
bool interior(polygon<double> const& polygon, double scale_factor, point<double> & pt);

} }


#include "RBGL.hpp"
#include "mincut.hpp"
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/kolmogorov_max_flow.hpp>

typedef enum { E_MF_Push_Relabel, E_MF_Edmonds_Karp, E_MF_Kolmogorov } E_MF_METHOD;

static SEXP BGL_max_flow_internal(SEXP num_verts_in, SEXP num_edges_in,
                                  SEXP R_edges_in, SEXP R_capacity_in,
                                  SEXP src, SEXP sink, E_MF_METHOD method )
{
    using namespace boost;

    typedef adjacency_list_traits<vecS, vecS, directedS> Tr;
    typedef Tr::edge_descriptor Tr_edge_desc;

    typedef adjacency_list<vecS, vecS, directedS, no_property,
    		property<edge_capacity_t, double,
    		property<edge_residual_capacity_t, double,
    		property<edge_reverse_t, Tr_edge_desc> > > >
		FlowGraph;

    typedef graph_traits<FlowGraph>::edge_iterator   edge_iterator;
    typedef graph_traits<FlowGraph>::vertex_descriptor vertex_descriptor;

    FlowGraph flow_g;

    property_map < FlowGraph, edge_capacity_t >::type
    cap = get(edge_capacity, flow_g);
    property_map < FlowGraph, edge_residual_capacity_t >::type
    res_cap = get(edge_residual_capacity, flow_g);
    property_map < FlowGraph, edge_reverse_t >::type
    rev_edge = get(edge_reverse, flow_g);

    Tr::edge_descriptor e1, e2;
    bool in1, in2;

    if (!isInteger(R_edges_in)) error("R_edges_in should be integer");

    int NV = INTEGER(num_verts_in)[0];
    int NE = asInteger(num_edges_in);
    int* edges_in = INTEGER(R_edges_in);
    int*    capacity_i = (isReal(R_capacity_in)) ? 0 : INTEGER(R_capacity_in);
    double* capacity_d = (isReal(R_capacity_in)) ? REAL(R_capacity_in) : 0;

    for (int i = 0; i < NE ; i++, edges_in += 2)
    {
        tie(e1, in1) = boost::add_edge(*edges_in, *(edges_in+1), flow_g);
        tie(e2, in2) = boost::add_edge(*(edges_in+1), *edges_in, flow_g);
        if ( !in1 || !in2 )
            error("unable to add edge: (%d, %d)", *edges_in, *(edges_in+1));

        // fill in capacity_map
        cap[e1] = capacity_i ? (*capacity_i++) : (*capacity_d++);
        cap[e2] = 0;

        // fill in reverse_edge_map
        rev_edge[e1] = e2;
        rev_edge[e2] = e1;
    }

    double maxflow = 0;

    int vsrc = INTEGER(src)[0];
    int vsink = INTEGER(sink)[0];

    if ( 0 <= vsrc && vsrc < NV && 0 <= vsink && vsink < NV )
    {
    	vertex_descriptor s = vertex(vsrc, flow_g);
    	vertex_descriptor t = vertex(vsink, flow_g);

    	if ( method == E_MF_Push_Relabel )
	     maxflow = push_relabel_max_flow(flow_g, s, t);
	else if ( method == E_MF_Edmonds_Karp )
             maxflow = edmonds_karp_max_flow(flow_g, s, t);
	else if ( method == E_MF_Kolmogorov )
	{
	     error("kolmogorov_max_flow from BGL doesn't work");
	     //maxflow = kolmogorov_max_flow(flow_g, s, t);
	}
	else
	     error("unknown method for max_flow");
    }

    SEXP ansList, conn, eList, fList;
    PROTECT(ansList = allocVector(VECSXP,3));
    PROTECT(conn = NEW_NUMERIC(1));
    PROTECT(eList = allocMatrix(INTSXP, 2, asInteger(num_edges_in)));
    PROTECT(fList = allocMatrix(REALSXP, 1, asInteger(num_edges_in)));

    REAL(conn)[0] = maxflow;

    edge_iterator ei, e_end;
    int i = 0, j = 0;
    for (tie(ei, e_end) = edges(flow_g); ei != e_end; ++ei)
        if (cap[*ei] > 0) {
            INTEGER(eList)[i++] = source(*ei, flow_g);
            INTEGER(eList)[i++] = target(*ei, flow_g);
            REAL(fList)[j++] = cap[*ei] - res_cap[*ei];
        }

    SET_VECTOR_ELT(ansList,0,conn);
    SET_VECTOR_ELT(ansList,1,eList);
    SET_VECTOR_ELT(ansList,2,fList);
    UNPROTECT(4);

    return(ansList);
}

extern "C"
{
    SEXP BGL_min_cut_U (SEXP num_verts_in, SEXP num_edges_in,
                        SEXP R_edges_in,   SEXP R_weights_in )
    {
        using namespace boost;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        typedef graph_traits < Graph_ud >::vertex_descriptor Vertex;
        typedef graph_traits < Graph_ud >::edges_size_type dst;
        std::vector<Vertex> s_set, vs_set;
        dst cut_capacity = min_cut(g, std::back_inserter(s_set), std::back_inserter(vs_set) );

        SEXP ansList, conn;
        SEXP sList, vsList;  // for subsets of nodes in mincut: S, V - S

        PROTECT(ansList = allocVector(VECSXP,3));
        PROTECT(conn = NEW_NUMERIC(1));
        PROTECT(sList = allocVector(INTSXP, s_set.size()));
        PROTECT(vsList = allocVector(INTSXP,vs_set.size()));

        REAL(conn)[0] = (double)cut_capacity;

        std::vector<Vertex>::iterator vi;
        int i = 0;
        for ( i = 0, vi = s_set.begin(); vi != s_set.end(); i++, ++vi)
            INTEGER(sList)[i] = *vi;

        for ( i = 0, vi = vs_set.begin(); vi != vs_set.end(); i++, ++vi)
            INTEGER(vsList)[i] = *vi;

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,sList);
        SET_VECTOR_ELT(ansList,2,vsList);
        UNPROTECT(4);

        return(ansList);
    }

    SEXP BGL_edmonds_karp_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Edmonds_Karp);
        return(ansList);
    }

    SEXP BGL_push_relabel_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Push_Relabel);
        return(ansList);
    }

    SEXP BGL_kolmogorov_max_flow(SEXP num_verts_in, SEXP num_edges_in,
                                   SEXP R_edges_in, SEXP R_capacity_in,
				   SEXP src, SEXP sink )
    {
        SEXP ansList = BGL_max_flow_internal(num_verts_in, num_edges_in,
                                             R_edges_in, R_capacity_in,
					     src, sink, E_MF_Kolmogorov);
        return(ansList);
    }

}


/* ------------Includes---------------------- */
#include <cmath>
#include <iostream>
#include <utility>
#include <unistd.h>
#include "../include/CAN_utils.h"
#include "../include/CO_message.h"
#include "../include/vehicle.h"
#include "../include/motor.h"
#include "../include/caster.h"
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <omniveyor_common/definitions.h>

// #define VELOCITY_SATURATION
// #define SEND_ZERO_TORQUES
#define CLAMP_TORQUES

using std::cout;
using std::endl;

/* --------------------------- STATIC FUNCTION PROTOTYPES ------------------------------- */
static double casterPosXSign(int casterNum);
static double casterPosYSign(int casterNum);
static Eigen::Vector3d clampVelocity(Eigen::Vector3d xd_com_in);
static Eigen::Vector3d clampAcceleration(Eigen::Vector3d xdd_com_in);
static float saturate(float x);

/* --------------------------- PUBLIC MEMBER FUNCTIONS ------------------------------- */

/* Constructor. Caster number starts at 1 */
Vehicle::Vehicle()
{
	_initialized = false;
	_enabled = false;
	_stopped = false;
	_control = VELOCITY;

	_x_local.setZero();
	_xd_local.setZero();
	_gx.setZero();
	_gxd.setZero();
	_gxd_com.setZero();
	_gxd_des.setZero();
	_gxdd.setZero();

	_g_cf.setZero();
	_cf_des_local.setZero();

	_q_steer.setZero();
	_qd.setZero();
	_qd_des.setZero();
	_tq.setZero();
	_tq_des.setZero();
	_lambda.setZero();
	_mu.setZero();
	_C.setZero();
	_Cp.setZero();
	_Jq.setZero();
	_C_pinv.setZero();

	_heading = 0;

	_Kp.setZero(); // zero gains for torque control initially
	_Kv.setZero(); // zero gains for torque control initially

	// create casters
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		casters[i] = new Caster(i+1);
	}

	/* create a socket for the control thread */
	s = create_can_socket (UNUSED_NODE_ID, 0xF);
	if (s < 0)
	{
		perror ("Unable to open control socket\n");
		exit (-1);
	}
	//CO_Set_bitrate(s, 500000);
}

/* Destructor */
Vehicle::~Vehicle()
{
	_initialized = false;
	/* quick stop to motors - wait (x amount of time)*/

	/* destroy caster objects */
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		delete casters[i];
	}
	/* reset comm */
	struct CO_message msg;
	msg.type = NMT;
	msg.m.NMT.data = 0x81;
	CO_send_message (s, 0, &msg);
	shutdown(s,1);
	//close(s);
}

/* Intializes all casters */
int Vehicle::init()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->init())
		{
			return -1;
		}
	}
	_initialized = true;
	return 0;
}

/* Enable all casters*/
int Vehicle::enable()
{
	if(!_initialized)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->enable())
		{
			return -1;
		}
	}

	_enabled = true;
	_stopped = false;
	return 0;
}

/* Disable all casters */
int Vehicle::disable()
{
	if(!_initialized)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->disable())
		{
			return -1;
		}
	}

	_enabled = false;
	return 0;
}

/* Stop all motors */
// HAVEN'T GOTTEN THIS TO WORK YET - CAN USE WHATEVER METHOD YOU WANT
int Vehicle::stop()
{
	if (!_enabled)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->stop() == 0)
		{
			cout << "stopping caster " << i << endl;
			// return -1;
		}
	}

	_stopped = true; // wait for stop?
	return 0;
}

int Vehicle::start()
{
	if (!_enabled)
	{
		return -1;
	}

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->start() == 0)
		{
			cout << "starting caster " << i << endl;
			// return -1;
		}
	}

	_stopped = false; // wait for stop?
	return 0;
}

/* Set control mode to velocity or torque control.
 * This can only be set when the motors are disabled.
 */
int Vehicle::setCtrlMode(enum ctrl_mode cm)
{
	if(_enabled && !_stopped)
	{
		return -1; // only set mode if disabled
	}

	// Set mode
	_control = cm;
	cout << "Setting vehicle control mode to : " << cm << endl;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setCtrlMode(cm))
		{
			return -1;
		}
	}

	return 0;
}

/* Update the local and global coordinates of the
 * operational point of the mobile base
 */
void Vehicle::updateOdometry(){
	updateJointData();
	// Eigen::Matrix<double, NUM_MOTORS, 1> deltaQ = getDeltaQ();

	// Calculate C_p^+
	updateConstraintPinvMatrix();

	// Find velocity and position of the base in the frame of the base with respect to the world origin
	// _xd_local = _C_pinv * deltaQ / CONTROL_PERIOD_s;
	_xd_local = _C_pinv * _qd;
	_x_local += _xd_local * CONTROL_PERIOD_s;

	_heading = _gx(2)+ 0.5*_xd_local(2)*CONTROL_PERIOD_s;

	// Rotation matrix to convert vectors from local frame to global frame
	Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
	R(0, 0) =  cos(_heading);
    R(0, 1) = -sin(_heading);
    R(1, 0) =  sin(_heading);
    R(1, 1) =  cos(_heading);
    R(2, 2) =  1.0;

    _gxd = R * _xd_local;
    _gx += _gxd * CONTROL_PERIOD_s;
}

/* Returns the change in steer and roll angle */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDeltaQ(){
	Eigen::Matrix<double, NUM_MOTORS, 1> deltaQ;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		deltaQ(2*i) = casters[i]->getSteerDeltaQ();
		deltaQ(2*i + 1) = casters[i]->getRollDeltaQ();
	}
	return deltaQ;
}

/* Set vehicle velocities in x, y, and theta. vx,vy in m/s and vth in rad/s */
// (6.1)
int Vehicle::setGlobalVelocity(Eigen::Vector3d xd_com_in)
{
	// Retrieve latest joint data
	updateJointData();

	// Calculate C
	updateConstraintMatrix();

	// Store velocity command and determine current xd_des (limit acceleration)
	_gxd_com = clampVelocity(xd_com_in);
	_gxd_des = _gxd_des + clampAcceleration(_gxd_com - _gxd_des);

	// Calculate joint velocities to command
	_qd_des = _C * _gxd_des;

	// Set velocities for each caster
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setVelocities(_qd_des(2*i),_qd_des(2*i+1)))
		{
			return -1;
		}
	}

	return 0;
}

/* Set desired vehicle position, velocity, and acceleration for torque controller */
// (6.4)
void Vehicle::setTargets(Eigen::Array3d gx_des_in, Eigen::Array3d gxd_des_in, Eigen::Array3d gxdd_des_in)
{
	// update _lambda and mu
	updateDynamics();

	_gx_des   = gx_des_in;
	_gxd_des  = gxd_des_in;
	_gxdd_des = gxdd_des_in;

	// Calculate and set control forces/torques
#ifdef VELOCITY_SATURATION
	Eigen::Array3d dxd;
	dxd = _Kp / _Kv * (gx_des_in - _gx.array());
	Eigen::Array3d nu;
	nu(0) = saturate(MAX_VEL_X/abs(dxd(0)));
	nu(1) = saturate(MAX_VEL_Y/abs(dxd(1)));
	nu(2) = saturate(MAX_VEL_TH/abs(dxd(2)));
	Eigen::Vector3d g_cf_unit = -_Kv * (_gxd.array() - nu * dxd);
#else
	Eigen::Vector3d g_cf_unit = -_Kp * (_gx.array() - gx_des_in) - _Kv * (_gxd.array() - gxd_des_in) + gxdd_des_in;

#endif

	// Rotation matrix to convert vector from global frame to local frame
	Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
	R(0, 0) = cos(_heading);
    R(0, 1) = sin(_heading);
    R(1, 0) = -sin(_heading);
    R(1, 1) = cos(_heading);
    R(2, 2) = 1.0;

	// Map global command to local coordinates
	Eigen::Array3d cf_unit_local = R * g_cf_unit;

	_cf_des_local = _lambda * cf_unit_local.matrix();
	// cf_des = _lambda * cf_unit.matrix() + mu;

	setTorque(_cf_des_local);
}

/* set _Kp gains */
void Vehicle::setKp(Eigen::Array3d Kp_des){
	_Kp = Kp_des;
}

/* set Kv gains */
void Vehicle::setKv(Eigen::Array3d Kv_des){
	_Kv = Kv_des;
}

/* Get control mode (velocity/torque) */
enum ctrl_mode Vehicle::getCtrlMode() const
{
	return _control;
}

/* Get position in m for x and y and rad for theta */
Eigen::Vector3d Vehicle::getGlobalPosition() const
{
	return _gx;
}

Eigen::Vector3d Vehicle::getLocalPosition() const
{
	return  _x_local;
}

/* Get vehicle velocity in m/s for x and y and rad/s for theta */
Eigen::Vector3d Vehicle::getGlobalVelocity() const
{
	return _gxd;
}

Eigen::Vector3d Vehicle::getLocalVelocity() const
{
	return _xd_local;
}

/* Get joint steering angles in rad */
Eigen::Matrix<double, NUM_CASTERS, 1> Vehicle::getJointSteeringAngles() const
{
	// updateJointData();
	return _q_steer;
}

/* Get desired joint velocities in rad/s */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDesJointVelocities() const
{
	return _qd_des;
}

/* Get joint velocities in rad/s */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getJointVelocities() const
{
	return _qd;
}

/* Get vehicle (operational space) forces in x and y in N and torque in theta in Nm */
Eigen::Vector3d Vehicle::getLocalCommandForces() const
{
	return _cf_des_local;
}

/* Get joint torques in Nm */
Eigen::Matrix<double, NUM_MOTORS, 1> Vehicle::getDesJointTorques() const
{
	return _tq_des;
}

/* Get initialization status of vehicle */
bool Vehicle::isInitialized() const
{
	return _initialized;
}

/* Get enable status of vehicle */
bool Vehicle::isEnabled() const
{
	return _enabled;
}

/* Get status of vehicle */
bool Vehicle::isStopped() const
{
	return _stopped;
}

Eigen::Matrix3d Vehicle::getLambda() const
{
	return _lambda;
}

Eigen::MatrixXd Vehicle::getCPinv() const
{
	return _C_pinv;
}

/* Returns true if at least one bumper is hit */
bool Vehicle::isBumperHit() const
{
	bool retVal = false;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if (casters[i]->getBumperState()){
			retVal = true;
		}
	}
	return retVal;
}

/* Returns a 4D vector containing the bumper state of each bumper */
Eigen::Vector4d Vehicle::getBumperState() const
{
	Eigen::Vector4d bumper_state;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		bumper_state[i] = casters[i]->getBumperState();
	}
	return bumper_state;
}

double Vehicle::getHeading() const
{
	return _heading;
}

void Vehicle::getElectricalStatus (robotElectrical_T *status)
{
	for (int i=0; i<NUM_CASTERS; i++){
		casters[i]->getAmps(&(status->steerMotorCurrent[i]), &(status->rollMotorCurrent[i]));
		casters[i]->getVolts(&(status->steerMotorVoltage[i]), &(status->rollMotorVoltage[i]));
	}
}

bool Vehicle::reachedTarget(Eigen::Vector3d curr_pos, Eigen::Vector3d curr_target, double max_linear_dist, double max_rot_dist) const
{
	return ((fabs(curr_pos[0]-curr_target[0]) < max_linear_dist) && (fabs(curr_pos[1]-curr_target[1]) < max_linear_dist) && (fabs(curr_pos[2]-curr_target[2]) < max_rot_dist));
}

/* --------------------------- PRIVATE MEMBER FUNCTIONS ------------------------------- */

/* Input: operational space forces fx,fy (N) and torque tz (Nm), all in local frame */
/* Outputs torques to casters */
int Vehicle::setTorque(const Eigen::Vector3d& cf_command)
{
	// Retrieve latest joint data
	updateJointData();

	// Calculate C#
	//updateConstraintMatrix();

	// Calculate C_p^+
	updateConstraintPinvMatrix();

	// Calculate joint torques
	// (6.11)
	_tq_des = _C_pinv.transpose() * cf_command;

#ifdef CLAMP_TORQUES
	clampTorque();
#endif

#ifdef SEND_ZERO_TORQUES
	_tq_des.setZero();
#endif

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		if(casters[i]->setTorques(_tq_des(2*i),_tq_des(2*i+1)))
		{
			return -1;
		}
	}
	return 0;
}

/* Update joint data */
void Vehicle::updateJointData()
{
	std::pair<double,double> jointVel;
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		_q_steer(i) = casters[i]->getSteerPosition();
		jointVel = casters[i]->getVelocities();
		_qd(2*i) = jointVel.first;
		_qd(2*i + 1) = jointVel.second;
	}
}

/* Mass Matrix taken from pg 20 of PCV Dyanmics.pdf*/
void Vehicle::updateDynamics()
{
	_lambda = Eigen::Matrix3d::Zero();
	_mu = Eigen::Vector3d::Zero();
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Calculate relevant matrices and vectors
		Eigen::Matrix3d C_th = calcC_thMatrix(i);
		Eigen::Matrix3d A = calcAMatrix();
		Eigen::Vector3d p = calcpVector(i);
		Eigen::Matrix3d J_dot = calcJ_dotMatrix(i);
		Eigen::Vector3d Q_dot = calcQ_dotVector(i);

		//Calculate lambda_pumpkin and mu_pumpkin
		Eigen::Matrix3d lambda_pumpkin = calcLambdaPumpkinMatrix(i);
		Eigen::Vector3d mu_pumpkin = calcMuPumpkinVector(i);

		Eigen::Matrix3d lambda_i = C_th.transpose() * A * C_th + lambda_pumpkin;
		_lambda += lambda_i;

		Eigen::Vector3d mu_i = C_th.transpose()*(p - A*C_th*J_dot*Q_dot) + mu_pumpkin;
 		_mu += mu_i;
	}
	_lambda += calcLambdaVehicleMatrix();
}

/* Update kinematic constraint matrix, C#. Assumes joint data is up to date.
   Converts from operational space (xdot) to joint space (qdot). See pg 27 PCV_Dyanmics.pdf*/
void Vehicle::updateConstraintMatrix()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

		// Take sin/cos of steeering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Compute steer row
		/**************** Alternative sign convention *******************/
		// C(steerRow,0) = -steerSin / PC_b;
		// C(steerRow,1) = steerCos / PC_b;
		// C(steerRow,2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;
		/****************************************************************/
		_C(steerRow,0) = steerSin / PC_b;
		_C(steerRow,1) = -steerCos / PC_b;
		_C(steerRow,2) = -(hx * steerCos + hy * steerSin) / PC_b - 1.0;

		// Compute roll row
		_C(rollRow,0) = steerCos / PC_r;
		_C(rollRow,1) = steerSin / PC_r;
		_C(rollRow,2) = (hx * steerSin - hy * steerCos) / PC_r;
	}
}

/* Update Moore-Penrose pseudoinverse of the constraint matrix C_p. Assumes constraint matrix, C#, is up to date */
void Vehicle::updateConstraintPinvMatrix()
{
	// Update Cp matrix with current data
	updateCpMatrix();

	Eigen::MatrixXd CptCli = Eigen::MatrixXd::Zero(3, NUM_MOTORS);

	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Cli IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
	    // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

	    int j = 2*i;

	    // Note: sign of p-dot is: wheel as viewed from ground
	    CptCli(0, j ) =  PC_b * steerSin;
	    CptCli(0,j+1) =  PC_r * steerCos;
	    CptCli(1, j ) = -PC_b * steerCos;
	    CptCli(1,j+1) =  PC_r * steerSin;
	    CptCli(2, j ) = -PC_b * ((hx * steerCos + hy * steerSin) + PC_b);
	    CptCli(2,j+1) =  PC_r * (hx * steerSin - hy * steerCos);
	}

	_C_pinv = (_Cp.transpose()*_Cp).llt().solve(CptCli);
}

Eigen::Matrix3d Vehicle::calcAMatrix(){
	Eigen::Matrix3d A;
	A(0, 0) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih + PC_Is*pow(Ns, 2) + PC_It*Nr;
	A(0, 1) = PC_Ii*Nw - PC_Ih*Nw - PC_It*pow(Nr, 2)*Nw;
	A(0, 2) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih - PC_Is*Ns - PC_It*Nr;

	A(1, 0) = A(0, 1);
	A(1, 1) = PC_Mf*pow(PC_r, 2) + PC_Ii*pow(Nw, 2) + PC_Ih*pow(Nw, 2) + PC_It*pow(Nr, 2)*pow(Nw, 2) + PC_Ij;
	A(1, 2) = PC_Ii*Nw - PC_Ih*Nw + PC_It*Nr*Nw;

	A(2, 0) = A(0, 2);
	A(2, 1) = A(1, 2);
	A(2, 2) = PC_Mf*pow(PC_e, 2) + PC_If + PC_Ii + PC_Ih + PC_Is + PC_It;
	return A;
}

// PCV Dynamics page 21
Eigen::Vector3d Vehicle::calcpVector(int caster_no){
	double steer_d = _qd(2*caster_no);
	double roll_d  = _qd(2*caster_no+1);
	double gth_d   = _gxd(2);

	Eigen::Vector3d p;
	p(0) = PC_Mf*PC_r*PC_e*roll_d*(steer_d+gth_d);
	p(1) = -PC_Mf*PC_r*PC_e*pow((steer_d+gth_d), 2);
	p(2) = PC_Mf*PC_r*PC_e*roll_d*(steer_d+gth_d);
	return p;
}

// From PCV Dynamics page 29
Eigen::Matrix3d Vehicle::calcC_thMatrix(int caster_no)
{
	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	// Take sin/cos of steering angle
	double steerSin = sin(_q_steer(caster_no));
	double steerCos = cos(_q_steer(caster_no));

	Eigen::Matrix3d C_th;
	/***  Alternate sign convention ***/
	// C_th(0, 0) = -steerSin / PC_b;
	// C_th(0, 1) = steerCos / PC_b;
	// C_th(0, 2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;
	/**********************************/
	C_th(0, 0) = steerSin / PC_b;
	C_th(0, 1) = -steerCos / PC_b;
	C_th(0, 2) = -(hx * steerCos + hy * steerSin) / PC_b - 1.0;

	C_th(1, 0) = steerCos / PC_r;
	C_th(1, 1) = steerSin / PC_r;
	C_th(1, 2) = (hx * steerSin - hy * steerCos) / PC_r;

	C_th(2, 0) = 0.0;
	C_th(2, 1) = 0.0;
	C_th(2, 2) = 1.0;

	return C_th;
}

// From PCV Dynamics page 25
Eigen::Matrix3d Vehicle::calcJ_dotMatrix(int caster_no)
{
	Eigen::Matrix3d J_dot;
	double steer_d = _qd(2*caster_no);
	double gth_d = _gxd(2);

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	// Take sin/cos of steeering angle
	double steerSin = sin(_q_steer(caster_no));
	double steerCos = cos(_q_steer(caster_no));

	// Take sin/cos of steer angle velocity plus PCV angle velocity
	double Sum = steer_d + gth_d;

	J_dot(0, 0) = PC_b * steerCos * Sum;
	J_dot(0, 1) = -PC_r * steerSin * Sum;
	J_dot(0, 2) = hx * gth_d + PC_b * steerCos * Sum;

	J_dot(1, 0) = PC_b * steerSin * Sum;
	J_dot(1, 1) = PC_r * steerCos * Sum;
	J_dot(1, 2) = hy * gth_d + PC_b * steerSin * Sum;

	J_dot(2, 0) = 0.0;
	J_dot(2, 1) = 0.0;
	J_dot(2, 2) = 0.0;

	return J_dot;
}

Eigen::Matrix3d Vehicle::calcLambdaPumpkinMatrix(int caster_no)
{
	Eigen::Matrix3d lambda_pumpkin = Eigen::Matrix3d::Zero();

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	lambda_pumpkin(0, 0) = PC_Mp;
	lambda_pumpkin(0, 2) = -PC_Mp*hy;

	lambda_pumpkin(1, 1) = PC_Mp;
	lambda_pumpkin(1, 2) = PC_Mp*hx;

	lambda_pumpkin(2, 0) = lambda_pumpkin(0, 2);
	lambda_pumpkin(2, 1) = lambda_pumpkin(1, 2);
	lambda_pumpkin(2, 2) = PC_Ip + PC_Mp*pow(DIST_TO_CASTER ,2);
	return lambda_pumpkin;
}

Eigen::Matrix3d Vehicle::calcLambdaVehicleMatrix()
{
	Eigen::Matrix3d lambda_vehicle = Eigen::Matrix3d::Zero();

	lambda_vehicle(0, 0) = PC_Mv; // Effective mass along the x direction is the mass of the vehicle
	lambda_vehicle(1, 1) = PC_Mv; // Effective mass along the y direction is the mass of the vehicle
	lambda_vehicle(2, 2) = PC_Iv; // Inertia of the vehicle about the z axis

	return lambda_vehicle;
}

Eigen::Vector3d Vehicle::calcQ_dotVector(int caster_no)
{
	Eigen::Vector3d Q_dot;

	double steer_d = _qd(2*caster_no);
	double roll_d = _qd(2*caster_no+1);
	double gth_d = _gxd(2);
	Q_dot << steer_d, roll_d, gth_d;

	return Q_dot;
}

Eigen::Vector3d Vehicle::calcMuPumpkinVector(int caster_no)
{
	Eigen::Vector3d mu_pumpkin;
	double gth_d = _gxd(2);

	// Caster position values
	double hx = casterPosXSign(caster_no+1) * DIST_TO_CASTER_X;
	double hy = casterPosYSign(caster_no+1) * DIST_TO_CASTER_Y;

	mu_pumpkin << -PC_Mp * hx * pow(gth_d, 2), -PC_Mp * hy * pow(gth_d, 2), 0.0;

	return mu_pumpkin;
}

/* Update Jq matrix. Transforms q_d's into p_d's (p_d = Jq*q_d). In documentation, Jq = C_q^{-1} */
void Vehicle::updateJqMatrix()
{
	_Jq = Eigen::MatrixXd::Zero(NUM_MOTORS, NUM_MOTORS);
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Which rows to calculate
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Compute steer row
		_Jq(steerRow,steerRow) = steerSin * PC_b;
		_Jq(steerRow,rollRow) = steerCos * PC_r;

		// Compute roll row
		_Jq(rollRow,steerRow) = -steerCos * PC_b;
		_Jq(rollRow,rollRow) = steerSin * PC_r;
	}
}

/* Update Cp matrix. Transforms x_d's into p_d's (p_d = C_p*x_d) */
void Vehicle::updateCpMatrix()
{
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		// Which rows to calculate
		int steerRow = 2*i;
		int rollRow = 2*i + 1;

		// Take sin/cos of steering angle
		double steerSin = sin(_q_steer(i));
		double steerCos = cos(_q_steer(i));

		// Caster position values
		double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
		double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

		// Cp = Jq*C, could calculate it this way instead
		// Compute steer row
		_Cp(steerRow,0) = 1.0;
		_Cp(steerRow,1) = 0.0;
		_Cp(steerRow,2) = -PC_b * steerSin - hy;

		// Compute roll row
		_Cp(rollRow,0) = 0.0;
		_Cp(rollRow,1) = 1.0;
		_Cp(rollRow,2) = PC_b * steerCos + hx;
	}
}

// Clamp the torque command to max values
void Vehicle::clampTorque(){
	// Clamp torque values to maximum allowed
	for(int i = 0; i < NUM_CASTERS; i++)
	{
		_tq_des(2*i)   = abs(_tq_des(2*i))   < MAX_STEER_TORQUE  ? _tq_des(2*i)   : copysign(MAX_STEER_TORQUE, _tq_des(2*i));
		_tq_des(2*i+1) = abs(_tq_des(2*i+1)) < MAX_ROLL_TORQUE   ? _tq_des(2*i+1) : copysign(MAX_ROLL_TORQUE,  _tq_des(2*i+1));
	}
}

/* ------------ Static functions-------------- */
// Determine sign of x position of caster based on number
static double casterPosXSign(int casterNum)
{
	switch(casterNum)
	{
		case 1:
		case 2:
			return 1.0;
		case 3:
		case 4:
			return -1.0;
		default:
			return 0;
	}
}

// Determine sign of y position of caster based on number
static double casterPosYSign(int casterNum)
{
	switch(casterNum)
	{
		case 2:
		case 3:
			return 1.0;
		case 1:
		case 4:
			return -1.0;
		default:
			return 0;
	}
}

/*  Saturation function
	Returns the input float value if its absolute value is less than or equal to 1.
	Otherwise, it returns 1.0 or -1.0 (depending on the sign of float x) */
static float saturate(float x) {
	x = abs(x) <= 1.0 ? x : copysign(1.0, x);
	return x;
}

// Function to clamp velocity command to max values
static Eigen::Vector3d clampVelocity(Eigen::Vector3d xd_com_in){
	// Clamp velocity values to maximum allowed
	xd_com_in(0) = abs(xd_com_in(0)) < MAX_VEL_TRANS ? xd_com_in(0) : copysign(MAX_VEL_TRANS, xd_com_in(0));
	xd_com_in(1) = abs(xd_com_in(1)) < MAX_VEL_TRANS ? xd_com_in(1) : copysign(MAX_VEL_TRANS, xd_com_in(1));
	xd_com_in(2) = abs(xd_com_in(2)) < MAX_VEL_ROT ? xd_com_in(2) : copysign(MAX_VEL_ROT, xd_com_in(2));

	return xd_com_in;
}

// Function to clamp acceleration command to max values (accel is change in xd_des for one control loop here)
static Eigen::Vector3d clampAcceleration(Eigen::Vector3d xdd_com_in){

	// Clamp acceleration values to maximum allowed

	// Check accel/decel for x
	if(xdd_com_in(0) >= 0){
		// Clamp trans accel
		xdd_com_in(0) = xdd_com_in(0) < MAX_VEL_TRANS_INC ? xdd_com_in(0) : MAX_VEL_TRANS_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(0) = xdd_com_in(0) > -MAX_VEL_TRANS_DEC ? xdd_com_in(0) : -MAX_VEL_TRANS_DEC;
	}

	// Check accel/decel for y
	if(xdd_com_in(1) >= 0){
		// Clamp trans accel
		xdd_com_in(1) = xdd_com_in(1) < MAX_VEL_TRANS_INC ? xdd_com_in(1) : MAX_VEL_TRANS_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(1) = xdd_com_in(1) > -MAX_VEL_TRANS_DEC ? xdd_com_in(1) : -MAX_VEL_TRANS_DEC;
	}

	// Check accel/decel for theta
	if(xdd_com_in(2) >= 0){
		// Clamp trans accel
		xdd_com_in(2) = xdd_com_in(2) < MAX_VEL_ROT_INC ? xdd_com_in(2) : MAX_VEL_ROT_INC;
	}
	else{
		// Clamp trans decel
		xdd_com_in(2) = xdd_com_in(2) > -MAX_VEL_ROT_DEC ? xdd_com_in(2) : -MAX_VEL_ROT_DEC;
	}

	return xdd_com_in;
}

/*********************** TEST HARNESS *****************************************/

#ifdef TEST_VEHICLE

// Different types of tests to run
enum test_type
{
	TEST_INIT,
	TEST_CONSTRAINT_MATRIX,
	TEST_DYNAMICS
};

int main (void)
{
	cout << "Begin vehicle test harness" << endl;

	/* instantiate vehicle */
	Vehicle *vehicle = new Vehicle ();

	// Which test to run
	enum test_type thisTest = TEST_DYNAMICS;

	int s = create_can_socket (0xF, 0xF);

	// Switch on type
	switch(thisTest){

		case TEST_DYNAMICS:
		{
			cout << "Testing Vehicle Dynamics" << endl;

		}

		case TEST_INIT:
			cout << "Testing Vehicle Initialization" << endl;

			if (vehicle->init())
				cout << "Vehicle initialization failed" << endl;
			else
				cout << "Vehicle initialization success" << endl;

			getchar ();
			break;

		case TEST_CONSTRAINT_MATRIX:
			{
				cout << "Testing Vehicle constraint matrix" << endl;

				Eigen::Matrix<double, NUM_MOTORS, 3> C; 		 		// constraint matrix
				Eigen::Matrix<double, NUM_CASTERS, 5> _q_steer;
				Eigen::Vector3d xd1;
				Eigen::Vector3d xd2;
				xd1 << 1,0,0;
				xd2 << 0,1,0;

				_q_steer << 0,0,0,0,
								   M_PI/2, M_PI/2, M_PI/2, M_PI/2,
									 M_PI,M_PI,M_PI,M_PI,
									 3*M_PI/2,3*M_PI/2,3*M_PI/2,3*M_PI/2,
									 -M_PI/2, -M_PI/2, -M_PI/2, -M_PI/2;

				// Re-compute constraint matrix C
				for(int j = 0; j < 5; j++){
					cout << "q_steer = [";
					for(int i = 0; i < NUM_CASTERS; i++)
					{
						// Which rows to calculate
						int steerRow = 2*i;
						int rollRow = 2*i + 1;

						// Caster position values
						double hx = casterPosXSign(i+1) * DIST_TO_CASTER_X;
						double hy = casterPosYSign(i+1) * DIST_TO_CASTER_Y;

						// Take sin/cos of steeering angle
						double steerSin = sin(_q_steer(i,j));
						double steerCos = cos(_q_steer(i,j));

						// Compute steer row
						C(steerRow,0) = -steerSin / PC_b;
						C(steerRow,1) =  steerCos / PC_b;
						C(steerRow,2) = (hx * steerCos + hy * steerSin) / PC_b - 1.0;

								// Compute roll row
						C(rollRow,0) = steerCos / PC_r;
						C(rollRow,1) = steerSin / PC_r;
						C(rollRow,2) = (hx * steerSin - hy * steerCos) / PC_r;

						cout << _q_steer(i,j) << ", ";
					}

					cout << "]" << endl;
					cout << endl << "C = " << endl;
					cout << C << endl;
					cout << "xd1 = " << xd1.transpose() << endl;
					cout << "qd1 = " << (C*xd1).transpose() << endl;
					cout << "xd2 = " << xd2.transpose() << endl;
					cout << "qd2 = " << (C*xd2).transpose() << endl;

					getchar();
				}
			}
			break;

		default:
			break;
	}

	// Delete caster
	delete vehicle;

	getchar ();
	cout << "End vehicle test harness" << endl;
	return 0;
}

#endif


//
// Created by philipp on 1/16/20.
//

#ifndef FUNNELS_CPP_EX1_FIXED_HH
#define FUNNELS_CPP_EX1_FIXED_HH

#include <Eigen/Core>
#include <cmath>
#include <sstream>
#include <algorithm>
#include <array>

#include <funnels/utils.hh>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795d
#endif

// Attention, in example 1, velocity coincides with the control
// signal
// Attention, assumes constant velocity/input

const double max_r_step = 0.5; // different funnel sizes in a family
const double min_r = 0.001; // Minimal radius
const double max_r = 0.5+5.*min_r; // Minimal radius
const double max_alpha_step = max_r_step*max_r_step; // Minimal step for alpha
const double min_alpha = min_r*min_r; // Minimal alpha for ellipsoids
const double max_alpha = max_r*max_r; // Minimal alpha for ellipsoids
const double max_vel = 1.; // Norm of velocity
const double min_vel_diff = 0.1; // Norm difference in velocity
const double min_ang_diff = 20.*M_PI/180.;
const double gamma_conv = 10.; // exponential convergence rate
const double max_traj_length = 3; // Length of the new segment
const double dt_step = 0.05; //time step between two verification points
const size_t n_max = 100;
const double max_time = 100.;
const double min_englobe_fac = 1.1;

const double plane_dist = 0.3;
const double inter_dist = 0.3;

Eigen::Matrix2d getR(double alpha);

template <class FUN_PTR_T>
std::pair<FUN_PTR_T, FUN_PTR_T> get_this_funnel(const FUN_PTR_T &src, double radius,
                          const Eigen::Vector4d &x0_O, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  Eigen::Vector4d x0 = x0_O;
  double n_u, t_max;
  if (u.norm() > 1e-3){
    n_u = u.norm()+1e-200;
    t_max = std::min(max_time, max_traj_length/n_u);
    // distance to x/y-border
    for (size_t i=0; i<2; i++){
      if(u(i)>=0.){
        t_max = std::min(t_max, (1.2-x0(i))/(u(i)+1e-200));
      }else{
        t_max = std::min(t_max, (-1.2-x0(i))/(u(i)+1e-200));
      }
    }
    if (t_max < 3.*dt_step){
      // trajectory is too short
      // initial is on the boundary and we are pointing outwards
      return std::pair(nullptr, nullptr);
    }
  }else{
    t_max = 3.*dt_step;
  }

  // Convert to number of points
  size_t this_n_verif = std::max((size_t)3, std::min((size_t) (t_max/dt_step), n_max));

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  // Update the initial pos
  x0 = x0_O;

  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return std::pair(nullptr, nullptr);
  }

  FUN_PTR_T new_fun1 = src->make_copy(name, 2.*this_n_verif);
  FUN_PTR_T new_fun2 = src->make_copy(name+"_twin", 2.*this_n_verif);

  // set the new radius
  new_fun1->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  new_fun2->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  // and convergence
  new_fun1->set_gamma(gamma_conv);
  new_fun2->set_gamma(gamma_conv);

  // Update pos and compute traj
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun1->compute(x0, 0., 2.*t_max, u);
  // Mirror
  x0 = -x0_O;
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun2->compute(x0, 0., 2.*t_max, u);

  // He starts a family
  new_fun1->start_family(new_fun1);
  new_fun2->start_family(new_fun2);

  return std::pair(new_fun1, new_fun2);
}

template <class FUN_PTR_T>
FUN_PTR_T get_this_funnel_1(const FUN_PTR_T &src, double radius,
    const Eigen::Vector4d &x0_O, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  Eigen::Vector4d x0 = x0_O;
  double n_u, t_max;
  if (u.norm() > 1e-3){
    n_u = u.norm()+1e-200;
    t_max = std::min(max_time, max_traj_length/n_u);
    // distance to x/y-border
    for (size_t i=0; i<2; i++){
      if(u(i)>=0.){
        t_max = std::min(t_max, (1.2-x0(i))/(u(i)+1e-200));
      }else{
        t_max = std::min(t_max, (-1.2-x0(i))/(u(i)+1e-200));
      }
    }
    if (t_max < 3.*dt_step){
      // trajectory is too short
      // initial is on the boundary and we are pointing outwards
      return nullptr;
    }
  }else{
    t_max = 3.*dt_step;
  }

  // Convert to number of points
  size_t this_n_verif = std::max((size_t)3, std::min((size_t) (t_max/dt_step), n_max));

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  // Update the initial pos
  x0 = x0_O;

  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return nullptr;
  }

  FUN_PTR_T new_fun = src->make_copy(name, this_n_verif);

  // set the new radius
  new_fun->set_P(src->get_P() * (src->get_alpha() / (radius * radius)));
  // and convergence
  new_fun->set_gamma(gamma_conv);

  // Update pos and compute traj
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun->compute(x0, 0., 2. * t_max, u);
  // Mirror
  x0 = -x0_O;
  x0.block(2,0,2,1) -= -t_max*u;

  // He starts a family
  new_fun->start_family(new_fun);

  return new_fun;
}

template <class FUN_PTR_T>
std::vector<FUN_PTR_T> get_children(FUN_PTR_T &parent){

  using fun_t = typename FUN_PTR_T::element_type;
  using matrix_t = typename fun_t::matrix_t;

  FUN_PTR_T this_child_fun;
  FUN_PTR_T this_parent_fun = parent;

  std::stringstream sstream;
  std::string name;

  // Original unscaled matrix
  matrix_t P_p = parent->get_P()*parent->get_alpha();

  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  std::vector<FUN_PTR_T> fun_vec_child;

  const double alpha_step = max_alpha_step;
  const double r_step = max_r_step;

  // Radius of parent
  double alpha = parent->get_alpha();

  if(std::sqrt(alpha) < min_r + r_step){
    return fun_vec_child;
  }

  alpha = std::max(min_alpha, (std::sqrt(alpha)-r_step)*(std::sqrt(alpha)-r_step));

  while(alpha>=min_alpha){
    // Get the name
    sstream.str(std::string());
    sstream << "fun_";
    auto x00 = parent->x0();
    for(size_t i=0; i<4; i++){
      sstream << (double) x00(i) << "_";
    }
    sstream << std::sqrt(alpha);
    name = sstream.str();
    std::replace( name.begin(), name.end(), '.', 'p');
    std::replace( name.begin(), name.end(), '-', 'm');
    if (utils_ext::loc_map.find(name)){
      // Already exists
      return fun_vec_child;
    }

    this_child_fun = this_parent_fun->make_copy(name, this_parent_fun->size());
    // set the new radius
    this_child_fun->set_P(P_p/alpha);
    this_child_fun->set_cyclic(this_parent_fun->is_cyclic());
    // And convergence
    this_child_fun->set_gamma(gamma_conv);

    fun_t::set_parent(this_parent_fun, this_child_fun);
    fun_vec_child.emplace_back(this_child_fun);
    this_parent_fun = this_child_fun;
    this_child_fun = nullptr;
    // Update alpha
    alpha = (std::sqrt(alpha)-r_step); // New alpha as r
    if (alpha <= min_r){
      break; // Necessary as negative radii become positive in next step
    }
    alpha *= alpha; // Convert to alpha
  }
  return fun_vec_child;
}

template<class FUN_PTR_T, class FUN_SYS>
void add_new_funnels(const FUN_PTR_T& src,
                     double t_src, FUN_SYS &fun_sys){

  // Src is dummy, only to keep the same signature

  using fun_t = typename FUN_PTR_T::element_type;

  // Return val
  FUN_PTR_T fun_parent1, fun_parent2;
  std::vector<FUN_PTR_T> fun_vec_child;

  Eigen::Vector2d pos_0;

  Eigen::Vector2d orth_p, vel_p, vel_1 = Eigen::Vector2d::Zero();
  vel_1(0) = 1.;

  Eigen::Vector4d x0;

  // The problem is purely symmetric

  // Grid of static funnels
  std::array<int, 2> signs = {-1,1};
  x0.setZero();
  vel_p.setZero();
  for (double x_s = plane_dist/2.; x_s <= 1.; x_s+=plane_dist){
    for (double y_s = plane_dist/2.; y_s <= 1.; y_s+=plane_dist) {
      for (auto s_x : signs){
        for (auto s_y : signs){
          x0(0) = s_x*x_s;
          x0(1) = s_y*y_s;
          fun_parent1 = get_this_funnel_1(src, max_r, x0, vel_p);
          if (fun_parent1 != nullptr) {
            // add
            fun_sys.add_funnel(fun_parent1, true);
            // all_children
            fun_vec_child = get_children(fun_parent1);
            for (auto a_f : fun_vec_child) {
              fun_sys.add_funnel(a_f);
            }
          }
        }
      }
    }
  }

  // "Dynamic" funnels
  // Loop over all velocities <-> planes
  for (double n_vel=inter_dist; n_vel<=1.0; n_vel+=inter_dist) {
      // Loop over all angles
    for (double d_alpha = 0.; d_alpha <= 2 * M_PI - d_alpha / 2;
         d_alpha += min_ang_diff) {
      // New velocity
      vel_p = n_vel * (getR(d_alpha) * vel_1);
      // New orthogonal
      orth_p = getR(M_PI / 2.) * getR(d_alpha) * vel_1;
      // Loop over all funnel origins
      pos_0 = orth_p * (plane_dist / 2.);
      while (true) {
        if ((pos_0.array().abs() >= 1.).any()) {
          // Out of the box
          break;
        }

        // Now we have a new parent funnel candidate
        x0.block(0, 0, 2, 1) = pos_0;
        // todo refactor norm vs normalized
        x0.block(2, 0, 2, 1) = vel_p;
        // Parent
        std::tie(fun_parent1, fun_parent2) = get_this_funnel(src, max_r, x0,
                                                             vel_p);

        if (fun_parent1 != nullptr) {
          // add
          fun_sys.add_funnel(fun_parent1, true);
          // all_children
          fun_vec_child = get_children(fun_parent1);
          for (auto a_f : fun_vec_child) {
            fun_sys.add_funnel(a_f);
          }
        }
        if (fun_parent2 != nullptr) {
          // add
          fun_sys.add_funnel(fun_parent2, true);
          // all_children
          fun_vec_child = get_children(fun_parent2);
          for (auto a_f : fun_vec_child) {
            fun_sys.add_funnel(a_f);
          }
        }
        // Update
        pos_0 += orth_p * plane_dist;
      } // origin pos
    } // angle
  } // plane
  return;
}

#endif //FUNNELS_CPP_EX1_FIXED_HH


#include <iostream>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "util.hpp"

#include "elias_fano.hpp"
#include "mapper.hpp"

#include "perftest_common.hpp"

struct monotone_generator
{
    monotone_generator(uint64_t m, uint8_t bits, unsigned int seed)
	: m_gen(seed)
        , m_bits(bits)
    {
	m_stack.push_back(state_t(0, m, 0));
    }

    uint64_t next()
    {
	uint64_t cur_word, cur_m;
	uint8_t cur_depth;

	assert(m_stack.size());
	boost::tie(cur_word, cur_m, cur_depth) = m_stack.back();
	m_stack.pop_back();

	while (cur_depth < m_bits) {
	    boost::random::uniform_int_distribution<uint64_t> dist(0, cur_m);
	    uint64_t left_m = dist(m_gen);
	    uint64_t right_m = cur_m - left_m;

	    // push left and right children, if present
	    if (right_m > 0) {
		m_stack.push_back(state_t(cur_word | (uint64_t(1) << (m_bits - cur_depth - 1)),
					  right_m, cur_depth + 1));
	    }
	    if (left_m > 0) {
		m_stack.push_back(state_t(cur_word, left_m, cur_depth + 1));

	    }

	    // pop next child in visit
	    boost::tie(cur_word, cur_m, cur_depth) = m_stack.back();
	    m_stack.pop_back();
	}

	if (cur_m > 1) {
	    // push back the current leaf, with cur_m decreased by one
	    m_stack.push_back(state_t(cur_word, cur_m - 1, cur_depth));
	}

	return cur_word;
    }

    bool done() const
    {
	return m_stack.empty();
    }

private:
    typedef boost::tuple<uint64_t /* cur_word */,
			 uint64_t /* cur_m */,
			 uint64_t /* cur_depth */> state_t;
    std::vector<state_t> m_stack;
    boost::random::mt19937 m_gen;
    uint8_t m_bits;
};

void ef_enumeration_benchmark(uint64_t m, uint8_t bits)
{
    succinct::elias_fano::elias_fano_builder bvb(uint64_t(1) << bits, m);
    monotone_generator mgen(m, bits, 37);
    for (size_t i = 0; i < m; ++i) {
	bvb.push_back(mgen.next());
    }
    assert(mgen.done());

    succinct::elias_fano ef(&bvb);
    succinct::mapper::size_tree_of(ef)->dump();

    double elapsed;
    uint64_t foo = 0;
    SUCCINCT_TIMEIT(elapsed) {
	succinct::elias_fano::select_enumerator it(ef, 0);
	for (size_t i = 0; i < m; ++i) {
	    foo ^= it.next();
	}
    }
    volatile uint64_t vfoo = foo;
    (void)vfoo; // silence warning

    std::cerr << "Elapsed: " << elapsed / 1000 << " msec\n"
	      << double(m) / elapsed << " Mcodes/s" << std::endl;
}

int main(int argc, char** argv)
{
    if (argc != 3) {
        std::cerr << "Invalid arguments" << std::endl;
        std::terminate();
    }
    size_t m = boost::lexical_cast<uint64_t>(argv[1]);
    uint8_t bits = uint8_t(boost::lexical_cast<int>(argv[2]));

    ef_enumeration_benchmark(m, bits);
}


/***********************************************************************************************************************
 *
 * Copyright (c)
 * 2015, ABB Schweiz AG
 * 2021, JOiiNT LAB, Fondazione Istituto Italiano di Tecnologia, Intellimech Consorzio per la Meccatronica.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with
 * or without modification, are permitted provided that
 * the following conditions are met:
 *
 *    * Redistributions of source code must retain the
 *      above copyright notice, this list of conditions
 *      and the following disclaimer.
 *    * Redistributions in binary form must reproduce the
 *      above copyright notice, this list of conditions
 *      and the following disclaimer in the documentation
 *      and/or other materials provided with the
 *      distribution.
 *    * Neither the name of ABB nor the names of its
 *      contributors may be used to endorse or promote
 *      products derived from this software without
 *      specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ***********************************************************************************************************************
 *
 * Authors: Gianluca Lentini, Ugo Alberto Simioni
 * Date:18/01/2022
 * Version 1.0
 * Description: this package provides a ROS node that communicates with the controller using Robot Web Services 2.0, original code can be retrieved at https://github.com/ros-industrial/abb_libegm
 *
 ***********************************************************************************************************************
 */

#define _USE_MATH_DEFINES

#include <cmath>

#include <boost/math/quaternion.hpp>

#include "abb_libegm/egm_common_auxiliary.h"

namespace abb
{
namespace egm
{
/***********************************************************************************************************************
 * Math functions
 */

double saturate(const double value, const double lower, const double upper)
{
  return (value < lower ? lower : (value > upper ? upper : value));
}

void multiply(wrapper::Joints* p_j, const double factor)
{
  if (p_j)
  {
    for (int i = 0; i < p_j->values_size(); ++i)
    {
      p_j->set_values(i, p_j->values(i)*factor);
    }
  }
}

void multiply(wrapper::Cartesian* p_c, const double factor)
{
  if (p_c)
  {
    p_c->set_x(p_c->x()*factor);
    p_c->set_y(p_c->y()*factor);
    p_c->set_z(p_c->z()*factor);
  }
}

void multiply(wrapper::Euler* p_e, const double factor)
{
  if (p_e)
  {
    p_e->set_x(p_e->x()*factor);
    p_e->set_y(p_e->y()*factor);
    p_e->set_z(p_e->z()*factor);
  }
}

void multiply(wrapper::Quaternion* p_q, const double factor)
{
  if (p_q)
  {
    p_q->set_u0(p_q->u0()*factor);
    p_q->set_u1(p_q->u1()*factor);
    p_q->set_u2(p_q->u2()*factor);
    p_q->set_u3(p_q->u3()*factor);
  }
}

wrapper::Quaternion multiply(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2)
{
  wrapper::Quaternion result;

  result.set_u0(q1.u0()*q2.u0() - q1.u1()*q2.u1() - q1.u2()*q2.u2() - q1.u3()*q2.u3());
  result.set_u1(q1.u0()*q2.u1() + q1.u1()*q2.u0() + q1.u2()*q2.u3() - q1.u3()*q2.u2());
  result.set_u2(q1.u0()*q2.u2() + q1.u2()*q2.u0() + q1.u3()*q2.u1() - q1.u1()*q2.u3());
  result.set_u3(q1.u0()*q2.u3() + q1.u3()*q2.u0() + q1.u1()*q2.u2() - q1.u2()*q2.u1());

  return result;
}

double dotProduct(const wrapper::Quaternion& q1, const wrapper::Quaternion& q2)
{
  return q1.u0()*q2.u0() + q1.u1()*q2.u1() + q1.u2()*q2.u2() + q1.u3()*q2.u3();
}

double euclideanNorm(const wrapper::Quaternion& q)
{
  return std::sqrt(dotProduct(q, q));
}

void normalize(wrapper::Quaternion* p_q)
{
  if (p_q)
  {
    double norm = euclideanNorm(*p_q);
    if (norm != 0.0)
    {
      p_q->set_u0(p_q->u0() / norm);
      p_q->set_u1(p_q->u1() / norm);
      p_q->set_u2(p_q->u2() / norm);
      p_q->set_u3(p_q->u3() / norm);
    }
  }
}

void convert(wrapper::Quaternion* p_q, const wrapper::Euler& e)
{
  if (p_q)
  {
    double z = e.z() * Constants::Conversion::DEG_TO_RAD;
    double y = e.y() * Constants::Conversion::DEG_TO_RAD;
    double x = e.x() * Constants::Conversion::DEG_TO_RAD;

    // Convert ZYX Euler angles to a rotation matrix.
    // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations.

    double cx = std::cos(0.5*x);
    double sx = std::sin(0.5*x);
    double cy = std::cos(0.5*y);
    double sy = std::sin(0.5*y);
    double cz = std::cos(0.5*z);
    double sz = std::sin(0.5*z);

    p_q->set_u0(sx*sy*sz + cx*cy*cz);
    p_q->set_u1(-cx*sy*sz + sx*cy*cz);
    p_q->set_u2(sx*cy*sz + cx*sy*cz);
    p_q->set_u3(cx*cy*sz - sx*sy*cz);

    normalize(p_q);
  }
}

void convert(wrapper::Euler* p_e, const wrapper::Quaternion& q)
{
  if(p_e && euclideanNorm(q) != 0.0)
  {
    // Convert a quaternion to ZYX Euler angles.
    // See for example https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles for the equations.
    //
    // Handle singularities.
    // See for example http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
    // for indications of how to derive the equations.

    double y = 0.0;
    double z = 0.0;
    double x = 0.0;

    double u0 = q.u0();
    double u1 = q.u1();
    double u2 = q.u2();
    double u3 = q.u3();

    const double SINGULARITY_THRESHOLD = 0.000001;
    double singularity_check = u0*u2 - u1*u3;

    // Check for singularity (i.e. when y is close to +- 90 degrees).
    // This occur when the argument of y = sin(2*(u0*u2 - u1*u3)) is close to 1.0 -> u0*u2 - u1*u3 = 0.5.
    if (std::abs(singularity_check - 0.5) <= SINGULARITY_THRESHOLD)
    {
      // Y is close to 90 degrees.
      x = 2.0*std::atan2(u1, u0);
      y = M_PI_2;
    }
    else if (std::abs(singularity_check + 0.5) <= SINGULARITY_THRESHOLD)
    {
      // Y is close to -90 degrees.
      x = 2.0*std::atan2(u1, u0);
      y = -M_PI_2;
    }
    else
    {
      x = std::atan2(2.0*(u0*u1 + u2*u3), 1.0 - 2.0*(u1*u1 + u2*u2));
      y = std::asin(2.0*(u0*u2 - u1*u3));
      z = std::atan2(2.0*(u0*u3 + u1*u2), 1.0 - 2.0*(u2*u2 + u3*u3));
    }

    p_e->set_x(x*Constants::Conversion::RAD_TO_DEG);
    p_e->set_y(y*Constants::Conversion::RAD_TO_DEG);
    p_e->set_z(z*Constants::Conversion::RAD_TO_DEG);
  }
}

void convert(wrapper::Quaternion* p_dq, const wrapper::Quaternion& previous_q, const wrapper::Euler& av)
{
  if (p_dq)
  {
    wrapper::Quaternion temp;
    temp.set_u0(0.0);
    temp.set_u1(av.x()*Constants::Conversion::DEG_TO_RAD);
    temp.set_u2(av.y()*Constants::Conversion::DEG_TO_RAD);
    temp.set_u3(av.z()*Constants::Conversion::DEG_TO_RAD);

    p_dq->CopyFrom(multiply(temp, previous_q));
    multiply(p_dq, 0.5);
  }
}

/***********************************************************************************************************************
 * Estimation functions
 */

bool estimateVelocities(wrapper::Joints* p_estimate,
                        const wrapper::Joints& current,
                        const wrapper::Joints& previous,
                        const double sample_time)
{
  bool success = false;
  double delta_speed = 0.0;

  if (p_estimate && sample_time > 0.0)
  {
    p_estimate->Clear();

    for (int i = 0; i < current.values_size() && i < previous.values_size(); ++i)
    {
      delta_speed = current.values(i) - previous.values(i);
      p_estimate->add_values(delta_speed / sample_time);
    }
    success = true;
  }

  return success;
}

bool estimateVelocities(wrapper::Euler* p_estimate,
                        const wrapper::Quaternion& current,
                        const wrapper::Quaternion& previous,
                        const double sample_time)
{
  bool success = false;

  if (p_estimate && sample_time > 0.0)
  {
    // Estimate the angular velocity.
    // See for example https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions for equations.
    // Note: Only valid for orientations, for the same object, at two points close in time.
    // Also assumes constant angular velocity between the points.
    boost::math::quaternion<double> q1(previous.u0(), previous.u1(),
                                       previous.u2(), previous.u3());

    boost::math::quaternion<double> q2(current.u0(), current.u1(),
                                       current.u2(), current.u3());

    boost::math::quaternion<double> estimation = ((2.0*(q2 - q1) / sample_time)*boost::math::conj(q1));

    p_estimate->set_x(estimation.R_component_2()*Constants::Conversion::RAD_TO_DEG);
    p_estimate->set_y(estimation.R_component_3()*Constants::Conversion::RAD_TO_DEG);
    p_estimate->set_z(estimation.R_component_4()*Constants::Conversion::RAD_TO_DEG);

    success = true;
  }

  return success;
}

bool estimateVelocities(wrapper::CartesianVelocity* p_estimate,
                        const wrapper::CartesianPose& current,
                        const wrapper::CartesianPose& previous,
                        const double sample_time)
{
  bool success = false;

  if (p_estimate && sample_time > 0.0)
  {
    // Estimate the linear velocity.
    p_estimate->mutable_linear()->set_x((current.position().x() - previous.position().x()) / sample_time);
    p_estimate->mutable_linear()->set_y((current.position().y() - previous.position().y()) / sample_time);
    p_estimate->mutable_linear()->set_z((current.position().z() - previous.position().z()) / sample_time);

    // Estimate the angular velocity.
    success = estimateVelocities(p_estimate->mutable_angular(),
                                 current.quaternion(),
                                 previous.quaternion(),
                                 sample_time);
  }

  return success;
}

/***********************************************************************************************************************
 * Find functions
 */

double findMaxDifference(const wrapper::Joints& j1, const wrapper::Joints& j2)
{
  double max_difference = 0.0;

  for (int i = 0; i < j1.values_size() && i < j2.values_size(); ++i)
  {
    max_difference = std::max(max_difference, std::abs(j1.values(i) - j2.values(i)));
  }

  return max_difference;
}

double findMaxDifference(const wrapper::Cartesian& c1, const wrapper::Cartesian& c2)
{
  double max_difference = std::abs(c1.x() - c2.x());
  max_difference = std::max(max_difference, std::abs(c1.y() - c2.y()));
  max_difference = std::max(max_difference, std::abs(c1.z() - c2.z()));

  return max_difference;
}

double findMaxDifference(const wrapper::Euler& e1, const wrapper::Euler& e2)
{
  double max_difference = std::abs(e1.x() - e2.x());
  max_difference = std::max(max_difference, std::abs(e1.y() - e2.y()));
  max_difference = std::max(max_difference, std::abs(e1.z() - e2.z()));

  return max_difference;
}

/***********************************************************************************************************************
 * Copy functions
 */

void copyPresent(wrapper::Joints* p_target, const wrapper::Joints& source)
{
  if (p_target)
  {
    for (int i = 0; i < source.values_size() && i < p_target->values_size(); ++i)
    {
      p_target->set_values(i, source.values(i));
    }
  }
}

void copyPresent(wrapper::JointSpace* p_target, const wrapper::JointSpace& source)
{
  if (p_target)
  {
    if (source.has_position())
    {
      copyPresent(p_target->mutable_position(), source.position());
    }

    if (source.has_velocity())
    {
      copyPresent(p_target->mutable_velocity(), source.velocity());
    }
  }
}

void copyPresent(wrapper::Cartesian* p_target, const wrapper::Cartesian& source)
{
  if (p_target)
  {
    if (source.has_x())
    {
      p_target->set_x(source.x());
    }

    if (source.has_y())
    {
      p_target->set_y(source.y());
    }

    if (source.has_z())
    {
      p_target->set_z(source.z());
    }
  }
}

void copyPresent(wrapper::Euler* p_target, const wrapper::Euler& source)
{
  if (p_target)
  {
    if (source.has_x())
    {
      p_target->set_x(source.x());
    }

    if (source.has_y())
    {
      p_target->set_y(source.y());
    }

    if (source.has_z())
    {
      p_target->set_z(source.z());
    }
  }
}

void copyPresent(wrapper::Quaternion* p_target, const wrapper::Quaternion& source)
{
  if (p_target)
  {
    if (source.has_u0())
    {
      p_target->set_u0(source.u0());
    }

    if (source.has_u1())
    {
      p_target->set_u1(source.u1());
    }

    if (source.has_u2())
    {
      p_target->set_u2(source.u2());
    }

    if (source.has_u3())
    {
      p_target->set_u3(source.u3());
    }

    normalize(p_target);
  }
}

void copyPresent(wrapper::CartesianPose* p_target, const wrapper::CartesianPose& source)
{
  if (p_target)
  {
    if (source.has_position())
    {
      copyPresent(p_target->mutable_position(), source.position());
    }

    if (source.has_euler())
    {
      copyPresent(p_target->mutable_euler(), source.euler());
      convert(p_target->mutable_quaternion(), p_target->euler());
    }
    else if (source.has_quaternion())
    {
      copyPresent(p_target->mutable_quaternion(), source.quaternion());
      normalize(p_target->mutable_quaternion());
      convert(p_target->mutable_euler(), p_target->quaternion());
    }
  }
}

void copyPresent(wrapper::CartesianVelocity* p_target, const wrapper::CartesianVelocity& source)
{
  if (p_target)
  {
    if (source.has_linear())
    {
      copyPresent(p_target->mutable_linear(), source.linear());
    }

    if (source.has_angular())
    {
      copyPresent(p_target->mutable_angular(), source.angular());
    }
  }
}

void copyPresent(wrapper::CartesianSpace* p_target, const wrapper::CartesianSpace& source)
{
  if (p_target)
  {
    if (source.has_pose())
    {
      copyPresent(p_target->mutable_pose(), source.pose());
    }

    if (source.has_velocity())
    {
      copyPresent(p_target->mutable_velocity(), source.velocity());
    }
  }
}

void copyPresent(wrapper::Robot* p_target, const wrapper::Robot& source)
{
  if (p_target)
  {
    if (source.has_joints())
    {
      copyPresent(p_target->mutable_joints(), source.joints());
    }

    if (source.has_cartesian())
    {
      copyPresent(p_target->mutable_cartesian(), source.cartesian());
    }
  }
}

void copyPresent(wrapper::External* p_target, const wrapper::External& source)
{
  if (p_target)
  {
    if (source.has_joints())
    {
      copyPresent(p_target->mutable_joints(), source.joints());
    }
  }
}

void copyPresent(wrapper::Output* p_target, const wrapper::Output& source)
{
  if (p_target)
  {
    if (source.has_robot())
    {
      copyPresent(p_target->mutable_robot(), source.robot());
    }

    if (source.has_external())
    {
      copyPresent(p_target->mutable_external(), source.external());
    }
  }
}

/***********************************************************************************************************************
 * Parse functions
 */

bool parse(wrapper::Header* p_target, const EgmHeader& source)
{
  bool success = false;

  if (p_target && source.has_seqno() && source.has_tm() && source.has_mtype())
  {
    p_target->set_sequence_number(source.seqno());
    p_target->set_time_stamp(source.tm());

    switch (source.mtype())
    {
      case EgmHeader_MessageType_MSGTYPE_DATA:
      {
        p_target->set_message_type(wrapper::Header_MessageType_DATA);
        success = true;
      }
      break;

      case EgmHeader_MessageType_MSGTYPE_UNDEFINED:
      case EgmHeader_MessageType_MSGTYPE_COMMAND:
      case EgmHeader_MessageType_MSGTYPE_CORRECTION:
      case EgmHeader_MessageType_MSGTYPE_PATH_CORRECTION:
      default:
      {
        p_target->set_message_type(wrapper::Header_MessageType_UNDEFINED);
      }
    }
  }

  return success;
}

bool parse(wrapper::Status* p_target, const EgmRobot& source)
{
  bool success = false;

  if (p_target &&
      source.has_motorstate() && source.motorstate().has_state() &&
      source.has_mcistate() && source.mcistate().has_state() &&
      source.has_rapidexecstate() && source.rapidexecstate().has_state() &&
      source.has_mciconvergencemet())
  {
    switch (source.motorstate().state())
    {
      case EgmMotorState_MotorStateType_MOTORS_UNDEFINED:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED);
      }
      break;

      case EgmMotorState_MotorStateType_MOTORS_ON:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_ON);
      }
      break;

      case EgmMotorState_MotorStateType_MOTORS_OFF:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_OFF);
      }
      break;

      default:
      {
        p_target->set_motor_state(wrapper::Status_MotorState_MOTORS_UNDEFINED);
      }
    }

    switch (source.mcistate().state())
    {
      case EgmMCIState_MCIStateType_MCI_UNDEFINED:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_ERROR:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_ERROR);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_STOPPED:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_STOPPED);
      }
      break;

      case EgmMCIState_MCIStateType_MCI_RUNNING:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_RUNNING);
      }
      break;

      default:
      {
        p_target->set_egm_state(wrapper::Status_EGMState_EGM_UNDEFINED);
      }
    }

    switch (source.rapidexecstate().state())
    {
      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_UNDEFINED:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED);
      }
      break;

      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_STOPPED:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_STOPPED);
      }
      break;

      case EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_RUNNING:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_RUNNING);
      }
      break;

      default:
      {
        p_target->set_rapid_execution_state(wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED);
      }
    }

    p_target->set_egm_convergence_met(source.mciconvergencemet());

    success = true;
  }

  return success;
}

bool parse(wrapper::Clock* p_target, const EgmClock& source)
{
  bool success = true;

  if (p_target && source.has_sec() && source.has_usec())
  {
    p_target->set_sec(source.sec());
    p_target->set_usec(source.usec());
  }
  else
  {
    success = false;
  }

  return success;
}

bool parse(wrapper::Joints* p_target_robot,
           wrapper::Joints* p_target_external,
           const EgmJoints& source_robot,
           const EgmJoints& source_external,
           const RobotAxes axes)
{
  bool success = false;

  if (p_target_robot && p_target_external)
  {
    p_target_robot->Clear();
    p_target_external->Clear();

    switch (axes)
    {
      case Six:
      {
        if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS)
        {
          for (int i = 0; i < source_robot.joints_size(); ++i)
          {
            p_target_robot->add_values(source_robot.joints(i));
          }

          for (int i = 0; i < source_external.joints_size(); ++i)
          {
            p_target_external->add_values(source_external.joints(i));
          }

          success = true;
        }
      }
      break;

      case Seven:
      {
        // If using a seven axes robot (e.g. IRB14000): Map to special case.
        if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS &&
            source_external.joints_size() >= 1)
        {
          p_target_robot->add_values(source_robot.joints(0));
          p_target_robot->add_values(source_robot.joints(1));
          p_target_robot->add_values(source_external.joints(0));
          p_target_robot->add_values(source_robot.joints(2));
          p_target_robot->add_values(source_robot.joints(3));
          p_target_robot->add_values(source_robot.joints(4));
          p_target_robot->add_values(source_robot.joints(5));

          for (int i = 1; i < source_external.joints_size(); ++i)
          {
            p_target_external->add_values(source_external.joints(i));
          }

          success = true;
        }
      }
      break;
    }
  }

  return success;
}

bool parse(wrapper::CartesianPose* p_target, const EgmPose& source)
{
  bool success = true;

  if (p_target)
  {
    p_target->Clear();

    if (source.has_pos() && source.pos().has_x() && source.pos().has_y() && source.pos().has_z())
    {
      p_target->mutable_position()->set_x(source.pos().x());
      p_target->mutable_position()->set_y(source.pos().y());
      p_target->mutable_position()->set_z(source.pos().z());
    }
    else
    {
      success = false;
    }

    if (success &&
        source.has_orient() &&
        source.orient().has_u0() &&
        source.orient().has_u1() &&
        source.orient().has_u2() &&
        source.orient().has_u3())
    {
      p_target->mutable_quaternion()->set_u0(source.orient().u0());
      p_target->mutable_quaternion()->set_u1(source.orient().u1());
      p_target->mutable_quaternion()->set_u2(source.orient().u2());
      p_target->mutable_quaternion()->set_u3(source.orient().u3());
    }
    else
    {
      success = false;
    }

    if (success)
    {
      if (source.has_euler() && source.euler().has_x() && source.euler().has_y() && source.euler().has_z())
      {
        p_target->mutable_euler()->set_x(source.euler().x());
        p_target->mutable_euler()->set_y(source.euler().y());
        p_target->mutable_euler()->set_z(source.euler().z());
      }
      else
      {
        convert(p_target->mutable_euler(), p_target->quaternion());
      }
    }
  }

  return success;
}

bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAxes axes)
{
  bool success = false;

  if (p_target)
  {
    success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(),
                    p_target->mutable_external()->mutable_joints()->mutable_position(),
                    source.joints(), source.externaljoints(), axes);

    if (success)
    {
      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

      if (success)
      {
        success = parse(p_target->mutable_time(), source.time());
      }
    }
  }

  return success;
}

bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes axes)
{
  bool success = false;

  if (p_target)
  {
    success = parse(p_target->mutable_robot()->mutable_joints()->mutable_position(),
                    p_target->mutable_external()->mutable_joints()->mutable_position(),
                    source.joints(), source.externaljoints(), axes);

    if (success)
    {
      success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

      if (success)
      {
        success = parse(p_target->mutable_time(), source.time());
      }
    }
  }

  return success;
}

/***********************************************************************************************************************
 * Reset functions
 */

void reset(wrapper::Joints* p_joints, const unsigned int number_of_joints)
{
  if (p_joints)
  {
    p_joints->Clear();

    for (unsigned int i = 0; i < number_of_joints; ++i)
    {
      p_joints->add_values(0.0);
    }
  }
}

void reset(wrapper::Cartesian* p_cartesian)
{
  if (p_cartesian)
  {
    p_cartesian->set_x(0.0);
    p_cartesian->set_y(0.0);
    p_cartesian->set_z(0.0);
  }
}

void reset(wrapper::Euler* p_euler)
{
  if (p_euler)
  {
    p_euler->set_x(0.0);
    p_euler->set_y(0.0);
    p_euler->set_z(0.0);
  }
}

} // end namespace egm
} // end namespace abb