//=======================================================================
// Copyright 1997-2001 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef BOOST_GRAPH_SGB_GRAPH_HPP
#define BOOST_GRAPH_SGB_GRAPH_HPP

#include <boost/config.hpp>
#include <boost/operators.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

// Thanks to Andreas Scherer for numerous suggestions and fixes!

// This file adapts a Stanford GraphBase (SGB) Graph pointer into a
// VertexListGraph. Note that a graph adaptor class is not needed,
// SGB's Graph* is used as is. The VertexListGraph concept is fulfilled by
// defining the appropriate non-member functions for Graph*.
//
// The PROTOTYPES change file extensions to SGB must be applied so
// that the SGB functions have real prototypes which are necessary for
// the C++ compiler. To apply the PROTOTYPES extensions, before you do
// "make tests install" for SGB do "ln -s PROTOTYPES/* ." to the SGB
// root directory (or just copy all the files from the PROTOTYPES
// directory to the SGB root directory).
//
extern "C"
{
    // We include all global definitions for the general stuff
    // of The Stanford GraphBase and its various graph generator
    // functions by reading all SGB headerfiles as in section 2 of
    // the "test_sample" program.
#include <gb_graph.h> /* SGB data structures */
#include <gb_io.h> /* SGB input/output routines */
#include <gb_flip.h> /* random number generator */
#include <gb_dijk.h> /* routines for shortest paths */
#include <gb_basic.h> /* the basic graph operations */
#undef empty /* avoid name clash with C++ standard library */
    inline Graph* empty(long n) /* and provide workaround */
    {
        return board(n, 0L, 0L, 0L, 2L, 0L, 0L);
    }
#include <gb_books.h> /* graphs based on literature */
#include <gb_econ.h> /* graphs based on economic data */
#include <gb_games.h> /* graphs based on football scores */
#undef ap /* avoid name clash with BGL parameter */
    // ap ==> Vertex::u.I
#include <gb_gates.h> /* graphs based on logic circuits */
#undef val /* avoid name clash with g++ headerfile stl_tempbuf.h */
    // val ==> Vertex::x.I
#include <gb_lisa.h> /* graphs based on Mona Lisa */
#include <gb_miles.h> /* graphs based on mileage data */
#include <gb_plane.h> /* planar graphs */
#include <gb_raman.h> /* Ramanujan graphs */
#include <gb_rand.h> /* random graphs */
#include <gb_roget.h> /* graphs based on Roget's Thesaurus */
#include <gb_save.h> /* we save results in ASCII format */
#include <gb_words.h> /* five-letter-word graphs */
#undef weight /* avoid name clash with BGL parameter */
    // weight ==> Vertex::u.I
}

namespace boost
{
class sgb_edge;
}

class sgb_out_edge_iterator;
class sgb_adj_iterator;
class sgb_vertex_iterator;

namespace boost
{
typedef Graph* sgb_graph_ptr;
typedef const Graph* sgb_const_graph_ptr;

struct sgb_traversal_tag : public virtual vertex_list_graph_tag,
                           public virtual incidence_graph_tag,
                           public virtual adjacency_graph_tag
{
};

template <> struct graph_traits< sgb_graph_ptr >
{
    typedef Vertex* vertex_descriptor;
    typedef boost::sgb_edge edge_descriptor;
    typedef sgb_out_edge_iterator out_edge_iterator;
    typedef void in_edge_iterator;
    typedef sgb_adj_iterator adjacency_iterator;
    typedef sgb_vertex_iterator vertex_iterator;
    typedef void edge_iterator;
    typedef long vertices_size_type;
    typedef long edge_size_type;
    typedef long degree_size_type;
    typedef directed_tag directed_category;
    typedef sgb_traversal_tag traversal_category;
    typedef allow_parallel_edge_tag edge_parallel_category;
    /** Return a null descriptor */
    static vertex_descriptor null_vertex() { return NULL; }
};
template <> struct graph_traits< sgb_const_graph_ptr >
{
    typedef Vertex* vertex_descriptor;
    typedef boost::sgb_edge edge_descriptor;
    typedef sgb_out_edge_iterator out_edge_iterator;
    typedef void in_edge_iterator;
    typedef sgb_adj_iterator adjacency_iterator;
    typedef sgb_vertex_iterator vertex_iterator;
    typedef void edge_iterator;
    typedef long vertices_size_type;
    typedef long edge_size_type;
    typedef long degree_size_type;
    typedef directed_tag directed_category;
    typedef sgb_traversal_tag traversal_category;
    typedef allow_parallel_edge_tag edge_parallel_category;
    /** Return a null descriptor */
    static vertex_descriptor null_vertex() { return NULL; }
};
}

namespace boost
{

struct edge_length_t
{
    typedef edge_property_tag kind;
};

// We could just use Arc* as the edge descriptor type, but
// we want to add the source(e,g) function which requires
// that we carry along a pointer to the source vertex.
class sgb_edge
{
    typedef sgb_edge self;

public:
    sgb_edge() : _arc(0), _src(0) {}
    sgb_edge(Arc* a, Vertex* s) : _arc(a), _src(s) {}
    friend Vertex* source(self e, sgb_const_graph_ptr) { return e._src; }
    friend Vertex* target(self e, sgb_const_graph_ptr) { return e._arc->tip; }
    friend bool operator==(const self& a, const self& b)
    {
        return a._arc == b._arc;
    }
    friend bool operator!=(const self& a, const self& b)
    {
        return a._arc != b._arc;
    }
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
    template < class Ref > friend class sgb_edge_length_map;
    template < class Tag, class Ref > friend class sgb_edge_util_map;
    friend long get(edge_length_t, const sgb_graph_ptr&, const sgb_edge& key);
    friend long get(
        edge_length_t, const sgb_const_graph_ptr&, const sgb_edge& key);
    friend void put(
        edge_length_t, sgb_graph_ptr&, const sgb_edge& key, long value);

protected:
#endif
    Arc* _arc;
    Vertex* _src;
};
} // namespace boost

class sgb_out_edge_iterator
: public boost::forward_iterator_helper< sgb_out_edge_iterator, boost::sgb_edge,
      std::ptrdiff_t, boost::sgb_edge*, boost::sgb_edge >
{
    typedef sgb_out_edge_iterator self;

public:
    sgb_out_edge_iterator() : _src(0), _arc(0) {}
    sgb_out_edge_iterator(Vertex* s, Arc* d) : _src(s), _arc(d) {}
    boost::sgb_edge operator*() { return boost::sgb_edge(_arc, _src); }
    self& operator++()
    {
        _arc = _arc->next;
        return *this;
    }
    friend bool operator==(const self& x, const self& y)
    {
        return x._arc == y._arc;
    }

protected:
    Vertex* _src;
    Arc* _arc;
};

class sgb_adj_iterator
: public boost::forward_iterator_helper< sgb_adj_iterator, Vertex*,
      std::ptrdiff_t, Vertex**, Vertex* >
{
    typedef sgb_adj_iterator self;

public:
    sgb_adj_iterator() : _arc(0) {}
    sgb_adj_iterator(Arc* d) : _arc(d) {}
    Vertex* operator*() { return _arc->tip; }
    self& operator++()
    {
        _arc = _arc->next;
        return *this;
    }
    friend bool operator==(const self& x, const self& y)
    {
        return x._arc == y._arc;
    }

protected:
    Arc* _arc;
};

// The reason we have this instead of just using Vertex* is that we
// want to use Vertex* as the vertex_descriptor instead of just
// Vertex, which avoids problems with boost passing vertex descriptors
// by value and how that interacts with the sgb_vertex_id_map.
class sgb_vertex_iterator
: public boost::forward_iterator_helper< sgb_vertex_iterator, Vertex*,
      std::ptrdiff_t, Vertex**, Vertex* >
{
    typedef sgb_vertex_iterator self;

public:
    sgb_vertex_iterator() : _v(0) {}
    sgb_vertex_iterator(Vertex* v) : _v(v) {}
    Vertex* operator*() { return _v; }
    self& operator++()
    {
        ++_v;
        return *this;
    }
    friend bool operator==(const self& x, const self& y)
    {
        return x._v == y._v;
    }

protected:
    Vertex* _v;
};

