/* +------------------------------------------------------------------------+
   |                     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/CClientTCPSocket.h>
#include <mrpt/hwdrivers/CGPSInterface.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <chrono>
#include <iostream>
#include <list>
#include <mutex>
#include <thread>

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

IMPLEMENTS_GENERIC_SENSOR(CGPSInterface, mrpt::hwdrivers)

struct TParsersRegistry
{
  std::list<CGPSInterface::ptr_parser_t> all_parsers;

  static const TParsersRegistry& getInstance()
  {
    static TParsersRegistry reg;
    return reg;
  }

 private:
  TParsersRegistry()
  {
    all_parsers.push_back(&CGPSInterface::implement_parser_NMEA);
    all_parsers.push_back(&CGPSInterface::implement_parser_NOVATEL_OEM6);
  }
};

/* -----------------------------------------------------
        Constructor
   ----------------------------------------------------- */
CGPSInterface::CGPSInterface() :
    mrpt::system::COutputLogger("CGPSInterface"),
    m_customInit(),
    m_rx_buffer(0x10000),
    m_raw_dump_file_prefix(),
    m_COMname(),
    m_last_timestamp(INVALID_TIMESTAMP),
    m_JAVAD_rtk_src_port(),
    m_JAVAD_rtk_format("cmr")
{
  m_sensorLabel = "GPS";
}

/* -----------------------------------------------------
        loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
  m_parser = configSource.read_enum<CGPSInterface::PARSERS>(
      iniSection, "parser", m_parser, false /*Allow default values*/);
  m_raw_dump_file_prefix = configSource.read_string(
      iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix, false /*Allow default values*/);

#ifdef _WIN32
  m_COMname = configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true);
#else
  m_COMname = configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true);
