/* +------------------------------------------------------------------------+
   |                     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/hwdrivers/CSICKTim561Eth_2050101.h>
#include <mrpt/system/string_utils.h>

#include <cstring>
#include <iostream>
#include <sstream>

#define APPERTURE 4.712385  // in radian <=> 270

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

// CODE
IMPLEMENTS_GENERIC_SENSOR(CSICKTim561Eth, mrpt::hwdrivers)

/** Default Lidar IP: 192.168.0.1
 *  Default IP_port: 2111
 *  Maximum Range: 10 Meters
 *  Default SensorPose: Depend on robot(0, 270, 105)mm, orientation(0, 21.25, 0)
 */
CSICKTim561Eth::CSICKTim561Eth(string _ip, unsigned int _port) :
    m_ip(_ip),
    m_port(_port),
    m_client(),
    m_cmd(),
    m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
    m_beamApperture(.25 * M_PI / 180.0)
{
  setVerbosityLevel(mrpt::system::LVL_DEBUG);
}

CSICKTim561Eth::~CSICKTim561Eth()
{
  if (m_connected) m_client.close();
}

void CSICKTim561Eth::initialize()
{
  if (!checkIsConnected())
  {
    THROW_EXCEPTION(
        "Cannot connect to SICK Tim561 Ethernet Sensor check your "
        "configuration"
        "file.");
  }
  turnOn();
}

void CSICKTim561Eth::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
  float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;

  pose_x = configSource.read_float(iniSection, "pose_x", 0, false);
  pose_y = configSource.read_float(iniSection, "pose_y", 0, false);
  pose_z = configSource.read_float(iniSection, "pose_z", 0, false);
  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, false);
  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, false);
  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, false);
  m_ip = configSource.read_string(iniSection, "ip_address", "192.168.0.1", false);
  m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
  m_process_rate = configSource.read_int(iniSection, string("process_rate"), 15, false);
  m_sensorLabel = configSource.read_string(iniSection, "sensorLabel", "SICK", false);
  m_sensorPose =
      CPose3D(pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch), DEG2RAD(pose_roll));
}

bool CSICKTim561Eth::checkIsConnected()
{
  if (m_connected)
  {
    return true;
  }
  else
  {
    try
    {
      m_client.connect(m_ip, m_port);
    }
    catch (const std::exception& e)
    {
      MRPT_LOG_ERROR_FMT("[SICKTIM561ETH] ERROR TRYING TO OPEN Ethernet DEVICE:\n%s", e.what());
      return false;
    }
  }
  m_connected = true;
  return true;
}

bool CSICKTim561Eth::rebootDev()
{
  {
    // Set Maintenance access mode to allow reboot to be sent
    char msg[] = {"sMN SetAccessMode 03 F4724744"};
    char msgIn[100];
    sendCommand(msg);

    size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
    msgIn[read - 1] = 0;
    MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
    MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

    if (!read)
    {
      MRPT_LOG_ERROR("SOPAS - Error setting access mode, unexpected response");
      return false;
    }
  }
  {
    // Send reboot command
    char msg[] = {"sMN mSCreboot"};
    char msgIn[100];
    sendCommand(msg);

    size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
    msgIn[read - 1] = 0;
    MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
    MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

    if (!read)
    {
      MRPT_LOG_ERROR("SOPAS - Error rebootting scanner, unexpected response.");
      return false;
    }
  }
  return true;
}

bool CSICKTim561Eth::turnOff()
{
  if (m_client.isConnected()) m_client.close();
  m_connected = false;
  m_turnedOn = false;
  return true;
}