namespace boost
{

inline std::pair< sgb_vertex_iterator, sgb_vertex_iterator > vertices(
    sgb_const_graph_ptr g)
{
    return std::make_pair(sgb_vertex_iterator(g->vertices),
        sgb_vertex_iterator(g->vertices + g->n));
}

inline std::pair< sgb_out_edge_iterator, sgb_out_edge_iterator > out_edges(
    Vertex* u, sgb_const_graph_ptr)
{
    return std::make_pair(
        sgb_out_edge_iterator(u, u->arcs), sgb_out_edge_iterator(u, 0));
}

inline boost::graph_traits< sgb_graph_ptr >::degree_size_type out_degree(
    Vertex* u, sgb_const_graph_ptr g)
{
    boost::graph_traits< sgb_graph_ptr >::out_edge_iterator i, i_end;
    boost::tie(i, i_end) = out_edges(u, g);
    return std::distance(i, i_end);
}

// in_edges?

inline std::pair< sgb_adj_iterator, sgb_adj_iterator > adjacent_vertices(
    Vertex* u, sgb_const_graph_ptr)
{
    return std::make_pair(sgb_adj_iterator(u->arcs), sgb_adj_iterator(0));
}

inline long num_vertices(sgb_const_graph_ptr g) { return g->n; }
inline long num_edges(sgb_const_graph_ptr g) { return g->m; }

inline Vertex* vertex(long v, sgb_const_graph_ptr g) { return g->vertices + v; }

// Various Property Maps

// Vertex ID
class sgb_vertex_id_map
: public boost::put_get_helper< long, sgb_vertex_id_map >
{
public:
    typedef boost::readable_property_map_tag category;
    typedef long value_type;
    typedef long reference;
    typedef Vertex* key_type;
    sgb_vertex_id_map() : _g(0) {}
    sgb_vertex_id_map(sgb_graph_ptr g) : _g(g) {}
    long operator[](Vertex* v) const { return v - _g->vertices; }

protected:
    sgb_graph_ptr _g;
};
inline sgb_vertex_id_map get(vertex_index_t, sgb_graph_ptr g)
{
    return sgb_vertex_id_map(g);
}

// Vertex Name
class sgb_vertex_name_map
: public boost::put_get_helper< char*, sgb_vertex_name_map >
{
public:
    typedef boost::readable_property_map_tag category;
    typedef char* value_type;
    typedef char* reference;
    typedef Vertex* key_type;
    char* operator[](Vertex* v) const { return v->name; }
};
inline sgb_vertex_name_map get(vertex_name_t, sgb_graph_ptr)
{
    return sgb_vertex_name_map();
}

// Vertex Property Tags
#define SGB_PROPERTY_TAG(KIND, TAG)            \
    template < class T > struct TAG##_property \
    {                                          \
        typedef KIND##_property_tag kind;      \
        typedef T type;                        \
    };
SGB_PROPERTY_TAG(vertex, u)
SGB_PROPERTY_TAG(vertex, v)
SGB_PROPERTY_TAG(vertex, w)
SGB_PROPERTY_TAG(vertex, x)
SGB_PROPERTY_TAG(vertex, y)
SGB_PROPERTY_TAG(vertex, z)

// Edge Property Tags
SGB_PROPERTY_TAG(edge, a)
SGB_PROPERTY_TAG(edge, b)

// Various Utility Maps

// helpers
inline Vertex*& get_util(util& u, Vertex*) { return u.V; }
inline Arc*& get_util(util& u, Arc*) { return u.A; }
inline sgb_graph_ptr& get_util(util& u, sgb_graph_ptr) { return u.G; }
inline char*& get_util(util& u, char*) { return u.S; }
inline long& get_util(util& u, long) { return u.I; }

#define SGB_GET_UTIL_FIELD(KIND, X)                                           \
    template < class T > inline T& get_util_field(KIND* k, X##_property< T >) \
    {                                                                         \
        return get_util(k->X, T());                                           \
    }

SGB_GET_UTIL_FIELD(Vertex, u)
SGB_GET_UTIL_FIELD(Vertex, v)
SGB_GET_UTIL_FIELD(Vertex, w)
SGB_GET_UTIL_FIELD(Vertex, x)
SGB_GET_UTIL_FIELD(Vertex, y)
SGB_GET_UTIL_FIELD(Vertex, z)

SGB_GET_UTIL_FIELD(Arc, a)
SGB_GET_UTIL_FIELD(Arc, b)

// Vertex Utility Map
template < class Tag, class Ref >
class sgb_vertex_util_map
: public boost::put_get_helper< Ref, sgb_vertex_util_map< Tag, Ref > >
{
    Tag tag;

public:
    explicit sgb_vertex_util_map(Tag tag = Tag()) : tag(tag) {}
    typedef boost::lvalue_property_map_tag category;
    typedef typename Tag::type value_type;
    typedef Vertex* key_type;
    typedef Ref reference;
    reference operator[](Vertex* v) const { return get_util_field(v, tag); }
};

// Edge Utility Map
template < class Tag, class Ref >
class sgb_edge_util_map
: public boost::put_get_helper< Ref, sgb_edge_util_map< Tag, Ref > >
{
    Tag tag;

public:
    explicit sgb_edge_util_map(Tag tag = Tag()) : tag(tag) {}
    typedef boost::lvalue_property_map_tag category;
    typedef typename Tag::type value_type;
    typedef Vertex* key_type;
    typedef Ref reference;
    reference operator[](const sgb_edge& e) const
    {
        return get_util_field(e._arc, tag);
    }
};

template < class Tag >
inline sgb_vertex_util_map< Tag, const typename Tag::type& > get_property_map(
    Tag, const sgb_graph_ptr& g, vertex_property_tag)
{
    return sgb_vertex_util_map< Tag, const typename Tag::type& >();
}
template < class Tag >
inline sgb_vertex_util_map< Tag, typename Tag::type& > get_property_map(
    Tag, sgb_graph_ptr& g, vertex_property_tag)
{
    return sgb_vertex_util_map< Tag, typename Tag::type& >();
}

template < class Tag >
inline sgb_edge_util_map< Tag, const typename Tag::type& > get_property_map(
    Tag, const sgb_graph_ptr& g, edge_property_tag)
{
    return sgb_edge_util_map< Tag, const typename Tag::type& >();
}
template < class Tag >
inline sgb_edge_util_map< Tag, typename Tag::type& > get_property_map(
    Tag, sgb_graph_ptr& g, edge_property_tag)
{
    return sgb_edge_util_map< Tag, typename Tag::type& >();
}

// Edge Length Access
template < class Ref >
class sgb_edge_length_map
: public boost::put_get_helper< Ref, sgb_edge_length_map< Ref > >
{
public:
    typedef boost::lvalue_property_map_tag category;
    typedef long value_type;
    typedef sgb_edge key_type;
    typedef Ref reference;
    reference operator[](const sgb_edge& e) const { return e._arc->len; }
};

inline sgb_edge_length_map< const long& > get(
    edge_length_t, const sgb_graph_ptr&)
{
    return sgb_edge_length_map< const long& >();
}
inline sgb_edge_length_map< const long& > get(
    edge_length_t, const sgb_const_graph_ptr&)
{
    return sgb_edge_length_map< const long& >();
}
inline sgb_edge_length_map< long& > get(edge_length_t, sgb_graph_ptr&)
{
    return sgb_edge_length_map< long& >();
}
inline long get(edge_length_t, const sgb_graph_ptr&, const sgb_edge& key)
{
    return key._arc->len;
}
inline long get(edge_length_t, const sgb_const_graph_ptr&, const sgb_edge& key)
{
    return key._arc->len;
}
inline void put(edge_length_t, sgb_graph_ptr&, const sgb_edge& key, long value)
{
    key._arc->len = value;
}

// Property Map Traits Classes
template <> struct property_map< sgb_graph_ptr, edge_length_t >
{
    typedef sgb_edge_length_map< long& > type;
    typedef sgb_edge_length_map< const long& > const_type;
};
template <> struct property_map< sgb_graph_ptr, vertex_index_t >
{
    typedef sgb_vertex_id_map type;
    typedef sgb_vertex_id_map const_type;
};
template <> struct property_map< sgb_graph_ptr, vertex_name_t >
{
    typedef sgb_vertex_name_map type;
    typedef sgb_vertex_name_map const_type;
};

template <> struct property_map< sgb_const_graph_ptr, edge_length_t >
{
    typedef sgb_edge_length_map< const long& > const_type;
};
template <> struct property_map< sgb_const_graph_ptr, vertex_index_t >
{
    typedef sgb_vertex_id_map const_type;
};
template <> struct property_map< sgb_const_graph_ptr, vertex_name_t >
{
    typedef sgb_vertex_name_map const_type;
};

namespace detail
{
    template < class Kind, class PropertyTag > struct sgb_choose_property_map
    {
    };
    template < class PropertyTag >
    struct sgb_choose_property_map< vertex_property_tag, PropertyTag >
    {
        typedef typename PropertyTag::type value_type;
        typedef sgb_vertex_util_map< PropertyTag, value_type& > type;
        typedef sgb_vertex_util_map< PropertyTag, const value_type& >
            const_type;
    };
    template < class PropertyTag >
    struct sgb_choose_property_map< edge_property_tag, PropertyTag >
    {
        typedef typename PropertyTag::type value_type;
        typedef sgb_edge_util_map< PropertyTag, value_type& > type;
        typedef sgb_edge_util_map< PropertyTag, const value_type& > const_type;
    };
} // namespace detail
template < class PropertyTag > struct property_map< sgb_graph_ptr, PropertyTag >
{
    typedef typename property_kind< PropertyTag >::type Kind;
    typedef detail::sgb_choose_property_map< Kind, PropertyTag > Choice;
    typedef typename Choice::type type;
    typedef typename Choice::const_type const_type;
};
template < class PropertyTag >
struct property_map< sgb_const_graph_ptr, PropertyTag >
{
    typedef typename property_kind< PropertyTag >::type Kind;
    typedef detail::sgb_choose_property_map< Kind, PropertyTag > Choice;
    typedef typename Choice::const_type const_type;
};

#define SGB_UTIL_ACCESSOR(KIND, X)                                             \
    template < class T >                                                       \
    inline sgb_##KIND##_util_map< X##_property< T >, T& > get(                 \
        X##_property< T >, sgb_graph_ptr&)                                     \
    {                                                                          \
        return sgb_##KIND##_util_map< X##_property< T >, T& >();               \
    }                                                                          \
    template < class T >                                                       \
    inline sgb_##KIND##_util_map< X##_property< T >, const T& > get(           \
        X##_property< T >, const sgb_graph_ptr&)                               \
    {                                                                          \
        return sgb_##KIND##_util_map< X##_property< T >, const T& >();         \
    }                                                                          \
    template < class T >                                                       \
    inline sgb_##KIND##_util_map< X##_property< T >, const T& > get(           \
        X##_property< T >, const sgb_const_graph_ptr&)                         \
    {                                                                          \
        return sgb_##KIND##_util_map< X##_property< T >, const T& >();         \
    }                                                                          \
    template < class T, class Key >                                            \
    inline typename sgb_##KIND##_util_map< X##_property< T >,                  \
        const T& >::value_type                                                 \
    get(X##_property< T >, const sgb_graph_ptr&, const Key& key)               \
    {                                                                          \
        return sgb_##KIND##_util_map< X##_property< T >, const T& >()[key];    \
    }                                                                          \
    template < class T, class Key >                                            \
    inline typename sgb_##KIND##_util_map< X##_property< T >,                  \
        const T& >::value_type                                                 \
    get(X##_property< T >, const sgb_const_graph_ptr&, const Key& key)         \
    {                                                                          \
        return sgb_##KIND##_util_map< X##_property< T >, const T& >()[key];    \
    }                                                                          \
    template < class T, class Key, class Value >                               \
    inline void put(                                                           \
        X##_property< T >, sgb_graph_ptr&, const Key& key, const Value& value) \
    {                                                                          \
        sgb_##KIND##_util_map< X##_property< T >, T& >()[key] = value;         \
    }

SGB_UTIL_ACCESSOR(vertex, u)
SGB_UTIL_ACCESSOR(vertex, v)
SGB_UTIL_ACCESSOR(vertex, w)
SGB_UTIL_ACCESSOR(vertex, x)
SGB_UTIL_ACCESSOR(vertex, y)
SGB_UTIL_ACCESSOR(vertex, z)

SGB_UTIL_ACCESSOR(edge, a)
SGB_UTIL_ACCESSOR(edge, b)

} // namespace boost

#endif // BOOST_GRAPH_SGB_GRAPH_HPP


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

#include "platooning_tactical_plugin/platooning_tactical_plugin.h"

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <platooning_tactical_plugin/smoothing/SplineI.h>
#include <platooning_tactical_plugin/smoothing/BSpline.h>
#include <platooning_tactical_plugin/log/log.h>
#include <platooning_tactical_plugin/smoothing/filters.h>
#include <unordered_set>

using oss = std::ostringstream;

namespace platooning_tactical_plugin
{
PlatooningTacticalPlugin::PlatooningTacticalPlugin(carma_wm::WorldModelConstPtr wm, PlatooningTacticalPluginConfig config,
                                           PublishPluginDiscoveryCB plugin_discovery_publisher)
  : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher)
{
  plugin_discovery_msg_.name = "PlatooningTacticalPlugin";
  plugin_discovery_msg_.versionId = "v1.0";
  plugin_discovery_msg_.available = true;
  plugin_discovery_msg_.activated = false;
  plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
  plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";
}

bool PlatooningTacticalPlugin::onSpin()
{
  plugin_discovery_publisher_(plugin_discovery_msg_);
  return true;
}

bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req,
                                              cav_srvs::PlanTrajectoryResponse& resp)
{
  ros::WallTime start_time = ros::WallTime::now(); // Start timeing the execution time for planning so it can be logged

  lanelet::BasicPoint2d veh_pos(req.vehicle_state.X_pos_global, req.vehicle_state.Y_pos_global);
  double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;

  // Only plan the trajectory for the initial LANE_FOLLOWING maneuver and any immediately sequential maneuvers of the same type
  std::vector<cav_msgs::Maneuver> maneuver_plan;
  for(size_t i = req.maneuver_index_to_plan; i < req.maneuver_plan.maneuvers.size(); i++)
  {
    if(req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::LANE_FOLLOWING)
    {
      if (req.maneuver_plan.maneuvers[i].lane_following_maneuver.parameters.negotiation_type != cav_msgs::ManeuverParameters::NO_NEGOTIATION)
      {
        maneuver_plan.push_back(req.maneuver_plan.maneuvers[i]);
        resp.related_maneuvers.push_back(i);
      }
    }
    else
    {
      break;
    }
  }

  auto points_and_target_speeds = maneuvers_to_points(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance), wm_); // Convert maneuvers to points

  ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size());

  ROS_DEBUG_STREAM("PlanTrajectory");

  cav_msgs::TrajectoryPlan original_trajectory;
  original_trajectory.header.frame_id = "map";
  original_trajectory.header.stamp = ros::Time::now();
  original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());

