/* +------------------------------------------------------------------------+
   |                     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 "comms-precomp.h"  // Precompiled headers
//
#include <mrpt/comms/CSerialPort.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/system/os.h>

#if defined(MRPT_OS_LINUX) || defined(__APPLE__)
// Linux implementation: refer to
//  http://www.easysw.com/~mike/serial/serial.html

#include <fcntl.h>      /* File control definitions */
#include <sys/ioctl.h>  // FIONREAD,...
#include <sys/time.h>   // gettimeofday
#include <termios.h>    /* POSIX terminal control definitions */
#include <unistd.h>     /* UNIX standard function definitions */

#include <cerrno> /* Error number definitions */
#include <csignal>
#include <cstdio>  /* Standard input/output definitions */
#include <cstring> /* String function definitions */

#ifdef HAVE_LINUX_SERIAL_H
#include <linux/serial.h>
#endif

#include <map>

#endif  // Linux | Apple

#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#endif

#include <iostream>
#include <thread>

using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::io;
using namespace std;
using namespace std::literals;

// ctor
CSerialPort::CSerialPort(const string& portName, bool openNow) : m_serialName(portName)
{
  if (openNow) open();
}

// Dtor:
CSerialPort::~CSerialPort()
{
  try
  {
    if (isOpen()) close();
  }
  catch (const std::exception& e)
  {
    std::cerr << "[~CSerialPort] Exception:\n" << mrpt::exception_to_str(e);
  }
}

void CSerialPort::open(const std::string& COM_name)
{
  if (isOpen() && m_serialName != COM_name) THROW_EXCEPTION("Cannot change serial port while open");
  if (!isOpen())
  {
    setSerialPortName(COM_name);
    open();
  }
}

void CSerialPort::setSerialPortName(const std::string& COM_name)
{
  if (isOpen()) THROW_EXCEPTION("Cannot change serial port while open");
  m_serialName = COM_name;
}

/* -----------------------------------------------------
        Open
   ----------------------------------------------------- */
void CSerialPort::open()
{
  MRPT_START
#ifdef _WIN32
  // Check name:
  if (!m_serialName.size()) THROW_EXCEPTION("Serial port name is empty!!");

  // Is it COMX, X>4? ->  "\\.\COMX"
  if (tolower(m_serialName[0]) == 'c' && tolower(m_serialName[1]) == 'o' &&
      tolower(m_serialName[2]) == 'm')
  {
    // Need to add "\\.\"?
    if (m_serialName.size() > 4 || m_serialName[3] > '4')
      m_serialName = std::string("\\\\.\\") + m_serialName;
  }

  // Open the serial port:
  if (INVALID_HANDLE_VALUE == (hCOM = CreateFileA(
                                   m_serialName.c_str(),  // Serial Port name
                                   GENERIC_READ | GENERIC_WRITE, 0, nullptr, OPEN_EXISTING, 0, 0)))
  {
    hCOM = nullptr;
    THROW_EXCEPTION_FMT("Error trying to open serial port: %s", m_serialName.c_str());
  }

  // Set recommended buffer sizes:
  SetupComm(hCOM, 4096, 4096);

#else
  // Check name:
  if (!m_serialName.size()) THROW_EXCEPTION("Serial port name is empty!!");
  if (m_serialName[0] != '/') m_serialName = string("/dev/") + m_serialName;

  // Open the serial port:
  // The O_NOCTTY flag tells UNIX that this program doesn't want to be the
  // "controlling terminal" for that port.
  // The O_NDELAY flag tells UNIX that this program doesn't care what state
  // the DCD signal line is in - whether the other end of the port is up and
  // running.
  if (-1 == (hCOM = ::open(m_serialName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)))
    THROW_EXCEPTION_FMT("Error trying to open the serial port %s!!", m_serialName.c_str());

  // Clear flags:
  fcntl(hCOM, F_SETFL, 0);

  //
  // Start assembling the new port settings.
  //
  termios port_settings;
  bzero(&port_settings, sizeof(port_settings));

  //
  // Enable the receiver (CREAD) and ignore modem control lines
  // (CLOCAL).
  //
  port_settings.c_cflag |= CREAD | CLOCAL;

  //
  // Set the VMIN and VTIME parameters to zero by default. VMIN is
  // the minimum number of characters for non-canonical read and
  // VTIME is the timeout in deciseconds for non-canonical
  // read. Setting both of these parameters to zero implies that a
  // read will return immediately only giving the currently
  // available characters.
  //
  port_settings.c_cc[VMIN] = 0;
  port_settings.c_cc[VTIME] = 0;

  /*
   * Flush the input buffer associated with the port.
   */
  if (tcflush(hCOM, TCIFLUSH) < 0)
    THROW_EXCEPTION_FMT("Cannot flush serial port: %s", strerror(errno));

  /*
   * Write the new settings to the port.
   */
  if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
    THROW_EXCEPTION_FMT("Cannot set the new config to the serial port: %s", strerror(errno));

  // Do NOT block on read.
  fcntl(hCOM, F_SETFL, FNDELAY);

// Success!
#endif
  MRPT_END
}