bool CSICKTim561Eth::turnOn()
{
  /** From the SICK TIM561 datasheet:
   * * 1. Login: "sMN SetAccessMode 03 F4724744"
   * *    -wait answer: "sAN SetAccessMode 1"
   * * 2. Set Freq and Resolution: "sMN mLMPsetscancfg +1500 0 1800000"
   * *    -wait answer(0-180): "sAN mLMPsetscancfg 0 +1500 0 1800000"
   * * 3. Start measurement: "sMN LMCstartmeas"
   * *    -wait answer(0/1 yes/no): "sAN LMCstartmeas 0"
   * * 4. Stop measurement: "sMN LMCstopmeas"
   * *    -wait answer(0/1 yes/no): "sAN LMCstopmeas 0"
   *
   *
   * * z. Configure scandata context: "sWN LMDscandatacfg"
   * *    -wait answer
   *
   * * x. Read Freq and Angular:
   * *    -Get params: "sRN LMPscancfg"
   * *    -Read params(Freq, Sect, Resolution): "sRA LMPscancfg 1500 0
   * 1800000"
   */
  if (checkIsConnected())
  {
    try
    {
      /** Init scanner
       */
      {
        // Read 'DeviceIdent'
        char msg[] = {"sRIO"};
        char msgIn[100];
        sendCommand(msg);

        size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
        msgIn[read - 1] = 0;
        MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
        MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

        if (!read)
        {
          MRPT_LOG_ERROR("SOPAS - Error reading variable 'DeviceIdent'.");
          return false;
        }
      }

      {
        // Read 'SerialNumber'
        char msg[] = {"sRN SerialNumber"};
        char msgIn[100];
        sendCommand(msg);

        size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
        msgIn[read - 1] = 0;
        MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
        MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

        if (!read)
        {
          MRPT_LOG_ERROR("SOPAS - Error reading variable 'SerialNumber'.");
          return false;
        }
      }

      {
        // Read 'FirmwareVersion'
        char msg[] = {"sRN FirmwareVersion"};
        char msgIn[100];
        sendCommand(msg);

        size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
        msgIn[read - 1] = 0;
        MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
        MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

        if (!read)
        {
          MRPT_LOG_ERROR("SOPAS - Error reading variable 'FirmwareVersion'.");
          return false;
        }
      }

      {
        // Read 'Device state'
        char msg[] = {"sRN SCdevicestate"};
        char msgIn[100];
        sendCommand(msg);

        size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
        msgIn[read - 1] = 0;
        MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
        MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());

        if (!read)
        {
          MRPT_LOG_ERROR("SOPAS - Error reading variable 'devicestate'.");
          return false;
        }
      }

      /** End Init Scanner
       */
      // {
      //   /** Login  TIM561
      //    */
      //   char msg[] = {"sMN SetAccessMode 03 F4724744"};
      //   char msgIn[100];
      //   sendCommand(msg);

      //   size_t  read = m_client.readAsync(msgIn, 100, 1000, 1000);
      //   msgIn[read - 1] = 0;
      //   MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
      //   MRPT_LOG_DEBUG_FMT("message : %s\n",
      //   string(&msgIn[1]).c_str());

      //   if (!read )
      //   {
      //     return false;
      //   }
      // }
      // while(true);
      /** Set Freq and Resolution
       */
      // {
      //   char msg[] = {"sMN mLMPsetscancfg +1500 +1 0 1800000"};
      //   char msgIn[100];
      //   sendCommand(msg);

      //   size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
      //   msgIn[read - 1] = 0;
      //   MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
      //   MRPT_LOG_DEBUG_FMT("message : %s\n",
      //   string(&msgIn[1]).c_str());

      //    if (!read)
      //   {
      //     return false;
      //   }
      // }

      /** Set scandatacfg
       */
      // {
      //   char msg[] = {"sWN LMDscandatacfg 01 00 0 1 0 00 00 0 0 0 0
      //   +1"}; char msgIn[100]; sendCommand(msg);

      //   size_t read =m_client.readAsync(msgIn, 100, 1000, 1000);
      //   msgIn[read - 1] = 0;
      //   MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
      //   MRPT_LOG_DEBUG_FMT("message : %s\n",
      //   string(&msgIn[1]).c_str()); if (!read){
      //     return false;
      //   }
      // }

      /** Set send data permanently
       */
      {
        char msg[] = {"sEN LMDscandata 1"};
        char msgIn[100];
        sendCommand(msg);
        size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
        msgIn[read - 1] = 0;
        MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
        MRPT_LOG_DEBUG_FMT("message : %s\n", string(&msgIn[1]).c_str());
        if (!read)
        {
          MRPT_LOG_DEBUG("No LMSDATA");
          return false;
        }
      }

      // {
      //    /** Start measurement
      //    */
      //   char msg[] = {"sMN LMCstartmeas"};
      //   char msgIn[100];
      //   sendCommand(msg);
      //   size_t read = m_client.readAsync(msgIn, 100, 1000, 1000);
      //   msgIn[read - 1] = 0;
      //   MRPT_LOG_DEBUG_FMT("read : %u\n", (unsigned int)read);
      //   MRPT_LOG_DEBUG_FMT("message : %s\n",
      //   string(&msgIn[1]).c_str()); if (!read)
      //   {
      //     return false;
      //   }
      // }
      m_turnedOn = true;
    }
    catch (const std::exception& e)
    {
      MRPT_LOG_ERROR_FMT("%s", e.what());
      return false;
    }
  }
  else
  {
    return false;
  }
  return true;
}

void CSICKTim561Eth::sendCommand(const char* cmd)
{
  generateCmd(cmd);
  if (!m_cmd.empty())
  {
    m_client.writeAsync(&m_cmd[0], m_cmd.size());
  }
}

