text: #pragma once
  
  /*
   * Copyright (C) 2019-2021 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 <vector>
  #include <tuple>
  #include <utility>
  #include <lanelet2_core/primitives/LineString.h>
  #include <lanelet2_core/geometry/Point.h>
  #include <unordered_map>
  #include <math.h>
  
  namespace carma_wm
  {
  /*!
   * \brief O(1) distance lookup structure for quickly accessing route distance information. 
   *        NOTE: This structure is used internally in the world model and is not intended for use by WorldModel users.
   *
   * This class is meant to be used when a route update occurs to precompute distances along the route to support rapid
   * queries later on Insertion is O(n) where n is the number of points in the linestring This structure does not support
   * element reinsertion Linestrings added to this structure should be fully unique (NO DUPLICATE IDs for linestring or
   * point objects)
   *
   *
   * NOTE: Pre-computing route distances make queries much faster but could slow down route loading for large routes and
   * cause them to use more memory.
   */
  class IndexedDistanceMap
  {
  private:
    // Distance storage structure
    // A vector of tuples where the first vector is indexed by line segment and the second vector is index by point index
    // on that line segment. The double value in the tuple stores the total along-line distance to the start of that line
    // segment from the first point on the first line segment The double value inside the interior vector store the
    // along-line distance of the point from the start of its line segment
    std::vector<std::tuple<std::vector<double>, double>> accum_lengths;
  
    // Id mapping structure
    // Sores the linestring and point index's as values with their lanelet Ids as the key
    std::unordered_map<lanelet::Id, std::pair<size_t, size_t>> id_index_map;
  
  public:
    /*!
     * \brief Add a linestring to this structure. This function will iterate over the line string to compute distances
     * between each segment.
     *
     * \throws std::invalid_argument If the provided linstring was already added to this structure
     *
     * \param ls The linestring to add
     */
    void pushBack(const lanelet::LineString2d& ls);
  
    /*!
     * \brief Get the length of the linestring located at the provided index
     *
     * NOTE: No bounds checking is performed
     *
     * \param index The index of the linestring
     *
     * \return The length of the linestring
     */
    double elementLength(size_t index) const;
  
    /*!
     * \brief Get the distance to the start of the linestring at the specified index
     *
     * NOTE: No bounds checking is performed
     *
     * \param index The index of the linestring
     *
     * \return The along-line distance to the start of the linestring at index
     */
    double distanceToElement(size_t index) const;
  
    /*!
     * \brief Get the distance between two points on the same linestring
     *
     * NOTE: No bounds checking is performed
     *
     * \param index The index of the linestring
     * \param p1_index The index of the first point on the linestring
     * \param p2_index The index of the second point
     *
     * \return The along-line distance between the two provided points on the same linestring
     */
    double distanceBetween(size_t index, size_t p1_index, size_t p2_index) const;
  
    /*!
     * \brief Get the along-line distance to the point on the provided linestring
     *
     * NOTE: No bounds checking is performed
     *
     * \param index The linestring index
     * \param point_index The point index in the linestring at index
     *
     * \return The along-line distance to the point from the start of the requested linestring
     */
    double distanceToPointAlongElement(size_t index, size_t point_index) const;
  
    /*!
     * \brief Returns the total along-line length of this structure
     *
     * \return The length
     */
    double totalLength() const;
  
    /*!
     * \brief Returns the indexes of the element identified by the provided Id
     *        NOTE: It is up to the user to know if the id is for a linestring or a point. By default index values will be
     * 0
     *
     * \throws std::out_of_range if the container does not have an element with the specified id
     *
     * \return An std::pair of the matched index where the first element is the linestring index and the second is the
     * point index in that linestring
     */
    std::pair<size_t, size_t> getIndexFromId(const lanelet::Id& id) const;
  
    /*!
     * \brief Returns number of linestrings in this structure
     *
     * \return The element count
     */
    size_t size() const;
  
    /*!
     * \brief Returns the size of the linestring at the specified index
     *
     * \return The linestring point count
     */
    size_t size(size_t index) const;
  
    /*!
     * \brief Returns index of the linestring which the provided distance is within.
     *        NOTE: Unlike the rest of this class, this method runs in O(log n) where n is this.size()
     *
     * \throw std::invalid_argument if distance does not fit within bounds [0, totalLength()]
     * \return The linestring index which this distance is inside
     */
    size_t getElementIndexByDistance(double distance) const;
  };
  }  // namespace carma_wm
  