  original_trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp); // Compute the trajectory
  original_trajectory.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, config_.minimum_speed);

  resp.trajectory_plan = original_trajectory;

  resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);

  ros::WallTime end_time = ros::WallTime::now();  // Planning complete

  ros::WallDuration duration = end_time - start_time;
  ROS_DEBUG_STREAM("ExecutionTime: " << duration.toSec());

  return true;

}

std::vector<double> PlatooningTacticalPlugin::apply_speed_limits(const std::vector<double> speeds,
                                                             const std::vector<double> speed_limits)
{
  ROS_DEBUG_STREAM("Speeds list size: " << speeds.size());
  ROS_DEBUG_STREAM("SpeedLimits list size: " << speed_limits.size());

  if (speeds.size() != speed_limits.size())
  {
    throw std::invalid_argument("Speeds and speed limit lists not same size");
  }
  std::vector<double> out;
  for (size_t i = 0; i < speeds.size(); i++)
  {
    out.push_back(std::min(speeds[i], speed_limits[i]));
  }

  return out;
}

Eigen::Isometry2d PlatooningTacticalPlugin::compute_heading_frame(const lanelet::BasicPoint2d& p1,
                                                              const lanelet::BasicPoint2d& p2)
{
  Eigen::Rotation2Dd yaw(atan2(p2.y() - p1.y(), p2.x() - p1.x()));

  return carma_wm::geometry::build2dEigenTransform(p1, yaw);
}

std::pair<double, size_t> PlatooningTacticalPlugin::min_with_exclusions(const std::vector<double>& values, const std::unordered_set<size_t>& excluded) const {
  double min = std::numeric_limits<double>::max();
  size_t best_idx = -1;
  for (size_t i = 0; i < values.size(); i++) {
    if (excluded.find(i) != excluded.end()) {
      continue;
    }

    if (values[i] < min) {
      min = values[i];
      best_idx = i;
    }
  }

  return std::make_pair(min, best_idx);
}

std::vector<double> PlatooningTacticalPlugin::optimize_speed(const std::vector<double>& downtracks, const std::vector<double>& curv_speeds, double accel_limit)
{
  if (downtracks.size() != curv_speeds.size())
  {
    throw std::invalid_argument("Downtracks and speeds do not have the same size");
  }

  if (accel_limit <= 0)
  {
    throw std::invalid_argument("Accel limits should be positive");
  }

  bool optimize = true;
  std::unordered_set<size_t> visited_idx;
  visited_idx.reserve(curv_speeds.size());

  std::vector<double> output = curv_speeds;

  while (optimize)
  {
    auto min_pair = min_with_exclusions(curv_speeds, visited_idx);
    size_t min_idx = std::get<1>(min_pair);
    if (min_idx == -1) {
      break;
    }

    visited_idx.insert(min_idx); // Mark this point as visited
    double v_i = std::get<0>(min_pair);
    double x_i = downtracks[min_idx];
    for (int i = min_idx - 1; i > 0; i--) { // NOTE: Do not use size_t for i type here as -- with > 0 will result in overflow
                                            //       First point's speed is left unchanged as it is current speed of the vehicle
      double v_f = curv_speeds[i];
      double dv = v_f - v_i;

      double x_f = downtracks[i];
      double dx = x_f - x_i;

      if(dv > 0) {
        v_f = std::min(v_f, sqrt(v_i * v_i - 2 * accel_limit * dx)); // inverting accel as we are only visiting deceleration case
        visited_idx.insert(i);
      } else if (dv < 0) {
        break;
      }
      output[i] = v_f;
      v_i = v_f;
      x_i = x_f;
    }
  }

  log::printDoublesPerLineWithPrefix("only_reverse[i]: ", output);

  output = trajectory_utils::apply_accel_limits_by_distance(downtracks, output, accel_limit, accel_limit);
  log::printDoublesPerLineWithPrefix("after_forward[i]: ", output);

  return output;
}

std::vector<PointSpeedPair> PlatooningTacticalPlugin::constrain_to_time_boundary(const std::vector<PointSpeedPair>& points,
                                                                             double time_span)
{
  std::vector<lanelet::BasicPoint2d> basic_points;
  std::vector<double> speeds;
  splitPointSpeedPairs(points, &basic_points, &speeds);

  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(basic_points);

  size_t time_boundary_exclusive_index =
      trajectory_utils::time_boundary_index(downtracks, speeds, config_.trajectory_time_length);

  if (time_boundary_exclusive_index == 0)
  {
    throw std::invalid_argument("No points to fit in timespan");
  }

  std::vector<PointSpeedPair> time_bound_points;
  time_bound_points.reserve(time_boundary_exclusive_index);

  if (time_boundary_exclusive_index == points.size())
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.end());  // All points fit within time boundary
  }
  else
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.begin() + time_boundary_exclusive_index - 1);  // Limit points by time boundary
  }

  return time_bound_points;
}

std::vector<cav_msgs::TrajectoryPlanPoint> PlatooningTacticalPlugin::trajectory_from_points_times_orientations(
    const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& times, const std::vector<double>& yaws,
    ros::Time startTime)
{
  if (points.size() != times.size() || points.size() != yaws.size())
  {
    throw std::invalid_argument("All input vectors must have the same size");
  }

  std::vector<cav_msgs::TrajectoryPlanPoint> traj;
  traj.reserve(points.size());

  for (int i = 0; i < points.size(); i++)
  {
    cav_msgs::TrajectoryPlanPoint tpp;
    ros::Duration relative_time(times[i]);
    tpp.target_time = startTime + relative_time;
    tpp.x = points[i].x();
    tpp.y = points[i].y();
    tpp.yaw = yaws[i];

    // TODO: update the name
    tpp.controller_plugin_name = "PlatooningControlPlugin";
    tpp.planner_plugin_name = plugin_discovery_msg_.name;

    traj.push_back(tpp);
  }

  return traj;
}

std::vector<PointSpeedPair> PlatooningTacticalPlugin::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
                                                                      double max_starting_downtrack,
                                                                      const carma_wm::WorldModelConstPtr& wm)
{
  std::vector<PointSpeedPair> points_and_target_speeds;
  std::unordered_set<lanelet::Id> visited_lanelets;

  bool first = true;
  ROS_DEBUG_STREAM("VehDowntrack: " << max_starting_downtrack);
  for (const auto& manuever : maneuvers)
  {
    if (manuever.type != cav_msgs::Maneuver::LANE_FOLLOWING)
    {
      throw std::invalid_argument("Platooning tactical plugin does not support this maneuver type");
    }

    if (manuever.lane_following_maneuver.parameters.negotiation_type == cav_msgs::ManeuverParameters::NO_NEGOTIATION)
    {
      throw std::invalid_argument("NO_NEGOTIATION maneuver is not supported by this plugin");
    }

    // if (manuever.lane_following_maneuver.parameters.negotiation_type == cav_msgs::ManeuverParameters::PLATOONING)
    // {
    //   // TODO: start and end speed are floats, sp compare them with epsilon
    //   // if ( manuever.lane_following_maneuver.start_speed != manuever.lane_following_maneuver.end_speed)
    //   // {
    //   //   throw std::invalid_argument("Invalid Platooning Maneuver");
    //   // }
    // }

    cav_msgs::LaneFollowingManeuver lane_following_maneuver = manuever.lane_following_maneuver;

    double starting_downtrack = lane_following_maneuver.start_dist;
    if (first)
    {
      if (starting_downtrack > max_starting_downtrack)
      {
        starting_downtrack = max_starting_downtrack;
      }
      first = false;
    }

    ROS_DEBUG_STREAM("Used downtrack: " << starting_downtrack);

    auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist, true, true);

    if (lanelets.empty())
    {
      ROS_ERROR_STREAM("Detected no lanelets between starting_downtrack: " << starting_downtrack  << ", and lane_following_maneuver.end_dist: " << lane_following_maneuver.end_dist);
      throw std::invalid_argument("Detected no lanelets between starting_downtrack and end_dist");
    }

    ROS_DEBUG_STREAM("Maneuver");
    lanelet::BasicLineString2d downsampled_centerline;
    downsampled_centerline.reserve(200);

    // getLaneletsBetween is inclusive lanelets between its two boundaries
    // which may return lanechange lanelets, so
    // exclude lanechanges and plan for only the straight part
    int curr_idx = 0;
    auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
    lanelet::ConstLanelets straight_lanelets;

    if (lanelets.size() <= 1) // no lane change anyways if only size 1
    {
      ROS_DEBUG_STREAM("Detected one straight lanelet Id: " << lanelets[curr_idx].id());
      straight_lanelets = lanelets;
    }
    else
    {
      // skip all lanechanges until lane follow starts
      while (curr_idx + 1 < lanelets.size() &&
              std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) == following_lanelets.end())
      {
        ROS_DEBUG_STREAM("As there were no directly following lanelets after this, skipping lanelet id: " << lanelets[curr_idx].id());
        curr_idx ++;
        following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
      }

      ROS_DEBUG_STREAM("Added lanelet Id for lane follow: " << lanelets[curr_idx].id());
      // guaranteed to have at least one "straight" lanelet (e.g the last one in the list)
      straight_lanelets.push_back(lanelets[curr_idx]);

      // add all lanelets on the straight road until next lanechange
      while (curr_idx + 1 < lanelets.size() &&
              std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) != following_lanelets.end())
      {
        curr_idx++;
        ROS_DEBUG_STREAM("Added lanelet Id forlane follow: " << lanelets[curr_idx].id());
        straight_lanelets.push_back(lanelets[curr_idx]);
        following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
      }
    }

    for (auto l : straight_lanelets)
    {
      ROS_DEBUG_STREAM("Processing lanelet ID: " << l.id());
      if (visited_lanelets.find(l.id()) == visited_lanelets.end())
      {

        bool is_turn = false;
        if(l.hasAttribute("turn_direction")) {
          std::string turn_direction = l.attribute("turn_direction").value();
          is_turn = turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0;
        }

        lanelet::BasicLineString2d centerline = l.centerline2d().basicLineString();
        lanelet::BasicLineString2d downsampled_points;
        if (is_turn) {
          downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.turn_downsample_ratio);
        } else {
          downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.default_downsample_ratio);
        }
        downsampled_centerline = carma_wm::geometry::concatenate_line_strings(downsampled_centerline, downsampled_points);
        visited_lanelets.insert(l.id());
      }
    }

    first = true;
    for (auto p : downsampled_centerline)
    {
      if (first && points_and_target_speeds.size() != 0)
      {
        first = false;
        continue;  // Skip the first point if we have already added points from a previous maneuver to avoid duplicates
      }
      PointSpeedPair pair;
      pair.point = p;
      pair.speed = lane_following_maneuver.end_speed;
      points_and_target_speeds.push_back(pair);
    }
  }

  // Here we are limiting the trajectory length to the given length by maneuver end dist as opposed to the end of lanelets involved.
    double starting_route_downtrack = wm_->routeTrackPos(points_and_target_speeds.front().point).downtrack;
    double ending_downtrack = maneuvers.back().lane_following_maneuver.end_dist;

    if(ending_downtrack + config_.buffer_ending_downtrack < wm_->getRouteEndTrackPos().downtrack){
      ending_downtrack = ending_downtrack + config_.buffer_ending_downtrack;
    }
    else
    {
      ending_downtrack = wm_->getRouteEndTrackPos().downtrack;
    }

    size_t max_i = points_and_target_speeds.size() - 1;
    size_t unbuffered_idx = points_and_target_speeds.size() - 1;
    bool found_unbuffered_idx = false;
    double dist_accumulator = starting_route_downtrack;
    lanelet::BasicPoint2d prev_point;

  for (int i = 0; i < points_and_target_speeds.size(); i ++) {
      auto current_point = points_and_target_speeds[i].point;
      if (i == 0) {
        prev_point = current_point;
        continue;
      }

      double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
      ROS_DEBUG_STREAM("Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() <<
        "current_point.y():" << current_point.y());
      dist_accumulator += delta_d;
      if (dist_accumulator > maneuvers.back().lane_following_maneuver.end_dist && !found_unbuffered_idx)
      {
        unbuffered_idx = i - 1;
        ROS_DEBUG_STREAM("Found index unbuffered_idx at: " << unbuffered_idx);
        found_unbuffered_idx = true;
      }
      if (dist_accumulator > ending_downtrack) {
        max_i = i - 1;
        ROS_DEBUG_STREAM("Max_i breaking at: i: " << i << ", max_i: " << max_i);
        break;
      }
      prev_point = current_point;
    }
    ending_state_before_buffer.X_pos_global = points_and_target_speeds[unbuffered_idx].point.x();
    ending_state_before_buffer.Y_pos_global = points_and_target_speeds[unbuffered_idx].point.y();
    ROS_DEBUG_STREAM("Here ending_state_before_buffer.X_pos_global: " << ending_state_before_buffer.X_pos_global <<
      ", and Y_pos_global" << ending_state_before_buffer.Y_pos_global);

    std::vector<PointSpeedPair> constrained_points(points_and_target_speeds.begin(), points_and_target_speeds.begin() + max_i);
    return constrained_points;