/* -----------------------------------------------------
        isOpen
   ----------------------------------------------------- */
bool CSerialPort::isOpen() const
{
#ifdef _WIN32
  return hCOM != nullptr;
#else
  return hCOM != -1;
#endif
}

/* -----------------------------------------------------
        setConfig
   ----------------------------------------------------- */
void CSerialPort::setConfig(
    int baudRate, int parity, int bits, int nStopBits, bool enableFlowControl)
{
  MRPT_START
#ifdef _WIN32

  DCB dcb_conf;
  dcb_conf.DCBlength = sizeof(DCB);

  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The serial port is not open");

  if (!GetCommState(hCOM, &dcb_conf)) THROW_EXCEPTION("Error retrieving COM state");

  //
  // Apply baud rate
  //
  int BR;
  switch (baudRate)
  {
    case 300:
      BR = CBR_300;
      break;
    case 600:
      BR = CBR_600;
      break;
    case 1200:
      BR = CBR_1200;
      break;
    case 2400:
      BR = CBR_2400;
      break;
    case 4800:
      BR = CBR_4800;
      break;
    case 9600:
      BR = CBR_9600;
      break;
    case 19200:
      BR = CBR_19200;
      break;
    case 38400:
      BR = CBR_38400;
      break;
    case 57600:
      BR = CBR_57600;
      break;
    case 115200:
      BR = CBR_115200;
      break;
    default:
      BR = baudRate;
      // THROW_EXCEPTION_FMT("Invalid desired baud rate value:
      // %i",baudRate ) ;
      break;
  }

  dcb_conf.BaudRate = BR;

  dcb_conf.ByteSize = (BYTE)bits;
  dcb_conf.Parity = (BYTE)parity;

  // stop bits:
  switch (nStopBits)
  {
    case 1:
      dcb_conf.StopBits = ONESTOPBIT;
      break;
    case 2:
      dcb_conf.StopBits = TWOSTOPBITS;
      break;
    default:
      THROW_EXCEPTION_FMT("Invalid number of stop bits: %i", nStopBits);
      break;
  }

  dcb_conf.fBinary = true;
  dcb_conf.fParity = parity != 0;

  dcb_conf.fRtsControl = enableFlowControl ? RTS_CONTROL_HANDSHAKE : RTS_CONTROL_DISABLE;
  dcb_conf.fOutxCtsFlow = enableFlowControl;

  // Apply:
  if (!SetCommState(hCOM, &dcb_conf)) THROW_EXCEPTION("Error changing COM state");

  // Assure:
  if (!GetCommState(hCOM, &dcb_conf)) THROW_EXCEPTION("Error retrieving COM state");
  if (((int)dcb_conf.BaudRate) != baudRate)
    THROW_EXCEPTION("COM state verification after writing failed");

  m_baudRate = baudRate;

#else
  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");

  ASSERT_(baudRate > 0);

  //
  // Apply baud rate
  //
  int BR;
  bool special_rate = false;
  switch (baudRate)
  {
    case 50:
      BR = B50;
      break;
    case 75:
      BR = B75;
      break;
    case 110:
      BR = B110;
      break;
    case 134:
      BR = B134;
      break;
    case 200:
      BR = B200;
      break;
    case 300:
      BR = B300;
      break;
    case 600:
      BR = B600;
      break;
    case 1200:
      BR = B1200;
      break;
    case 2400:
      BR = B2400;
      break;
    case 4800:
      BR = B4800;
      break;
    case 9600:
      BR = B9600;
      break;
    case 19200:
      BR = B19200;
      break;
    case 38400:
      BR = B38400;
      break;
    case 57600:
      BR = B57600;
      break;
    case 115200:
      BR = B115200;
      break;
    case 230400:
      BR = B230400;
      break;
#ifdef B460800
    case 460800:
      BR = B460800;
      break;
#endif
#ifdef B500000
    case 500000:
      BR = B500000;
      break;
#endif
#ifdef B4000000
    case 576000:
      BR = B576000;
      break;
    case 921600:
      BR = B921600;
      break;
    case 1000000:
      BR = B1000000;
      break;
    case 1152000:
      BR = B1152000;
      break;
    case 1500000:
      BR = B1500000;
      break;
    case 2000000:
      BR = B2000000;
      break;
    case 2500000:
      BR = B2500000;
      break;
    case 3000000:
      BR = B3000000;
      break;
    case 3500000:
      BR = B3500000;
      break;
    case 4000000:
      BR = B4000000;
      break;
#endif
    default:
#ifdef HAVE_LINUX_SERIAL_H
      special_rate = true;
#else
      BR = baudRate;  // This is all we can try in that case...
#endif
      break;
  }

  if (special_rate)
  {
#ifdef HAVE_LINUX_SERIAL_H
    struct serial_struct serial;
    if (ioctl(hCOM, TIOCGSERIAL, &serial) < 0) THROW_EXCEPTION("error on TIOCGSERIAL ioctl");

    serial.custom_divisor = serial.baud_base / baudRate;
    if (!serial.custom_divisor) serial.custom_divisor = 1;
    const int actual_rate = serial.baud_base / serial.custom_divisor;

    serial.flags &= ~ASYNC_SPD_MASK;
    serial.flags |= ASYNC_SPD_CUST;  // We want to use our CUSTOM divisor.

    if (ioctl(hCOM, TIOCSSERIAL, &serial) < 0) THROW_EXCEPTION("error on TIOCSSERIAL ioctl");

    BR = B38400;  // It seems that 38400 means to the driver here to use our
    // custom divisor

    if (actual_rate != baudRate)
      cout << "[CSerialPort::setConfig] Setting custom baud rate to " << actual_rate
           << ", the closer I can make to " << baudRate << endl;
#else
    THROW_EXCEPTION("Custom serial port baud rates require linux/serial.h");
#endif
  }  // end specialRate
  else
  {
    // Normal baudrate: Just in case, undo possible custom divisors:
    // #ifdef HAVE_LINUX_SERIAL_H
    //		struct serial_struct serial;
    //		if (ioctl(hCOM, TIOCGSERIAL, &serial) < 0)
    //            THROW_EXCEPTION("error on TIOCGSERIAL ioctl");
    //
    //        serial.flags &= ~ASYNC_SPD_MASK;
    //
    //		if (ioctl(hCOM, TIOCSSERIAL, &serial) < 0)
    //            THROW_EXCEPTION("error on TIOCSSERIAL ioctl");
    // #endif
  }

  termios port_settings;
  if (tcgetattr(hCOM, &port_settings) < 0)
    THROW_EXCEPTION_FMT("Cannot get the current settings: %s", strerror(errno));

  if ((cfsetispeed(&port_settings, BR) < 0) || (cfsetospeed(&port_settings, BR) < 0))
    THROW_EXCEPTION_FMT("Cannot change baudRate in setting structure: %s", strerror(errno));

  //
  // Set the character size.
  //
  port_settings.c_cflag &= ~CSIZE;
  switch (bits)
  {
    case 5:
      port_settings.c_cflag |= CS5;
      break;
    case 6:
      port_settings.c_cflag |= CS6;
      break;
    case 7:
      port_settings.c_cflag |= CS7;
      break;
    case 8:
      port_settings.c_cflag |= CS8;
      break;
    default:
      THROW_EXCEPTION_FMT("Invalid character size: %i", bits);
      break;
  }

  // parity  0:No parity, 1:Odd, 2:Even
  switch (parity)
  {
    case 2:
      port_settings.c_cflag |= PARENB;
      port_settings.c_cflag &= ~PARODD;
      port_settings.c_iflag |= INPCK;
      break;
    case 1:
      port_settings.c_cflag |= (PARENB | PARODD);
      port_settings.c_iflag |= INPCK;
      break;
    case 0:
      port_settings.c_cflag &= ~(PARENB);
      port_settings.c_iflag |= IGNPAR;
      break;
    default:
      THROW_EXCEPTION_FMT("Invalid parity selection: %i", parity);
      break;
  }

  // stop bits:
  switch (nStopBits)
  {
    case 1:
      port_settings.c_cflag &= ~(CSTOPB);
      break;
    case 2:
      port_settings.c_cflag |= CSTOPB;
      break;
    default:
      THROW_EXCEPTION_FMT("Invalid number of stop bits: %i", nStopBits);
      break;
  }

  //
  // Set the flow control.
  //
  if (enableFlowControl)
  {
    // RTS/CTS ON:
    port_settings.c_cflag |= CRTSCTS;
  }
  else
  {
    // none
    port_settings.c_cflag &= ~(CRTSCTS);
  }

  /* Write the new settings to the port.
   */
  if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
    THROW_EXCEPTION_FMT("Cannot set the new settings: %s", strerror(errno));

  // Check:
  termios port_settings_verif;
  if (tcgetattr(hCOM, &port_settings_verif) < 0)
    THROW_EXCEPTION_FMT("Cannot get the settings to verify: %s", strerror(errno));

#if 0
	if (!special_rate)
	{
		if (port_settings_verif.c_ispeed != port_settings.c_ispeed)
			THROW_EXCEPTION("Verification of changed baudrate(i) failed");
		if (port_settings_verif.c_ospeed != port_settings.c_ospeed)
			THROW_EXCEPTION("Verification of changed baudrate(i) failed");
	}

	if (port_settings_verif.c_cflag !=  port_settings.c_cflag)
		THROW_EXCEPTION("Verification of serial port flags failed");
#endif

  m_baudRate = baudRate;
#endif
  MRPT_END
}