#endif

  m_COMbauds = configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
  m_sensorLabelAppendMsgType = configSource.read_bool(
      iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);

  // legacy custom cmds:
  m_customInit = configSource.read_string(iniSection, "customInit", m_customInit, false);

  // new custom cmds:
  m_custom_cmds_delay =
      configSource.read_float(iniSection, "custom_cmds_delay", m_custom_cmds_delay);
  m_custom_cmds_append_CRLF =
      configSource.read_bool(iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
  // Load as many strings as found on the way:
  m_setup_cmds.clear();
  for (int i = 1; true; i++)
  {
    std::string sLine =
        configSource.read_string(iniSection, mrpt::format("setup_cmd%i", i), std::string());
    sLine = mrpt::system::trim(sLine);
    if (sLine.empty()) break;
    m_setup_cmds.push_back(sLine);
  }

  m_shutdown_cmds.clear();
  for (int i = 1; true; i++)
  {
    std::string sLine =
        configSource.read_string(iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
    sLine = mrpt::system::trim(sLine);
    if (sLine.empty()) break;
    m_shutdown_cmds.push_back(sLine);
  }

  m_sensorPose.setFromValues(
      configSource.read_float(iniSection, "pose_x", 0),
      configSource.read_float(iniSection, "pose_y", 0),
      configSource.read_float(iniSection, "pose_z", 0),
      DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
      DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
      DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));

  m_JAVAD_rtk_src_port =
      configSource.read_string(iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
  m_JAVAD_rtk_src_baud =
      configSource.read_int(iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
  m_JAVAD_rtk_format = configSource.read_string(iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);

  m_topcon_useAIMMode = configSource.read_bool(iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
  m_topcon_data_period =
      1.0 / configSource.read_double(iniSection, "outputRate", m_topcon_data_period);
}

CGPSInterface::~CGPSInterface() { OnConnectionShutdown(); }

void CGPSInterface::setParser(CGPSInterface::PARSERS parser) { m_parser = parser; }
CGPSInterface::PARSERS CGPSInterface::getParser() const { return m_parser; }
void CGPSInterface::bindStream(
    const std::shared_ptr<mrpt::io::CStream>& external_stream,
    const std::shared_ptr<std::mutex>& csOptionalExternalStream)
{
  m_data_stream_is_external = true;
  m_data_stream = external_stream;
  m_data_stream_cs = csOptionalExternalStream ? csOptionalExternalStream : m_data_stream_mine_cs;
}
void CGPSInterface::setSetupCommandsDelay(const double delay_secs)
{
  m_custom_cmds_delay = delay_secs;
}
double CGPSInterface::getSetupCommandsDelay() const { return m_custom_cmds_delay; }
void CGPSInterface::setSetupCommands(const std::vector<std::string>& cmds) { m_setup_cmds = cmds; }
const std::vector<std::string>& CGPSInterface::getSetupCommands() const { return m_setup_cmds; }
void CGPSInterface::setShutdownCommands(const std::vector<std::string>& cmds)
{
  m_shutdown_cmds = cmds;
}
const std::vector<std::string>& CGPSInterface::getShutdownCommands() const
{
  return m_shutdown_cmds;
}
void CGPSInterface::enableSetupCommandsAppendCRLF(const bool enable)
{
  m_custom_cmds_append_CRLF = enable;
}
bool CGPSInterface::isEnabledSetupCommandsAppendCRLF() const { return m_custom_cmds_append_CRLF; }

/* -----------------------------------------------------
        setSerialPortName
----------------------------------------------------- */
void CGPSInterface::setSerialPortName(const std::string& COM_port)
{
  // Dont allow changing the serial port if:
  if (m_data_stream_is_external)
    THROW_EXCEPTION(
        "Cannot change serial port name: an external stream has been "
        "already bound manually.");

  if (m_data_stream)
  {
    std::lock_guard<std::mutex> lock(*m_data_stream_cs);
    auto serial = dynamic_cast<mrpt::comms::CSerialPort*>(m_data_stream.get());
    if (serial && serial->isOpen())
      THROW_EXCEPTION("Cannot change serial port name when it is already open");
  }

  // OK:
  m_COMname = COM_port;
}

/* -----------------------------------------------------
        getSerialPortName
----------------------------------------------------- */
std::string CGPSInterface::getSerialPortName() const { return m_COMname; }
/* -----------------------------------------------------
        tryToOpenTheCOM
----------------------------------------------------- */
bool CGPSInterface::tryToOpenTheCOM()
{
  // If this is the first use of the COM port, create it:
  if (!m_data_stream)
  {
    m_data_stream = std::make_shared<mrpt::comms::CSerialPort>();
    m_data_stream_is_external = false;
    m_data_stream_cs = m_data_stream_mine_cs;
  }

  auto serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
  if (serial)
  {
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      if (serial->isOpen()) return true;  // Already open

      if (m_verbose) cout << "[CGPSInterface] Opening " << m_COMname << " @ " << m_COMbauds << endl;
    }
    try
    {
      serial->open(m_COMname);
      // Config:
      serial->setConfig(m_COMbauds, 0, 8, 1);
      serial->setTimeouts(1, 0, 1, 1, 1);

      // Do extra initialization?
      if (!OnConnectionEstablished())
      {
        serial->close();
        return false;
      }
      return true;  // All OK
    }
    catch (const std::exception& e)
    {
      std::cerr << "[CGPSInterface::tryToOpenTheCOM] Error opening or "
                   "configuring serial port:"
                << std::endl
                << e.what();
      serial->close();
      return false;
    }
  }  // end of this is a serial port

  return true;  // All OK
}

/* -----------------------------------------------------
        isGPS_connected
----------------------------------------------------- */
bool CGPSInterface::isGPS_connected() { return m_GPS_comsWork; }
/* -----------------------------------------------------
        doProcess
----------------------------------------------------- */
void CGPSInterface::doProcess()
{
  // Is the COM open?
  if (!tryToOpenTheCOM())
  {
    m_state = ssError;
    THROW_EXCEPTION("Could not open the input stream");
  }
  ASSERT_(m_data_stream != nullptr);
  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
  auto* stream_tcpip = dynamic_cast<CClientTCPSocket*>(m_data_stream.get());

  // Read as many bytes as available:
  uint8_t buf[0x1000];
  const size_t to_read = std::min(m_rx_buffer.available() - 1, sizeof(buf) - 1);
  try
  {
    size_t nRead = 0;
    if (to_read > 0)
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      if (stream_tcpip)
      {
        nRead = stream_tcpip->readAsync(buf, to_read, 100, 10);
      }
      else if (stream_serial)
      {
        nRead = stream_serial->Read(buf, to_read);
      }
      else
      {
        nRead = m_data_stream->Read(buf, to_read);
      }
    }

    if (nRead) m_rx_buffer.push_many(buf, nRead);

    // Also dump to raw file:
    if (!m_raw_dump_file_prefix.empty() && !m_raw_output_file.fileOpenCorrectly())
    {
      // 1st time open:
      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_raw_dump_file_prefix +
                               mrpt::system::fileNameStripInvalidChars(sFilePostfix) +
                               string(".gps");

      if (m_verbose) std::cout << "[CGPSInterface] Creating RAW dump file: `" << sFileName << "`\n";
      m_raw_output_file.open(sFileName, mrpt::io::OpenMode::TRUNCATE);
    }
    if (nRead && m_raw_output_file.fileOpenCorrectly())
    {
      m_raw_output_file.Write(buf, nRead);
    }
  }
  catch (std::exception&)
  {
    // ERROR:
    MRPT_LOG_ERROR(
        "[CGPSInterface::doProcess] Error reading stream of data: Closing "
        "communications\n");
    if (stream_serial)
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      stream_serial->close();
    }
    m_GPS_comsWork = false;
    return;
  }

  // Try to parse incoming data as messages:
  parseBuffer();

  // Decide whether to push out a new observation in old legacy mode.
  if (!m_customInit.empty())
  {  // "Advanced" (RTK,mmGPS) device  (kept for
     // backwards-compatibility)
    bool do_append_obs = false;
    // FAMD
    // Append observation if:
    // 0. the timestamp seems to be correct!
    // 1. it contains both synched GGA and RMC data
    // 2. it contains only GGA or RMC but the next one is not synched with
    // it
    if (m_last_timestamp == INVALID_TIMESTAMP)
    {
      if (m_verbose)
        cout << "[CGPSInterface] Initial timestamp: "
             << mrpt::system::timeToString(m_parsed_messages->timestamp) << endl;
      // Check if the initial timestamp seems to be OK (not a spurio one)
      TTimeStamp tmNow = mrpt::Clock::now();
      const double tdif = mrpt::system::timeDifference(m_parsed_messages->timestamp, tmNow);
      if (tdif >= 0 && tdif < 7500 /*Up to two hours*/)
        m_last_timestamp = m_parsed_messages->timestamp;
      else
      {
        if (m_verbose)
          cout << "[CGPSInterface] Warning: The initial timestamp "
                  "seems to be wrong! : "
               << tdif << endl;
      }
    }  // end-if
    else
    {
      const double time_diff =
          mrpt::system::timeDifference(m_last_timestamp, m_parsed_messages->timestamp);
      if (time_diff < 0 || time_diff > 300)  // Assert that the current
      // timestamp is after the
      // previous one and not more
      // than 5 minutes later ->
      // remove spurious
      {
        if (m_verbose) cout << "[CGPSInterface ] Bad timestamp difference" << endl;
        return;
      }

      if (time_diff - m_topcon_data_period > 0.25 * m_topcon_data_period)
      {
        if (m_verbose)
          cout << "[CGPSInterface] WARNING: According to the "
                  "timestamps, we probably skipped one frame!"
               << endl;
      }

      // a. These GPS data have both synched RMC and GGA data
      // don't append observation until we have both data
      do_append_obs = (m_parsed_messages->has_GGA_datum() && m_parsed_messages->has_RMC_datum());
    }  // end-else

    if (do_append_obs) flushParsedMessagesNow();
  }
}

