//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_BITSET_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_BITSET_HPP_INCLUDED

#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/simd/shift_left.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( bitset_
                         , (typename A0, typename A1)
                         , bd::cpu_
                         , bd::generic_< bd::arithmetic_<A0> >
                         , bd::generic_< bd::integer_<A1> >
                         )
 {
   BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const& a1
                                   , typename std::enable_if<bs::cardinal_of<A1>::value
                                     == bs::cardinal_of<A0>::value>::type* = 0) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0, unsigned>;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(dec(a1)), "bitset :index is out of range");
      return bitwise_or(a0, shift_left(One<i_t>(), dec(a1)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( bitset_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0, unsigned>;
      BOOST_ASSERT_MSG(assert_good_shift<A0>(dec(a1)), "bitset :index is out of range");
      return bitwise_or(a0, shift_left(One<i_t>(), dec(a1)));
    }
  };
} } }

#endif


#include <string>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <cstdio>

#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/dinast_grabber.h>

#define FPS_CALC(_WHAT_) \
do \
{ \
    static unsigned count = 0;\
    static double last = pcl::getTime ();\
    double now = pcl::getTime (); \
    ++count; \
    if (now - last >= 1.0) \
    { \
      std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
      count = 0; \
      last = now; \
    } \
}while(false)

void
savePGM (const unsigned char *image, const std::string& filename)
{
  FILE *file = fopen (filename.c_str (), "w");
  if (!file)
  {
    std::cerr << "Unable to open file '" << filename << "' for writing." << std::endl;
    return;
  }

  // PGM header
  fprintf (file, "P2\n%d %d\n255\n", IMAGE_WIDTH, IMAGE_HEIGHT);

  // Write data as ASCII
  for (int i = 0; i < IMAGE_HEIGHT; ++i)
  {
    for (int j = 0; j < IMAGE_WIDTH; ++j)
    {
      fprintf (file, "%3d ", (int)*image++);
    }
    fprintf (file, "\n");
  }

  fclose (file);
}

void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void* data)
{
  static int NUM_IMAGES = 0;
  if (event.getKeySym () == "s" && event.keyDown ())
  {
    char filename[16];
    snprintf (filename, sizeof(filename), "image%.2d.pgm", NUM_IMAGES++);
    unsigned char *image = reinterpret_cast<unsigned char*> (data);
    savePGM (image, filename);
    printf ("Wrote %s\n", filename);
  }
}

void
convertImageToCloud (const unsigned char *image, pcl::PointCloud<pcl::PointXYZI> &cloud)
{
  cloud.points.resize (IMAGE_WIDTH * IMAGE_HEIGHT);
  cloud.width = IMAGE_WIDTH;
  cloud.height = IMAGE_HEIGHT;
  cloud.is_dense = false;

  int depth_idx = 0;
  int pxl[9];

  for (int x = 0; x < cloud.width; ++x)
  {
    for (int y = 0; y < cloud.height; ++y, ++depth_idx)
    {
      float xc = (float)(x - 160);
      float yc = (float)(y - 120);
      double r1 = sqrt (xc * xc + yc * yc);
      double r2 = r1 * r1;
      double r3 = r1 * r2;
      double A = -2e-5 * r3 + 0.004 * r2 + 0.1719 * r1 + 350.03;
      double B = -2e-9 * r3 + 3e-7 * r2 - 1e-5 * r1 - 0.01;

      // Low pass filtering
      /// @todo Try a bilateral filter to avoid blurring over depth boundaries
      int measure = 0;
      if ((y > 0) && (y < (IMAGE_HEIGHT - 1)) && (x > 0) && (x < (IMAGE_WIDTH - 1)))
      {
        int ipx = x + IMAGE_WIDTH * y;
#if 1
        pxl[0] = image[ipx];
        pxl[1] = image[ipx-1];
        pxl[2] = image[ipx+1];
        pxl[3] = image[ipx - IMAGE_WIDTH];
        pxl[4] = image[ipx - IMAGE_WIDTH - 1];
        pxl[5] = image[ipx - IMAGE_WIDTH + 1];
        pxl[6] = image[ipx + IMAGE_WIDTH];
        pxl[7] = image[ipx + IMAGE_WIDTH - 1];
        pxl[8] = image[ipx + IMAGE_WIDTH + 1];

        for (int ii = 0; ii < 9; ii++)
          measure += pxl[ii];
        measure /= 9;
#else
        // No blurring
        measure = image[ipx];
#endif
      }
      if (measure > 255)
        measure = 255;  // saturation for display

      unsigned char pixel = measure;//image[depth_idx];
      if (pixel < 1)
      {
        cloud.points[depth_idx].x = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].z = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].intensity = pixel;
        continue;
      }

      if (pixel > A)
        pixel = A;

      float dy = y*0.1;
      double dist = (log((double)pixel/A)/B-dy)*(7E-07*r3 - 0.0001*r2 + 0.004*r1 + 0.9985)*1.5;
      double dist_2d = r1;

      static const double dist_max_2d = 1 / 160.0; /// @todo Why not 200?
      //static const double dist_max_2d = 1 / 200.0;
      static const double FOV = 64.0 * M_PI / 180.0; // diagonal FOV?

      double theta_colati = FOV * r1 * dist_max_2d;
      double c_theta = cos (theta_colati);
      double s_theta = sin (theta_colati);
      double c_ksai = ((double)(x - 160.)) / r1;
      double s_ksai = ((double)(y - 120.)) / r1;

      cloud.points[depth_idx].x = (dist * s_theta * c_ksai) / 500.0 + 0.5; //cartesian x
      cloud.points[depth_idx].y = (dist * s_theta * s_ksai) / 500.0 + 0.5; //cartesian y
      cloud.points[depth_idx].z = (dist * c_theta);                        //cartesian z
      /// @todo This looks weird, can it cause artifacts?
      if (cloud.points[depth_idx].z < 0.01)
#if 1
        cloud.points[depth_idx].z = 0.01;
#else
        cloud.points[depth_idx].x = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].y = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].z = std::numeric_limits<float>::quiet_NaN ();
        cloud.points[depth_idx].intensity = pixel;
        continue;
