/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/comms/net_utils.h>
#include <mrpt/core/reverse_bytes.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/hwdrivers/CVelodyneScanner.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/datetime.h>  // timeDifference
#include <mrpt/system/filesystem.h>

#include <chrono>
#include <iostream>
#include <thread>

// socket's hdrs:
#ifdef _WIN32
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#if defined(_WIN32_WINNT) && (_WIN32_WINNT < 0x600)
#undef _WIN32_WINNT
#define _WIN32_WINNT 0x600  // Minimum: Windows Vista (required to pollfd)
#endif

#include <winsock2.h>
using socklen_t = int;

#if defined(_MSC_VER)
#pragma comment(lib, "WS2_32.LIB")
#endif

#else
#define INVALID_SOCKET (-1)
#include <arpa/inet.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>  // gettimeofday()
#include <sys/types.h>
#include <unistd.h>

#include <cerrno>
#endif

#if MRPT_HAS_LIBPCAP
#include <pcap.h>
#if !defined(PCAP_NETMASK_UNKNOWN)  // for older pcap versions
#define PCAP_NETMASK_UNKNOWN 0xffffffff
#endif
#endif

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CVelodyneScanner, mrpt::hwdrivers)

short int CVelodyneScanner::VELODYNE_DATA_UDP_PORT = 2368;
short int CVelodyneScanner::VELODYNE_POSITION_UDP_PORT = 8308;

const CVelodyneScanner::model_properties_list_t& CVelodyneScanner::TModelPropertiesFactory::get()
{
  static CVelodyneScanner::model_properties_list_t modelProperties;
  static bool init = false;
  if (!init)
  {
    init = true;
    modelProperties[CVelodyneScanner::VLP16].maxRange = 120.0;
    modelProperties[CVelodyneScanner::HDL32].maxRange = 70.0;
    modelProperties[CVelodyneScanner::HDL64].maxRange = 120.0;
  }
  return modelProperties;
}
std::string CVelodyneScanner::TModelPropertiesFactory::getListKnownModels()
{
  const CVelodyneScanner::model_properties_list_t& lst =
      CVelodyneScanner::TModelPropertiesFactory::get();
  std::string s;
  for (auto it : lst)
    s += mrpt::format(
        "`%s`,",
        mrpt::typemeta::TEnumType<CVelodyneScanner::model_t>::value2name(it.first).c_str());
  return s;
}

CVelodyneScanner::CVelodyneScanner() :
    m_device_ip(""),

    m_last_pos_packet_timestamp(INVALID_TIMESTAMP),

    m_hDataSock(INVALID_SOCKET),
    m_hPositionSock(INVALID_SOCKET),
    m_last_gps_rmc_age(INVALID_TIMESTAMP)

{
  m_sensorLabel = "Velodyne";

#if MRPT_HAS_LIBPCAP
  m_pcap_bpf_program = new bpf_program[1];
#endif

#ifdef _WIN32
  // Init the WinSock Library:
  WORD wVersionRequested = MAKEWORD(2, 0);
  WSADATA wsaData;
  if (WSAStartup(wVersionRequested, &wsaData)) THROW_EXCEPTION("Error calling WSAStartup");
#endif
}

CVelodyneScanner::~CVelodyneScanner()
{
  this->close();
#ifdef _WIN32
  WSACleanup();
#endif

#if MRPT_HAS_LIBPCAP
  delete[] reinterpret_cast<bpf_program*>(m_pcap_bpf_program);
  m_pcap_bpf_program = nullptr;
#endif
}

bool CVelodyneScanner::loadCalibrationFile(const std::string& velodyne_xml_calib_file_path)
{
  return m_velodyne_calib.loadFromXMLFile(velodyne_xml_calib_file_path);
}