/** Queue out now the messages in \a m_just_parsed_messages, leaving it empty */

void CGPSInterface::flushParsedMessagesNow()
{
  // Generic observation data:
  m_parsed_messages->sensorPose = m_sensorPose;
  if (m_sensorLabelAppendMsgType)
    m_parsed_messages->sensorLabel = m_sensorLabel + string("_") + m_parsed_messages->sensorLabel;
  else
    m_parsed_messages->sensorLabel = m_sensorLabel;

  m_last_timestamp = m_parsed_messages->timestamp;

  // Add observation to the output queue:
  CGenericSensor::appendObservation(m_parsed_messages);

  m_parsed_messages = mrpt::obs::CObservationGPS::Create();

  // And this means the comms works:
  m_GPS_comsWork = true;
  m_state = ssWorking;
}

/* -----------------------------------------------------
          parseBuffer
----------------------------------------------------- */
void CGPSInterface::parseBuffer()
{
  if (m_parser == CGPSInterface::NONE) return;  // Dont try to parse data

  // Only one parser selected?
  ptr_parser_t parser_ptr = nullptr;
  switch (m_parser)
  {
    case CGPSInterface::NMEA:
      parser_ptr = &CGPSInterface::implement_parser_NMEA;
      break;
    case CGPSInterface::NOVATEL_OEM6:
      parser_ptr = &CGPSInterface::implement_parser_NOVATEL_OEM6;
      break;
    case CGPSInterface::AUTO:
      break;  // Leave it as NULL
    default:
      throw std::runtime_error("[CGPSInterface] Unknown parser!");
  };
  if (parser_ptr)
  {
    // Use only one parser ----------
    size_t min_bytes;
    do
    {
      if (!(*this.*parser_ptr)(min_bytes))
      {
        if (m_rx_buffer.size() != 0) m_rx_buffer.pop();  // Not the start of a frame, skip 1 byte
      }
      if (m_customInit.empty() /* If we are not in old legacy mode */ &&
          !m_parsed_messages->messages.empty())
        flushParsedMessagesNow();
    } while (m_rx_buffer.size() >= min_bytes);
  }  // end one parser mode ----------
  else
  {
    // AUTO mode --------
    const std::list<CGPSInterface::ptr_parser_t>& all_parsers =
        TParsersRegistry::getInstance().all_parsers;

    size_t global_min_bytes_max = 0;
    do
    {
      bool all_parsers_want_to_skip = true;
      for (auto all_parser : all_parsers)
      {
        parser_ptr = all_parser;
        size_t this_parser_min_bytes;
        if ((*this.*parser_ptr)(this_parser_min_bytes)) all_parsers_want_to_skip = false;
        mrpt::keep_max(global_min_bytes_max, this_parser_min_bytes);
      }

      if (all_parsers_want_to_skip && m_rx_buffer.size() != 0)
        m_rx_buffer.pop();  // Not the start of a frame, skip 1 byte

      if (m_customInit.empty() /* If we are not in old legacy mode */ &&
          !m_parsed_messages->messages.empty())
        flushParsedMessagesNow();

    } while (m_rx_buffer.size() >= global_min_bytes_max);
  }  // end AUTO mode ----
}

