/* +------------------------------------------------------------------------+
   |                     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/CIbeoLuxETH.h>  // Precompiled headers

#include <bitset>
#include <chrono>
#include <thread>

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

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

// TODO: Use enum instead
const unsigned char SearchForAF = 0;
const unsigned char SearchForFE = 1;
const unsigned char SearchForC0 = 2;
const unsigned char SearchForC2 = 3;
const unsigned char PacketFound = 4;
const unsigned char SaveData = 5;

IMPLEMENTS_GENERIC_SENSOR(CIbeoLuxETH, mrpt::hwdrivers)

CIbeoLuxETH::CIbeoLuxETH(string _ip, unsigned int _port) :
    m_ip(_ip),
    m_port(_port),
    m_sensorPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
    m_beamApperture(.25 * M_PI / 180.0)
{
}

CIbeoLuxETH::~CIbeoLuxETH()
{
  m_run = false;
  dataCollectionThread.join();
  // Wait a little for the thread to come down
  // Don't ask why, it just works
  // TODO: Try without the delay
  std::this_thread::sleep_for(10ms);
}

void CIbeoLuxETH::dataCollection()
{
  unsigned char state = SearchForAF;
  unsigned char msgIn[1], Header[20], ScanListHeader[44], ScanPointData[10];
  unsigned int datatype, /*scannumber,*/ numScanpoints, angleTicks, SPlayer,
      SPdistance;  // SPecho;
  int SPHangle;
  unsigned char msg[32];

  // Start TCP-connection to laserscanner
  m_client.connect(m_ip, m_port);

  // Send filter command
  makeCommandHeader(msg);
  makeTypeCommand(msg);
  m_client.writeAsync(&msg[0], 32);

  // Send start command
  makeCommandHeader(msg);
  makeStartCommand(msg);
  m_client.writeAsync(&msg[0], 28);

  while (m_run)
  {
    switch (state)
    {
      case SearchForAF:
        m_client.readAsync(msgIn, 1, 100, 10);
        if (msgIn[0] == 0xAF) state = SearchForFE;
        break;
      case SearchForFE:
        m_client.readAsync(msgIn, 1, 100, 10);
        if (msgIn[0] == 0xFE)
          state = SearchForC0;
        else
          state = SearchForAF;
        break;
      case SearchForC0:
        m_client.readAsync(msgIn, 1, 100, 10);
        if (msgIn[0] == 0xC0)
          state = SearchForC2;
        else
          state = SearchForAF;
        break;
      case SearchForC2:
        m_client.readAsync(msgIn, 1, 100, 10);
        if (msgIn[0] == 0xC2)
          state = PacketFound;
        else
          state = SearchForAF;
        break;
      case PacketFound:
        m_client.readAsync(Header, 20, 100, 10);
        datatype = Header[10] * 0x100 + Header[11];
        switch (datatype)
        {
          case 0x2030:
            // do nothing
            state = SearchForAF;
            break;
          case 0x2221:
            // do nothing
            state = SearchForAF;
            break;
          case 0x2805:
            // do nothing
            state = SearchForAF;
            break;
          case 0x2020:
            // do nothing
            state = SearchForAF;
            break;
          case 0x2202:
            state = SaveData;
            break;
          default:
            std::cerr << "UNKNOWN packet of type " << hex << datatype << " received!!\n";
            state = SearchForAF;
        }
        break;
      case SaveData:
        // Create new observation object pointer
        CObservation3DRangeScan::Ptr newObs = std::make_shared<CObservation3DRangeScan>();
        newObs->hasPoints3D = true;
        newObs->maxRange = 200.00;

        m_client.readAsync(ScanListHeader, 44, 10, 10);
        /*scannumber =
        ScanListHeader[1] * 0x100 + ScanListHeader[0]; */
        numScanpoints = ScanListHeader[29] * 0x100 + ScanListHeader[28];
        angleTicks = ScanListHeader[23] * 0x100 + ScanListHeader[22];

        for (unsigned int i = 0; i < numScanpoints; ++i)
        {
          bool dropPacket = false;

          m_client.readAsync(ScanPointData, 10, 10, 10);
          SPlayer = ScanPointData[0] & 0x0F;  // two lower bits denote layer
          // SPecho  = ScanPointData[0] >> 4; // two higher bits
          // denote echo
          SPHangle = (char)ScanPointData[3] * 0x100 + ScanPointData[2];  // signed INT16 here
          SPdistance = ScanPointData[5] * 0x100 + ScanPointData[4];

          // Sanity checks
          if (SPlayer > 4)
          {
            dropPacket = true;
            // std::cerr << "Invalid layer: " << SPlayer << " should
            // be element of [0,3] Scanpoint dropped.\n";
          }
          if ((SPHangle < -((int)angleTicks) / 2) || (SPHangle > (int)angleTicks / 2))
          {
            dropPacket = true;
            // std::cerr << "Invalid horizontal angle: " <<
            // (int)-angleTicks/2 << "< " << SPHangle << " <" <<
            // angleTicks/2 << " Scanpoint dropped.\n";
          }
          if ((SPdistance < 30) || (SPdistance > 20000))
          {
            dropPacket = true;
            // std::cerr << "Invalid distance: 30< " << SPdistance
            // << " <20000 Scanpoint dropped.\n";
          }

          if (!dropPacket)
          {
            // TODO: Process point information correctly
            CPoint3D cartesianPoint = convertToCartesian(
                convertLayerToRad(SPlayer),  // vertikal coord of scanner
                convertTicksToHRad(
                    SPHangle,
                    angleTicks),  // horizontal coord of scanner
                SPdistance);

            // write scanpoint data to observation object
            newObs->points3D_x.push_back(cartesianPoint.x());
            newObs->points3D_y.push_back(cartesianPoint.y());
            newObs->points3D_z.push_back(cartesianPoint.z());
          }
        }  // for

        // return observation to framework
        appendObservation(newObs);

        state = SearchForAF;
        break;  // SaveData
    }           // Switch
  }             // While

  // Send stop command
  makeCommandHeader(msg);
  makeStopCommand(msg);
  m_client.writeAsync(&msg[0], 28);

  m_client.close();
}  // dataCollection