#endif

      cloud.points[depth_idx].z /= 500.0;
      cloud.points[depth_idx].intensity = pixel;
    }
  }
}

/* --[ */

int
main (int argc, char** argv)
{

  pcl::DinastGrabber grabber;

  grabber.findDevice (1);

  grabber.openDevice();

  std::cerr << "Device version/revision number: " << grabber.getDeviceVersion () << std::endl;

  grabber.start ();

  pcl::visualization::ImageViewer vis_img ("Dinast Image Viewer");
  pcl::visualization::PCLVisualizer vis_cld (argc, argv, "Dinast Cloud Viewer");

  unsigned char *img1 = (unsigned char*)malloc (IMAGE_SIZE);
  unsigned char *img2 = (unsigned char*)malloc (IMAGE_SIZE);

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);

  while (true)
  {
    if (grabber.readImage ( img1, img2) == 0)
      continue;

     convertImageToCloud (img1, *cloud);

    FPS_CALC ("grabber + visualization");
    vis_img.showMonoImage (img1, IMAGE_WIDTH, IMAGE_HEIGHT);

    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> handler (cloud, "intensity");
    if (!vis_cld.updatePointCloud (cloud, handler, "DinastCloud"))
    {
      vis_cld.addPointCloud (cloud, handler, "DinastCloud");
      vis_cld.resetCameraViewpoint ("DinastCloud");
    }

    vis_img.spinOnce ();
    vis_cld.spinOnce ();

  }

  grabber.stop ();
  grabber.closeDevice ();

}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2019-2020 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "opendrive/geometry/GeometryGenerator.hpp"

#include "opendrive/geometry/CenterLine.hpp"
#include "opendrive/geometry/LaneUtils.hpp"

#include <algorithm>
#include <boost/math/tools/rational.hpp>
#include <iostream>

#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include <proj_api.h>