/* -----------------------------------------------------
          JAVAD_sendMessage
----------------------------------------------------- */
void CGPSInterface::JAVAD_sendMessage(const char* str, bool waitForAnswer)
{
  if (!str) return;
  const size_t len = strlen(str);
  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
  if (!stream_serial) return;

  size_t written;

  {
    std::lock_guard<std::mutex> lock(*m_data_stream_cs);
    written = stream_serial->Write(str, len);
  }

  if (m_verbose) std::cout << "[CGPSInterface] TX: " << str;

  if (written != len) throw std::runtime_error(format("Error sending command: '%s'", str).c_str());
  std::this_thread::sleep_for(5ms);

  if (!waitForAnswer) return;

  std::this_thread::sleep_for(200ms);
  char buf[200];
  buf[0] = '\0';

  int bad_counter = 0;
  while (bad_counter < 10)
  {
    size_t nRead;
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      written = stream_serial->Write(str, len);
      nRead = stream_serial->Read(buf, sizeof(buf));
    }

    if (m_verbose) std::cout << "[CGPSInterface] RX: " << buf << std::endl;

    if (nRead < 3)
      throw std::runtime_error(format("ERROR: Invalid response '%s' for command '%s'", buf, str));

    if (nRead >= 3 && buf[0] == 'R' && buf[1] == 'E')
      return;  // Ok!
    else
      ++bad_counter;
  }
  throw std::runtime_error(format("ERROR: Invalid response '%s' for command '%s'", buf, str));
}