CPoint3D CIbeoLuxETH::convertToCartesian(float vrad, float hrad, float distance)
{
  float x, y, z;
  float rho, phi, theta;

  // Convert from laserscanner coordinate system to spherical coordinates
  rho = distance / 100;      // cm to meter
  phi = -hrad + (M_PI / 2);  // start with 0 pointing straight up
  theta = vrad + M_PI;       // 0 is straight ahead, going clockwise for 2 Pi

  x = rho * sin(phi) * cos(theta);
  y = rho * sin(phi) * sin(theta);
  z = rho * cos(phi);

  CPoint3D point(x, y, z);
  return point;
}

double CIbeoLuxETH::convertTicksToHRad(int hticks, int hticksPerRotation)
{
  return M_PI * 2 * hticks / hticksPerRotation;
}

double CIbeoLuxETH::convertLayerToRad(int scanlayer)
{
  double vangle;

  switch (scanlayer)
  {
    case 0:
      vangle = -0.02094395103;
      break;
    case 1:
      vangle = -0.006981317009;
      break;
    case 2:
      vangle = 0.006981317009;
      break;
    case 3:
      vangle = 0.02094395103;
      break;
    default:
      vangle = 0;
      std::cerr << "Layer: " << scanlayer << "! Returning " << vangle << " as angle.\n";
      break;
  }

  return vangle;
}

void CIbeoLuxETH::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
  float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
  bool faillNotFound = false;
  pose_x = configSource.read_float(iniSection, "pose_x", 0, faillNotFound);
  pose_y = configSource.read_float(iniSection, "pose_y", 0, faillNotFound);
  pose_z = configSource.read_float(iniSection, "pose_z", 0, faillNotFound);
  pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, faillNotFound);
  pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, faillNotFound);
  pose_roll = configSource.read_float(iniSection, "pose_roll", 0, faillNotFound);

  m_sensorPose =
      CPose3D(pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch), DEG2RAD(pose_roll));
}

void CIbeoLuxETH::makeCommandHeader(unsigned char* buffer)
{
  // Header - all big endian
  buffer[0] = 0xAF;  // magic word
  buffer[1] = 0xFE;
  buffer[2] = 0xC0;
  buffer[3] = 0xC2;
  buffer[4] = 0x00;  // Size of previous message, here just left to null
  buffer[5] = 0x00;
  buffer[6] = 0x00;
  buffer[7] = 0x00;
  buffer[8] = 0x00;  // Size of data block
  buffer[9] = 0x00;
  buffer[10] = 0x00;
  buffer[11] = 0x00;  // to be set by the command function
  buffer[12] = 0x00;  // Reserved + source Id
  buffer[13] = 0x78;  // source ID of 0x78 as observed
  buffer[14] = 0x20;  // Data Type - 2010 = command
  buffer[15] = 0x10;
  buffer[16] = 0x00;  // 4* ntpp time (s) + 4* fractions of a second
  buffer[17] = 0x00;
  buffer[18] = 0x00;
  buffer[19] = 0x00;
  buffer[20] = 0x00;
  buffer[21] = 0x00;
  buffer[22] = 0x00;
  buffer[23] = 0x00;
}

void CIbeoLuxETH::makeStartCommand(unsigned char* buffer)
{
  // Header - all big endian
  buffer[11] = 0x04;  // Size of data block
  // Data Block - all little endian
  buffer[24] = 0x20;  // Start Measure 0x0020
  buffer[25] = 0x00;
  buffer[26] = 0x00;  // Reserved, but obligatory
  buffer[27] = 0x00;
}

void CIbeoLuxETH::makeStopCommand(unsigned char* buffer)
{
  // Header - all big endian
  buffer[11] = 0x04;  // Size of data block
  // Data Block - all little endian
  buffer[24] = 0x21;  // Stop Measure 0x0021
  buffer[25] = 0x00;
  buffer[26] = 0x00;  // Reserved, but obligatory
  buffer[27] = 0x00;
}

void CIbeoLuxETH::makeTypeCommand(unsigned char* buffer)
{
  // Header - all big endian
  buffer[11] = 0x08;  // Size of data block
  // Data Block - big endian (for filter command!)
  buffer[24] = 0x00;  // Command Type - 0005 = set datatype filter
  buffer[25] = 0x05;
  buffer[26] = 0x00;  // Data type filter length
  buffer[27] = 0x02;
  buffer[28] = 0x22;  // start value
  buffer[29] = 0x00;
  buffer[30] = 0x22;  // end value
  buffer[31] = 0x10;
}

/** This method can or cannot be implemented in the derived class, depending on
 * the need for it.
 *  \exception This method must throw an exception with a descriptive message if
 * some critical error is found.
 */
void CIbeoLuxETH::initialize()
{
  m_run = true;

  // Start a thread to collect and interpret scandata
  // std::cout << "Start dataCollectionThread\n";

  // boost threads:
  // boost::thread
  // dataCollectionThread(boost::bind(&CIbeoLuxETH::dataCollection,this));

  dataCollectionThread = std::thread(&CIbeoLuxETH::dataCollection, this);
}

void CIbeoLuxETH::doProcess()
{
  // nothing is done here
  // data is collected in the dataCollection thread
}