namespace opendrive {
namespace geometry {

double laneWidth(std::vector<LaneWidth> const &laneWidthVector, double s)
{
  if (s < 0.)
  {
    return laneWidth(laneWidthVector, 0.);
  }

  for (auto it = laneWidthVector.rbegin(); it != laneWidthVector.rend(); it++)
  {
    if (s >= it->soffset)
    {
      auto poly = boost::array<double, 4>{{it->a, it->b, it->c, it->d}};
      return boost::math::tools::evaluate_polynomial(poly, s - it->soffset);
    }
  }
  return 0.0;
}

bool isInvalidLaneSection(::opendrive::LaneSection const &laneSection)
{
  auto length = laneSection.end_position - laneSection.start_position;
  if (length < MinimumSegmentLength)
  {
    std::cerr << "Invalid lane section of length " << length << "\n";
    return true;
  }
  return false;
}

void calculateLaneSectionBounds(std::vector<::opendrive::LaneSection> &laneSections, double totalLength)
{
  for (auto it = laneSections.begin(); it != laneSections.end(); it++)
  {
    if (it + 1 == laneSections.end())
    {
      it->end_position = totalLength;
    }
    else
    {
      it->end_position = (it + 1)->start_position;
    }
  }
}

bool fixInvalidLaneSections(std::vector<::opendrive::LaneSection> &laneSections)
{
  for (auto it = laneSections.begin(); it != laneSections.end(); it++)
  {
    // for the moment do nothing
    if (isInvalidLaneSection(*it))
    {
      return false;
    }
  }
  return true;
}

void sortLanesByIndex(std::vector<::opendrive::LaneSection> &laneSections)
{
  auto compare = [](::opendrive::LaneInfo const &infoA, ::opendrive::LaneInfo const &infoB) {
    return abs(infoA.attributes.id) < abs(infoB.attributes.id);
  };

  for (auto it = laneSections.begin(); it != laneSections.end(); it++)
  {
    std::sort(it->left.begin(), it->left.end(), compare);
    std::sort(it->right.begin(), it->right.end(), compare);
  }
}

void generateTrafficSignal(TrafficSignalInformation &trafficSignalInfo,
                           CenterLine const &centerLine,
                           ::opendrive::LandmarkMap &landmarks)
{
  Landmark landmark;
  auto directedPoint = centerLine.eval(trafficSignalInfo.start_position, false);
  directedPoint.ApplyLateralOffset(trafficSignalInfo.track_position);

  landmark.position = directedPoint.location;
  landmark.orientation = directedPoint.tangent + M_PI;

  if (trafficSignalInfo.orientation == "-")
  {
    landmark.orientation += M_PI;
  }

  landmark.type = stoi(trafficSignalInfo.type);
  landmark.id = trafficSignalInfo.id;
  landmark.subtype = stoi(trafficSignalInfo.type);

  if (landmark.id == -1)
  {
    landmark.id = landmarks.size() + 1;
    trafficSignalInfo.id = landmark.id;
  }
  landmarks[landmark.id] = landmark;
}

std::vector<LaneSection>::iterator getLaneSection(double s, RoadInformation &roadInfo)
{
  for (auto it = roadInfo.lanes.lane_sections.end() - 1; it >= roadInfo.lanes.lane_sections.begin(); --it)
  {
    if (s >= it->start_position)
    {
      return it;
    }
  }
  return roadInfo.lanes.lane_sections.end();
}

void addTrafficReferenceToLanes(TrafficSignalReference const &trafficSignalReference,
                                RoadInformation &roadInfo,
                                LaneMap &laneMap)
{
  auto laneSectionIt = getLaneSection(trafficSignalReference.start_position, roadInfo);

  if (laneSectionIt == roadInfo.lanes.lane_sections.end())
  {
    std::cerr << "addTrafficReferenceToLanes() traffic reference outside road\n";
    return;
  }

  auto laneSectionIndex = static_cast<uint32_t>(laneSectionIt - roadInfo.lanes.lane_sections.begin()) + 1;
  double s = parametricPosition(trafficSignalReference.start_position, *laneSectionIt);

  if (trafficSignalReference.orientation == "+")
  {
    for (auto &laneInfo : laneSectionIt->right)
    {
      SignalReference signalReference;
      signalReference.id = trafficSignalReference.id;
      signalReference.parametricPosition = s;
      signalReference.inLaneOrientation = true;
      auto id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
      laneMap[id].signalReferences.emplace_back(signalReference);
    }
  }
  else
  {
    for (auto &laneInfo : laneSectionIt->left)
    {
      SignalReference signalReference;
      signalReference.id = trafficSignalReference.id;
      signalReference.parametricPosition = s;
      signalReference.inLaneOrientation = false;
      auto id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
      laneMap[id].signalReferences.emplace_back(signalReference);
    }
  }
}

void addTrafficSignals(RoadInformation &roadInfo, CenterLine const &centerLine, opendrive::OpenDriveData &mapData)
{
  for (auto &trafficSignalInfo : roadInfo.traffic_signals)
  {
    generateTrafficSignal(trafficSignalInfo, centerLine, mapData.landmarks);

    // Create a signal reference to the given traffic signal
    TrafficSignalReference trafficSignalReference;
    trafficSignalReference.id = trafficSignalInfo.id;
    trafficSignalReference.orientation = trafficSignalInfo.orientation;
    trafficSignalReference.start_position = trafficSignalInfo.start_position;

    addTrafficReferenceToLanes(trafficSignalReference, roadInfo, mapData.laneMap);
  }

  for (auto &trafficSignalReference : roadInfo.traffic_signal_references)
  {
    addTrafficReferenceToLanes(trafficSignalReference, roadInfo, mapData.laneMap);
  }
}

bool addLane(OpenDriveData &mapData, RoadInformation const &roadInfo, LaneInfo const &laneInfo, Id laneId)
{
  if (mapData.laneMap.count(laneId) > 0)
  {
    std::cerr << "Duplicated lane Id " << laneId << "\n";
    return false;
  }

  mapData.laneMap[laneId].leftEdge.clear();
  mapData.laneMap[laneId].rightEdge.clear();
  mapData.laneMap[laneId].index = laneInfo.attributes.id;
  mapData.laneMap[laneId].id = laneId;
  mapData.laneMap[laneId].type = laneInfo.attributes.type;
  mapData.laneMap[laneId].junction = roadInfo.attributes.junction;

  if (roadInfo.attributes.junction != -1)
  {
    mapData.intersectionLaneIds[roadInfo.attributes.junction].push_back(laneId);
  }

  return true;
}

bool initializeLaneMap(opendrive::OpenDriveData &mapData)
{
  bool ok = true;
  for (auto const &roadInfo : mapData.roads)
  {
    auto const &roadId = roadInfo.attributes.id;
    auto const &laneSections = roadInfo.lanes.lane_sections;

    for (std::size_t k = 0; k != laneSections.size(); k++)
    {
      for (auto const &laneInfo : laneSections[k].left)
      {
        auto id = laneId(roadId, k + 1, laneInfo.attributes.id);
        if (!addLane(mapData, roadInfo, laneInfo, id))
        {
          ok = false;
        }
      }

      for (auto const &laneInfo : laneSections[k].right)
      {
        auto id = laneId(roadId, k + 1, laneInfo.attributes.id);
        if (!addLane(mapData, roadInfo, laneInfo, id))
        {
          ok = false;
        }
      }
    }
  }

  return ok;
}

bool generateRoadGeometry(RoadInformation &roadInfo, opendrive::OpenDriveData &mapData)
{
  bool ok = true;
  auto roadId = roadInfo.attributes.id;
  auto &laneSections = roadInfo.lanes.lane_sections;

  CenterLine centerLine;
  if (!generateCenterLine(roadInfo, centerLine))
  {
    ok = false;
  }

  calculateLaneSectionBounds(laneSections, centerLine.length);
  if (!fixInvalidLaneSections(laneSections)) // currently just checks consistency
  {
    ok = false;
  }
  sortLanesByIndex(laneSections);

  addTrafficSignals(roadInfo, centerLine, mapData);

  auto samplingPoints = centerLine.samplingPoints();

  for (auto it = laneSections.begin(); it != laneSections.end(); it++)
  {
    auto &laneSection = *it;
    auto laneSectionIndex = static_cast<uint32_t>(it - laneSections.begin()) + 1;

    // the tolerance at the end is necessary so that the lane offset applies only to the current lane section
    double s0 = laneSection.start_position;
    double s1 = (it + 1) == laneSections.end() ? centerLine.length : (it + 1)->start_position - 1e-12;

    if (s1 < s0)
    {
      std::cerr << "Invalid lane section length s1, s0\n";
      ok = false;
    }

    auto inRange = [&s0, &s1](double const &s) { return (s < s1) && (s > s0); };

    std::vector<double> localSamplingPoints;
    localSamplingPoints.push_back(s0);
    std::copy_if(samplingPoints.begin(), samplingPoints.end(), std::back_inserter(localSamplingPoints), inRange);
    localSamplingPoints.push_back(s1);

    for (auto s : localSamplingPoints)
    {
      auto centerPoint = centerLine.eval(s);

      double borderOffset = 0.;
      for (auto &laneInfo : laneSection.left)
      {
        auto width = laneWidth(laneInfo.lane_width, s - s0);
        auto rightPoint = centerPoint;
        auto leftPoint = centerPoint;

        leftPoint.ApplyLateralOffset(borderOffset + width);
        rightPoint.ApplyLateralOffset(borderOffset);

        auto id = laneId(roadId, laneSectionIndex, laneInfo.attributes.id);
        mapData.laneMap[id].leftEdge.push_back(leftPoint.location);
        mapData.laneMap[id].rightEdge.push_back(rightPoint.location);
        borderOffset += width;
      }

      // right lanes
      borderOffset = 0.;
      for (auto &laneInfo : laneSection.right)
      {
        auto width = laneWidth(laneInfo.lane_width, s - s0);
        auto rightPoint = centerPoint;
        auto leftPoint = centerPoint;

        leftPoint.ApplyLateralOffset(borderOffset);
        rightPoint.ApplyLateralOffset(borderOffset - width);

        auto id = laneId(roadId, laneSectionIndex, laneInfo.attributes.id);
        mapData.laneMap[id].leftEdge.push_back(leftPoint.location);
        mapData.laneMap[id].rightEdge.push_back(rightPoint.location);
        borderOffset -= width;
      }
    }
  }

  return ok;
}

double convertToMetersPerSecond(double const value, std::string const &units)
{
  if (units == "m/s")
  {
    return value;
  }
  else if (units == "km/h")
  {
    return value / 3.6;
  }
  else if (units == "mph")
  {
    return value * 0.4470389;
  }
  else
  {
    std::cerr << "Unrecognized speed units\n";
    return 0.0;
  }
}

double speedAt(double s, std::vector<RoadSpeed> speed)
{
  for (auto it = speed.rbegin(); it != speed.rend(); ++it)
  {
    if (s >= it->s)
    {
      return convertToMetersPerSecond(it->max, it->unit);
    }
  }

  std::cerr << "speedAt() Invalid parameter s\n";

  if (speed.size() > 0)
  {
    return convertToMetersPerSecond(speed.front().max, speed.front().unit);
  }

  return 0.;
}

// Creates a parametric speed entries vector using the given road speed as reference
// The range [s0, s1] is mapped to [0., 1.0]
std::vector<ParametricSpeed> parametricSpeed(double s0, double s1, std::vector<RoadSpeed> speed)
{
  double length = s1 - s0;

  if (length <= 0)
  {
    std::cerr << "parametricSpeed() Invalid parameters: s1 <= s0\n";
    return {ParametricSpeed(speedAt(s0, speed))};
  }

  if (fabs(length) < MinimumSegmentLength)
  {
    std::cerr << "parametricSpeed() road segment too short length=" << length << "\n";
    return {ParametricSpeed(speedAt(s0, speed))};
  }

  std::vector<double> speedPoints;
  std::vector<ParametricSpeed> roadParametricSpeed;

  speedPoints.push_back(s0);
  for (auto it = speed.begin(); it != speed.end(); ++it)
  {
    if ((it->s > s0) && (it->s < s1))
    {
      speedPoints.push_back(it->s);
    }
  }
  speedPoints.push_back(s1);

  for (auto it = speedPoints.begin(); it != speedPoints.end(); ++it)
  {
    ::opendrive::ParametricSpeed parametricSpeed;
    parametricSpeed.start = std::max(0., (*it - s0) / length);
    parametricSpeed.speed = speedAt(*it, speed);

    if ((it + 1 == speedPoints.end()) || (*(it + 1) > s1))
    {
      parametricSpeed.end = 1.0;
    }
    else
    {
      parametricSpeed.end = (*(it + 1) - s0) / length;
    }
    roadParametricSpeed.push_back(parametricSpeed);
  }
  return roadParametricSpeed;
}

std::vector<ParametricSpeed> calculateLaneSpeed(LaneInfo const &laneInfo, double laneSectionLength)
{
  if (laneSectionLength < MinimumSegmentLength)
  {
    std::cerr << "calculateLaneSpeed:: lane section length too short\n";
  }

  std::vector<ParametricSpeed> speed;
  for (auto it = laneInfo.lane_speed.begin(); it != laneInfo.lane_speed.end(); ++it)
  {
    ::opendrive::ParametricSpeed parametricSpeed;
    parametricSpeed.start = it->soffset / laneSectionLength;
    parametricSpeed.speed = convertToMetersPerSecond(it->max_speed, it->unit);
    if (it + 1 == laneInfo.lane_speed.end())
    {
      parametricSpeed.end = 1.0;
    }
    else
    {
      parametricSpeed.end = (it + 1)->soffset / laneSectionLength;
    }
    speed.push_back(parametricSpeed);
  }
  return speed;
}

void calculateSpeed(RoadInformation &roadInfo, LaneMap &laneMap)
{
  auto &laneSections = roadInfo.lanes.lane_sections;

  auto &roadSpeed = roadInfo.attributes.speed;

  uint32_t laneSectionIndex = 1;
  for (auto &laneSection : laneSections)
  {
    std::vector<ParametricSpeed> laneSectionSpeed;
    if (roadSpeed.empty())
    {
      // no speed info
      laneSectionSpeed.push_back(ParametricSpeed(convertToMetersPerSecond(50.0, "km/h")));
    }
    else
    {
      laneSectionSpeed = parametricSpeed(laneSection.start_position, laneSection.end_position, roadSpeed);
    }

    for (auto &laneInfo : laneSection.left)
    {
      auto id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
      if (laneInfo.lane_speed.empty())
      {
        laneMap[id].speed = laneSectionSpeed;
      }
      else
      {
        laneMap[id].speed = calculateLaneSpeed(laneInfo, laneSection.end_position - laneSection.start_position);
      }
    }
    for (auto &laneInfo : laneSection.right)
    {
      auto id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
      if (laneInfo.lane_speed.empty())
      {
        laneMap[id].speed = laneSectionSpeed;
      }
      else
      {
        laneMap[id].speed = calculateLaneSpeed(laneInfo, laneSection.end_position - laneSection.start_position);
      }
    }
    laneSectionIndex++;
  }
}

bool checkId(Id id, std::string const &text = "")
{
  if (id < 1)
  {
    std::cerr << "checkId() invalid id " << id << " " << text << "\n";
    return false;
  }
  return true;
}

bool belongsToIntersection(::opendrive::RoadInformation const &roadInfo)
{
  return roadInfo.attributes.junction == -1 ? false : true;
}

void setPredecessor(::opendrive::OpenDriveData &mapData,
                    ::opendrive::RoadInformation const &roadInfo,
                    ::opendrive::LaneInfo const &laneInfo)
{
  auto const &contact = roadInfo.road_link.predecessor->contact_point;
  auto roadIt = findRoad(mapData, roadInfo.road_link.predecessor->id);

  // predecessor road is connected to the first lane section
  uint32_t thisLaneSectionIndex = 1;
  Id thisLaneId = laneId(roadInfo.attributes.id, thisLaneSectionIndex, laneInfo.attributes.id);

  checkId(thisLaneId, "::SetPredecessor");

  if (roadIt != mapData.roads.end())
  {
    auto const &roadId = roadIt->attributes.id;
    Id predecessorId{0u};
    if (contact == ::opendrive::ContactPoint::End)
    {
      predecessorId = laneId(roadId, roadIt->lanes.lane_sections.size(), laneInfo.link->predecessor_id);
      mapData.laneMap[predecessorId].successors.push_back(thisLaneId);
    }
    else if (contact == ::opendrive::ContactPoint::Start)
    {
      predecessorId = laneId(roadId, 1, laneInfo.link->predecessor_id);
      mapData.laneMap[predecessorId].predecessors.push_back(thisLaneId);
    }
    mapData.laneMap[thisLaneId].predecessors.push_back(predecessorId);
  }
}

void setSuccessor(::opendrive::OpenDriveData &mapData,
                  ::opendrive::RoadInformation const &roadInfo,
                  ::opendrive::LaneInfo const &laneInfo)
{
  auto const &contact = roadInfo.road_link.successor->contact_point;
  auto roadIt = findRoad(mapData, roadInfo.road_link.successor->id);

  // successor road is connected to the last lane section
  uint32_t thisLaneSectionIndex = roadInfo.lanes.lane_sections.size();
  Id thisLaneId = laneId(roadInfo.attributes.id, thisLaneSectionIndex, laneInfo.attributes.id);

  checkId(thisLaneId, "::setSuccessor");

  if (roadIt != mapData.roads.end())
  {
    auto const &roadId = roadIt->attributes.id;
    Id successorId{0u};
    if (contact == ::opendrive::ContactPoint::End)
    {
      successorId = laneId(roadId, roadIt->lanes.lane_sections.size(), laneInfo.link->successor_id);
      mapData.laneMap[successorId].successors.push_back(thisLaneId);
    }
    else if (contact == ::opendrive::ContactPoint::Start)
    {
      successorId = laneId(roadId, 1, laneInfo.link->successor_id);
      mapData.laneMap[successorId].predecessors.push_back(thisLaneId);
    }
    mapData.laneMap[thisLaneId].successors.push_back(successorId);
  }
}

bool hasSuccessorRoad(RoadInformation const &roadInfo)
{
  auto &link = roadInfo.road_link;
  return link.successor != nullptr && link.successor->element_type == ::opendrive::ElementType::Road;
}

bool hasPredecessorRoad(RoadInformation const &roadInfo)
{
  auto &link = roadInfo.road_link;
  return link.predecessor != nullptr && link.predecessor->element_type == ::opendrive::ElementType::Road;
}

void setSuccessorPredecessor(::opendrive::OpenDriveData &mapData,
                             ::opendrive::RoadInformation const &roadInfo,
                             ::opendrive::LaneInfo const &laneInfo,
                             uint32_t const laneSectionIndex)
{
  if (laneInfo.link == nullptr)
  {
    return;
  }

  Id currentLaneId = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
  auto &laneSections = roadInfo.lanes.lane_sections;

  // First handle predecessors
  if (laneSectionIndex == 1)
  {
    // set predecessor
    if (laneInfo.link->predecessor_id != 0 && hasPredecessorRoad(roadInfo))
    {
      setPredecessor(mapData, roadInfo, laneInfo);
    }
  }
  else
  {
    if (laneInfo.link->predecessor_id != 0)
    {
      Id predecessorId = laneId(roadInfo.attributes.id, laneSectionIndex - 1, laneInfo.link->predecessor_id);
      mapData.laneMap[currentLaneId].predecessors.push_back(predecessorId);
    }
  }

  // Then handle successors
  if (laneSectionIndex < laneSections.size())
  {
    if (laneInfo.link->successor_id != 0)
    {
      Id successorId = laneId(roadInfo.attributes.id, laneSectionIndex + 1, laneInfo.link->successor_id);
      mapData.laneMap[currentLaneId].successors.push_back(successorId);
    }
  }
  else
  {
    // set successor
    if (laneInfo.link->successor_id != 0 && hasSuccessorRoad(roadInfo))
    {
      setSuccessor(mapData, roadInfo, laneInfo);
    }
  }
}

void setLeftRightNeighbor(::opendrive::OpenDriveData &mapData,
                          ::opendrive::RoadInformation const &roadInfo,
                          uint32_t const laneSectionIndex)
{
  auto &laneSection = roadInfo.lanes.lane_sections[laneSectionIndex - 1];
  auto numLeftLanes = laneSection.left.size();
  auto numRightLanes = laneSection.right.size();

  for (std::size_t k = 0; k < laneSection.left.size(); k++)
  {
    auto &laneInfo = laneSection.left[k];
    Id thisLaneId = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
    Id rightNeighborId{0u};
    Id leftNeighborId{0u};

    // right neighbor
    if (k == 0)
    {
      if (numRightLanes > 0)
      {
        rightNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.right.front().attributes.id);
      }
    }
    else
    {
      rightNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.left[k - 1].attributes.id);
    }