bool CGPSInterface::OnConnectionShutdown()
{
  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());

  if (stream_serial && !stream_serial->isOpen()) return false;

  // Send commands:
  for (const auto& m_shutdown_cmd : m_shutdown_cmds)
  {
    if (m_verbose) cout << "[CGPSInterface] TX shutdown command: `" << m_shutdown_cmd << "`\n";

    std::string sTx = m_shutdown_cmd;
    if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");
    try
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      m_data_stream->Write(&sTx[0], sTx.size());
    }
    catch (...)
    {
      return false;  // On any I/O error
    }

    std::this_thread::sleep_for(
        std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
  }
  return true;
}

/* -----------------------------------------------------
          OnConnectionEstablished
----------------------------------------------------- */
bool CGPSInterface::OnConnectionEstablished()
{
  m_last_GGA.clear();  // On comms reset, empty this cache
  m_parsed_messages->clear();

  // Legacy behavior:
  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") || !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
  {
    return legacy_topcon_setup_commands();
  }

  // Purge input:
  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
  if (stream_serial)
  {
    std::lock_guard<std::mutex> lock(*m_data_stream_cs);
    stream_serial->purgeBuffers();
  }

  // New behavior: Send generic commands set-up by the user in the config
  // file.

  // Send commands:
  for (const auto& m_setup_cmd : m_setup_cmds)
  {
    if (m_verbose) cout << "[CGPSInterface] TX setup command: `" << m_setup_cmd << "`\n";

    std::string sTx = m_setup_cmd;
    if (m_custom_cmds_append_CRLF) sTx += std::string("\r\n");

    try
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      m_data_stream->Write(&sTx[0], sTx.size());
    }
    catch (const std::exception& e)
    {
      std::cerr << "[CGPSInterface::OnConnectionEstablished] Error "
                   "sending setup cmds: "
                << e.what() << std::endl;
      return false;
    }
    std::this_thread::sleep_for(
        std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
  }
  std::this_thread::sleep_for(
      std::chrono::duration<double, std::milli>(m_custom_cmds_delay * 1000));
  return true;
}

bool CGPSInterface::unsetJAVAD_AIM_mode()
{
  MRPT_START
  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") || !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
  {
    // Stop messaging:
    JAVAD_sendMessage("%%dm\r\n", false);
    std::this_thread::sleep_for(500ms);
    JAVAD_sendMessage("%%dm\r\n", false);
    std::this_thread::sleep_for(1000ms);

    // Purge input:
    auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
    if (stream_serial)
    {
      std::lock_guard<std::mutex> lock(*m_data_stream_cs);
      stream_serial->purgeBuffers();
    }

    JAVAD_sendMessage("%%set,/par/cur/term/imode,cmd\r\n");  // set the
    // current port
    // in command
    // mode
    return true;
  }
  else
    return true;
  MRPT_END
}  // end-unsetJAVAD_AIM_mode

