<commit_before>#ifndef DESCARTES_LADDER_GRAPH_H
#define DESCARTES_LADDER_GRAPH_H

#include "descartes_core/trajectory_id.h"
#include "descartes_core/trajectory_timing_constraint.h"

namespace descartes_planner
{

struct Rung
{
  descartes_core::TrajectoryID id; // corresponds to user's input ID
  descartes_core::TimingConstraint timing; // user input timing
  std::vector<double> data; // joint values stored in one contiguous array
};

struct __attribute__ ((__packed__)) Edge
{
  double cost; // transition cost from vertex who owns this object to 'idx' in next rung
  unsigned idx; // from THIS rung to 'idx' into the NEXT rung
};

/**
 * @brief LadderGraph is an adjacency list based, directed graph structure with vertices
 *        arranged into "rungs" which have connections only to vertices in the adjacent
 *        rungs. Assumes a fixed DOF.
 */
class LadderGraph
{
public:
  using size_type = std::size_t;
  using EdgeList = std::vector<Edge>;

  /**
   * @brief LadderGraph
   * @param dof The number of joints that constitute a single 'DOF'
   */
  explicit LadderGraph(size_type dof) noexcept
    : dof_(dof)
  {
    assert(dof != 0);
  }

  /**
   * @brief resize Resizes the internal ladder to have 'n_rung' rungs
   * @param n_rungs Number of individual rungs
   */
  void resize(size_type n_rungs)
  {
    rungs_.resize(n_rungs);
    edges_.resize(n_rungs);
  }

  Rung& getRung(size_type index) noexcept
  {
    return const_cast<Rung&>(static_cast<const LadderGraph&>(*this).getRung(index));
  }

  const Rung& getRung(size_type index) const noexcept
  {
    assert(index < rungs_.size());
    return rungs_[index];
  }

  std::vector<EdgeList>& getEdges(size_type index) noexcept // see p.23 Effective C++ (<NAME>)
  {
    return const_cast<std::vector<EdgeList>&>(static_cast<const LadderGraph&>(*this).getEdges(index));
  }

  const std::vector<EdgeList>& getEdges(size_type index) const noexcept
  {
    return edges_[index];
  }

  size_type rungSize(size_type index) const noexcept
  {
    return getRung(index).data.size() / dof_;
  }

  /**
   * @brief numVertices Counts the total number of vertices in the graph
   */
  size_type numVertices() const noexcept
  {
    size_type count = 0; // Add the size of each rung d
    for (const auto& rung : rungs_) count += (rung.data.size() / dof_);
    return count;
  }

  /**
   * @brief indexOf returns a pair describing whether the given ID is in the graph and if so, what
   *        index it has.
   * @param id The ID to
   * @return std::pair(index, was_found)
   */
  std::pair<size_type, bool> indexOf(descartes_core::TrajectoryID id) const noexcept
  {
    auto it = std::find_if(rungs_.cbegin(), rungs_.cend(), [id] (const Rung& r) {
      return id == r.id;
    });
    if (it == rungs_.cend())<commit_msg>Added assert check to getEdges()<commit_after>#ifndef DESCARTES_LADDER_GRAPH_H
#define DESCARTES_LADDER_GRAPH_H

#include "descartes_core/trajectory_id.h"
#include "descartes_core/trajectory_timing_constraint.h"

namespace descartes_planner
{

struct Rung
{
  descartes_core::TrajectoryID id; // corresponds to user's input ID
  descartes_core::TimingConstraint timing; // user input timing
  std::vector<double> data; // joint values stored in one contiguous array
};

struct __attribute__ ((__packed__)) Edge
{
  double cost; // transition cost from vertex who owns this object to 'idx' in next rung
  unsigned idx; // from THIS rung to 'idx' into the NEXT rung
};

/**
 * @brief LadderGraph is an adjacency list based, directed graph structure with vertices
 *        arranged into "rungs" which have connections only to vertices in the adjacent
 *        rungs. Assumes a fixed DOF.
 */
class LadderGraph
{
public:
  using size_type = std::size_t;
  using EdgeList = std::vector<Edge>;

  /**
   * @brief LadderGraph
   * @param dof The number of joints that constitute a single 'DOF'
   */
  explicit LadderGraph(size_type dof) noexcept
    : dof_(dof)
  {
    assert(dof != 0);
  }

  /**
   * @brief resize Resizes the internal ladder to have 'n_rung' rungs
   * @param n_rungs Number of individual rungs
   */
  void resize(size_type n_rungs)
  {
    rungs_.resize(n_rungs);
    edges_.resize(n_rungs);
  }

  Rung& getRung(size_type index) noexcept
  {
    return const_cast<Rung&>(static_cast<const LadderGraph&>(*this).getRung(index));
  }

  const Rung& getRung(size_type index) const noexcept
  {
    assert(index < rungs_.size());
    return rungs_[index];
  }

  std::vector<EdgeList>& getEdges(size_type index) noexcept // see p.23 Effective C++ (Scott Meyers)
  {
    return const_cast<std::vector<EdgeList>&>(static_cast<const LadderGraph&>(*this).getEdges(index));
  }

  const std::vector<EdgeList>& getEdges(size_type index) const noexcept
  {
    assert(index < rungs_.size());
    return edges_[index];
  }

  size_type rungSize(size_type index) const noexcept
  {
    return getRung(index).data.size() / dof_;
  }

  /**
   * @brief numVertices Counts the total number of vertices in the graph
   */
  size_type numVertices() const noexcept
  {
    size_type count = 0; // Add the size of each rung d
    for (const auto& rung : rungs_) count += (rung.data.size() / dof_);
    return count;
  }

  /**
   * @brief indexOf returns a pair describing whether the given ID is in the graph and if so, what
   *        index it has.
   * @param id The ID to
   * @return std::pair(index, was_found)
   */
  std::pair<size_type, bool> indexOf(descartes_core::TrajectoryID id) const noexcept
  {
    auto it = std::find_if(rungs_.cbegin(), rungs_.cend(), [id] (const Rung& r) {
      return id == r.id;
    });
    if (it == rungs_.cend())
{'fraction_non_alphanumeric': 0.1120353982300885, 'fraction_numerical': 0.001415929203539823, 'mean_word_length': 3.609298531810767, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 24, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2650074', 'n_tokens_mistral': 1957, 'n_tokens_neox': 1910, 'n_words': 548}