void CSerialPort::setTimeouts(
    int ReadIntervalTimeout,
    int ReadTotalTimeoutMultiplier,
    int ReadTotalTimeoutConstant,
    int WriteTotalTimeoutMultiplier,
    int WriteTotalTimeoutConstant)
{
  MRPT_START
#ifdef _WIN32
  COMMTIMEOUTS timeouts;

  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The COM port is not open");

  // Config:
  timeouts.ReadIntervalTimeout = ReadIntervalTimeout;  // Milisegundos entre dos bytes recibidos
  timeouts.ReadTotalTimeoutMultiplier =
      ReadTotalTimeoutMultiplier;  // Milisegundos de espera por cada byte a
  // recibir
  timeouts.ReadTotalTimeoutConstant =
      ReadTotalTimeoutConstant;  // Milisegundos de espera en cada operacion
  // de recepcion
  timeouts.WriteTotalTimeoutMultiplier =
      WriteTotalTimeoutMultiplier;                                 // Timeout de escritura no usado
  timeouts.WriteTotalTimeoutConstant = WriteTotalTimeoutConstant;  // Timeout de escritura no usado

  if (!SetCommTimeouts(hCOM, &timeouts)) THROW_EXCEPTION("Error changing COM port timeout config");

// Success
#else
  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");

  // Save variables which are used in other methods:
  m_totalTimeout_ms = ReadTotalTimeoutConstant;
  m_interBytesTimeout_ms = ReadIntervalTimeout;

  // http://www.unixwiz.net/techtips/termios-vmin-vtime.html
  // VMIN & VTIME
  termios port_settings;
  if (tcgetattr(hCOM, &port_settings) < 0)
    THROW_EXCEPTION_FMT("Cannot get the current settings: %s", strerror(errno));

  // We set VMIN=0 and VTIME=ReadIntervalTimeout (in thenth of seconds)
  //
  port_settings.c_cc[VMIN] = 0;
  port_settings.c_cc[VTIME] = max(1, ReadTotalTimeoutConstant / 100);

  /* Write the new settings to the port.
   */
  if (tcsetattr(hCOM, TCSANOW, &port_settings) < 0)
    THROW_EXCEPTION_FMT("Cannot set the new settings: %s", strerror(errno));
#endif
  MRPT_END
}