void CVelodyneScanner::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& cfg, const std::string& sect)
{
  MRPT_START

  cfg.read_enum<CVelodyneScanner::model_t>(sect, "model", m_model);
  MRPT_LOAD_HERE_CONFIG_VAR(device_ip, string, m_device_ip, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pcap_input, string, m_pcap_input_file, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pcap_output, string, m_pcap_output_file, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_once, bool, m_pcap_read_once, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_fast, bool, m_pcap_read_fast, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(
      pcap_read_full_scan_delay_ms, double, m_pcap_read_full_scan_delay_ms, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pcap_repeat_delay, double, m_pcap_repeat_delay, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(
      pos_packets_timing_timeout, double, m_pos_packets_timing_timeout, cfg, sect);
  MRPT_LOAD_HERE_CONFIG_VAR(pos_packets_min_period, double, m_pos_packets_min_period, cfg, sect);

  using mrpt::DEG2RAD;
  m_sensorPose = mrpt::poses::CPose3D(
      cfg.read_float(sect, "pose_x", 0), cfg.read_float(sect, "pose_y", 0),
      cfg.read_float(sect, "pose_z", 0), DEG2RAD(cfg.read_float(sect, "pose_yaw", 0)),
      DEG2RAD(cfg.read_float(sect, "pose_pitch", 0)),
      DEG2RAD(cfg.read_float(sect, "pose_roll", 0)));

  std::string calibration_file;
  MRPT_LOAD_CONFIG_VAR(calibration_file, string, cfg, sect);
  if (!calibration_file.empty())
  {
    bool calibLoadOk = loadCalibrationFile(calibration_file);
    ASSERT_(calibLoadOk);
  }

  // Check validity:
  const model_properties_list_t& lstModels = TModelPropertiesFactory::get();
  if (lstModels.find(m_model) == lstModels.end())
  {
    THROW_EXCEPTION(mrpt::format(
        "Unrecognized `model` parameter: `%u` . Known values are: %s",
        static_cast<unsigned int>(m_model), TModelPropertiesFactory::getListKnownModels().c_str()));
  }

  // Optional HTTP-based settings:
  MRPT_LOAD_HERE_CONFIG_VAR(rpm, int, m_lidar_rpm, cfg, sect);
  m_lidar_return = cfg.read_enum<return_type_t>(sect, "return_type", m_lidar_return);

  MRPT_END
}

bool CVelodyneScanner::getNextObservation(
    mrpt::obs::CObservationVelodyneScan::Ptr& outScan, mrpt::obs::CObservationGPS::Ptr& outGPS)
{
  try
  {
    ASSERTMSG_(m_initialized, "initialize() has not been called yet!");

    // Init with empty smart pointers:
    outScan = mrpt::obs::CObservationVelodyneScan::Ptr();
    outGPS = mrpt::obs::CObservationGPS::Ptr();

    // Try to get data & pos packets:
    mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket rx_pkt;
    mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket rx_pos_pkt;
    mrpt::system::TTimeStamp data_pkt_timestamp, pos_pkt_timestamp;
    bool rx_all_ok =
        this->receivePackets(data_pkt_timestamp, rx_pkt, pos_pkt_timestamp, rx_pos_pkt);

    if (!rx_all_ok)
    {
      // PCAP EOF:
      return false;
    }

    if (pos_pkt_timestamp != INVALID_TIMESTAMP)
    {
      mrpt::obs::CObservationGPS::Ptr gps_obs = mrpt::obs::CObservationGPS::Create();
      gps_obs->sensorLabel = this->m_sensorLabel + std::string("_GPS");
      gps_obs->sensorPose = m_sensorPose;

      gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;

      bool parsed_ok = CGPSInterface::parse_NMEA(std::string(rx_pos_pkt.NMEA_GPRMC), *gps_obs);
      const mrpt::obs::gnss::Message_NMEA_RMC* msg_rmc =
          gps_obs->getMsgByClassPtr<mrpt::obs::gnss::Message_NMEA_RMC>();
      if (!parsed_ok || !msg_rmc || msg_rmc->fields.validity_char != 'A')
      {
        gps_obs->has_satellite_timestamp = false;
        gps_obs->timestamp = pos_pkt_timestamp;
      }
      else
      {
        // We have live GPS signal and a recent RMC frame:
        m_last_gps_rmc_age = pos_pkt_timestamp;
        m_last_gps_rmc = *msg_rmc;
      }
      outGPS = gps_obs;  // save in output object
    }

    if (data_pkt_timestamp != INVALID_TIMESTAMP)
    {
      m_state = ssWorking;

      // Break into a new observation object when the azimuth passes
      // 360->0 deg:
      const uint16_t rx_pkt_start_angle = rx_pkt.blocks[0].rotation();
      // const uint16_t rx_pkt_end_angle   =
      // rx_pkt.blocks[CObservationVelodyneScan::BLOCKS_PER_PACKET-1].rotation;

      // Return the observation as done when a complete 360 deg scan is
      // ready:
      if (m_rx_scan && !m_rx_scan->scan_packets.empty())
      {
        if ((rx_pkt_start_angle < m_rx_scan->scan_packets.rbegin()->blocks[0].rotation()) ||
            !m_return_frames)
        {
          outScan = m_rx_scan;
          m_rx_scan.reset();

          if (m_pcap)
          {
            // Keep the reader from blowing through the file.
            if (!m_pcap_read_fast)
              std::this_thread::sleep_for(
                  std::chrono::duration<double, std::milli>(m_pcap_read_full_scan_delay_ms));
          }
        }
      }

      // Create smart ptr to new in-progress observation:
      if (!m_rx_scan)
      {
        m_rx_scan = mrpt::obs::CObservationVelodyneScan::Create();
        m_rx_scan->sensorLabel = this->m_sensorLabel + std::string("_SCAN");
        m_rx_scan->sensorPose = m_sensorPose;
        m_rx_scan->calibration = m_velodyne_calib;  // Embed a copy of the calibration info

        {
          const model_properties_list_t& lstModels = TModelPropertiesFactory::get();
          auto it = lstModels.find(this->m_model);
          if (it != lstModels.end())
          {  // Model params:
            m_rx_scan->maxRange = it->second.maxRange;
          }
          else  // default params:
          {
            m_rx_scan->maxRange = 120.0;
          }
        }
      }

      // For the first packet, set timestamp:
      if (m_rx_scan->scan_packets.empty())
      {
        m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
        // Using GPS, if available:
        if (m_last_gps_rmc.fields.validity_char == 'A' &&
            mrpt::system::timeDifference(m_last_gps_rmc_age, data_pkt_timestamp) <
                m_pos_packets_timing_timeout)
        {
          // Each Velodyne data packet has a timestamp field,
          // with the number of us since the top of the current HOUR:
          // take the date and time from the GPS, then modify minutes
          // and seconds from data pkt:
          const mrpt::system::TTimeStamp gps_tim =
              m_last_gps_rmc.fields.UTCTime.getAsTimestamp(m_last_gps_rmc.getDateAsTimestamp());

          mrpt::system::TTimeParts tim_parts;
          mrpt::system::timestampToParts(gps_tim, tim_parts);
          tim_parts.minute = rx_pkt.gps_timestamp() /*us from top of hour*/ / 60000000ul;
          tim_parts.second = (rx_pkt.gps_timestamp() /*us from top of hour*/ % 60000000ul) * 1e-6;

          const mrpt::system::TTimeStamp data_pkt_tim =
              mrpt::system::buildTimestampFromParts(tim_parts);

          m_rx_scan->timestamp = data_pkt_tim;
          m_rx_scan->has_satellite_timestamp = true;
        }
        else
        {
          m_rx_scan->has_satellite_timestamp = false;
          m_rx_scan->timestamp = data_pkt_timestamp;
        }
      }

      // Accumulate pkts in the observation object:
      m_rx_scan->scan_packets.push_back(rx_pkt);
    }

    return true;
  }
  catch (exception& e)
  {
    cerr << "[CVelodyneScanner::getObservation] Returning false due to "
            "exception: "
         << endl;
    cerr << e.what() << endl;
    return false;
  }
}

void CVelodyneScanner::doProcess()
{
  CObservationVelodyneScan::Ptr obs;
  CObservationGPS::Ptr obs_gps;

  if (getNextObservation(obs, obs_gps))
  {
    m_state = ssWorking;
    if (obs) appendObservation(obs);
    if (obs_gps) appendObservation(obs_gps);
  }
  else
  {
    m_state = ssError;
    cerr << "ERROR receiving data from Velodyne devic!" << endl;
  }
}

/** Tries to initialize the sensor, after setting all the parameters with a call
 * to loadConfig.
 *  \exception This method must throw an exception with a descriptive message if
 * some critical error is found. */
void CVelodyneScanner::initialize()
{
  this->close();

  // (0) Preparation:
  // --------------------------------
  // Make sure we have calibration data:
  if (m_velodyne_calib.empty())
  {
    // Try to load default data:
    m_velodyne_calib = VelodyneCalibration::LoadDefaultCalibration(
        mrpt::typemeta::TEnumType<CVelodyneScanner::model_t>::value2name(m_model));
    if (m_velodyne_calib.empty())
      THROW_EXCEPTION(
          "Could not find default calibration data for the given LIDAR "
          "`model` name. Please, specify a valid `model` or load a valid "
          "XML configuration file first.");
  }

  // online vs off line operation??
  // -------------------------------
  if (m_pcap_input_file.empty())
  {  // Online

    if (m_lidar_rpm > 0)
    {
      if (!setLidarRPM(m_lidar_rpm)) THROW_EXCEPTION("Error in setLidarRPM();");
    }
    if (m_lidar_return != UNCHANGED)
    {
      if (!setLidarReturnType(m_lidar_return)) THROW_EXCEPTION("Error in setLidarReturnType();");
    }

    // (1) Create LIDAR DATA socket
    // --------------------------------
    if (INVALID_SOCKET == (m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)))
      THROW_EXCEPTION(format(
          "Error creating UDP socket:\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));

    struct sockaddr_in bindAddr;
    memset(&bindAddr, 0, sizeof(bindAddr));
    bindAddr.sin_family = AF_INET;
    bindAddr.sin_port = htons(VELODYNE_DATA_UDP_PORT);
    bindAddr.sin_addr.s_addr = INADDR_ANY;

    if (int(INVALID_SOCKET) == ::bind(m_hDataSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
      THROW_EXCEPTION(mrpt::comms::net::getLastSocketErrorStr());

#ifdef _WIN32
    unsigned long non_block_mode = 1;
    if (ioctlsocket(m_hDataSock, FIONBIO, &non_block_mode))
      THROW_EXCEPTION("Error entering non-blocking mode with ioctlsocket().");
#else
    int oldflags = fcntl(m_hDataSock, F_GETFL, 0);
    if (oldflags == -1) THROW_EXCEPTION("Error retrieving fcntl() of socket.");
    oldflags |= O_NONBLOCK | FASYNC;
    if (-1 == fcntl(m_hDataSock, F_SETFL, oldflags))
      THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
#endif

    // (2) Create LIDAR POSITION socket
    // --------------------------------
    if (INVALID_SOCKET == (m_hPositionSock = socket(PF_INET, SOCK_DGRAM, 0)))
      THROW_EXCEPTION(format(
          "Error creating UDP socket:\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));

    bindAddr.sin_port = htons(VELODYNE_POSITION_UDP_PORT);

    if (int(INVALID_SOCKET) ==
        ::bind(m_hPositionSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
      THROW_EXCEPTION(mrpt::comms::net::getLastSocketErrorStr());

#ifdef _WIN32
    if (ioctlsocket(m_hPositionSock, FIONBIO, &non_block_mode))
      THROW_EXCEPTION("Error entering non-blocking mode with ioctlsocket().");
#else
    oldflags = fcntl(m_hPositionSock, F_GETFL, 0);
    if (oldflags == -1) THROW_EXCEPTION("Error retrieving fcntl() of socket.");
    oldflags |= O_NONBLOCK | FASYNC;
    if (-1 == fcntl(m_hPositionSock, F_SETFL, oldflags))
      THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
#endif
  }
  else
  {  // Offline:
#if MRPT_HAS_LIBPCAP
    char errbuf[PCAP_ERRBUF_SIZE];

    if (m_pcap_verbose)
      printf("\n[CVelodyneScanner] Opening PCAP file \"%s\"\n", m_pcap_input_file.c_str());
    if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) == nullptr)
    {
      THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
    }

    // Build PCAP filter:
    {
      std::string filter_str = mrpt::format(
          "(udp dst port %d || udp dst port %d)",
          static_cast<int>(CVelodyneScanner::VELODYNE_DATA_UDP_PORT),
          static_cast<int>(CVelodyneScanner::VELODYNE_POSITION_UDP_PORT));
      if (!m_device_ip.empty()) filter_str += "&& src host " + m_device_ip;

      static std::string sMsgError = "[CVelodyneScanner] Error calling pcap_compile: ";  // This is
      // to avoid
      // the
      // ill-formed
      // signature
      // of
      // pcap_error()
      // accepting
      // "char*",
      // not
      // "const
      // char*"...
      // sigh
      if (pcap_compile(
              reinterpret_cast<pcap_t*>(m_pcap), reinterpret_cast<bpf_program*>(m_pcap_bpf_program),
              filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) < 0)
        pcap_perror(reinterpret_cast<pcap_t*>(m_pcap), &sMsgError[0]);
    }

    m_pcap_file_empty = true;
    m_pcap_read_count = 0;

#else
    THROW_EXCEPTION(
        "Velodyne: Reading from PCAP requires building MRPT with libpcap "
        "support!");
#endif
  }

  m_last_pos_packet_timestamp = INVALID_TIMESTAMP;
  m_last_gps_rmc_age = INVALID_TIMESTAMP;
  m_state = ssInitializing;

  m_initialized = true;
}

void CVelodyneScanner::close()
{
  if (m_hDataSock != INVALID_SOCKET)
  {
    shutdown(m_hDataSock, 2);  // SD_BOTH  );
#ifdef _WIN32
    closesocket(m_hDataSock);
#else
    ::close(m_hDataSock);
#endif
    m_hDataSock = INVALID_SOCKET;
  }

  if (m_hPositionSock != INVALID_SOCKET)
  {
    shutdown(m_hPositionSock, 2);  // SD_BOTH  );
#ifdef _WIN32
    closesocket(m_hPositionSock);
#else
    ::close(m_hPositionSock);
#endif
    m_hPositionSock = INVALID_SOCKET;
  }
#if MRPT_HAS_LIBPCAP
  if (m_pcap)
  {
    pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
    m_pcap = nullptr;
  }
  if (m_pcap_dumper)
  {
    pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(m_pcap_dumper));
    m_pcap_dumper = nullptr;
  }
  if (m_pcap_out)
  {
    pcap_close(reinterpret_cast<pcap_t*>(m_pcap_out));
    m_pcap_out = nullptr;
  }
#endif
  m_initialized = false;
}

// Fixed Ethernet headers for PCAP capture --------
#if MRPT_HAS_LIBPCAP
const uint16_t LidarPacketHeader[21] = {
    0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204, 0x0000, 0x0040, 0x11ff,
    0xaab4, 0xa8c0, 0xc801,
    0xffff,  // checksum 0xa9b4 //source ip
    // 0xa8c0, 0xc801 is
    // 192.168.1.200
    0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
const uint16_t PositionPacketHeader[21] = {
    0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204, 0x0000, 0x0040, 0x11ff,
    0xaab4, 0xa8c0, 0xc801, 0xffff,  // checksum 0xa9b4
    // //source ip
    // 0xa8c0, 0xc801
    // is 192.168.1.200
    0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
#endif

#if defined(_WIN32) && MRPT_HAS_LIBPCAP
int gettimeofday(struct timeval* tp, void*)
{
  FILETIME ft;
  ::GetSystemTimeAsFileTime(&ft);
  long long t = (static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
  t -= 116444736000000000LL;
  t /= 10;  // microseconds
  tp->tv_sec = static_cast<long>(t / 1000000UL);
  tp->tv_usec = static_cast<long>(t % 1000000UL);
  return 0;
}
#endif

bool CVelodyneScanner::receivePackets(
    mrpt::system::TTimeStamp& data_pkt_timestamp,
    mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket& out_data_pkt,
    mrpt::system::TTimeStamp& pos_pkt_timestamp,
    mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket& out_pos_pkt)
{
  static_assert(
      sizeof(mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket) ==
          CObservationVelodyneScan::POS_PACKET_SIZE,
      "Velodyne pos packet: wrong size!");
  static_assert(
      sizeof(mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket) ==
          CObservationVelodyneScan::PACKET_SIZE,
      "Velodyne raw packet: wrong size!");

  bool ret = true;  // all ok
  if (m_pcap)
  {
    ret = internal_read_PCAP_packet(
        data_pkt_timestamp, (uint8_t*)&out_data_pkt, pos_pkt_timestamp, (uint8_t*)&out_pos_pkt);
  }
  else
  {
    data_pkt_timestamp = internal_receive_UDP_packet(
        m_hDataSock, (uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE, m_device_ip);
    pos_pkt_timestamp = internal_receive_UDP_packet(
        m_hPositionSock, (uint8_t*)&out_pos_pkt, CObservationVelodyneScan::POS_PACKET_SIZE,
        m_device_ip);
  }

// Optional PCAP dump:
#if MRPT_HAS_LIBPCAP
  // Save to PCAP file?
  if (!m_pcap_output_file.empty() && !m_pcap_out)  // 1st time: create output file
  {
#if MRPT_HAS_LIBPCAP
    char errbuf[PCAP_ERRBUF_SIZE];

    mrpt::system::TTimeParts parts;
    mrpt::system::timestampToParts(mrpt::Clock::now(), parts, true);
    string sFilePostfix = "_";
    sFilePostfix += format(
        "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year, (unsigned int)parts.month,
        (unsigned int)parts.day, (unsigned int)parts.hour, (unsigned int)parts.minute,
        (unsigned int)parts.second);
    const string sFileName = m_pcap_output_file +
                             mrpt::system::fileNameStripInvalidChars(sFilePostfix) +
                             string(".pcap");

    m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
    ASSERTMSG_(m_pcap_out != nullptr, "Error creating PCAP live capture handle");

    printf("\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n", sFileName.c_str());
    if ((m_pcap_dumper =
             pcap_dump_open(reinterpret_cast<pcap_t*>(m_pcap_out), sFileName.c_str())) == nullptr)
    {
      THROW_EXCEPTION_FMT("Error creating PCAP live dumper: '%s'", errbuf);
    }
#else
    THROW_EXCEPTION(
        "Velodyne: Writing PCAP files requires building MRPT with libpcap "
        "support!");
#endif
  }

  if (m_pcap_out && m_pcap_dumper &&
      (data_pkt_timestamp != INVALID_TIMESTAMP || pos_pkt_timestamp != INVALID_TIMESTAMP))
  {
    struct pcap_pkthdr header;
    struct timeval currentTime;
    gettimeofday(&currentTime, nullptr);
    std::vector<unsigned char> packetBuffer;

    // Data pkt:
    if (data_pkt_timestamp != INVALID_TIMESTAMP)
    {
      header.caplen = header.len = CObservationVelodyneScan::PACKET_SIZE + 42;
      packetBuffer.resize(header.caplen);
      header.ts = currentTime;

      memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
      memcpy(
          &(packetBuffer[0]) + 42, (uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE);
      pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
    }
    // Pos pkt:
    if (pos_pkt_timestamp != INVALID_TIMESTAMP)
    {
      header.caplen = header.len = CObservationVelodyneScan::POS_PACKET_SIZE + 42;
      packetBuffer.resize(header.caplen);
      header.ts = currentTime;

      memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
      memcpy(
          &(packetBuffer[0]) + 42, (uint8_t*)&out_pos_pkt,
          CObservationVelodyneScan::POS_PACKET_SIZE);
      pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
    }
  }
#endif

  // Convert from Velodyne's standard little-endian ordering to host byte
  // ordering (done AFTER saving the pckg as is to pcap above).
  // 2019-NOV: Removed. Don't do this here, since it's problematic to
  // "remember" whether internal data is already in big or little endian.

  // Position packet decimation:
  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
  {
    if ((m_last_pos_packet_timestamp != INVALID_TIMESTAMP) &&
        mrpt::system::timeDifference(m_last_pos_packet_timestamp, pos_pkt_timestamp) <
            m_pos_packets_min_period)
    {
      // Ignore this packet
      pos_pkt_timestamp = INVALID_TIMESTAMP;
    }
    else
    {
      // Reset time watch:
      m_last_pos_packet_timestamp = pos_pkt_timestamp;
    }
  }

  return ret;
}

// static method:
mrpt::system::TTimeStamp CVelodyneScanner::internal_receive_UDP_packet(
    platform_socket_t hSocket,
    uint8_t* out_buffer,
    size_t expected_packet_size,
    const std::string& filter_only_from_IP)
{
  if (hSocket == INVALID_SOCKET)
    THROW_EXCEPTION(
        "Error: UDP socket is not open yet! Have you called initialize() "
        "first?");

  unsigned long devip_addr = 0;
  if (!filter_only_from_IP.empty()) devip_addr = inet_addr(filter_only_from_IP.c_str());

  const mrpt::system::TTimeStamp time1 = mrpt::Clock::now();

  struct pollfd fds[1];
  fds[0].fd = hSocket;
  fds[0].events = POLLIN;
  static const int POLL_TIMEOUT = 1;  // (ms)

  sockaddr_in sender_address;
  socklen_t sender_address_len = sizeof(sender_address);

  while (true)
  {
    // Unfortunately, the Linux kernel recvfrom() implementation
    // uses a non-interruptible std::this_thread::sleep_for(ms) when waiting
    // for data,
    // which would cause this method to hang if the device is not
    // providing data.  We poll() the device first to make sure
    // the recvfrom() will not block.
    //
    // Note, however, that there is a known Linux kernel bug:
    //
    //   Under Linux, select() may report a socket file descriptor
    //   as "ready for reading", while nevertheless a subsequent
    //   read blocks.  This could for example happen when data has
    //   arrived but upon examination has wrong checksum and is
    //   discarded.  There may be other circumstances in which a
    //   file descriptor is spuriously reported as ready.  Thus it
    //   may be safer to use O_NONBLOCK on sockets that should not
    //   block.

    // poll() until input available
    do
    {
      int retval =
#if !defined(_WIN32)
          poll
#else
          WSAPoll
#endif
          (fds, 1, POLL_TIMEOUT);
      if (retval < 0)  // poll() error?
      {
        if (errno != EINTR)
          THROW_EXCEPTION(format(
              "Error in UDP poll():\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));
      }
      if (retval == 0)  // poll() timeout?
      {
        return INVALID_TIMESTAMP;
      }
      if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
          (fds[0].revents & POLLNVAL))  // device error?
      {
        THROW_EXCEPTION("Error in UDP poll() (seems Velodyne device error?)");
      }
    } while ((fds[0].revents & POLLIN) == 0);

    // Receive packets that should now be available from the
    // socket using a blocking read.
    int nbytes = recvfrom(
        hSocket, (char*)&out_buffer[0], expected_packet_size, 0, (sockaddr*)&sender_address,
        &sender_address_len);

    if (nbytes < 0)
    {
      if (errno != EWOULDBLOCK) THROW_EXCEPTION("recvfrom() failed!?!");
    }
    else if ((size_t)nbytes == expected_packet_size)
    {
      // read successful,
      // if packet is not from the lidar scanner we selected by IP,
      // continue otherwise we are done
      if (!filter_only_from_IP.empty() && sender_address.sin_addr.s_addr != devip_addr)
        continue;
      else
        break;  // done
    }

    std::cerr << "[CVelodyneScanner] Warning: incomplete Velodyne packet read: " << nbytes
              << " bytes\n";
  }

  // Average the times at which we begin and end reading.  Use that to
  // estimate when the scan occurred.
  const mrpt::system::TTimeStamp time2 = mrpt::Clock::now();

  return mrpt::Clock::time_point(mrpt::Clock::duration(
      time1.time_since_epoch().count() / 2 + time2.time_since_epoch().count() / 2));
}

bool CVelodyneScanner::internal_read_PCAP_packet(
    mrpt::system::TTimeStamp& data_pkt_time,
    uint8_t* out_data_buffer,
    mrpt::system::TTimeStamp& pos_pkt_time,
    uint8_t* out_pos_buffer)
{
#if MRPT_HAS_LIBPCAP
  ASSERT_(m_pcap);

  data_pkt_time = INVALID_TIMESTAMP;
  pos_pkt_time = INVALID_TIMESTAMP;

  char errbuf[PCAP_ERRBUF_SIZE];
  struct pcap_pkthdr* header;
  const u_char* pkt_data;

  while (true)
  {
    int res;
    if ((res = pcap_next_ex(reinterpret_cast<pcap_t*>(m_pcap), &header, &pkt_data)) >= 0)
    {
      ++m_pcap_read_count;

      // if packet is not from the lidar scanner we selected by IP,
      // continue
      if (pcap_offline_filter(
              reinterpret_cast<bpf_program*>(m_pcap_bpf_program), header, pkt_data) == 0)
      {
        if (m_verbose)
          std::cout << "[CVelodyneScanner] DEBUG: Filtering out packet #" << m_pcap_read_count
                    << " in PCAP file.\n";
        continue;
      }

      // Determine whether it is a DATA or POSITION packet:
      m_pcap_file_empty = false;
      const mrpt::system::TTimeStamp tim = mrpt::Clock::now();
      const uint16_t udp_dst_port = ntohs(*reinterpret_cast<const uint16_t*>(pkt_data + 0x24));

      if (udp_dst_port == CVelodyneScanner::VELODYNE_POSITION_UDP_PORT)
      {
        if (m_verbose)
          std::cout << "[CVelodyneScanner] DEBUG: Packet #" << m_pcap_read_count
                    << " in PCAP file is POSITION pkt.\n";
        memcpy(out_pos_buffer, pkt_data + 42, CObservationVelodyneScan::POS_PACKET_SIZE);
        pos_pkt_time = tim;  // success
        return true;
      }
      else if (udp_dst_port == CVelodyneScanner::VELODYNE_DATA_UDP_PORT)
      {
        if (m_verbose)
          std::cout << "[CVelodyneScanner] DEBUG: Packet #" << m_pcap_read_count
                    << " in PCAP file is DATA pkt.\n";
        memcpy(out_data_buffer, pkt_data + 42, CObservationVelodyneScan::PACKET_SIZE);
        data_pkt_time = tim;  // success
        return true;
      }
      else
      {
        std::cerr << "[CVelodyneScanner] ERROR: Packet " << m_pcap_read_count
                  << " in PCAP file passed the filter but does not "
                     "match expected UDP port numbers! Skipping it.\n";
      }
    }

    if (m_pcap_file_empty)  // no data in file?
    {
      fprintf(
          stderr,
          "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d "
          "reading Velodyne packet: `%s`\n",
          res, pcap_geterr(reinterpret_cast<pcap_t*>(m_pcap)));
      return true;
    }

    if (m_pcap_read_once)
    {
      if (m_pcap_verbose)
        printf(
            "[CVelodyneScanner] INFO: end of file reached -- done "
            "reading.\n");
      std::this_thread::sleep_for(250ms);
      return false;
    }

    if (m_pcap_repeat_delay > 0.0)
    {
      if (m_pcap_verbose)
        printf(
            "[CVelodyneScanner] INFO: end of file reached -- delaying "
            "%.3f seconds.\n",
            m_pcap_repeat_delay);
      std::this_thread::sleep_for(
          std::chrono::duration<double, std::milli>(m_pcap_repeat_delay * 1000.0));
    }

    if (m_pcap_verbose) printf("[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");

    // rewind the file
    pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
    if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) == nullptr)
    {
      THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
    }
    m_pcap_file_empty = true;  // maybe the file disappeared?
  }                            // loop back and try again
#else
  THROW_EXCEPTION("MRPT needs to be built against libpcap to enable this functionality");
#endif
}

bool CVelodyneScanner::setLidarReturnType(return_type_t ret_type)
{
  /* HTTP-based config: http://10.0.0.100/tab/config.html
    <form name='returns' method="post" action="/cgi/setting">
    <span>Return&nbsp;Type:&nbsp;</span> <select name="returns"
    onchange='javascript:this.form.submit()'>
    <option>Strongest</option>
    <option>Last</option>
    <option>Dual</option>
    </select>
    </form>
  */
  MRPT_START
  std::string strRet;
  switch (ret_type)
  {
    case STRONGEST:
      strRet = "Strongest";
      break;
    case DUAL:
      strRet = "Dual";
      break;
    case LAST:
      strRet = "Last";
      break;
    case UNCHANGED:
      return true;
    default:
      THROW_EXCEPTION("Invalid value for return type!");
  };

  const std::string cmd = mrpt::format("returns=%s", strRet.c_str());
  return this->internal_send_http_post(cmd);
  MRPT_END
}

bool CVelodyneScanner::setLidarRPM(int rpm)
{
  /* HTTP-based config: http://10.0.0.100/tab/config.html
  <form name='rpm' method="post" action="/cgi/setting">
  Motor
  &nbsp;RPM:&nbsp;<input type="text" name="rpm" size="5"
  style="text-align:right" /><input type="button" value="+"
  onclick="javascript:this.form.rpm.value++;this.form.submit()" /><input
  type="button" value="-"
  onclick="javascript:this.form.rpm.value--;this.form.submit()" />
  &nbsp;<input type="submit" value="Set" />
  </form>
  */

  MRPT_START
  const std::string cmd = mrpt::format("rpm=%i", rpm);
  return this->internal_send_http_post(cmd);
  MRPT_END
}

bool CVelodyneScanner::setLidarOnOff(bool on)
{
  // laser = on|off
  MRPT_START
  const std::string cmd = mrpt::format("laser=%s", on ? "on" : "off");
  return this->internal_send_http_post(cmd);
  MRPT_END
}

void CVelodyneScanner::setFramePublishing(bool on)
{
  // frame publishing | data packet publishing = on|off
  MRPT_START
  m_return_frames = on;
  MRPT_END
}

bool CVelodyneScanner::internal_send_http_post(const std::string& post_data)
{
  MRPT_START

  ASSERTMSG_(!m_device_ip.empty(), "A device IP address must be specified first!");

  using namespace mrpt::comms::net;

  std::vector<uint8_t> post_out;

  HttpRequestOutput httpOut;
  HttpRequestOptions ho;

  ho.extra_headers["Origin"] = mrpt::format("http://%s", m_device_ip.c_str());
  ho.extra_headers["Referer"] = mrpt::format("http://%s", m_device_ip.c_str());
  ho.extra_headers["Upgrade-Insecure-Requests"] = "1";
  ho.extra_headers["Content-Type"] = "application/x-www-form-urlencoded";

  http_errorcode ret = http_request(
      "POST", post_data, mrpt::format("http://%s/cgi/setting", m_device_ip.c_str()), post_out, ho,
      httpOut);

  const int http_rep_code = httpOut.http_responsecode;

  return mrpt::comms::net::http_errorcode::Ok == ret &&
         (http_rep_code == 200 || http_rep_code == 204);  // OK codes

  MRPT_END
}
