/**
 * Read one scan from buffer and decode it (ColaB decoding)
 *
 * Example:
 * SN LMDscandata 1 1 9981BB 0 0 C82F 5 7C6D3430 7C6D395E 0 0 7 0 0 9C4 B4 0 1 DI ....
 *
 */
void MRS6xxxB::readAndDecodeMRS6xxxScan(uint8_t* inputBuffer, uint16_t frameLength)
{
  // boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();

  //#ifdef _WIN32
  // LARGE_INTEGER StartingTime, EndingTime, ElapsedMicroseconds;
  // LARGE_INTEGER Frequency;
  //	QueryPerformanceFrequency(&Frequency);
  //	QueryPerformanceCounter(&StartingTime);
  //#endif

  SensorStateInfo sensorStateInfo;
  MeasurementList measureList;
  if (m_enableSensorStateInfoOutput)
  {
    sensorStateInfo.setDeviceID(getDeviceID());
    measureList.setGroupName("SensorState");
    measureList.setListName("SensorStates");
    measureList.add(Measurement::DeviceName, m_scannerName);
    measureList.add(Measurement::DeviceVersion, m_scannerVersion);
    measureList.add(Measurement::ScanFreq, m_scanFreq);
    measureList.add(Measurement::ScanStartAngle, m_scanStartAngle);
    measureList.add(Measurement::ScanStopAngle, m_scanStopAngle);
    measureList.add(Measurement::ScanResolution, m_scanAngleRes);
  }

  if (m_beVerbose == true)
  {
    traceNote(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We are decoding a received scan from "
                                << getLongName() << ". Data length is " << frameLength << " bytes." << std::endl;
  }

  //
  // Debug: Print the input buffer contents
  //
  //	std::string text = bufferToHexString(inputBuffer, frameLength);
  //	traceNote(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Input buffer = " << text << "." <<
  // std::endl;

  // Trace once that scan data was received
  if (m_weHaveReceivedScans == false)
  {
    if (m_beVerbose == true)
    {
      traceNote(MRS6xxxB_VERSION) << "We are receiving scans from " << getLongName() << "." << std::endl;
    }
    m_weHaveReceivedScans = true;
  }

  //
  // Parse Scan
  //
  uint16_t pos = 0;
  uint16_t versionNo = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Version of the following structure
  if (versionNo > 1)
  {
    // Es ist eine unbekannte Versionsnummer!
    traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We are receiving scan data structures with "
                                    "an unknown version number. Received version is "
                                 << versionNo << ", but we can only handle version 0 and 1!" << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  uint16_t ident = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);         // Logical number of the scanner - 2
  uint32_t serialNo = colab::getIntegerFromBuffer<uint32_t>(inputBuffer, pos);      // Serial number of the scanner - 4
  uint16_t state = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);         // Geraetestatus - 8
  uint16_t telegramCount = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Telegramm-Nummer - 10
  uint16_t scanCount = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);     // Scan number - 12
  uint32_t systemCountScan = colab::getIntegerFromBuffer<uint32_t>(
      inputBuffer, pos); // Microseconds from power-on to scan generation - mirror start position - 14
  uint32_t systemCountTransmit =
      colab::getIntegerFromBuffer<uint32_t>(inputBuffer, pos); // Microseconds from power-on to scan transmission - 18
  uint16_t inputs = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);  // Status der Digital-Eingaenge - 22
  uint16_t outputs = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Status der Digital-Ausgaenge - 24
  int16_t layerAngle =
      colab::getIntegerFromBuffer<int16_t>(inputBuffer, pos); // this should be the MRS6xxxB Layer Angle - 26
  uint32_t scanFreq = colab::getIntegerFromBuffer<uint32_t>(inputBuffer, pos); // Scan frequency, in [1/100 Hz] - 28
  uint32_t measFreq =
      colab::getIntegerFromBuffer<uint32_t>(inputBuffer, pos); // Spot-to-spot frequency, in [100 Hz] - 32

  if (serialNo < 17000000) // no devices before 01.01.2017 produced
  {
    // Es ist eine unbekannte Versionsnummer!
    traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We are receiving scan data structures with "
                                    "an invalid serial number. Received version is "
                                 << serialNo << ", but we can only handle version 0 and 1!" << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  if (scanCount != m_lastScanCount)
  {
    m_lastScanCount = uint32_t(scanCount);
    m_lastLayerAngle = int32_t(layerAngle);
  }

  uint16_t encoders =
      colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Number of encoder blocks following - 34
  if (encoders > 0)
  {
    // Error: No encoder blocks allowed
    traceError(MRS6xxxB_VERSION)
        << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We have decoded an invalid number of encoders (" << encoders
        << "), but none are allowed!" << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  // Data channel 16 (=16-bit scan data)
  const uint16_t maxPointsPerChannel = 1001; // arrays will be allocated staticly, so we have to define a value here

  uint16_t dataChannel16s = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Number of 16 bit channels - 36
  if (dataChannel16s > 9)
  {
    // Error: Only up to 9 channels allowed (4 for distance and 4 for remission) + one for vertical angles
    traceError(MRS6xxxB_VERSION)
        << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We have decoded an invalid number of 16-bit data channels ("
        << dataChannel16s << "), but only up to 9 are allowed!" << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  std::string contentType[9];
  double scaleFactor[9];
  double scaleOffset[9];
  int32_t startAngle[9];
  int16_t angleRes[9];
  uint16_t numberChannelEntries[9];
  uint16_t channelValues[9][maxPointsPerChannel];

  for (uint16_t i = 0; i < dataChannel16s; i++)
  {
    // Header
    // uint16_t stringlength = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
    contentType[i] =
        std::string((char*)(&inputBuffer[pos]), 5); // Channel content descriptor (text) - should be 5 chars ???
    if (contentType[i].length() != 5)
    {
      // Warning: The length is not what we expected.
      traceNote(MRS6xxxB_VERSION)
          << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We have decoded an invalid content string of data channel " << i
          << ". Read text is " << contentType[i] << "." << std::endl;
      m_alreadyReceivedBytes = 0; // reset in case of an error
      return;
    }

    pos += 5;

    scaleFactor[i] = colab::getFloatFromBuffer<float>(inputBuffer, pos);    // scale factor, depends on content
    scaleOffset[i] = colab::getFloatFromBuffer<float>(inputBuffer, pos);    // scale offset, depends on content
    startAngle[i] = colab::getIntegerFromBuffer<int32_t>(inputBuffer, pos); // Start angle [1/10000 deg]
    angleRes[i] = colab::getIntegerFromBuffer<int16_t>(inputBuffer, pos);   // Angular resolution [1/10000 deg]

    // Scan data

    numberChannelEntries[i] = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Number of scanpoints
    if (numberChannelEntries[i] > maxPointsPerChannel)
    {
      // Error: Only up to ???? scanpoints per channel allowed
      traceError(MRS6xxxB_VERSION)
          << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We have decoded an invalid number of scanpoints ("
          << numberChannelEntries[i] << ") in channel " << i << ", but only up to ???? are allowed!" << std::endl;
      m_alreadyReceivedBytes = 0; // reset in case of an error
      return;
    }

    for (uint16_t s = 0; s < numberChannelEntries[i]; ++s)
    {
      channelValues[i][s] = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Scanpoint
    }
  }

  // data channel 8-bit. Remission values 8 bit with MRS6xxxB
  uint16_t dataChannel8s =
      colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos); // Number of 8 bit channels. MRS6xxxB: should be 0

  if (dataChannel8s != 0)
  {
    traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): number dataChannel8s must be 0!"
                                 << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  uint16_t position = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
  if (position != 0)
  {
    traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Message \"Position\" must be 0!"
                                 << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  uint16_t deviceNameAvailable = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
  if (deviceNameAvailable != 0)
  {
    uint16_t nameLength = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
    std::string deviceName = std::string((char*)(&inputBuffer[pos]), nameLength);
    pos += nameLength;
  }

  uint16_t comment = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
  if (comment != 0)
  {
    traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Message \"Comment\" must be 0!" << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  uint16_t UTCTimestampfromTelegramAvailable = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
  boost::posix_time::ptime UTCTimestampFromTelegram; // this corresponds transmission time of the telegram
  if (UTCTimestampfromTelegramAvailable != 0)
  {

    uint16_t year = colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
    uint8_t month = colab::getIntegerFromBuffer<uint8_t>(inputBuffer, pos);
    uint8_t day = colab::getIntegerFromBuffer<uint8_t>(inputBuffer, pos);
    uint8_t hour = colab::getIntegerFromBuffer<uint8_t>(inputBuffer, pos);
    uint8_t minute = colab::getIntegerFromBuffer<uint8_t>(inputBuffer, pos);
    uint8_t second = colab::getIntegerFromBuffer<uint8_t>(inputBuffer, pos);
    uint32_t microSeconds = colab::getIntegerFromBuffer<uint32_t>(inputBuffer, pos);
    // check range of variables
    if (year < 1970 || year > 2200 || month > 12 || day > 31 || hour > 23 || minute > 60 || second > 60
        || microSeconds > 999999)
    {
      traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Invalid UTC time" << std::endl;
      m_alreadyReceivedBytes = 0; // reset in case of an error
      return;
    }
    UTCTimestampFromTelegram = boost::posix_time::ptime(boost::gregorian::date(year, month, day))
                               + boost::posix_time::hours(hour) + boost::posix_time::minutes(minute)
                               + boost::posix_time::seconds(second) + boost::posix_time::microseconds(microSeconds);
  }

  /*uint16_t eventInfo = */
  colab::getIntegerFromBuffer<uint16_t>(inputBuffer, pos);
  // if (eventInfo != 0)
  // {
  // traceError(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): We have decoded an invalid number of
  // eventInfo (" << eventInfo << "), but only up to 3 are allowed!" << std::endl; m_alreadyReceivedBytes = 0; // reset
  // in case of an error return;
  // }

  // END of buffer parsing

  // To avoid compiler warnings (unused variables)
  uint32_t dummy;
  dummy = versionNo + ident + serialNo + state + telegramCount;
  dummy += scanCount + systemCountTransmit + inputs;
  dummy += outputs + layerAngle;

  std::bitset<16> stateBits(state);
  std::bitset<16> inputBits(inputs);
  std::bitset<16> outputBits(outputs);

  //
  // Convert to ResearchAPI scan format
  //

  // Check for distance channels
  int16_t distanceChannel[4] = {-1, -1, -1, -1};
  int16_t intensityChannel[4] = {-1, -1, -1, -1};
  int16_t verticalAngleChannel = -1;

  // The MRS6xxxB has up to 4 echo per spot
  uint16_t numberEchos = 4; // Echos

  for (int16_t i = 0; i < 9; i++)
  {
    if ((dataChannel16s + dataChannel8s) > i)
    {
      // Channel contains content
      if (contentType[i] == "DIST1")
      {
        distanceChannel[0] = i;
      }
      else if (contentType[i] == "DIST2")
      {
        distanceChannel[1] = i;
      }
      else if (contentType[i] == "DIST3")
      {
        distanceChannel[2] = i;
      }
      else if (contentType[i] == "DIST4")
      {
        distanceChannel[3] = i;
      }
      else if (contentType[i] == "RSSI1")
      {
        intensityChannel[0] = i;
      }
      else if (contentType[i] == "RSSI2")
      {
        intensityChannel[1] = i;
      }
      else if (contentType[i] == "RSSI3")
      {
        intensityChannel[2] = i;
      }
      else if (contentType[i] == "RSSI4")
      {
        intensityChannel[3] = i;
      }
      else if (contentType[i] == "VANGL") // vertical angle channel
      {
        verticalAngleChannel = i;
      }
    }
    else
    {
      // No more channels with data
      break;
    }
  }

  // Is scan data included anywhere?
  if ((distanceChannel[0] < 0) && (distanceChannel[1] < 0) && (distanceChannel[2] < 0) && (distanceChannel[3] < 0))
  {
    // No valid scan data
    traceError(MRS6xxxB_VERSION)
        << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Scan contains no scan data! Aborting decoding and ignoring this scan."
        << std::endl;
    m_alreadyReceivedBytes = 0; // reset in case of an error
    return;
  }

  // Set unit of distance data to mm
  double resolution = 0.001;

  // Start angle, stop angle and angle step.
  // Contains a correction (90 deg) from MRS6xxxB coordinate system where the 0-deg-beam
  // points to the right side (+90 deg)
  double startAngleReal, endAngleReal, angleStepReal;
  startAngleReal = (((double)startAngle[0] / 10000.0) - 90.0)
                   * deg2rad; ///< right limiting angle => as the scanner rotates clockwise, this is the angle where the
                              ///< measurement stops (unfortunately)
  angleStepReal = ((double)angleRes[0] / 10000.0) * deg2rad;
  endAngleReal = normalizeRadians(
      startAngleReal
      + ((numberChannelEntries[0] - 1) * angleStepReal)); ///< left limiting angle => as the scanner rotates clockwise,
                                                          ///< this is the angle where the measurement starts

  // Create Scanner Info
  ScannerInfo si;
  si.setStartAngle(m_scanStartAngle);
  si.setEndAngle(m_scanStopAngle);
  si.setDeviceIdent(DeviceIdent({m_scannerName, m_scannerVersion}));

  // the MRS6xxxB cannot have a variable scan resolution because there is only one sector
  ScannerInfo::ResolutionMap rm;
  rm.push_back(std::make_pair(m_scanStartAngle, m_scanAngleRes));
  si.setResolutionMap(rm);

  // Do set-up and received angle ranges match?
  // Physical scanrange is -60 to 60 deg.
  // But the range can be limited further
  double allowedAngleDifference = 0.13 * research::deg2rad;
  uint32_t startScanPoint, stopScanPoint;
  if ((endAngleReal - m_scanStopAngle) > allowedAngleDifference)
  {
    // The target stop angle before the end of the scan. Discard scan data at the end of the scan
    stopScanPoint = uint32_t(numberChannelEntries[0] - floor((endAngleReal - m_scanStopAngle) / angleStepReal));
    if (stopScanPoint > maxPointsPerChannel)
    {
      // The target angle is invalid, it may be outside of the physical scan range
      if (m_scanAreaWarningPrinted == false)
      {
        traceWarning(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): The scan end angle is outside of the "
                                          "scanner scan range. Setting to default scan area (120 deg)."
                                       << std::endl;
        stopScanPoint = numberChannelEntries[0];
      }
    }

    if (m_scanAreaWarningPrinted == false)
    {
      m_scanAreaWarningPrinted = true;
      traceNote(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Dropping scan points because configured "
                                     "scan area is smaller than the default 120 deg scan area."
                                  << std::endl;
    }
  }
  else
  {
    // Default: Full scanrange
    stopScanPoint = numberChannelEntries[0];
  }

  // Start angle
  if (m_scanStartAngle - startAngleReal > allowedAngleDifference)
  {
    startScanPoint = uint32_t(floor((m_scanStartAngle - startAngleReal) / angleStepReal));
    if (!m_scanAreaWarningPrinted)
    {
      m_scanAreaWarningPrinted = true;
      traceNote(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Dropping scan points because configured "
                                     "scan area is smaller than the default 120 deg scan area."
                                  << std::endl;
    }
  }
  else
  {
    startScanPoint = 0;
  }

  //
  // Parse scan data
  //
  float dist = 0.f;

  // Spot-to-spot time in [s] - should be 18.05 us - Measurement freq stored in [100Hz]
  double spotTime = 1. / (double(measFreq) * 100.);

  float layerAngles[24]; // vertical angle at 0 deg (x-axis) in sopas telegram as layer identifier

  // create array of vertical angles at 0 deg for each layer to be able to recognize the layers
  for (size_t i = 0; i < 24; ++i)
  {
    layerAngles[23 - i] = 13.5f - (0.62f / 2.f) - (float(i) * 0.625f);
    if (m_positiveVerticalAnglesPointDownwards)
    {
      layerAngles[23 - i] *= (-1);
    }
  }

  float verticalAngleDeg = static_cast<float>(layerAngle) / 200.0; // MRS6000 has 200 as conversion factor
  float verticalAngle = research::degToRad(verticalAngleDeg);

  // The layer number (channel) must be derived from the layer angle: bottom layer is layer 0, top layer 23
  uint8_t layerNumber = 0;

  for (size_t ch = 0; ch < 24; ++ch)
  {
    if (research::fuzzyCompare(verticalAngleDeg, layerAngles[ch]))
    {
      layerNumber = uint8_t(ch);
      break;
    }
  }

  // layer 23 is the first layer that is transmitted (transmitted from top to bottom)
  if (layerNumber == 23 || scanCount != m_currentScanNumber)
  {
    // we have received a new scan with a new scan number
    m_scan.clear();
    m_scan.reserve(24 * numberChannelEntries[0]);

    if (uint32_t(scanCount - 1) != m_currentScanNumber
        || uint32_t(scanCount - 1) != m_lastScanNumber) // last transmitted scan number
    {
      if (!(m_lastScanNumber == 0 && m_allLayersReceived.to_ulong() == 0)
          && !(m_lastScanNumber == 65535 && scanCount == 0))
      {
        traceNote(MRS6xxxB_VERSION) << "Missing one scan: Received scan " << uint32_t(scanCount)
                                    << " after last sent scan " << m_lastScanNumber
                                    << " - layers: " << m_allLayersReceived << std::endl;
      }
    }
    m_currentScanNumber = scanCount;
    m_allLayersReceived = std::bitset<24>(0);
  }

  if (m_allLayersReceived.test(layerNumber))
  {
    traceNote(MRS6xxxB_VERSION) << "Received data for layer " << uint32_t(layerNumber) << " twice." << std::endl;
  }

  m_allLayersReceived.set(layerNumber);

  // time offset for the different mirror faces in us - (0, 25, 50, 75) [ms]
  uint32_t timeOffsetUS[4] = {0, 25000, 50000, 75000};

  if (m_beVerbose == true)
  {
    traceNote(MRS6xxxB_VERSION) << "MRS6xxxB::readAndDecodeMRS6xxxScan(): Received layer " << uint32_t(layerNumber)
                                << " scan " << scanCount << std::endl;
  }

  for (uint16_t p = startScanPoint; p < stopScanPoint; p++)
  {
    // calc horizontal angle of the scan point
    float hAngle = startAngleReal + (p * angleStepReal);
    // calc vertical angle of the scan point
    float vAngle = -verticalAngle;
    if (verticalAngleChannel >= 0)
    {
      // In SOPAS ET, positive angles point upwards (positive z-Axis) (wrong understanding in devices with fw version
      // 0.*) here it is the other way round
      vAngle = (-deg2rad)
               * (float(channelValues[verticalAngleChannel][p]) * scaleFactor[verticalAngleChannel]
                  + scaleOffset[verticalAngleChannel]);
    }
    if (m_positiveVerticalAnglesPointDownwards)
    {
      vAngle *= (-1);
    }

    for (uint16_t echo = 0; echo < numberEchos; echo++)
    {
      int16_t distanceIndex = distanceChannel[echo];
      int16_t intensityIndex = intensityChannel[echo];

      if (distanceIndex >= 0)
      {
        // Distance
        uint16_t value = channelValues[distanceIndex][p];
        if ((value > 15) && (value < 0xFFF0))
        {
          dist =
              double(value) * resolution * scaleFactor[distanceIndex] + scaleOffset[distanceIndex]; // Scan data in [m]

          // Append new scan point at end of scan point list
          ScanPoint& newPoint = m_scan.addNewPoint();

          newPoint.setPolar(dist, hAngle, vAngle);

          // Copy data to new scan point
          // If the sensor has sent data, otherwise 0.0, convert echo pulse width to meters (max 1 m since TiM sends 8
          // bit remission values). valid RSSI values are in range [0, 1200] for reflectors  and [0,256] for white
          // targets depending on distance
          //              => normalized to 1m pulse width at close distance for white target
          if (intensityIndex >= 0)
          {
            newPoint.setEchoWidth(float(channelValues[intensityIndex][p]) / 256.0f);

            // just a line through (0m/300)<=>(100m/0) to distinguish
            float threshold = 300.f - 3.f * float(dist);
            if (float(channelValues[intensityIndex][p]) > threshold)
            {
              newPoint.setFlags(ScanPoint::FlagReflector);
            }
            else
            {
              newPoint.setFlags(0);
            }
          }
          else
          {
            newPoint.setEchoWidth(0.0f);
            newPoint.setFlags(0); // Not available
          }
          newPoint.setDeviceID(getDeviceID());

          newPoint.setChannel(layerNumber);

          newPoint.setSubchannel(echo);
          newPoint.setTimeOffset(boost::posix_time::microseconds(
              uint32_t(p * spotTime * 1000000.0)
              + timeOffsetUS[(23 - layerNumber)
                             / 6])); // Time offset of scan point (4 mirror faces that are measured after each other)
          newPoint.setSegmentId(0);  // Not available
        }
      }
    }
  }

  //***************************
  //* Time stamp code section *
  //***************************
  // fill just once if we have received the first layer
  if (layerNumber == uint8_t(23))
  {
    boost::posix_time::ptime UTCSyncTimestamp;  // UTC time stamp when sync pulse was emitted, e.g. when Laserscanner
                                                // mirror was at sync angle position
    boost::posix_time::ptime UTCStartTimestamp; // UTC time stamp when Laserscanner mirror was at start angle position
    boost::posix_time::ptime UTCEndTimestamp;   // UTC time stamp when Laserscanner mirror was at end angle position

    // Duration of the scan of one mirror side.
    const boost::posix_time::time_duration scanDurationMirrorSide =
        boost::posix_time::microseconds((uint32_t)(double(stopScanPoint - startScanPoint) * spotTime * 1000000.0));

    // Calculate all hardware time stamps
    const boost::posix_time::time_duration HWStartTimestamp = boost::posix_time::microseconds(
        systemCountScan); // Time counter of scanner hardware when Laserscanner mirror was at start index position
    const boost::posix_time::time_duration HWSyncTimestamp = HWStartTimestamp;
    const boost::posix_time::time_duration HWEndTimestamp =
        HWStartTimestamp + scanDurationMirrorSide + boost::posix_time::microseconds(timeOffsetUS[3]);
    const boost::posix_time::time_duration HWTransmissionTimestamp =
        boost::posix_time::microseconds(systemCountTransmit); // of first layer

    //////
    // Time stamps from scan telegram
    boost::posix_time::ptime UTCSyncTimestampNTP;  // Sync time stamp from NTP time from scan telegram
    boost::posix_time::ptime UTCStartTimestampNTP; // Start time stamp from NTP time from scan telegram
    boost::posix_time::ptime UTCEndTimestampNTP;   // End time stamp from NTP time from scan telegram
    if (UTCTimestampfromTelegramAvailable)
    {
      UTCStartTimestampNTP =
          UTCTimestampFromTelegram - boost::posix_time::microseconds(systemCountTransmit - systemCountScan);
      // sync timestamp = start of measurement
      UTCSyncTimestampNTP = UTCStartTimestampNTP;
      // end of measurement of all 4 mirror sides
      UTCEndTimestampNTP = UTCTimestampFromTelegram
                           - boost::posix_time::microseconds(systemCountTransmit - systemCountScan)
                           + boost::posix_time::microseconds(timeOffsetUS[3]) + scanDurationMirrorSide;
    }

    //////
    // Best guess of UTC time stamps based on arival time. The HW time stamps can help us with this.
    boost::posix_time::ptime UTCSyncTimestampFallback;  // Sync time stamp based on estimated arival time
    boost::posix_time::ptime UTCStartTimestampFallback; // Start time stamp based on estimated arival time
    boost::posix_time::ptime UTCEndTimestampFallback;   // End time stamp based on estimated arival time

    // Update time offset compensation filter using the HW time stamp which is closest to system time stamp
    m_timeOffsetFilter.update(HWTransmissionTimestamp, getName());

    // Use HW time stamp to discipline UTC time stamp
    UTCStartTimestampFallback = m_timeOffsetFilter.getEquivalentUTC(HWStartTimestamp);
    UTCEndTimestampFallback = m_timeOffsetFilter.getEquivalentUTC(HWEndTimestamp);
    UTCSyncTimestampFallback = m_timeOffsetFilter.getEquivalentUTC(HWSyncTimestamp);

    if (m_useNTPTimestampFromScan && UTCTimestampfromTelegramAvailable)
    {
      const boost::posix_time::time_duration ntpOffsetSec = universalTime() - UTCTimestampFromTelegram;
      if (m_ntpShouldBeSyncdToLocalClock && ntpOffsetSec > m_maxNTPOffset)
      {
        traceWarning("") << "The offset of the NTP time stamp from the scan telegram is higher than " << m_maxNTPOffset
                         << " s. "
                         << "The internal clock of " << getName() << " seems not to be synchronized with your PC. "
                         << "Either check this or increase the parameter \"maxNTPOffset\" to a value highr than "
                         << ntpOffsetSec << std::endl;
      }

      UTCStartTimestamp = UTCStartTimestampNTP;
      UTCEndTimestamp = UTCEndTimestampNTP;
      UTCSyncTimestamp = UTCSyncTimestampNTP;
    }
    else
    {
      UTCStartTimestamp = UTCStartTimestampFallback;
      UTCEndTimestamp = UTCEndTimestampFallback;
      UTCSyncTimestamp = UTCSyncTimestampFallback;
    }

    // Actually set time stamps.
    // Also store hardware scanner counter values for start and end time stamp.
    // This can be used to reconstruct scanner timing if we messed up above somehow.
    m_scan.setSyncTimestamp(UTCSyncTimestamp); // Here the scan class only has room for a UTC value.
    m_scan.setStartTimestamp(DeviceSpecificTime(HWStartTimestamp, UTCStartTimestamp));
    m_scan.setEndTimestamp(DeviceSpecificTime(HWEndTimestamp, UTCEndTimestamp));

    uint32_t scanFlags = 0;
    if (m_enableCoordTransformation == true)
    {
      scanFlags = Scan::FlagVehicleCoordinates;
    }
    m_scan.setFlags(scanFlags);

    // Scan number
    uint32_t scanNumber = (uint32_t)scanCount; // Scan counter
    m_scan.setScanNumber(scanNumber);

    // For internal test purposes:
    m_scan.m_sensorNotification = 0;

    // For new 0x2205
    si.setTimestamps(UTCStartTimestamp, UTCEndTimestamp);
    si.setDeviceTimestamps(NTPTime::epoch() + HWStartTimestamp, NTPTime::epoch() + HWEndTimestamp);
    si.setScanFrequency(static_cast<float>(scanFreq) / 100.0f);

    // Write the current effective beam tilt into the
    // ScannerInfo. Front mirror side: beam tilted/pitched down
    // (positive pitch), rear mirror side: beam tilted/pitched up
    // (negative pitch)
    si.setBeamTilt(0.0);

    // Sensor state (dirt detection)
    si.setScannerStatus(state);

    si.setScanFlags(scanFlags);
    // End 2205

    si.setScanNumber(scanNumber);
    si.setDeviceID(getDeviceID());

    si.setScannerType(ScannerType::TypeSickMRS6000); // for compatibility, if no value is set in the
                                                     // scanner's config.

    // Mounting position
    MountingPosition mp(m_yawAngle, m_pitchAngle, m_rollAngle, m_offsetX, m_offsetY, m_offsetZ);
    si.setMountingPosition(mp);
    m_scan.setScannerInfos(Scan::ScannerInfoVector(1, si));

    if (m_beVerbose == true)
    {
      traceDebug(MRS6xxxB_VERSION) << "New scan from " << getLongName() << " received." << std::endl;
    }
  }

  //
  // Finish scan
  //

  // Send the scan
  if (layerNumber == uint8_t(0))
  {
    // check if we have received a full scan (otherwise content might be not correctly initialized)
    if (m_allLayersReceived == m_allLayersFull)
    {
      // build sensorStateInfo
      SensorStateInfo::StateMap stateMap;

      stateMap[STATENAME_DEVICE_ERROR] = stateBits[0];
      stateMap[STATENAME_CONTAMINATION_WARNING] = stateBits[1];
      stateMap[STATENAME_CONTAMINATION_ERROR] = stateBits[2];
      sensorStateInfo.setStateMap(stateMap);

      SensorStateInfo::StateVector inputStates(16, SensorStateInfo::OFF);
      SensorStateInfo::StateVector outputStates(16, SensorStateInfo::OFF);
      for (uint32_t j = 0; j < 16; ++j)
      {
        inputStates[j] = inputBits[j] ? SensorStateInfo::ON : SensorStateInfo::OFF;
        outputStates[j] = outputBits[j] ? SensorStateInfo::ON : SensorStateInfo::OFF;
      }
      sensorStateInfo.setInputStates(inputStates);
      sensorStateInfo.setOutputStates(outputStates);

      measureList.add(Measurement::SerialNumber, serialNo);

      // Send the SensorStateInfo - but only if requested. Default is "on".
      if (m_enableSensorStateInfoOutput == true)
      {
        sensorStateInfo.setMeasurementList(measureList);
        getSerializer().notifyMessageHandlers(sensorStateInfo, getDeviceID());
      }

      getSerializer().notifyMessageHandlers(m_scan, getDeviceID());
      traceDebug(MRS6xxxB_VERSION) << "Sent scan " << m_scan.getScanNumber() << std::endl;
      m_lastScanNumber = m_scan.getScanNumber();
    }
  }
}