/* -----------------------------------------------------
        Close
   ----------------------------------------------------- */
void CSerialPort::close()
{
  MRPT_START
#ifdef _WIN32
  if (hCOM) CloseHandle(hCOM);
  hCOM = nullptr;
#else
  if (hCOM < 0) return;  // Already closed

  //    PosixSignalDispatcher& signal_dispatcher =
  //    PosixSignalDispatcher::Instance() ;
  //    signal_dispatcher.DetachHandler( SIGIO, *this ) ;
  // Close the serial port file descriptor.
  ::close(hCOM);

  hCOM = -1;  // Means the port is closed
#endif
  MRPT_END
}

/* -----------------------------------------------------
        read
   ----------------------------------------------------- */
size_t CSerialPort::Read(void* Buffer, size_t Count)
{
  MRPT_START
#ifdef _WIN32
  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

  DWORD actuallyRead;

  if (!ReadFile(
          hCOM,           // Handle,
          Buffer,         // Buffer
          (DWORD)Count,   // Max expected bytes
          &actuallyRead,  // Actually read bytes
          nullptr))
    THROW_EXCEPTION("Error reading from port!");

  return actuallyRead;
#else

  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

  if (!Count) return 0;

  // Use the "m_totalTimeout_ms" global timeout
  //  and the "m_interBytesTimeout_ms" for inter-bytes:
  m_timer.Tic();

  size_t alreadyRead = 0;
  int leftTime = m_totalTimeout_ms - (int)(m_timer.Tac() * 1000);

  while (alreadyRead < Count && leftTime >= 0)
  {
    // Bytes waiting in the queue?
    // Check if we are still connected or there is an error...
    int waiting_bytes = 0;
    if (ioctl(hCOM, FIONREAD, &waiting_bytes) < 0)
    {
      if (errno == EIO)
      {
        // The port has been disconnect (for USB ports)
        this->close();
        return alreadyRead;
      }
    }

    // Are there any bytes??
    int nRead = 0;

    if (waiting_bytes > 0)
    {
      int nToRead = min((size_t)waiting_bytes, Count - alreadyRead);

      if ((nRead = ::read(hCOM, ((char*)Buffer) + alreadyRead, nToRead)) < 0)
      {
        cerr << "[CSerialPort] read() returned " << nRead << ", errno=" << errno << endl;
      }
      else
      {
        alreadyRead += nRead;
      }
    }
    else
    {
      // Nope...
    }

    if (alreadyRead < Count)
    {
      // Wait 1 more ms for new data to arrive.
      std::this_thread::sleep_for(1ms);
    }

    // Reset interbytes timer:
    leftTime = m_totalTimeout_ms - (int)(m_timer.Tac() * 1000);
    if (nRead > 0) leftTime = max(leftTime, m_interBytesTimeout_ms);
  }

  //    cout << "READ DONE: "<< alreadyRead << endl;
  return alreadyRead;
#endif
  MRPT_END
}