////
  //   std::vector<lanelet::ConstLanelet> lanelets_to_add;
  //   for (auto l : lanelets)
  //   {
  //     ROS_DEBUG_STREAM("Lanelet ID: " << l.id());
  //     if (visited_lanelets.find(l.id()) == visited_lanelets.end())
  //     {
  //       lanelets_to_add.push_back(l);
  //       visited_lanelets.insert(l.id());
  //     }
  //   }

  //   lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);

  //   first = true;
  //   for (auto p : route_geometry)
  //   {
  //     if (first && points_and_target_speeds.size() != 0)
  //     {
  //       first = false;
  //       continue;  // Skip the first point if we have already added points from a previous maneuver to avoid duplicates
  //     }
  //     PointSpeedPair pair;
  //     pair.point = p;
  //     pair.speed = lane_following_maneuver.end_speed;
  //     points_and_target_speeds.push_back(pair);
  //   }
  // }

  // return points_and_target_speeds;
}

int PlatooningTacticalPlugin::getNearestPointIndex(const std::vector<PointSpeedPair>& points,
                                               const cav_msgs::VehicleState& state)
{
  lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
  ROS_DEBUG_STREAM("veh_point: " << veh_point.x() << ", " << veh_point.y());
  double min_distance = std::numeric_limits<double>::max();
  int i = 0;
  int best_index = 0;
  for (const auto& p : points)
  {
    double distance = lanelet::geometry::distance2d(p.point, veh_point);
    ROS_DEBUG_STREAM("distance: " << distance);
    ROS_DEBUG_STREAM("p: " << p.point.x() << ", " << p.point.y());
    if (distance < min_distance)
    {
      best_index = i;
      min_distance = distance;
    }
    i++;
  }

  return best_index;
}

void PlatooningTacticalPlugin::splitPointSpeedPairs(const std::vector<PointSpeedPair>& points,
                                                std::vector<lanelet::BasicPoint2d>* basic_points,
                                                std::vector<double>* speeds)
{
  basic_points->reserve(points.size());
  speeds->reserve(points.size());

  for (const auto& p : points)
  {
    basic_points->push_back(p.point);
    speeds->push_back(p.speed);
  }
}

std::unique_ptr<smoothing::SplineI>
PlatooningTacticalPlugin::compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points)
{
  if (basic_points.size() < 4)
  {
    ROS_WARN_STREAM("Insufficient Spline Points");
    return nullptr;
  }

  std::unique_ptr<smoothing::SplineI> spl = std::make_unique<smoothing::BSpline>();
  spl->setPoints(basic_points);

  return spl;
}

std::vector<cav_msgs::TrajectoryPlanPoint> PlatooningTacticalPlugin::compose_trajectory_from_centerline(
    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state, const ros::Time& state_time)
{
  ROS_DEBUG_STREAM("VehicleState: "
                   << " x: " << state.X_pos_global << " y: " << state.Y_pos_global << " yaw: " << state.orientation
                   << " speed: " << state.longitudinal_vel);

  ROS_DEBUG_STREAM("points size: " << points.size());

  log::printDebugPerLine(points, &log::pointSpeedPairToStream);

  int nearest_pt_index = getNearestPointIndex(points, state);

  ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index);

  std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position

  auto time_bound_points = constrain_to_time_boundary(future_points, config_.trajectory_time_length);

  ROS_DEBUG_STREAM("time_bound_points size: " << time_bound_points.size());

  log::printDebugPerLine(time_bound_points, &log::pointSpeedPairToStream);

  ROS_DEBUG("Got basic points ");

  std::vector<PointSpeedPair> back_and_future = attach_back_points(points,nearest_pt_index,time_bound_points, config_.back_distance);

  ROS_DEBUG_STREAM("Got back_and_future points with size" <<back_and_future.size());
  log::printDebugPerLine(back_and_future, &log::pointSpeedPairToStream);

  std::vector<double> speed_limits;
  std::vector<lanelet::BasicPoint2d> curve_points;

  splitPointSpeedPairs(back_and_future, &curve_points, &speed_limits);

  std::unique_ptr<smoothing::SplineI> fit_curve = compute_fit(curve_points); // Compute splines based on curve points
  if (!fit_curve)
  {
    throw std::invalid_argument("Could not fit a spline curve along the given trajectory!");
  }

  ROS_DEBUG("Got fit");

  ROS_DEBUG_STREAM("speed_limits.size() " << speed_limits.size());

  std::vector<lanelet::BasicPoint2d> all_sampling_points;
  all_sampling_points.reserve(1 + curve_points.size() * 2);

  std::vector<double> distributed_speed_limits;
  distributed_speed_limits.reserve(1 + curve_points.size() * 2);

  // compute total length of the trajectory to get correct number of points
  // we expect using curve_resample_step_size
  std::vector<double> downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points);

  int total_step_along_curve = static_cast<int>(downtracks_raw.back() / config_.curve_resample_step_size);

  int current_speed_index = 0;
  size_t total_point_size = curve_points.size();

  double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size;
  double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory
  std::vector<double> better_curvature;
  better_curvature.reserve(1 + curve_points.size() * 2);

  for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution
  {
    lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve);

    all_sampling_points.push_back(p);
    double c = compute_curvature_at((*fit_curve), scaled_steps_along_curve);
    better_curvature.push_back(c);
    if ((double)steps_along_curve > step_threshold_for_next_speed)
    {
      step_threshold_for_next_speed += (double)total_step_along_curve / (double) total_point_size;
      current_speed_index ++;
    }
    distributed_speed_limits.push_back(speed_limits[current_speed_index]); // Identify speed limits for resampled points
    scaled_steps_along_curve += 1.0/total_step_along_curve; //adding steps_along_curve_step_size
  }

  ROS_DEBUG_STREAM("Got sampled points with size:" << all_sampling_points.size());
  log::printDebugPerLine(all_sampling_points, &log::basicPointToStream);

  std::vector<double> final_yaw_values = carma_wm::geometry::compute_tangent_orientations(all_sampling_points);

  log::printDoublesPerLineWithPrefix("raw_curvatures[i]: ", better_curvature);

  std::vector<double> curvatures = smoothing::moving_average_filter(better_curvature, config_.curvature_moving_average_window_size);

  std::vector<double> ideal_speeds =
      trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);

  log::printDoublesPerLineWithPrefix("curvatures[i]: ", curvatures);
  log::printDoublesPerLineWithPrefix("ideal_speeds: ", ideal_speeds);
  log::printDoublesPerLineWithPrefix("final_yaw_values[i]: ", final_yaw_values);

  std::vector<double> constrained_speed_limits = apply_speed_limits(ideal_speeds, distributed_speed_limits);
  log::printDoublesPerLineWithPrefix("constrained_speed_limits: ", constrained_speed_limits);

  ROS_DEBUG("Processed all points in computed fit");

  if (all_sampling_points.size() == 0)
  {
    ROS_WARN_STREAM("No trajectory points could be generated");
    return {};
  }

  // Add current vehicle point to front of the trajectory

  nearest_pt_index = get_nearest_index_by_downtrack(all_sampling_points, state);
  ROS_DEBUG_STREAM("Current state's nearest_pt_index: " << nearest_pt_index);
  ROS_DEBUG_STREAM("Curvature right now: " << better_curvature[nearest_pt_index] << ", at state x: " << state.X_pos_global << ", state y: " << state.Y_pos_global);
  ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[nearest_pt_index].x() << ", y:" << all_sampling_points[nearest_pt_index].y());

  int buffer_pt_index = get_nearest_index_by_downtrack(all_sampling_points, ending_state_before_buffer);
  ROS_DEBUG_STREAM("Ending state's index before applying buffer (buffer_pt_index): " << buffer_pt_index);
  ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[buffer_pt_index].x() << ", y:" << all_sampling_points[buffer_pt_index].y());

  if (nearest_pt_index + 1 >= buffer_pt_index)
  {
    ROS_WARN_STREAM("Current state is at or passed the planned end distance. Couldn't generate trajectory");
    return {};
  }

  //drop buffer points here
  std::vector<lanelet::BasicPoint2d> future_basic_points(all_sampling_points.begin() + nearest_pt_index + 1,
                                            all_sampling_points.begin()+ buffer_pt_index);  // Points in front of current vehicle position

  std::vector<double> future_speeds(constrained_speed_limits.begin() + nearest_pt_index + 1,
                                            constrained_speed_limits.begin() + buffer_pt_index);  // Points in front of current vehicle position
  std::vector<double> future_yaw(final_yaw_values.begin() + nearest_pt_index + 1,
                                            final_yaw_values.begin() + buffer_pt_index);  // Points in front of current vehicle position
  std::vector<double>  final_actual_speeds = future_speeds;
  all_sampling_points = future_basic_points;
  final_yaw_values = future_yaw;
  ROS_DEBUG_STREAM("Trimmed future points to size: " << future_basic_points.size() );

  lanelet::BasicPoint2d cur_veh_point(state.X_pos_global, state.Y_pos_global);

  all_sampling_points.insert(all_sampling_points.begin(),
                             cur_veh_point);  // Add current vehicle position to front of sample points

  final_actual_speeds.insert(final_actual_speeds.begin(), state.longitudinal_vel);

  final_yaw_values.insert(final_yaw_values.begin(), state.orientation);

  // Compute points to local downtracks
  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(all_sampling_points);

  // Apply accel limits
  final_actual_speeds = optimize_speed(downtracks, final_actual_speeds, config_.max_accel);

  log::printDoublesPerLineWithPrefix("postAccel[i]: ", final_actual_speeds);

  final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, config_.speed_moving_average_window_size);
  log::printDoublesPerLineWithPrefix("post_average[i]: ", final_actual_speeds);

  for (auto& s : final_actual_speeds)  // Limit minimum speed. TODO how to handle stopping?
  {
    s = std::max(s, config_.minimum_speed);
  }

  log::printDoublesPerLineWithPrefix("post_min_speed[i]: ", final_actual_speeds);

  // Convert speeds to times
  std::vector<double> times;
  trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);

  log::printDoublesPerLineWithPrefix("times[i]: ", times);

  // Build trajectory points
  std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
      trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time);

  if (config_.publish_debug) {
    carma_debug_msgs::TrajectoryCurvatureSpeeds msg;
    msg.velocity_profile = final_actual_speeds;
    msg.relative_downtrack = downtracks;
    msg.tangent_headings = final_yaw_values;
    std::vector<double> aligned_speed_limits(constrained_speed_limits.begin() + nearest_pt_index,
                                            constrained_speed_limits.end());
    msg.speed_limits = aligned_speed_limits;
    std::vector<double> aligned_curvatures(curvatures.begin() + nearest_pt_index,
                                            curvatures.end());
    msg.curvatures = aligned_curvatures;
    msg.lat_accel_limit = config_.lateral_accel_limit;
    msg.lon_accel_limit = config_.max_accel;
    msg.starting_state = state;
    debug_msg_ = msg;
  }
  return traj_points;
}