    // left neighbor
    if (k + 1 < numLeftLanes)
    {
      leftNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.left[k + 1].attributes.id);
    }

    mapData.laneMap[thisLaneId].rightNeighbor = rightNeighborId;
    mapData.laneMap[thisLaneId].leftNeighbor = leftNeighborId;
  }
  for (std::size_t k = 0; k < laneSection.right.size(); k++)
  {
    auto &laneInfo = laneSection.right[k];
    Id thisLaneId = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
    Id rightNeighborId{0u};
    Id leftNeighborId{0u};

    // left neighbor
    if (k == 0)
    {
      if (numLeftLanes > 0)
      {
        leftNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.left.front().attributes.id);
      }
    }
    else
    {
      leftNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.right[k - 1].attributes.id);
    }

    // right neighbor
    if (k + 1 < numRightLanes)
    {
      rightNeighborId = laneId(roadInfo.attributes.id, laneSectionIndex, laneSection.right[k + 1].attributes.id);
    }

    mapData.laneMap[thisLaneId].rightNeighbor = rightNeighborId;
    mapData.laneMap[thisLaneId].leftNeighbor = leftNeighborId;
  }
}

void generateTopology(RoadInformation &roadInfo, opendrive::OpenDriveData &mapData)
{
  auto &laneSections = roadInfo.lanes.lane_sections;

  // left and right neighbors
  for (std::size_t i = 0; i < laneSections.size(); ++i)
  {
    setLeftRightNeighbor(mapData, roadInfo, i + 1);
  }
  for (std::size_t i = 0; i < laneSections.size(); ++i)
  {
    auto &laneSection = laneSections[i];
    auto laneSectionIndex = i + 1;

    for (std::size_t k = 0; k < laneSection.left.size(); k++)
    {
      auto &laneInfo = laneSection.left[k];
      setSuccessorPredecessor(mapData, roadInfo, laneInfo, laneSectionIndex);
    }
    for (std::size_t k = 0; k < laneSection.right.size(); k++)
    {
      auto &laneInfo = laneSection.right[k];
      setSuccessorPredecessor(mapData, roadInfo, laneInfo, laneSectionIndex);
    }
  }
}