void CSICKTim561Eth::generateCmd(const char* cmd)
{
  if (strlen(cmd) > 995)
  {
    MRPT_LOG_ERROR("Error: command is too long.");
    return;
  }
  // m_cmd = format("%c%s%c", 0x02, cmd, 0x03);
  m_cmd = format("%c%s%c%c", 0x02, cmd, 0x03, 0);
}

bool CSICKTim561Eth::decodeScan(char* buff, CObservation2DRangeScan& outObservation)
{
  char* next;
  unsigned int idx = 0;
  unsigned int scanCount = 0;
  char* tmp;

  next = strtok(buff, " ", &tmp);

  while (next && scanCount == 0)
  {
    switch (++idx)
    {
      case 1:
        // If no "sRA" and also no "sSN", return false reading
        if (strncmp(&next[1], "sRA", 3) && strncmp(&next[1], "sSN", 3))
        {
          return false;
        }
        break;
      case 2:
        if (strcmp(next, "LMDscandata"))
        {
          return false;
        }
        break;
      case 6:
        if (strcmp(next, "1"))
        {
          MRPT_LOG_DEBUG("Laser is ready");
        }
        else if (strcmp(next, "0"))
        {
          MRPT_LOG_DEBUG("Laser is busy");
        }
        else
        {
          MRPT_LOG_DEBUG("Laser reports error");
          rebootDev();
        }
        break;
      case 21:
        if (strcmp(next, "DIST1"))
        {
          THROW_EXCEPTION("TIM561 is not configured to send distances");
          return false;
        }
        MRPT_LOG_DEBUG("Distance : OK\n");
        break;
      case 26:
        scanCount = strtoul(next, nullptr, 16);
        MRPT_LOG_DEBUG_FMT("Scan Count : %d\n", scanCount);
        break;
      default:
        break;
    }
    next = strtok(nullptr, " ", &tmp);
  }
  outObservation.aperture = (float)APPERTURE;
  outObservation.rightToLeft = false;
  outObservation.stdError = 0.012f;
  outObservation.sensorPose = m_sensorPose;
  outObservation.beamAperture = m_beamApperture;
  outObservation.maxRange = m_maxRange;
  outObservation.timestamp = mrpt::Clock::now();
  outObservation.sensorLabel = m_sensorLabel;

  outObservation.resizeScan(scanCount);
  unsigned int i;
  for (i = 0; i < scanCount && next; i++, next = strtok(nullptr, " ", &tmp))
  {
    outObservation.setScanRange(i, double(strtoul(next, nullptr, 16)) / 1000.0);
    outObservation.setScanRangeValidity(
        i, outObservation.getScanRange(i) <= outObservation.maxRange);
  }
  outObservation.resizeScan(i);
  return i >= scanCount;
}

void CSICKTim561Eth::doProcessSimple(
    bool& outThereIsObservation, CObservation2DRangeScan& outObservation, bool& hardwareError)
{
  if (!m_turnedOn)
  {
    hardwareError = true;
    outThereIsObservation = false;
    return;
  }
  hardwareError = false;

  char msg[] = {"sRN LMDscandata"};
  sendCommand(msg);
  char buffIn[16 * 1024];

  m_client.readAsync(buffIn, sizeof(buffIn), 40, 40);

  if (decodeScan(buffIn, outObservation))
  {
    // Filter:
    C2DRangeFinderAbstract::filterByExclusionAreas(outObservation);
    C2DRangeFinderAbstract::filterByExclusionAngles(outObservation);
    // Do show preview:
    C2DRangeFinderAbstract::processPreview(outObservation);
    MRPT_LOG_DEBUG("doProcessSimple Show");

    outThereIsObservation = true;
    hardwareError = false;
  }
  else
  {
    hardwareError = true;
    outThereIsObservation = false;
    MRPT_LOG_ERROR("doProcessSimple failed\n");
  }
}

void CSICKTim561Eth::doProcess()
{
  CObservation2DRangeScan::Ptr obs = std::make_shared<CObservation2DRangeScan>();
  try
  {
    bool isThereObservation, hwError;
    doProcessSimple(isThereObservation, *obs, hwError);
    if (hwError)
    {
      m_state = ssError;
      MRPT_LOG_DEBUG("state Error");
    }
    else
    {
      m_state = ssWorking;
      MRPT_LOG_DEBUG("state working");
    }
    // if at least one data have been sensed:
    if (isThereObservation)
    {
      appendObservation(obs);
    }
  }
  catch (...)
  {
    m_state = ssError;
    THROW_EXCEPTION("No observation received from the Phidget board!");
  }
}

/** A method to set the sensor pose on the robot.
 */
void CSICKTim561Eth::setSensorPose(const CPose3D& _pose) { m_sensorPose = _pose; }