int PlatooningTacticalPlugin::get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points,
                                               const cav_msgs::VehicleState& state) const
{
  lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
  double min_distance = std::numeric_limits<double>::max();
  int i = 0;
  int best_index = 0;
  for (const auto& p : points)
  {
    double distance = lanelet::geometry::distance2d(p, veh_point);
    if (distance < min_distance)
    {
      best_index = i;
      min_distance = distance;
    }
    i++;
  }

  return best_index;
}

double PlatooningTacticalPlugin::compute_curvature_at(const smoothing::SplineI& fit_curve, double step_along_the_curve) const
{
  lanelet::BasicPoint2d f_prime_pt = fit_curve.first_deriv(step_along_the_curve);
  lanelet::BasicPoint2d f_prime_prime_pt = fit_curve.second_deriv(step_along_the_curve);
  // Convert to 3d vector to do 3d vector operations like cross.
  Eigen::Vector3d f_prime = {f_prime_pt.x(), f_prime_pt.y(), 0};
  Eigen::Vector3d f_prime_prime = {f_prime_prime_pt.x(), f_prime_prime_pt.y(), 0};
  return (f_prime.cross(f_prime_prime)).norm()/(pow(f_prime.norm(),3));
}

std::vector<PointSpeedPair> PlatooningTacticalPlugin::attach_back_points(const std::vector<PointSpeedPair>& points,
                          const int nearest_pt_index, std::vector<PointSpeedPair> future_points, double back_distance) const
{
  std::vector<PointSpeedPair> back_and_future;
  back_and_future.reserve(points.size());
  double total_dist = 0;
  int min_i = 0;
  for (int i = nearest_pt_index; i > 0; --i) {
    min_i = i;
    total_dist += lanelet::geometry::distance2d(points[i].point, points[i-1].point);

    if (total_dist > back_distance) {
      break;
    }
  }

  back_and_future.insert(back_and_future.end(), points.begin() + min_i, points.begin() + nearest_pt_index + 1);
  back_and_future.insert(back_and_future.end(), future_points.begin(), future_points.end());
  return back_and_future;
}

}  // namespace platooning_tactical_plugin


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#ifndef __itkEvaluateDirectionImagesFilter_cpp
#define __itkEvaluateDirectionImagesFilter_cpp

#include "itkEvaluateDirectionImagesFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionIterator.h>
#include <itkImageDuplicator.h>
#include <boost/progress.hpp>

namespace itk {

template< class PixelType >
ComparePeakImagesFilter< PixelType >
::ComparePeakImagesFilter():
  m_IgnoreMissingDirections(false),
  m_IgnoreEmptyVoxels(false),
  m_Eps(0.0001)
{
  this->SetNumberOfIndexedOutputs(2);
}

template< class PixelType >
void ComparePeakImagesFilter< PixelType >::GenerateData()
{
  if (m_TestImage.IsNull() || m_ReferenceImage.IsNull())
    mitkThrow() << "Test or reference image not set!";
//  if (m_TestImage->GetLargestPossibleRegion().GetSize()!=m_ReferenceImage->GetLargestPossibleRegion().GetSize())
//    mitkThrow() << "Test and reference image need to have same sizes!";

  Vector<float, 4> spacing4 = m_TestImage->GetSpacing();
  Point<float, 4> origin4 = m_TestImage->GetOrigin();
  Matrix<double, 4, 4> direction4 = m_TestImage->GetDirection();
  ImageRegion<4> imageRegion4 = m_TestImage->GetLargestPossibleRegion();

  Vector<double, 3> spacing3;
  Point<float, 3> origin3;
  Matrix<double, 3, 3> direction3;
  ImageRegion<3> imageRegion3;

  spacing3[0] = spacing4[0]; spacing3[1] = spacing4[1]; spacing3[2] = spacing4[2];
  origin3[0] = origin4[0]; origin3[1] = origin4[1]; origin3[2] = origin4[2];
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction3[r][c] = direction4[r][c];
  imageRegion3.SetSize(0, imageRegion4.GetSize()[0]);
  imageRegion3.SetSize(1, imageRegion4.GetSize()[1]);
  imageRegion3.SetSize(2, imageRegion4.GetSize()[2]);

  // angular error image
  typename OutputImageType::Pointer outputImage = OutputImageType::New();
  outputImage->SetOrigin( origin3 );
  outputImage->SetRegions( imageRegion3 );
  outputImage->SetSpacing( spacing3 );
  outputImage->SetDirection( direction3 );
  outputImage->Allocate();
  outputImage->FillBuffer(0.0);
  this->SetNthOutput(0, outputImage);

  // length error image
  outputImage = OutputImageType::New();
  outputImage->SetOrigin( origin3 );
  outputImage->SetRegions( imageRegion3 );
  outputImage->SetSpacing( spacing3 );
  outputImage->SetDirection( direction3 );
  outputImage->Allocate();
  outputImage->FillBuffer(0.0);
  this->SetNthOutput(1, outputImage);

  if (m_MaskImage.IsNull())
  {
    m_MaskImage = UCharImageType::New();
    m_MaskImage->SetOrigin( origin3 );
    m_MaskImage->SetRegions( imageRegion3 );
    m_MaskImage->SetSpacing( spacing3 );
    m_MaskImage->SetDirection( direction3 );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }

  m_MeanAngularError = 0.0;
  m_MedianAngularError = 0;
  m_MaxAngularError = 0.0;
  m_MinAngularError = itk::NumericTraits<float>::max();
  m_VarAngularError = 0.0;
  m_AngularErrorVector.clear();

  m_MeanLengthError = 0.0;
  m_MedianLengthError = 0;
  m_MaxLengthError = 0.0;
  m_MinLengthError = itk::NumericTraits<float>::max();
  m_VarLengthError = 0.0;
  m_LengthErrorVector.clear();

  outputImage = static_cast< OutputImageType* >(this->ProcessObject::GetOutput(0));
  typename OutputImageType::Pointer outputImage2 = static_cast< OutputImageType* >(this->ProcessObject::GetOutput(1));

  ImageRegionIterator< OutputImageType > oit(outputImage, outputImage->GetLargestPossibleRegion());
  ImageRegionIterator< OutputImageType > oit2(outputImage2, outputImage2->GetLargestPossibleRegion());
  ImageRegionIterator< UCharImageType > mit(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());

  boost::progress_display disp(outputImage->GetLargestPossibleRegion().GetSize()[0]*outputImage->GetLargestPossibleRegion().GetSize()[1]*outputImage->GetLargestPossibleRegion().GetSize()[2]);
  while( !oit.IsAtEnd() )
  {
    ++disp;
    if( mit.Get()!=1 )
    {
      ++oit;
      ++oit2;
      ++mit;
      continue;
    }
    typename OutputImageType::IndexType index = oit.GetIndex();
    itk::Index<4> idx4;
    idx4[0] = index[0];
    idx4[1] = index[1];
    idx4[2] = index[2];

    float maxAngularError = 1.0;

    // get number of valid directions (length > 0)
    unsigned int numRefPeaks = 0;
    unsigned int numTestPeaks = 0;
    std::vector< vnl_vector_fixed< PixelType, 3 > > testPeaks;
    std::vector< vnl_vector_fixed< PixelType, 3 > > refPeaks;
    std::vector< PixelType > testPeakMagnitudes;
    std::vector< PixelType > refPeakMagnitudes;

    for (unsigned int i=0; i<m_ReferenceImage->GetLargestPossibleRegion().GetSize()[3]/3; i++)
    {
      vnl_vector_fixed< PixelType, 3 > peak;
      idx4[3] = i*3;
      peak[0] = m_ReferenceImage->GetPixel(idx4);
      idx4[3] = i*3 + 1;
      peak[1] = m_ReferenceImage->GetPixel(idx4);
      idx4[3] = i*3 + 2;
      peak[2] = m_ReferenceImage->GetPixel(idx4);

      PixelType mag = peak.magnitude();
      refPeakMagnitudes.push_back(mag);
      if (mag > m_Eps )
      {
        peak.normalize();
        refPeaks.push_back(peak);
        numRefPeaks++;
      }
    }
    for (unsigned int i=0; i<m_TestImage->GetLargestPossibleRegion().GetSize()[3]/3; i++)
    {
      vnl_vector_fixed< PixelType, 3 > peak;
      idx4[3] = i*3;
      peak[0] = m_TestImage->GetPixel(idx4);
      idx4[3] = i*3 + 1;
      peak[1] = m_TestImage->GetPixel(idx4);
      idx4[3] = i*3 + 2;
      peak[2] = m_TestImage->GetPixel(idx4);

      PixelType mag = peak.magnitude();
      testPeakMagnitudes.push_back(mag);
      if (mag > m_Eps )
      {
        peak.normalize();
        testPeaks.push_back(peak);
        numTestPeaks++;
      }
    }

    // matrix containing the angular error between the directions
    unsigned int maxNumPeaks = std::max(numTestPeaks, numRefPeaks);
    vnl_matrix< float > diffM; diffM.set_size(maxNumPeaks, maxNumPeaks);
    diffM.fill(0.0);
    diffM.fill(10); // initialize with invalid error value

    if (m_IgnoreEmptyVoxels && (numRefPeaks==0 || numTestPeaks==0) )
    {
      ++oit;
      ++oit2;
      ++mit;
      continue;
    }

    // i: index of reference direction
    // j: index of test direction
    for (unsigned int i=0; i<maxNumPeaks; i++)     // for each reference direction
    {
      bool missingDir = false;
      vnl_vector_fixed< PixelType, 3 > refDir;

      if (i<numRefPeaks)  // normalize if not null
        refDir = refPeaks.at(i);
      else if (m_IgnoreMissingDirections)
        continue;
      else
        missingDir = true;

      for (unsigned int j=0; j<maxNumPeaks; j++)     // and each test direction
      {
        vnl_vector_fixed< PixelType, 3 > testDir;
        if (j<numTestPeaks)  // normalize if not null
          testDir = testPeaks.at(j);
        else if (m_IgnoreMissingDirections || missingDir)
          continue;
        else
          missingDir = true;

        // found missing direction
        if (missingDir)
        {
          diffM[i][j] = -1;
          continue;
        }

        // calculate angle between directions
        diffM[i][j] = fabs(dot_product(refDir, testDir));

        if (diffM[i][j] < maxAngularError)
          maxAngularError = diffM[i][j];

        if (diffM[i][j]>1.0)
          diffM[i][j] = 1.0;
      }
    }

    float angularError = 0.0;
    float lengthError = 0.0;
    int counter = 0;
    vnl_matrix< float > diffM_copy = diffM;
    for (unsigned int k=0; k<maxNumPeaks; k++)
    {
      float error = -1;
      int a,b; a=-1; b=-1;
      bool missingDir = false;

      // i: index of reference direction
      // j: index of test direction
      // find smalles error between two directions (large value -> small error)
      for (unsigned int i=0; i<maxNumPeaks; i++)
        for (unsigned int j=0; j<maxNumPeaks; j++)
        {
          if (diffM[i][j]>error && diffM[i][j]<2) // found valid error entry
          {
            error = diffM[i][j];
            a = (int)i;
            b = (int)j;
            missingDir = false;
          }
          else if (diffM[i][j]<0 && error<0)    // found missing direction
          {
            a = (int)i;
            b = (int)j;
            missingDir = true;
          }
        }

      if (a<0 || b<0 || (m_IgnoreMissingDirections && missingDir))
        continue; // no more directions found

      if (a>=(int)numRefPeaks && b>=(int)numTestPeaks)
      {
        MITK_INFO << a << " " << numRefPeaks;
        MITK_INFO << b << " " << numTestPeaks;
        MITK_INFO << "ERROR: missing test and reference direction. should not be possible. check code.";
        continue;
      }

      // remove processed directions from error matrix
      diffM.set_row(a, 10.0);
      diffM.set_column(b, 10.0);

      if (a>=(int)numRefPeaks) // missing reference direction (find next closest)
      {
        for (unsigned int i=0; i<numRefPeaks; i++)
          if (diffM_copy[i][b]>error)
          {
            error = diffM_copy[i][b];
            a = i;
          }
      }
      else if (b>=(int)numTestPeaks) // missing test direction (find next closest)
      {
        for (unsigned int i=0; i<numTestPeaks; i++)
          if (diffM_copy[a][i]>error)
          {
            error = diffM_copy[a][i];
            b = i;
          }
      }

      float refLength = 0;
      float testLength = 1;

      if (a>=(int)numRefPeaks || b>=(int)numTestPeaks || error<0)
        error = 0;
      else
      {
        refLength = refPeakMagnitudes.at(a);
        testLength = testPeakMagnitudes.at(b);
      }

      m_LengthErrorVector.push_back( fabs(refLength-testLength) );
      m_AngularErrorVector.push_back( acos(error)*180.0/itk::Math::pi );

      m_MeanAngularError += m_AngularErrorVector.back();
      m_MeanLengthError += m_LengthErrorVector.back();

      angularError += m_AngularErrorVector.back();
      lengthError += m_LengthErrorVector.back();
      counter++;
    }

    if (counter>0)
    {
      lengthError /= counter;
      angularError /= counter;
    }
    oit2.Set(lengthError);
    oit.Set(angularError);

    ++oit;
    ++oit2;
    ++mit;
  }

  std::sort( m_AngularErrorVector.begin(), m_AngularErrorVector.end() );
  m_MeanAngularError /= m_AngularErrorVector.size();      // mean
  for (unsigned int i=0; i<m_AngularErrorVector.size(); i++)
  {
    float temp = m_AngularErrorVector.at(i) - m_MeanAngularError;
    m_VarAngularError += temp*temp;

    if ( m_AngularErrorVector.at(i)>m_MaxAngularError )
      m_MaxAngularError = m_AngularErrorVector.at(i);
    if ( m_AngularErrorVector.at(i)<m_MinAngularError )
      m_MinAngularError = m_AngularErrorVector.at(i);
  }
  if (m_AngularErrorVector.size()>1)
  {
    m_VarAngularError /= (m_AngularErrorVector.size()-1);   // variance

    // median
    if (m_AngularErrorVector.size()%2 == 0)
      m_MedianAngularError = 0.5*( m_AngularErrorVector.at( m_AngularErrorVector.size()/2 ) +  m_AngularErrorVector.at( m_AngularErrorVector.size()/2+1 ) );
    else
      m_MedianAngularError = m_AngularErrorVector.at( (m_AngularErrorVector.size()+1)/2 ) ;
  }

  std::sort( m_LengthErrorVector.begin(), m_LengthErrorVector.end() );
  m_MeanLengthError /= m_LengthErrorVector.size();      // mean
  for (unsigned int i=0; i<m_LengthErrorVector.size(); i++)
  {
    float temp = m_LengthErrorVector.at(i) - m_MeanLengthError;
    m_VarLengthError += temp*temp;

    if ( m_LengthErrorVector.at(i)>m_MaxLengthError )
      m_MaxLengthError = m_LengthErrorVector.at(i);
    if ( m_LengthErrorVector.at(i)<m_MinLengthError )
      m_MinLengthError = m_LengthErrorVector.at(i);
  }
  if (m_LengthErrorVector.size()>1)
  {
    m_VarLengthError /= (m_LengthErrorVector.size()-1);   // variance

    // median
    if (m_LengthErrorVector.size()%2 == 0)
      m_MedianLengthError = 0.5*( m_LengthErrorVector.at( m_LengthErrorVector.size()/2 ) +  m_LengthErrorVector.at( m_LengthErrorVector.size()/2+1 ) );
    else
      m_MedianLengthError = m_LengthErrorVector.at( (m_LengthErrorVector.size()+1)/2 ) ;
  }
}

}

