text: // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
  // de Barcelona (UAB).
  //
  // This work is licensed under the terms of the MIT license.
  // For a copy, see <https://opensource.org/licenses/MIT>.
  
  #include "carla/Exception.h"
  #include "carla/ListView.h"
  #include "carla/Logging.h"
  #include "carla/geom/CubicPolynomial.h"
  #include "carla/geom/Location.h"
  #include "carla/geom/Math.h"
  #include "carla/road/Lane.h"
  #include "carla/road/MapData.h"
  #include "carla/road/Road.h"
  #include "carla/road/element/RoadInfoElevation.h"
  #include "carla/road/element/RoadInfoGeometry.h"
  #include "carla/road/element/RoadInfoLaneOffset.h"
  #include "carla/road/element/RoadInfoLaneWidth.h"
  
  #include <stdexcept>
  
  namespace carla {
  namespace road {
  
    const MapData *Road::GetMap() const {
      return _map_data;
    }
  
    RoadId Road::GetId() const {
      return _id;
    }
  
    std::string Road::GetName() const {
      return _name;
    }
  
    double Road::GetLength() const {
      return _length;
    }
  
    bool Road::IsJunction() const {
      return _is_junction;
    }
  
    JuncId Road::GetJunctionId() const {
      return _junction_id;
    }
  
    RoadId Road::GetSuccessor() const {
      return _successor;
    }
  
    RoadId Road::GetPredecessor() const {
      return _predecessor;
    }
  
    std::vector<Road *> Road::GetNexts() const {
      return _nexts;
    }
  
    std::vector<Road *> Road::GetPrevs() const {
      return _prevs;
    }
  
    const geom::CubicPolynomial &Road::GetElevationOn(const double s) const {
      auto info = GetInfo<element::RoadInfoElevation>(s);
      if (info == nullptr) {
        throw_exception(std::runtime_error("failed to find road elevation."));
      }
      return info->GetPolynomial();
    }
  
    Lane &Road::GetLaneByDistance(double s, LaneId lane_id) {
      for (auto &section : GetLaneSectionsAt(s)) {
        auto *lane = section.GetLane(lane_id);
        if (lane != nullptr) {
          return *lane;
        }
      }
      throw_exception(std::runtime_error("lane not found"));
    }
  
    const Lane &Road::GetLaneByDistance(double s, LaneId lane_id) const {
      return const_cast<Road *>(this)->GetLaneByDistance(s, lane_id);
    }
  
    Lane &Road::GetLaneById(SectionId section_id, LaneId lane_id) {
      return GetLaneSectionById(section_id).GetLanes().at(lane_id);
    }
  
    const Lane &Road::GetLaneById(SectionId section_id, LaneId lane_id) const {
      return const_cast<Road *>(this)->GetLaneById(section_id, lane_id);
    }
  
    // get the lane on a section next to 's'
    Lane *Road::GetNextLane(const double s, const LaneId lane_id) {
  
      auto upper = _lane_sections.upper_bound(s);
  
      while (upper != _lane_sections.end()) {
        // check id
        Lane *ptr = upper->second.GetLane(lane_id);
        if (ptr != nullptr) {
          return ptr;
        }
        ++upper;
      }
  
      return nullptr;
    }
  
    // get the lane on a section previous to 's'
    Lane *Road::GetPrevLane(const double s, const LaneId lane_id) {
  
      auto lower = _lane_sections.lower_bound(s);
      auto rlower = std::make_reverse_iterator(lower);
  
      while (rlower != _lane_sections.rend()) {
        // check id
        Lane *ptr = rlower->second.GetLane(lane_id);
        if (ptr != nullptr) {
          return ptr;
        }
        ++rlower;
      }
  
      return nullptr;
    }
  
    // get the start and end section with a lan id
    LaneSection *Road::GetStartSection(LaneId id) {
      auto it = _lane_sections.begin();
      while (it != _lane_sections.end()) {
        // check id
        Lane *ptr = it->second.GetLane(id);
        if (ptr != nullptr) {
          return &(it->second);
        }
        ++it;
      }
      return nullptr;
    }
  
    LaneSection *Road::GetEndSection(LaneId id) {
      auto it = _lane_sections.rbegin();
      while (it != _lane_sections.rend()) {
        // check id
        Lane *ptr = it->second.GetLane(id);
        if (ptr != nullptr) {
          return &(it->second);
        }
        ++it;
      }
      return nullptr;
    }
  
    carla::road::signal::Signal *Road::GetSignal(const SignId id) {
      auto search = _signals.find(id);
      if (search != _signals.end()) {
        return &search->second;
      }
      return nullptr;
    }
  
    carla::road::signal::SignalReference *Road::GetSignalRef(const SignRefId id) {
      const auto search = _sign_ref.find(id);
      if (search != _sign_ref.end()) {
        return &search->second;
      }
      return nullptr;
    }
  
    std::unordered_map<SignId, signal::Signal> *Road::getSignals() {
      return &_signals;
    }
  
    std::unordered_map<SignId, signal::SignalReference> *Road::getSignalReferences() {
      return &_sign_ref;
    }
  
    element::DirectedPoint Road::GetDirectedPointIn(const double s) const {
      const auto clamped_s = geom::Math::clamp<double>(s, 0.0, _length);
      const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
  
      const auto lane_offset = _info.GetInfo<element::RoadInfoLaneOffset>(clamped_s);
      const auto offset = lane_offset->GetPolynomial().Evaluate(clamped_s);
  
      // Apply road's lane offset record
      element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
      // Unreal's Y axis hack (the minus on the offset)
      p.ApplyLateralOffset(-offset);
  
      // Apply road's elevation record
      const auto elevation_info = GetElevationOn(s);
      p.location.z = elevation_info.Evaluate(s);
      p.pitch = elevation_info.Tangent(s);
  
      return p;
    }
  
    const std::pair<double, double> Road::GetNearestPoint(const geom::Location &loc) const {
      std::pair<double, double> last = { 0.0, std::numeric_limits<double>::max() };
  
      auto geom_info_list = _info.GetInfos<element::RoadInfoGeometry>();
      decltype(geom_info_list)::iterator nearest_geom = geom_info_list.end();
  
      for (auto g = geom_info_list.begin(); g != geom_info_list.end(); ++g) {
        DEBUG_ASSERT(*g != nullptr);
        auto dist = (*g)->GetGeometry().DistanceTo(loc);
        if (dist.second < last.second) {
          last = dist;
          nearest_geom = g;
        }
      }
  
      for (auto g = geom_info_list.begin();
          g != geom_info_list.end() && g != nearest_geom;
          ++g) {
        DEBUG_ASSERT(*g != nullptr);
        last.first += (*g)->GetGeometry().GetLength();
      }
  
      return last;
    }
  
    const std::pair<const Lane *, double> Road::GetNearestLane(
        const double s,
        const geom::Location &loc,
        uint32_t lane_type) const {
      using namespace carla::road::element;
      std::map<LaneId, const Lane *> lanes(GetLanesAt(s));
      // negative right lanes
      auto right_lanes = MakeListView(
          std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
      // positive left lanes
      auto left_lanes = MakeListView(
          lanes.lower_bound(1), lanes.end());
  
      const DirectedPoint dp_lane_zero = GetDirectedPointIn(s);
      std::pair<const Lane *, double> result =
          std::make_pair(nullptr, std::numeric_limits<double>::max());
  
      DirectedPoint current_dp = dp_lane_zero;
      for (const auto &lane : right_lanes) {
        const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
        const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
  
        current_dp.ApplyLateralOffset(half_width);
        const auto current_dist = geom::Math::Distance(current_dp.location, loc);
  
        // if the current_dp is near to loc, we are in the right way
        if (current_dist <= result.second) {
          // only consider the lanes that match the type flag for result
          // candidates
          if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
            result.first = &(*lane.second);
            result.second = current_dist;
          }
        } else {
          // elsewhere, we are be moving away
          break;
        }
        current_dp.ApplyLateralOffset(half_width);
      }
  
      current_dp = dp_lane_zero;
      for (const auto &lane : left_lanes) {
        const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
        const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
  
        current_dp.ApplyLateralOffset(half_width);
        const auto current_dist = geom::Math::Distance(current_dp.location, loc);
  
        // if the current_dp is near to loc, we are in the right way
        if (current_dist <= result.second) {
          // only consider the lanes that match the type flag for result
          // candidates
          if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
            result.first = &(*lane.second);
            result.second = current_dist;
          }
        } else {
          // elsewhere, we are be moving away
          break;
        }
        current_dp.ApplyLateralOffset(half_width);
      }
  
      return result;
    }
  
    std::map<LaneId, const Lane *> Road::GetLanesAt(const double s) const {
      std::map<LaneId, const Lane *> map;
      for (auto &&lane_section : GetLaneSectionsAt(s)) {
        for (auto &&lane : lane_section.GetLanes()) {
          map[lane.first] = &(lane.second);
        }
      }
      return map;
    }
  
  } // road
  } // carla
  