void autoConnectIntersectionLanes(opendrive::OpenDriveData &mapData, double const overlapMargin)
{
  auto &laneMap = mapData.laneMap;
  auto &intersectionLaneIds = mapData.intersectionLaneIds;

  for (auto &element : intersectionLaneIds)
  {
    auto &lanes = element.second;
    for (auto leftIt = lanes.begin(); leftIt != lanes.end(); leftIt++)
    {
      for (auto rightIt = leftIt; rightIt != lanes.end(); rightIt++)
      {
        if (lanesOverlap(laneMap[*leftIt], laneMap[*rightIt], overlapMargin))
        {
          laneMap[*leftIt].overlaps.push_back(*rightIt);
          laneMap[*rightIt].overlaps.push_back(*leftIt);
        }
        switch (contactPlace(laneMap[*leftIt], laneMap[*rightIt]))
        {
          case ContactPlace::LeftLeft:
            laneMap[*leftIt].leftNeighbor = *rightIt;
            laneMap[*rightIt].leftNeighbor = *leftIt;
            // lane needs inversion
            invertLane(mapData.laneMap[*rightIt]);
            break;
          case ContactPlace::LeftRight:
            laneMap[*leftIt].leftNeighbor = *rightIt;
            laneMap[*rightIt].rightNeighbor = *leftIt;
            break;
          case ContactPlace::RightLeft:
            laneMap[*leftIt].rightNeighbor = *rightIt;
            laneMap[*rightIt].leftNeighbor = *leftIt;
            break;
          case ContactPlace::RightRight:
            laneMap[*leftIt].rightNeighbor = *rightIt;
            laneMap[*rightIt].rightNeighbor = *leftIt;
            // lane needs inversion
            invertLane(mapData.laneMap[*rightIt]);
            break;
          case ContactPlace::Overlap:
          case ContactPlace::None:
            // nothing to be done
            break;
        }
      }
    }
  }
}