#endif // __itkEvaluateDirectionImagesFilter_cpp


#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <boost/ref.hpp>

const double MAX_X_VEL = 0.003;
const double MAX_Y_VEL = 0.003;
const double MAX_Z_VEL = 0.003;
const double MAX_ANGULAR_Y_POS = M_PI / 8.0;
const double MAX_ANGULAR_X_POS = M_PI / 8.0;
const double BUTTON_LINEAR_DELTA_Z = 0.005;
const double BUTTON_ANGULAR_DELTA_Z = 0.01;

const double MAX_ANGULAR_Z_VEL = 0.01;

double mapX(double ratio)
{
  return ratio * -MAX_X_VEL;
}

double mapY(double ratio)
{
  return ratio * MAX_X_VEL;
}

double mapZ(double ratio)
{
  return ratio * MAX_Z_VEL;
}

double mapAngularZ(double ratio)
{
  return ratio * MAX_ANGULAR_Z_VEL;
}

double mapAngularY(double ratio)
{
  return ratio * MAX_ANGULAR_Y_POS;
}

double mapAngularX(double ratio)
{
  return ratio * MAX_ANGULAR_X_POS;
}

double buttonLinearZ(bool v) { return v ? BUTTON_LINEAR_DELTA_Z : 0.0; }
double buttonAngularZ(bool v) { return v ? BUTTON_ANGULAR_DELTA_Z : 0.0; }

geometry_msgs::Twist interpret(const sensor_msgs::Joy& joy)
{
  geometry_msgs::Twist twist;
  twist.linear.x = mapX(joy.axes[0]);
  twist.linear.y = mapY(joy.axes[1]);
  twist.linear.z = buttonLinearZ(joy.buttons[4]) - buttonLinearZ(joy.buttons[6]);

  twist.angular.x = mapAngularX(-joy.axes[3]);
  twist.angular.y = mapAngularY(-joy.axes[2]);
  twist.angular.z = buttonAngularZ(joy.buttons[7]) - buttonAngularZ(joy.buttons[5]);

  return twist;
}

bool isInactive(double v) { return std::abs(v) < 0.0001; }

void handleSixAxisPS3(const sensor_msgs::JoyConstPtr& joy,
                      ros::Publisher& output_pub,
                      ros::Publisher& mode_pub)
{
  static bool select_button_last_state = false;
  if (joy->buttons[0] != select_button_last_state)
  {
    if (joy->buttons[0]) {
      ROS_INFO("Rising edge... publishing");
      std_msgs::Empty empty;
      mode_pub.publish(empty);
    }
    select_button_last_state = joy->buttons[0];
  }

  if (isInactive(joy->axes[0]) && isInactive(joy->axes[1]) &&
      isInactive(joy->axes[2]) && isInactive(joy->axes[3]) &&
      isInactive(joy->axes[12]) && isInactive(joy->axes[13]) &&
      !joy->buttons[0] && !joy->buttons[4] && !joy->buttons[5] &&
      !joy->buttons[6] && !joy->buttons[7]

      ) return;

  geometry_msgs::Twist out = interpret(*joy);

  output_pub.publish(out);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "joystick_teleop_node");

  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("commands", 1);
  ros::Publisher mode_pub = nh.advertise<std_msgs::Empty>("change_mode", 1);

  ros::Subscriber sub = nh.subscribe<sensor_msgs::Joy>("joy", 1,
    boost::bind(handleSixAxisPS3, _1, boost::ref(pub), boost::ref(mode_pub)));

  ros::spin();

  return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <Eigen/Core>
#include <sstream>
#include "metro/mean_and_variance.hpp"
#include "genfile/VariantIdentifyingData.hpp"
#include "components/SampleSummaryComponent/SampleSummaryComputation.hpp"
#include "components/SampleSummaryComponent/RiskScoreComputation.hpp"

//#define DEBUG_RISK_SCORE_COMPUTATION 3

namespace sample_stats {
	RiskScoreComputation::RiskScoreComputation( genfile::CohortIndividualSource const& samples, genfile::VariantIdentifyingData::CompareFields comparator ):
		m_samples( samples ),
		m_number_of_samples( samples.get_number_of_individuals() ),
		m_map( comparator )
	{}

	void RiskScoreComputation::accumulate( genfile::VariantIdentifyingData const& snp, Genotypes const& genotypes, genfile::VariantDataReader& ) {
		// do nothing
#if	DEBUG_RISK_SCORE_COMPUTATION
				std::cerr << "Looking at SNP:" << snp << ".\n" ;
#endif
		RiskScoreMap::const_iterator const where = m_map.find( snp ) ;
		if( where != m_map.end() ) {
			RiskScoreIdBetaMap::const_iterator i = where->second.begin() ;
			RiskScoreIdBetaMap::const_iterator const end_i = where->second.end() ;
			for( ; i != end_i; ++i ) {
				std::string const& risk_score_identifier = i->first ;
#if	DEBUG_RISK_SCORE_COMPUTATION
				std::cerr << "Accumulating risk scores for " << snp << ", " << risk_score_identifier << "...\n" ;
#endif
				accumulate( genotypes, i->second, m_scores[ risk_score_identifier ], m_counts[ risk_score_identifier ] ) ;
			}
		}
	}

	void RiskScoreComputation::accumulate( Genotypes const& genotypes, Betas const& betas, Eigen::VectorXd& scores, Eigen::VectorXd& counts ) const {
		assert( genotypes.cols() == 3 ) ;
		Eigen::VectorXd non_missingness = ( genotypes.rowwise().sum().array() > 0.0 ).cast< double >() ;
		Eigen::VectorXd contribution = ( genotypes * betas ).rowwise().sum() ;

#if DEBUG_RISK_SCORE_COMPUTATION > 1
		std::cerr << "betas = \n"
			<< betas << ".\n" ;
		std::cerr << "non_missingness = " << non_missingness.head(10).transpose() << "...\n" ;
		std::cerr << "genotypes =\n" << genotypes.block(0,0,10,3) << "...\n" ;
		std::cerr << "contribution = " << contribution.head(10).transpose() << "...\n" ;
#endif

		scores += contribution ;
		counts += non_missingness ;
	}

	void RiskScoreComputation::compute( int sample, ResultCallback callback ) {
		for( Identifiers::const_iterator i = m_identifiers.begin(); i != m_identifiers.end(); ++i ) {
			compute_impl( sample, *i, callback ) ;
		}
	}

	void RiskScoreComputation::compute_impl( int sample, std::string const& risk_score_identifier, ResultCallback callback ) {
		Eigen::VectorXd const& scores = m_scores.at( risk_score_identifier ) ;
		Eigen::VectorXd const& counts = m_counts.at( risk_score_identifier ) ;
		assert( std::size_t( scores.size() ) == m_number_of_samples ) ;
		assert( std::size_t( counts.size() ) == m_number_of_samples ) ;
		callback( sample, risk_score_identifier + "_risk_score", scores( sample ) ) ;
		callback( sample, risk_score_identifier + "_risk_score_count", counts( sample ) ) ;
	}