bool CGPSInterface::setJAVAD_AIM_mode()
{
  MRPT_START
  if (!os::_strcmpi(m_customInit.c_str(), "JAVAD") || !os::_strcmpi(m_customInit.c_str(), "TOPCON"))
  {
    JAVAD_sendMessage(format("%%%%set,/par%s/imode,cmd\r\n", m_JAVAD_rtk_src_port.c_str())
                          .c_str());  // set the port in command mode
    JAVAD_sendMessage("%%set,/par/cur/term/jps/0,{nscmd,37,n,\"\"}\r\n");  // any command
    // starting
    // with % will
    // be treated
    // as normal

    ASSERT_(!m_JAVAD_rtk_format.empty());
    cout << "Formato de correcciones para GR3: " << m_JAVAD_rtk_format << endl;
    if (m_JAVAD_rtk_format == "cmr")
    {
      JAVAD_sendMessage(format(
                            "%%%%set,/par/cur/term/jps/1,{cmr,-1,y,%s}\r\n",
                            m_JAVAD_rtk_src_port.c_str())
                            .c_str());  // set corrections type CMR or CMR+
      JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
      JAVAD_sendMessage(
          format("%%%%set,/par%s/imode,cmr\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
    }
    else if (m_JAVAD_rtk_format == "rtcm")
    {
      JAVAD_sendMessage(format(
                            "%%%%set,/par/cur/term/jps/1,{rtcm,-1,y,%s}\r\n",
                            m_JAVAD_rtk_src_port.c_str())
                            .c_str());  // set corrections type RTCM
      JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
      JAVAD_sendMessage(
          format("%%%%set,/par%s/imode,rtcm\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
    }
    else if (m_JAVAD_rtk_format == "rtcm3")
    {
      JAVAD_sendMessage(format(
                            "%%%%set,/par/cur/term/jps/1,{rtcm3,-1,y,%s}\r\n",
                            m_JAVAD_rtk_src_port.c_str())
                            .c_str());  // set corrections type RTCM 3.x
      JAVAD_sendMessage("%%set,/par/cur/term/jps/2,{none,-1,n,\"\"}\r\n");
      JAVAD_sendMessage(
          format("%%%%set,/par%s/imode,rtcm3\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());
    }
    else
    {
      cout << "Unknown RTK corrections format. Only supported: CMR, RTCM "
              "or RTCM3"
           << endl;
      return false;
    }
    JAVAD_sendMessage("%%set,/par/cur/term/imode,jps\r\n");  // sets current
    // port into
    // "JPS" mode

    return true;

  }  // end-if
  else
    return true;
  MRPT_END
}  // end-setJAVAD_AIM_mode

std::string CGPSInterface::getLastGGA(bool reset)
{
  std::string ret = m_last_GGA;
  if (reset) m_last_GGA.clear();
  return ret;
}

bool CGPSInterface::legacy_topcon_setup_commands()
{
  // Stop messaging:
  JAVAD_sendMessage("%%dm\r\n", false);
  std::this_thread::sleep_for(500ms);
  JAVAD_sendMessage("%%dm\r\n", false);
  std::this_thread::sleep_for(1000ms);

  // Purge input:
  auto* stream_serial = dynamic_cast<CSerialPort*>(m_data_stream.get());
  if (stream_serial)
  {
    std::lock_guard<std::mutex> lock(*m_data_stream_cs);
    stream_serial->purgeBuffers();
  }

  // Configure RTK mode and source:
  if (m_verbose) cout << "[CGPSInterface] Configure RTK options" << endl;

  if (!m_JAVAD_rtk_src_port.empty())
  {
    const int elevation_mask = 5;  // Degs

    JAVAD_sendMessage(format("%%%%set,/par/lock/elm,%i\r\n", elevation_mask)
                          .c_str());                     // Set elevation mask to track satellites
    JAVAD_sendMessage("%%set,/par/base/mode/,off\r\n");  // Set Base Mode off
    JAVAD_sendMessage("%%set,/par/pos/pd/period,1.0\r\n");  // Differential
    // Correction
    // Interval
    // JAVAD_sendMessage("%%set,hd/mode,off\r\n");  // fixed distance to rtk
    // base: Off
    // JAVAD_sendMessage("%%set,/par/pos/pd/hd/mode,off\r\n");  // fixed
    // distance to rtk base: Off  <-- Not working with TopCon GR3! (option
    // disabled)
    JAVAD_sendMessage("%%set,/par/pos/pd/qcheck,off\r\n");  // Set Quality Checks Off
    JAVAD_sendMessage("%%set,/par/pos/mode/cur,pd\r\n");    // Pos Mode Phase Diff
    JAVAD_sendMessage("%%set,/par/pos/pd/textr,10\r\n");    // RTK Extrapolation Limit
    JAVAD_sendMessage("%%set,/par/pos/pd/inuse,-1\r\n");    // Set Rovers Reference Station
    JAVAD_sendMessage("%%set,/par/pos/pd/nrs/mode,y\r\n");  // Enable
    // Nearest
    // Reference
    // Station Mode
    JAVAD_sendMessage("%%set,/par/pos/pd/mode,extrap\r\n");  // Enable
    // EXTRAPOLATED
    // mode in RTK
    // corrections

    // Set Differential Correction Source
    JAVAD_sendMessage(
        format("%%%%set,/par/pos/pd/port,%s\r\n", m_JAVAD_rtk_src_port.c_str()).c_str());

    // Set port bauds:
    if (!m_topcon_useAIMMode && m_JAVAD_rtk_src_baud != 0 &&
        !mrpt::system::strCmp(m_JAVAD_rtk_src_port, "/dev/usb/a"))
      JAVAD_sendMessage(
          format("%%%%set,/par%s/rate,%u\r\n", m_JAVAD_rtk_src_port.c_str(), m_JAVAD_rtk_src_baud)
              .c_str());

    // Set Input Mode: CMR,RTCM,...
    if (!m_topcon_useAIMMode && !m_JAVAD_rtk_format.empty())
      JAVAD_sendMessage(format(
                            "%%%%set,/par%s/imode,%s\r\n", m_JAVAD_rtk_src_port.c_str(),
                            m_JAVAD_rtk_format.c_str())
                            .c_str());
  }

  // Start NMEA messaging:
  //		JAVAD_sendMessage("%%em,,/msg/nmea/GGA:0.2\r\n");
  //		JAVAD_sendMessage("%%em,,/msg/nmea/RMC:0.2\r\n");
  // JAVAD_sendMessage("%%em,,/msg/jps/PS:0.2\r\n");

  if (m_topcon_useAIMMode)
  {
    if (m_verbose) cout << "[CGPSInterface] Using Advanced Input Mode";
    m_topcon_AIMConfigured = setJAVAD_AIM_mode();
    if (m_verbose) cout << "... done" << endl;
  }
  JAVAD_sendMessage(format("%%%%em,,/msg/nmea/GGA:%.1f\r\n", m_topcon_data_period).c_str());
  JAVAD_sendMessage(
      format("%%%%em,,/msg/nmea/RMC:%.1f\r\n", m_topcon_data_period).c_str());  // FAMD: 10 Hz

  if (m_topcon_useAIMMode)
  {
    if (m_verbose)
      cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
              "commands sent successfully with AIM."
           << endl;
  }
  else
  {
    if (m_verbose)
      cout << "[CGPSInterface::OnConnectionEstablished] JAVAD/TopCon "
              "commands sent successfully."
           << endl;
  }

  return true;
}

/** Send a custom data block to the GNSS device right now. Can be used to change
 * its behavior online as needed. */
bool CGPSInterface::sendCustomCommand(const void* data, size_t datalen)
{
  try
  {
    std::lock_guard<std::mutex> lock(*m_data_stream_cs);
    m_data_stream->Write(data, datalen);
    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CGPSInterface::sendCustomCommand] Error sending cmd: " << e.what() << std::endl;
    return false;
  }
}