double length(::opendrive::Edge const &edge)
{
  double edgeLength = 0.0;
  for (std::size_t i = 1u; i < edge.size(); ++i)
  {
    edgeLength += (edge[i - 1] - edge[i]).norm();
  }
  return edgeLength;
}

bool checkLaneConsistency(opendrive::OpenDriveData &mapData)
{
  bool ok = true;
  auto &laneMap = mapData.laneMap;
  std::vector<Id> deletionLanes;
  for (auto element : laneMap)
  {
    auto &lane = element.second;
    double leftLength = length(lane.leftEdge);
    double rightLength = length(lane.rightEdge);

    if (leftLength < 2.0e-10 || rightLength < 2.0e-10)
    {
      std::cerr << "checkLaneConsistency:: Invalid lane geometry for lane " << lane.id << "\n";
      deletionLanes.push_back(lane.id);
      ok = false;
    }
  }

  return ok;
}

bool convertToGeoPoints(opendrive::OpenDriveData &mapData)
{
  auto projPtr = pj_init_plus(mapData.geoReference.projection.c_str());
  if (projPtr == nullptr)
  {
    if (std::isnan(mapData.geoReference.longitude) || std::isnan(mapData.geoReference.latitude))
    {
      return false;
    }

    std::string defaultProjection = "+proj=tmerc +ellps=WGS84 +lon_0=" + std::to_string(mapData.geoReference.longitude)
      + " +lat_0=" + std::to_string(mapData.geoReference.latitude);
    projPtr = pj_init_plus(defaultProjection.c_str());
    std::cerr << "Using default projection: " << defaultProjection << "\n";
    if (projPtr == nullptr)
    {
      std::cerr << "Unknown error while creating the projection\n";
      return false;
    }
  }

  auto convertENUToGeo = [&projPtr](Point &point) {
    projXY enuPoint;
    enuPoint.u = point.x;
    enuPoint.v = point.y;

    auto geoPoint = pj_inv(enuPoint, projPtr);
    point.x = geoPoint.u * RAD_TO_DEG;
    point.y = geoPoint.v * RAD_TO_DEG;

  };

  for (auto &element : mapData.laneMap)
  {
    auto &lane = element.second;
    for (auto &point : lane.leftEdge)
    {
      convertENUToGeo(point);
    }
    for (auto &point : lane.rightEdge)
    {
      convertENUToGeo(point);
    }
  }

  for (auto &element : mapData.landmarks)
  {
    convertENUToGeo(element.second.position);
  }

  pj_dalloc(projPtr);
  return true;
}

bool GenerateGeometry(opendrive::OpenDriveData &open_drive_data, double const overlapMargin)
{
  bool ok = initializeLaneMap(open_drive_data);

  for (auto &roadInfo : open_drive_data.roads)
  {
    if (!generateRoadGeometry(roadInfo, open_drive_data))
    {
      ok = false;
    }
  }

  for (auto &roadInfo : open_drive_data.roads)
  {
    calculateSpeed(roadInfo, open_drive_data.laneMap);
  }
  for (auto &roadInfo : open_drive_data.roads)
  {
    generateTopology(roadInfo, open_drive_data);
  }

  if (!checkLaneConsistency(open_drive_data))
  {
    ok = false;
  }
  autoConnectIntersectionLanes(open_drive_data, overlapMargin);

  if (!convertToGeoPoints(open_drive_data))
  {
    ok = false;
  }

  return ok;
}
}
}