	std::string RiskScoreComputation::get_summary( std::string const& prefix, std::size_t column_width ) const {
		std::ostringstream ostr ;
		ostr << prefix << "RiskScoreComputation\n"
			<< prefix << " - computing these risk scores:" ;
		std::size_t count = 0 ;
		for( std::set< std::string >::const_iterator i = m_identifiers.begin(); i != m_identifiers.end(); ++i, ++count ) {
			if( count > 10 ) {
				ostr << prefix << "     ..." ;
				break ;
			}
			ostr << prefix << "     " << *i << "(" << m_snp_counts.at(*i) << " SNPs)\n" ;
		}
		return ostr.str() ;
	}

	void RiskScoreComputation::add_effects( statfile::BuiltInTypeStatSource& source, ProgressCallback progress_callback ) {
		using namespace genfile::string_utils ;
		if(
			( source.number_of_columns() < 8)
			|| ( to_lower( source.name_of_column( 0 ) ) != "snpid" )
			|| ( to_lower( source.name_of_column( 1 ) ) != "rsid" )
			|| ( to_lower( source.name_of_column( 2 ) ) != "chromosome" )
			|| ( to_lower( source.name_of_column( 3 ) ) != "position" )
			|| ( to_lower( source.name_of_column( 4 ) ) != "allelea" )
			|| ( to_lower( source.name_of_column( 5 ) ) != "alleleb" )
		) {
			throw genfile::MalformedInputError(
				source.get_source_spec(),
				"Expected first six column names SNPID rsid chromosome position alleleA alleleB, but found: "
					+ source.name_of_column( 0 ) + " "
					+ source.name_of_column( 1 ) + " "
					+ source.name_of_column( 2 ) + " "
					+ source.name_of_column( 3 ) + " "
					+ source.name_of_column( 4 ) + " "
					+ source.name_of_column( 5 ),
				1
			) ;
		}

		if( !source.has_column( "risk_score_identifier" ) || !source.has_column( "additive_beta" ) || !source.has_column( "heterozygote_beta" ) ) {
			throw genfile::MalformedInputError( source.get_source_spec(), "Expected to find risk_score_identifier, additive_beta and heterozygote_beta columns.", 1 ) ;
		}

		std::size_t const identifier_column = source.index_of_column( "risk_score_identifier" ) ;
		std::size_t const additive_beta_column = source.index_of_column( "additive_beta" ) ;
		std::size_t const heterozygote_beta_column = source.index_of_column( "heterozygote_beta" ) ;
		std::size_t const min_beta_column = std::min( additive_beta_column, heterozygote_beta_column ) ;
		std::size_t const max_beta_column = std::max( additive_beta_column, heterozygote_beta_column ) ;
		bool additive_first = ( additive_beta_column < heterozygote_beta_column ) ;

		if( identifier_column > min_beta_column ) {
			throw genfile::MalformedInputError( source.get_source_spec(), "Expected to find the risk_score_identifier before the additive_beta and heterozygote_beta columns.", 1 ) ;
		}

		if( progress_callback ) {
			progress_callback( 0, source.number_of_rows() ) ;
		}

		std::set< std::string > identifiers ;

		std::string SNPID, rsid, allele1, allele2 ;
		genfile::GenomePosition position ;
		std::string risk_score_identifier ;
		Eigen::MatrixXd betas( 3, 2 ) ;
		double beta1, beta2 ;
		while( source >> SNPID >> rsid >> position.chromosome() >> position.position() >> allele1 >> allele2 ) {
			genfile::VariantIdentifyingData snp( SNPID, rsid, position, allele1, allele2 ) ;

			source >> statfile::ignore( identifier_column - 6 ) >> risk_score_identifier ;
			source >> statfile::ignore( min_beta_column - identifier_column - 1 ) >> beta1 ;
			source >> statfile::ignore( max_beta_column - min_beta_column - 1 ) >> beta2 ;

			betas.setZero() ;
			if( additive_first ) {
				betas(1,0) = beta1 ;
				betas(2,0) = 2 * beta1 ;
				betas(1,1) = beta2 ;
			} else {
				betas(1,0) = beta2 ;
				betas(2,0) = 2 * beta2 ;
				betas(1,1) = beta1 ;
			}

			RiskScoreIdBetaMap::const_iterator where = m_map[ snp ].find( risk_score_identifier ) ;
			if( where != m_map[ snp ].end() ) {
				throw genfile::MalformedInputError( source.get_source_spec(), "SNP " + std::string( snp.get_primary_id() ) + " has duplicated entries for risk score " + risk_score_identifier, source.number_of_rows_read() ) ;
			}

			m_map[ snp ][ risk_score_identifier ] = betas ;
			++m_snp_counts[ risk_score_identifier ] ;
			identifiers.insert( risk_score_identifier ) ;
			source >> statfile::ignore_all() ;
			if( progress_callback ) {
				progress_callback( source.number_of_rows_read(), source.number_of_rows() ) ;
			}
		}

		for( std::set< std::string >::const_iterator i = identifiers.begin(); i != identifiers.end(); ++i ) {
			m_scores[ *i ] = Eigen::VectorXd::Zero( m_number_of_samples ) ;
			m_counts[ *i ] = Eigen::VectorXd::Zero( m_number_of_samples ) ;
		}

		m_identifiers = identifiers ;

#if DEBUG_RISK_SCORE_COMPUTATION
		std::cerr << "I have these SNPs:\n" ;
		for( RiskScoreMap::const_iterator i = m_map.begin(); i != m_map.end(); ++i ) {
			std::cerr << i->first << ".\n" ;
		}
#endif
	}
}


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

// This source code is derived from rotors_simulator
//   (https://github.com/ethz-asl/rotors_simulator)
// * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland,
// licensed under the Apache-2.0 license,
// cf. 3rd-party-licenses.txt file in the root directory of this source tree.
//
// The original code was modified to:
// - be more consistent with other sensor plugins within uuv_simulator,
// - remove its dependency on ROS. The ROS interface is instead implemented in
//   a derived class within the uuv_sensor_plugins_ros package,
// - adhere to Gazebo's coding standards.

#ifndef UUV_SENSOR_PLUGINS_GAZEBO_IMU_PLUGIN_H_
#define UUV_SENSOR_PLUGINS_GAZEBO_IMU_PLUGIN_H_

#include <random>
#include <string>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/msgs/msgs.hh"
#include "SensorImu.pb.h"

#include "SensorPlugin.hh"

namespace gazebo {

// Default values for use with ADIS16448 IMU
static constexpr double kDefaultAdisGyroscopeNoiseDensity =
    2.0 * 35.0 / 3600.0 / 180.0 * M_PI;
static constexpr double kDefaultAdisGyroscopeRandomWalk =
    2.0 * 4.0 / 3600.0 / 180.0 * M_PI;
static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime =
    1.0e+3;
static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma =
    0.5 / 180.0 * M_PI;
static constexpr double kDefaultAdisAccelerometerNoiseDensity =
    2.0 * 2.0e-3;
static constexpr double kDefaultAdisAccelerometerRandomWalk =
    2.0 * 3.0e-3;
static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime =
    300.0;
static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma =
    20.0e-3 * 9.8;
static constexpr double kDefaultOrientationNoise = 0.5;

static const std::string kDefaultImuTopic = "imu";

/// \brief ImuParameters stores all IMU model parameters.
/// A description of these parameters can be found here:
/// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
struct ImuParameters {
  /// \brief Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)]
  double gyroscopeNoiseDensity;
  /// \brief Gyroscope bias random walk [rad/s/s/sqrt(Hz)]
  double gyroscopeRandomWalk;
  /// \brief Gyroscope bias correlation time constant [s]
  double gyroscopeBiasCorrelationTime;
  /// \brief Gyroscope turn on bias standard deviation [rad/s]
  double gyroscopeTurnOnBiasSigma;
  /// \brief Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)]
  double accelerometerNoiseDensity;
  /// \brief Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)]
  double accelerometerRandomWalk;
  /// \brief Accelerometer bias correlation time constant [s]
  double accelerometerBiasCorrelationTime;
  /// \brief Accelerometer turn on bias standard deviation [m/s^2]
  double accelerometerTurnOnBiasSigma;

  /// \brief Standard deviation of orientation noise per axis [rad].
  double orientationNoise;

  /// \brief Constructor.
  ImuParameters()
      : gyroscopeNoiseDensity(kDefaultAdisGyroscopeNoiseDensity),
        gyroscopeRandomWalk(kDefaultAdisGyroscopeRandomWalk),
        gyroscopeBiasCorrelationTime(
            kDefaultAdisGyroscopeBiasCorrelationTime),
        gyroscopeTurnOnBiasSigma(kDefaultAdisGyroscopeTurnOnBiasSigma),
        accelerometerNoiseDensity(kDefaultAdisAccelerometerNoiseDensity),
        accelerometerRandomWalk(kDefaultAdisAccelerometerRandomWalk),
        accelerometerBiasCorrelationTime(
            kDefaultAdisAccelerometerBiasCorrelationTime),
        accelerometerTurnOnBiasSigma(
            kDefaultAdisAccelerometerTurnOnBiasSigma),
        orientationNoise(kDefaultOrientationNoise)
  {}
};

/// \brief GazeboImuPlugin simulates an IMU with drifting gyro. and accel.
///        The implementation is based on the GazeboImuPlugin found in the
///        rotors simulator.
class GazeboImuPlugin : public GazeboSensorPlugin {
  /// \brief Constructor.
  public: GazeboImuPlugin();

  /// \brief Desctructor.
  public: virtual ~GazeboImuPlugin();

  /// \brief Load plugin and its configuration from sdf,
  protected: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

  /// \brief Apply and add nosie model to ideal measurements.
  protected: void AddNoise(Eigen::Vector3d &linear_acceleration,
                           Eigen::Vector3d &angular_velocity,
                           Eigen::Quaterniond &orientation,
                           double dt);

  /// \brief Generate a simulated measurement.
  protected: void SimulateMeasurement(const common::UpdateInfo& _info);

  /// \brief Update callback from simulation.
  protected: virtual bool OnUpdate(const common::UpdateInfo&);

  /// \brief Last measurement of linear acceleration..
  protected: Eigen::Vector3d measLinearAcc_;

  /// \brief Last measurement of angular velocity.
  protected: Eigen::Vector3d measAngularVel_;

  /// \brief (Simulation) time when the last sensor measurement was generated.
  protected: Eigen::Quaterniond measOrientation_;

  /// \brief Gazebo sensor message. Kept as member since largely constant.
  protected: sensor_msgs::msgs::Imu imuGazeboMessage_;

  /// \brief Gravity vector wrt. world frame according to physics engine.
  protected: math::Vector3 gravityW_;

  /// \brief Current (drifting) gyroscope bias.
  protected: Eigen::Vector3d gyroscopeBias_;

  /// \brief Current (drifting) accelerometer bias.
  protected: Eigen::Vector3d accelerometerBias_;

  /// \brief Constant turn-on gyroscope bias.
  protected: Eigen::Vector3d gyroscopeTurnOnBias_;

  /// \brief Constant turn-on accelerometer bias.
  protected: Eigen::Vector3d accelerometerTurnOnBias_;

  /// \brief IMU model parameters.
  protected: ImuParameters imuParameters_;
};
}

#endif  // UUV_SENSOR_PLUGINS_GAZEBO_IMU_PLUGIN_H_


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkVectorImageToFourDImageFilter_cpp
#define __itkVectorImageToFourDImageFilter_cpp

#include "itkVectorImageToFourDImageFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <vnl/vnl_vector.h>
#include <itkOrientationDistributionFunction.h>
#include <itkContinuousIndex.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/math/special_functions.hpp>
#include <boost/progress.hpp>

using namespace boost::math;

namespace itk {

template< class PixelType >
VectorImageToFourDImageFilter< PixelType>::VectorImageToFourDImageFilter()
{

}

template< class PixelType >
void VectorImageToFourDImageFilter< PixelType>
::GenerateData()
{
  Vector<float, 4> spacing4;
  Point<float, 4> origin4;
  Matrix<double, 4, 4> direction4; direction4.SetIdentity();
  ImageRegion<4> imageRegion4;

  Vector<double, 3> spacing3 = m_InputImage->GetSpacing();
  Point<float, 3> origin3 = m_InputImage->GetOrigin();
  Matrix<double, 3, 3> direction3 = m_InputImage->GetDirection();
  ImageRegion<3> imageRegion3 = m_InputImage->GetLargestPossibleRegion();

  spacing4[0] = spacing3[0]; spacing4[1] = spacing3[1]; spacing4[2] = spacing3[2]; spacing4[3] = 1;
  origin4[0] = origin3[0]; origin4[1] = origin3[1]; origin4[2] = origin3[2]; origin4[3] = 0;
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction4[r][c] = direction3[r][c];

  imageRegion4.SetSize(0, imageRegion3.GetSize()[0]);
  imageRegion4.SetSize(1, imageRegion3.GetSize()[1]);
  imageRegion4.SetSize(2, imageRegion3.GetSize()[2]);
  imageRegion4.SetSize(3, m_InputImage->GetVectorLength());

  m_OutputImage = OutputImageType::New();
  m_OutputImage->SetSpacing( spacing4 );
  m_OutputImage->SetOrigin( origin4 );
  m_OutputImage->SetDirection( direction4 );
  m_OutputImage->SetRegions( imageRegion4 );
  m_OutputImage->Allocate();
  m_OutputImage->FillBuffer(0);
  ImageRegionIterator< InputImageType > iit(m_InputImage, m_InputImage->GetLargestPossibleRegion() );

  while( !iit.IsAtEnd() )
  {
    auto idx3 = iit.GetIndex();

    typename InputImageType::PixelType pix = iit.Get();

    for (unsigned int i=0; i<pix.GetSize(); ++i)
    {
      typename OutputImageType::IndexType idx4;
      idx4[0] = idx3[0]; idx4[1] = idx3[1]; idx4[2] = idx3[2]; idx4[3] = i;
      m_OutputImage->SetPixel(idx4, pix[i]);
    }

    ++iit;
  }
}

}

#endif // __itkVectorImageToFourDImageFilter_cpp


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Modeling/ShapeInference.h"
#include "Molassembler/Shapes/Data.h"

#include "boost/optional.hpp"

#include <iostream>

using namespace Scine;
using namespace Molassembler;
using namespace ShapeInference;

using TestCaseType = std::tuple<
  std::string, // a Name for the current compound
  Utils::ElementType, // The central atom type
  std::vector<BindingSite>, // a list of ligand types
  int // charge centered on the central atom
>;

// Runs a specific test case type
void testVsepr(
  const Shapes::Shape& expectedShapeName,
  const std::vector<TestCaseType>& testCases
) {
  std::string complexName;
  int charge;
  std::vector<BindingSite> ligands;
  Utils::ElementType centerAtomType;

  for(const auto& testCase : testCases) {

    std::tie(complexName, centerAtomType, ligands, charge) = testCase;

    boost::optional<Shapes::Shape> shape;

    try {
      shape = vsepr(
        centerAtomType,
        ligands,
        charge
      );
    } catch(std::exception& e) {
      std::cout << "Caught an exception when trying "
        << complexName << "." << std::endl;
      throw e;
    }

    BOOST_CHECK_MESSAGE(
      shape,
      "VSEPR could not find a shape for this case"
    );

    if(shape) {
      BOOST_CHECK_MESSAGE(
        shape.value() == expectedShapeName,
        complexName << " has been determined as "
          << Shapes::name(shape.value())
          << ", expected: "
          << Shapes::name(expectedShapeName)
      );
    }
  }
}

// Helper function to compose ligand situations
auto makeLigand(
  const unsigned L,
  const unsigned X,
  const Utils::ElementType& type,
  const BondType& bty
) {
  return BindingSite {
    L,
    X,
    {type},
    bty
  };
}

// Helper function to compose ligand situations
std::vector<BindingSite> repeat(
  const BindingSite& ligand,
  const unsigned N
) {
  return std::vector<BindingSite> (N, ligand);
}

// Helper function to compose ligand situations
std::vector<BindingSite> merge(
  const std::vector<BindingSite>& a,
  const std::vector<BindingSite>& b
) {
  std::vector<BindingSite> ret = a;
  std::copy(
    b.begin(),
    b.end(),
    std::back_inserter(ret)
  );

  return ret;
}

BOOST_AUTO_TEST_CASE(VseprTests, *boost::unit_test::label("Molassembler")) {
  using Element = Utils::ElementType;

  testVsepr( // AX2E0
    Shapes::Shape::Line,
    {
      TestCaseType {
        "CO2",
        Element::C,
        repeat(
          makeLigand(0, 2, Element::O, BondType::Double),
          2
        ),
        0
      },
      TestCaseType {
        "BeF2",
        Element::Be,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          2
        ),
        0
      },
      TestCaseType {
        "C≡C-H",
        Element::C,
        {
          makeLigand(0, 3, Element::C, BondType::Triple),
          makeLigand(0, 1, Element::H, BondType::Single)
        },
        0
      }
    }
  );

  testVsepr( // AX3E0
    Shapes::Shape::EquilateralTriangle,
    {
      TestCaseType {
        "BCl3",
        Element::B,
        repeat(
          makeLigand(0, 1, Element::Cl, BondType::Single),
          3
        ),
        0
      },
      TestCaseType {
        "H2C=O",
        Element::C,
        {
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 2, Element::O, BondType::Double),
        },
        0
      },
      TestCaseType {
        "SO3",
        Element::S,
        repeat(
          makeLigand(0, 2, Element::O, BondType::Double),
          3
        ),
        0
      }
    }
  );

  testVsepr( // AX2E1 (V-shape / Bent)
    Shapes::Shape::Bent,
    {
      TestCaseType {
        "SO2",
        Element::S,
        repeat(
          makeLigand(0, 2, Element::O, BondType::Double),
          2
        ),
        0
      },
      TestCaseType {
        "CH2··",
        Element::C,
        repeat(
          makeLigand(0, 1, Element::H, BondType::Single),
          2
        ),
        0
      },
      TestCaseType {
        "NH2^+",
        Element::N,
        repeat(
          makeLigand(0, 1, Element::H, BondType::Single),
          2
        ),
        +1
      },
      TestCaseType {
        /* Lewis structure is
         *
         *  O = N⁺ – O⁻  < – >  ⁻O – N⁺ = O
         *
         */
        "NO2·",
        Element::N,
        {
          makeLigand(0, 1, Element::O, BondType::Single),
          makeLigand(0, 2, Element::O, BondType::Double),
        },
        +1
      }
    }
  );

  testVsepr( // AX4E0
    Shapes::Shape::Tetrahedron,
    {
      TestCaseType {
        "CH4",
        Element::C,
        repeat(
          makeLigand(0, 1, Element::H, BondType::Single),
          4
        ),
        0
      },
      TestCaseType {
        "XeO4",
        Element::Xe,
        repeat(
          makeLigand(0, 2, Element::O, BondType::Double),
          4
        ),
        0
      },
      TestCaseType {
        "ClO4-",
        Element::Cl,
        {
          makeLigand(0, 2, Element::O, BondType::Double),
          makeLigand(0, 2, Element::O, BondType::Double),
          makeLigand(0, 2, Element::O, BondType::Double),
          makeLigand(0, 1, Element::O, BondType::Single)
        },
        0 // charge is not on central chlorine atom!
      },
      TestCaseType {
        "N≡SF3",
        Element::S,
        {
          makeLigand(0, 1, Element::F, BondType::Single),
          makeLigand(0, 1, Element::F, BondType::Single),
          makeLigand(0, 1, Element::F, BondType::Single),
          makeLigand(0, 3, Element::N, BondType::Triple)
        },
        0
      }
    }
  );

  testVsepr( // AX3E1
    Shapes::Shape::VacantTetrahedron,
    {
      TestCaseType {
        "NH3",
        Element::N,
        repeat(
          makeLigand(0, 1, Element::H, BondType::Single),
          3
        ),
        0
      },
      TestCaseType {
        "XeO3",
        Element::Xe,
        repeat(
          makeLigand(0, 2, Element::O, BondType::Double),
          3
        ),
        0
      }
    }
  );

  testVsepr( // AX2E2
    Shapes::Shape::Bent,
    {
      TestCaseType {
        "OH2",
        Element::O,
        repeat(
          makeLigand(0, 1, Element::H, BondType::Single),
          2
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::TrigonalBipyramid,
    {
      TestCaseType {
        "PCl5",
        Element::P,
        repeat(
          makeLigand(0, 1, Element::Cl, BondType::Single),
          5
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::Seesaw,
    {
      TestCaseType {
        "SF4",
        Element::S,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          4
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::T,
    {
      TestCaseType {
        "ClF3",
        Element::Cl,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          3
        ),
        0
      }
    }
  );

  testVsepr( // AX2E3
    Shapes::Shape::Line,
    {
      TestCaseType {
        "I3-",
        Element::I,
        {
          makeLigand(0, 1, Element::I, BondType::Single),
          makeLigand(1, 0, Element::I, BondType::Single)
        },
        -1
      },
      TestCaseType {
        "XeF2",
        Element::Xe,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          2
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::Octahedron,
    {
      TestCaseType {
        "SF6",
        Element::S,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          6
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::SquarePyramid,
    {
      TestCaseType {
        "BrF5",
        Element::Br,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          5
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::Square,
    {
      TestCaseType {
        "XeF4",
        Element::Xe,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          4
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::PentagonalBipyramid,
    {
      TestCaseType {
        "IF7",
        Element::I,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          7
        ),
        0
      }
    }
  );

  testVsepr(
    Shapes::Shape::PentagonalPyramid,
    {
      TestCaseType {
        "XeOF5^-",
        Element::Xe,
        merge(
          repeat(
            makeLigand(0, 1, Element::F, BondType::Single),
            5
          ),
          repeat(
            makeLigand(0, 2, Element::O, BondType::Double),
            1
          )
        ),
        -1
      }
    }
  );

  testVsepr(
    Shapes::Shape::Pentagon,
    {
      TestCaseType {
        "XeF5-",
        Element::Xe,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          5
        ),
        -1
      }
    }
  );

  testVsepr(
    Shapes::Shape::SquareAntiprism,
    {
      TestCaseType {
        "XeF8^2-",
        Element::Xe,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          8
        ),
        0 // charge is not located on Xe!
      }, TestCaseType {
        "IF8^-",
        Element::I,
        repeat(
          makeLigand(0, 1, Element::F, BondType::Single),
          8
        ),
        -1
      }
    }
  );

  // dative bonding
  testVsepr(
    Shapes::Shape::Tetrahedron,
    {
      TestCaseType {
        "H3N -> BF3, on N",
        Element::N,
        {
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 0, Element::B, BondType::Single)
        },
        +1
      },
      TestCaseType {
        "H3N -> BF3, on B",
        Element::B,
        {
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(0, 1, Element::H, BondType::Single),
          makeLigand(1, 0, Element::N, BondType::Single)
        },
        -1
      }
    }
  );
}