/** Reads one text line from the serial port in POSIX "canonical mode".
 *  This method reads from the serial port until one of the characters in \a
 * eol are found.
 */
std::string CSerialPort::ReadString(
    const int total_timeout_ms, bool* out_timeout, const char* eol_chars)
{
  MRPT_TRY_START
  // Calling ::ReadBuffer() many times would be even worse, so replicate its
  // code here:

  ASSERT_(eol_chars != nullptr);

  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

  if (out_timeout) *out_timeout = false;  // Will be set to true on timeout

  m_timer.Tic();
  string receivedStr;  // Rx buffer

  while (total_timeout_ms < 0 || (m_timer.Tac() * 1e3 < total_timeout_ms))
  {
#ifdef _WIN32
    // Read just 1 byte:
    char buf[1];

    DWORD actuallyRead;
    if (!ReadFile(
            hCOM,           // Handle,
            buf,            // Buffer
            1,              // Max expected bytes
            &actuallyRead,  // Actually read bytes
            nullptr))
      THROW_EXCEPTION("Error reading from port!");

    if (actuallyRead)
    {  // Append to string, if it's not a control char:
      if (!strchr(eol_chars, buf[0]))
        receivedStr.push_back(buf[0]);
      else
      {  // end of string!
        return receivedStr;
      }
    }
    // If we are still here, string is not finished:
    std::this_thread::sleep_for(1ms);  // Wait 1 more ms for new data to arrive.
#else
    // Bytes waiting in the queue?
    // Check if we are still connected or there is an error...
    int waiting_bytes = 0;
    if (ioctl(hCOM, FIONREAD, &waiting_bytes) < 0)
    {
      if (errno == EIO)
      {  // The port has been disconnect (for USB ports)
        this->close();
        THROW_EXCEPTION("Error reading port before end of line");
      }
    }

    // Are there any bytes??
    int nRead = 0;
    if (waiting_bytes > 0)
    {
      // Read just 1 byte:
      char buf[1];
      if ((nRead = ::read(hCOM, buf, 1)) < 0)
      {
        cerr << "[CSerialPort] Error reading from port..." << endl;
      }
      if (nRead)
      {  // Append to string, if it's not a control char:
        if (!strchr(eol_chars, buf[0]))
          receivedStr.push_back(buf[0]);
        else
        {  // end of string!
          return receivedStr;
        }
      }
    }
    else
    {
      // we decide to move the sleep here to satisfy realtime requirement
      // in the case where we are waiting a n-length string at a frequency
      // greater than 1/n...
      std::this_thread::sleep_for(1ms);  // Wait 1 more ms for new data to arrive.
    }
// If we are still here, string is not finished:
#endif
  }

  // Timeout:
  if (out_timeout) *out_timeout = true;
  return receivedStr;
  MRPT_TRY_END
}

size_t CSerialPort::Write(const void* Buffer, size_t Count)
{
  MRPT_START
  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

#ifdef _WIN32
  DWORD actuallyWritten;
  if (!WriteFile(hCOM, Buffer, (DWORD)Count, &actuallyWritten, nullptr))
    THROW_EXCEPTION("Error writing to port!");
  return actuallyWritten;
#else
  // Write the data to the serial port. Keep retrying if EAGAIN
  // error is received.

  /** \todo Add support for write timeout here */
  struct timeval start = {0, 0}, end = {0, 0};
  int num_of_bytes_written = -1;
  size_t total_bytes_written = 0;
  do
  {
    gettimeofday(&start, nullptr);
    num_of_bytes_written = write(
        hCOM, reinterpret_cast<const char*>(Buffer) + total_bytes_written,
        Count - total_bytes_written);
    // cout << "wr: " << num_of_bytes_written << " tot: " <<
    // total_bytes_written << " of " << Count << " err: " << errno << endl;
    if (num_of_bytes_written > 0) total_bytes_written += num_of_bytes_written;

    if (num_of_bytes_written < (int)Count)
    {
      // JL: These few lines are from the Player/Stage project:

      // need to do this sort of busy wait to ensure the right timing
      // although I've noticed you will get some anamolies that are
      // in the ms range; this could be a problem...
      long usecs;
      do
      {
        gettimeofday(&end, nullptr);
        usecs = (end.tv_sec - start.tv_sec) * 1000000 + (end.tv_usec - start.tv_usec);
      } while (usecs < 60);
      // std::this_thread::sleep_for(1ms);
      // we'll continue writing in 1 ms.
    }
  } while ((total_bytes_written < Count) && (!errno || EAGAIN == errno));
  //
  if (num_of_bytes_written < 0)  // This means we exit the loop due to a bad "errno".
    THROW_EXCEPTION_FMT("Error writing data to the serial port: %s", strerror(errno));

  // Make sure the queue is drained
  // Synchronous IO doesnt always work
  ::tcdrain(hCOM);

  // OK:
  return total_bytes_written;
#endif

  MRPT_END
}

void CSerialPort::purgeBuffers()
{
  MRPT_START

  // Port must be open!
  if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

#ifdef _WIN32
  if (!PurgeComm(hCOM, PURGE_RXABORT | PURGE_RXCLEAR | PURGE_TXABORT | PURGE_TXCLEAR))
    THROW_EXCEPTION("Error during COM port purge");
#else
  /* Flush the input buffer associated with the port. */
  if (tcflush(hCOM, TCIFLUSH) < 0)
    THROW_EXCEPTION_FMT("Cannot flush serial port: %s", strerror(errno));
#endif

  MRPT_END
}

uint64_t CSerialPort::Seek(
    [[maybe_unused]] int64_t Offset, [[maybe_unused]] CStream::TSeekOrigin Origin)
{
  MRPT_START
  THROW_EXCEPTION("Method not applicable to serial communications port CStream!");
  MRPT_END
}

uint64_t CSerialPort::getTotalBytesCount() const
{
  MRPT_START
  THROW_EXCEPTION("Method not applicable to serial communications port CStream!");
  MRPT_END
}

uint64_t CSerialPort::getPosition() const
{
  MRPT_START
  THROW_EXCEPTION("Method not applicable to serial communications port CStream!");
  MRPT_END
}
