LuxBase.cpp

Go to the documentation of this file.

//
// LuxBase.cpp
// Generic functions of the LD-MRS laserscanner interface.
//

//
// HISTORY
//
// This device is indended as a customer reference for the LD-MRS interface and coding / decoding of
// messages. Its interaction with other parts of the system and its usage of functions is intentionally
// kept simple, although this results in a somewhat lower performance than the original LUX/MRS
// device classes.
//
// Usage notes
//
// This device opens a TCP connection to an LD-MRS scanner. It then sets the desired parameters and
// receives incoming data. Currently, this is:
// - scans
// - objects
// - warning and error messages
//
// Internally, it has 2 buffers. The first level, called inputBuffer, is filled with whatever data is
// received on the interface. Scans and Objects are decoded right out of this buffer. Other messages,
// such as command replies and error messages, are forwarded to the cmdReplyBuffer where they are
// decoded. Note the different thread contexts: Scans and Objects are decoded in the receive-thread
// context, while data in the cmdReplyBuffer is decoded in the main device context asynchronously.
//
//  1.0.0, 2011-06-20, VWi
//          Initial version.
//  1.1.0, 2013-02-12, VWi
//          Added reading binary data from file.

//
#define LUXBASE_VERSION "1.1.0"

#include "LuxBase.hpp"
#include "../tools/errorhandler.hpp"
#include "../tools/MathToolbox.hpp"     // for NAN_double
#include <ostream>      // fuer ostringstream
#include <iomanip>      // fuer std::setfill
#include "../datatypes/Msg.hpp"
#include "../datatypes/Object.hpp"              // for ObjectList

// Note that LuxBase is not a device, but a helper class.
namespace devices
{
        
using namespace datatypes;

LuxBase::LuxBase (Manager* manager, const UINT8 deviceId, const std::string longName,
                                  std::string ipAddress, UINT16 tcpPortNumber,
                                  double scanFrequency, double scanStartAngle, double scanEndAngle, double offsetX,
                                  double offsetY, double offsetZ, double yawAngle, double pitchAngle, double rollAngle,
                                  bool beVerbose, std::string inputFileName)
        : m_manager(manager),
                m_longName(longName),
                m_deviceId(deviceId),
                m_ipAddress(ipAddress),
                m_tcpPortNumber(tcpPortNumber),
                m_inputFileName(inputFileName),
                m_scanFrequency(scanFrequency),
                m_scanStartAngle(scanStartAngle),
                m_scanEndAngle(scanEndAngle),
                m_offsetX(offsetX),
                m_offsetY(offsetY),
                m_offsetZ(offsetZ),
                m_yawAngle(yawAngle),
                m_pitchAngle(pitchAngle),
                m_rollAngle(rollAngle),
                m_inBufferLevel(0),
                m_beVerbose(beVerbose)
{
        printInfoMessage("LuxBase::LuxBase: Constructor running.", m_beVerbose);
        
        m_weWantScanData = false;               // Flag if received data is to be decoded.
        m_weWantObjectData = false;             // Flag if received data is to be decoded.
        
        m_onScanReceiveCallback = NULL;
        m_onScanReceiveCallbackObjPtr = NULL;
        m_disconnectFunctionObjPtr = NULL;
        m_tcpDisconnectFunction = NULL;
        m_fileDisconnectFunction = NULL;
        
        m_beamTiltAngle = 0.0;
        m_upsideDownActive = false;
        
        printInfoMessage("LuxBase::LuxBase: Constructor done.", m_beVerbose);
}


//
// Destructor
//
LuxBase::~LuxBase()
{
        if (isRunning() == true)
        {
                stop();
        }
}

void LuxBase::readCallbackFunctionS(void* obj, BYTE* buffer, UINT32& numOfBytes)
{
        ((LuxBase*)(obj))->readCallbackFunction(buffer, numOfBytes);
}


void LuxBase::setOnScanReceiveCallbackFunction(OnScanReceiveCallbackFunction function, void* obj)
{
        m_onScanReceiveCallback = function;
        m_onScanReceiveCallbackObjPtr = obj;
}

//
// Initialisation:
// Set-up variables and call base-class to connect
//
bool LuxBase::initTcp(Tcp::DisconnectFunction function, void* obj)      // , bool beVerbose)
{
        // Initialise base variables
//      m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
        m_firmwareVersion = 0;
        m_isRunning = false;
        m_inBufferLevel = 0;
        
        
        // Select the interface (file or TCP)
        bool result = false;
        if (m_inputFileName != "")
        {
                // File
                printError("LuxBase::initTcp: Called in file mode - this is an error, aborting.");
                return false;
        }
        
        printInfoMessage("LuxBase::initTcp: Opening the tcp interface (Addr=" + m_ipAddress + ":" + toString(m_tcpPortNumber) + ").", m_beVerbose);

        // Open the interface. Here, we are using our TCP wrapper.
        result = m_tcp.open(m_ipAddress, m_tcpPortNumber, false);       // m_beVerbose);
        if (result == true)
        {
                m_tcpDisconnectFunction = function;
                m_disconnectFunctionObjPtr = obj;
                m_tcp.setDisconnectCallbackFunction(function, obj);

                printInfoMessage("LuxBase::initTcp(): TCP connection established.", m_beVerbose);
        }
        else
        {
                printError("LuxBase::initTcp(): ERROR: Failed to establish TCP connection, aborting.");
                return false;
        }
        
        // Set the data input callback for our TCP connection
        m_tcp.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this);     // , this, _1, _2));


        // Get the status of the scanner, e.g. its firmware version. 
        printInfoMessage("LuxBase::initTcp(): Calling cmd_getStatus().", m_beVerbose);
        result = cmd_getStatus();

        if (result == true)
        {
                // Success, we have a valid firmware version
                if (m_beVerbose == true)
                {
                        UINT16 first = m_firmwareVersion >> 12;                                         // Example: 0x3021 ==> 3
                        UINT16 second = ((m_firmwareVersion & 0x0F00) >> 8) * 10;       // Example: 0x3021 ==> 0
                        second += (m_firmwareVersion & 0x00F0) >> 4;                            // Example: 0x3021 ==> 02
                        UINT16 third = (m_firmwareVersion & 0x000F);                            // Example: 0x3021 ==> 1
                        infoMessage(m_longName + " Firmware version is " + toString(first)+ "." + toString(second) + "." + toString(third) + ".");
                }
        }
        else
        {
                printError("LuxBase::initTcp(): ERROR: Failed to read scanner status, aborting.");
                return false;
        }
        
        //
        // Read the beam tilt.
        // We need this parameter later for the calculation of cartesian scan point coordinates.
        //
        printInfoMessage("LuxBase::initTcp(): Calling readBeamTilt().", m_beVerbose);
        result = readBeamTilt();

        if (result == true)
        {
                // Success, we have a valid beam tilt
                infoMessage(m_longName + " Beam tilt angle is " + toString(m_beamTiltAngle * rad2deg, 1)+ " degrees.", m_beVerbose);
        }
        else
        {
                printError("LuxBase::initTcp(): ERROR: Failed to read scanner beam tilt angle, aborting.");
                return false;
        }
        

        //
        // Read the UpsideDown mode.
        // We need this parameter later for the calculation of cartesian scan point coordinates.
        //
        printInfoMessage("LuxBase::initTcp(): Calling readUpsideDown().", m_beVerbose);
        result = readUpsideDown();

        if (result == true)
        {
                // Success, we have a valid upsideDown flag
                if (m_upsideDownActive == true)
                {
                        infoMessage(m_longName + " UpsideDown is active.", m_beVerbose);
                }
                else
                {
                        infoMessage(m_longName + " UpsideDown is not active.", m_beVerbose);
                }
        }
        else
        {
                // Some devices dont have the UpsideDown flag so just ignore this error
                infoMessage(m_longName + " UpsideDown not supported by firmware.", m_beVerbose);
        }

        // Start thread for reading temperature once a minute
//      m_updateThread.run(this);
        
        return true;
}

//
// Initialisation:
// Set-up variables and call base-class to connect.
//
// Call this function when file read is required.
//
bool LuxBase::initFile(File::DisconnectFunction function, void* obj)    // , bool beVerbose)
{
        // Initialise base variables
//      m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
        m_firmwareVersion = 0;
        m_isRunning = false;
        m_inBufferLevel = 0;
        
        // Set these values here as we cannot request them from the scanner!
        m_weWantScanData = true;
        m_weWantObjectData = true;
                
        // Select the interface (file or TCP)
        bool result = false;
        if (m_inputFileName == "")
        {
                // Error - no file!
                printError("LuxBase::initFile: called without file - aborting!");
                return false;
        }

        // File
        printInfoMessage("LuxBase::init: Opening the input file (Name=" + m_inputFileName + ").", m_beVerbose);
        result = m_file.open(m_inputFileName, false);   // m_beVerbose);
        if (result == true)
        {
                printInfoMessage("LuxBase::initFile(): File opened successfully.", m_beVerbose);
        }
        else
        {
                printError("LuxBase::initFile(): ERROR: Failed to open file, aborting.");
                return false;
        }
        
        m_fileDisconnectFunction = function;
        m_disconnectFunctionObjPtr = obj;
        m_file.setDisconnectCallbackFunction(function, obj);
        m_file.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this);    // , this, _1, _2));

        return true;
}

//
//
//
void LuxBase::updateThreadFunction(bool& endThread, UINT16& sleepTimeMs)
{
        // Status-Update anfordern
        bool result = cmd_getStatus();  // bool result = ...

        if (result == true)
        {
                printInfoMessage("updateThreadFunction(): Got new status. Temp= " + toString(getTemperature(), 1) + " deg.C.", m_beVerbose);
        }
        else
        {
                printWarning("updateThreadFunction(): Failed to read the status.");
        }

        // Call next time in 60s.
        sleepTimeMs = 1000 * 60;
        endThread = false; // !( /*m_updateThreadShouldStop &&*/ result);
}


//
//
//
bool LuxBase::isRunning()
{
        return m_isRunning;
}



//
// Writes the mounting position (angles and offsets) to the scanner.
//
bool LuxBase::cmd_setMountingPos(Position3D mp)
{
        UINT32 uValue;
        bool result;

        // Mounting pos x
        INT16 value = (INT16)(mp.getX() * 100.0);       // in [cm]
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingX, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingX parameter!");
                return false;
        }

        // Mounting pos y
        value = (INT16)(mp.getY() * 100.0);     // in [cm]
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingY, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingY parameter!");
                return false;
        }

        // Mounting pos z
        value = (INT16)(mp.getZ() * 100.0);     // in [cm]
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingZ, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingZ parameter!");
                return false;
        }

        // Mounting pos yaw angle
        value = (INT16)(mp.getYawAngle() * rad2deg * 32.0);
        makeIntValueEven(value);
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingYaw, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingYaw parameter!");
                return false;
        }

        // Mounting pos pitch angle
        value = (INT16)(mp.getPitchAngle() * rad2deg * 32.0);
        makeIntValueEven(value);
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingPitch, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingPitch parameter!");
                return false;
        }

        // Mounting pos roll angle
        value = (INT16)(mp.getRollAngle() * rad2deg * 32.0);
        makeIntValueEven(value);
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaMountingRoll, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("Failed to set MountingRoll parameter!");
                return false;
        }

        return true;    // Success
}



//
// Helper function to create even values, used for angles.
//
void LuxBase::makeIntValueEven(INT16& value)
{
        if ((2 * (value / 2)) != value)
        {
                // Value is not even, add 1
                value += 1;
        }
}



//
// Sets scan frequency.
// valid values are 12.5, 25.0 and 50.0 [Hz].
//
bool LuxBase::cmd_setScanFrequency(double scanFreq)
{
        bool result;
        //
        // Set the scan frequency
        //
        UINT32 uValue = (UINT32)(scanFreq * 256.0);
        if ((uValue > 3100) && (uValue < 3300))
        {
                uValue = 3200;  // 12.5 Hz
        }
        else if ((uValue > 6300) && (uValue < 6500))
        {
                uValue = 6400;  // 25 Hz
        }
        else if ((uValue > 12700) && (uValue < 12900))
        {
                uValue = 12800; // 50 Hz
        }
        else
        {
                uValue = 3200;  // DEFAULT
                printWarning("Para 'ScanFrequency' out of range, setting a default scan frequency of 12.5 Hz!");
        }
        result = cmd_setParameter(ParaScanFrequency, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("The SetParameter command failed!");
                return false;
        }

        return true;
}



//
// Sets scan start and end angles.
//
// Note that the two angles are set one after the other. Still, in the sensor, there
// is a condition that the StartAngle must always be greater than the EndAngle. Obviously,
// there are cases in which the mechanism implemented here will fail as the correct order
// is violated. However, for most cases, this will work.
//
bool LuxBase::cmd_setScanAngles(double startAngle, double endAngle)
{
        //
        // Start angle
        //
        // ** NOTE **
        // Some combinations of start and end angles just do not work. The result is that no scans
        // are being sent by the scanner; this is a known issue of the scanner firmware.
        // Our workaround is to use even TIC values; this will work fine.
        //
        // ** NOTE 2 **
        // The scan start angle must always be greater than the scan end angle. Therefore, the method
        // used below will not work for all configurations. To set the angles correctly, you should
        // read the current angles, select the order in which to set the parameters, and then write
        // them.
        //

        if (endAngle > startAngle)
        {
                printError("Start angle must be greater than end angle!");
                return false;
        }
        bool result;
        UINT32 uValue;
        INT16 value = (INT16)(startAngle * rad2deg * 32.0);     // Note that startAngle is in radians
        // Value should be even
        makeIntValueEven(value);
        if (value > 1600)
        {
                value = 1600;   //
                printWarning("Para 'ScanStartAngle' out of range, limiting to 1600!");
        }
        if (value < -1918)
        {
                value = -1918;  //
                printWarning("Para 'ScanStartAngle' out of range, limiting to -1919!");
        }
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaStartAngle, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printWarning("The SetParameter command failed!");
                return false;
        }

        //
        // End angle
        //
        value = (INT16)(endAngle * rad2deg * 32.0);     // Note that endAngle is in radians
        // Value should be even
        makeIntValueEven(value);
        if (value > 1598)
        {
                value = 1598;   //
                printWarning("Para 'ScanEndAngle' out of range, limiting to 1599!");
        }
        if (value < -1920)
        {
                value = -1920;  //
                printWarning("Para 'ScanEndAngle' out of range, limiting to -1920!");
        }
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & 0x0000FFFF;
        result = cmd_setParameter(ParaEndAngle, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printError("The SetParameter command failed!");
                return false;
        }

        return true;
}

bool LuxBase::cmd_setSyncAngleOffset(double syncAngle)
{
        bool result;
        UINT32 uValue;
        INT16 value = (INT16)(syncAngle * rad2deg * 32.0);      // Note that syncAngle is in radians
        if (value > 5759)
        {
                value = 5759;
                printWarning("Para 'SyncAngleOffset' out of range, limiting to 5759!");
        }
        if (value < -5760)
        {
                value = -5760;
                printWarning("Para 'SyncAngleOffset' out of range, limiting to -5760!");
        }
        uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
        uValue = uValue & (1 << 14) - 1; // = 0x00003FFF  (SyncAngleOffset is an INT14)
        result = cmd_setParameter(ParaSyncAngleOffset, uValue);
        if (result == false)
        {
                // We failed to set the parameter
                printWarning("The SetParameter command failed!");
                return false;
        }

        return true;
}


bool LuxBase::cmd_getStatus()
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                infoMessage("cmd_getStatus() called, but there is no connection - aborting!");
                return false;
        }

        // Send command
        MrsCommandId cmdId = CmdMrsGetStatus;
        printInfoMessage("Sending MRS command 'GetStatus'.", m_beVerbose);
        bool result = sendMrsCommand(cmdId);

        if (result == true)
        {
                printInfoMessage("MRS command 'GetStatus' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_getStatus: " + m_longName +
                                                                " MRS Status was read successfully. Firmware version is 0x" +
                                                                toHexString(m_firmwareVersion) + ".", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to receive status reply, aborting.");
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to send command, aborting.");
        }

        return result;
}



bool LuxBase::readBeamTilt()
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                infoMessage("readBeamTilt() called, but there is no connection - aborting!");
                return false;
        }

        // Read beam tilt
        UINT32 value;
        bool success = cmd_getParameter(ParaBeamTilt, &value);
        if (success == true)
        {
                INT16 tilt = static_cast<INT16>(value & 0xFFFF);
                
                // decompress to angle, in [rad]
                m_beamTiltAngle = static_cast<double>(tilt) / 10000.0;

                infoMessage("LuxBase::readBeamTilt: " + m_longName + " Beam tilt read. Angle is " + toString(rad2deg * m_beamTiltAngle, 1) + " degrees.");
        }
        else
        {
                // Failed to read parameters
                printError("LuxBase::readBeamTilt: " + m_longName + " ERROR: Failed to read beam tilt angle, aborting.");
        }

        return success;
}

bool LuxBase::readUpsideDown()
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                infoMessage("readUpsideDown() called, but there is no connection - aborting!");
                return false;
        }

        // Read beam tilt
        UINT32 value;
        bool success = cmd_getParameter(ParaUpsideDownMode, &value);
        if (success == true)
        {
                if (value != 0)
                {
                        m_upsideDownActive = true;
                        infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is active.");
                }
                else
                {
                        m_upsideDownActive = false;
                        infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is not active.");
                }
        }
        else
        {
                // Failed to read parameter
                infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown not supported by firmware.");
                // cannot read parameter so there is no upsideDown support
                m_upsideDownActive = false;
        }

        return success;
}


//
// Stop measuring.
//
bool LuxBase::cmd_stopMeasure()
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
                return false;
        }

        // StopMeasure command
        MrsCommandId cmdId = CmdMrsStopMeasure;
        printInfoMessage("Sending MRS command 'StopMeasure'.", m_beVerbose);
        bool result = sendMrsCommand(cmdId);

        if (result == true)
        {
                printInfoMessage("MRS command 'StopMeasure' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_stopMeasure: " + m_longName + " StopMeasure was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to receive StopMeasure reply.");
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to send command, aborting.");
                return result;
        }

        return result;
}

//
// Set NTP time.
//
bool LuxBase::cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec)
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
                return false;
        }

        // Part 1:
        // setNTPTimestamp command
        MrsCommandId cmdId = CmdMrsSetNTPTimestampSec;
        printInfoMessage("Sending MRS command 'SetNTPTimestampSec'.", m_beVerbose);
        bool result = sendMrsCommand(cmdId, 0, seconds);

        std::stringstream secString;
        secString << "seconds: " << seconds << "; fractional: " << fractionalSec;
        printInfoMessage("LuxBase::cmd_setNtpTimestamp: " +  secString.str(), true);

        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampSec' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_setNtpTimestamp: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampSec' reply, aborting.");
                        return false;
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_setNtpTimestampSec: " + m_longName + " ERROR: Failed to send command, aborting.");
                return false;
        }
        
        // Now send the fractional seconds
        cmdId = CmdMrsSetNTPTimestampFracSec;
        printInfoMessage("Sending MRS command 'SetNTPTimestampFrac'.", m_beVerbose);
        result = sendMrsCommand(cmdId, 0, fractionalSec);
        
        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampFrac' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampFrac' reply, aborting.");
                        return false;
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to send command, aborting.");
                return false;
        }

        return result;
}



//
// Set a parameter.
//
bool LuxBase::cmd_setParameter(MrsParameterId parameter, UINT32 value)
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
                return false;
        }

        // SetParameter command
        MrsCommandId cmdId = CmdMrsSetParameter;
        
        printInfoMessage("LuxBase::cmd_setParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
                                                " and value=0x" + toHexString(value) + ".", m_beVerbose);
        
        bool result = sendMrsCommand(cmdId, parameter, value);

        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_setParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_setParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to receive SetParameter reply.");
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
        }

        return result;
}

bool LuxBase::cmd_getParameter(MrsParameterId parameter, UINT32* value)
{
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
                return false;
        }

        // SetParameter command
        MrsCommandId cmdId = CmdMrsGetParameter;

        printInfoMessage("LuxBase::cmd_getParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
                         ".", m_beVerbose);

        bool result = sendMrsCommand(cmdId, parameter);

        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_getParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000, value);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_getParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to receive GetParameter reply.");
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
        }

        return result;
}

//
// Enable or disable the data output according to our needs.
//
// Note that the CAN output is disabled here!
// Note that false = enabled, true = disabled!!!!
//
// Bit 0 = Scan data (Ethernet)
// Bit 1 = reserved
// Bit 2 = Object data (Ethernet)
// Bit 3 = Vehicle data (Ethernet)
// Bit 4 = Errors/warnings (Ethernet)
// Bit 5 = Errors/warnings (CAN)
// Bit 6 = Object data (CAN)
//
bool LuxBase::cmd_setDataOutputFlags()
{
        UINT16 flags = 0xFFFF;
        if (m_weWantScanData == true)
        {
                flags &= 0xFFFE;                // Eth scan data
        }
        if (m_weWantObjectData == true)
        {
                flags &= 0xFFFB;                // Eth object data
        }
        flags &= 0xFFEF;                        // Eth errors and warnings
        
        // Set the parameter
        bool result = cmd_setParameter(ParaDataOutputFlag, flags);
        
        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_setDataOutputFlags: Output flags were set successfully to " +
                                                 toHexString(flags) + ".", m_beVerbose);
        }
        else
        {
                // Failed 
                printError("LuxBase::cmd_setDataOutputFlags:" + m_longName + " ERROR: Failed to set output flags, aborting.");
                return false;
        }

        return result;
}

bool LuxBase::cmd_saveConfiguration()
{
        printInfoMessage("LuxBase::cmd_saveConfiguration called", m_beVerbose);
        return sendMrsCommand(CmdMrsSaveConfig);
}

//
// Start measuring.
//
// Set both bool flags according to the device needs.
//
// Note 1: Not every MRS/LUX sends object data.
// Note 2: These flags may not prevent the data to be transferred, but it will not be decoded and
//         posted into the system.
//
bool LuxBase::cmd_startMeasure(bool weWantScanData, bool weWantObjectData)
{
        printInfoMessage("LuxBase::cmd_startMeasure: Called.", m_beVerbose);
        
        if (m_tcp.isOpen() == false)
        {
                // There is no connection
                printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: startMeasure() called, but there is no connection, aborting!");
                return false;
        }

        m_weWantScanData = weWantScanData;
        m_weWantObjectData = weWantObjectData;
        if ((m_weWantScanData == false) && (m_weWantObjectData == false))
        {
                // We want no data?
                printWarning("LuxBase::cmd_startMeasure:" + m_longName + " Warning: StartMeasure called, but neither scans nor objects are requested!");
        }

        // Enable scan and/or object output
        printInfoMessage("LuxBase::cmd_startMeasure: Enabling data output.", m_beVerbose);
        bool result = cmd_setDataOutputFlags();
        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_startMeasure: Data output was successfully enabled.", m_beVerbose);
        }
        else
        {
                // Failed
                printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to set data output, aborting!");
                return false;
        }

        // Command
        MrsCommandId cmdId = CmdMrsStartMeasure;
        printInfoMessage("LuxBase::cmd_startMeasure: Sending MRS command 'StartMeasure'.", m_beVerbose);
        result = sendMrsCommand(cmdId);

        if (result == true)
        {
                printInfoMessage("LuxBase::cmd_startMeasure: MRS command 'StartMeasure' was sent successfully, waiting for reply.", m_beVerbose);

                // The command was sent
                result = receiveMrsReply(cmdId, 2000);
                if (result == true)
                {
                        printInfoMessage("LuxBase::cmd_startMeasure: " + m_longName + " StartMeasure was acknowledged by the scanner.", m_beVerbose);
                }
                else
                {
                        printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to receive StartMeasure reply.");
                }
        }
        else
        {
                // Failed to send command
                printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to send command, aborting.");
                return result;
        }

        return result;
}

/*
inline void memreadLE (const BYTE*& buf, T& value)
{
        BOOST_STATIC_ASSERT(boost::is_fundamental<T>::value);
        BOOST_STATIC_ASSERT(sizeof(T) == 1 || sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8);
        value = detail::memreadcLE<T>(buf);
        buf += sizeof(T);
}
*/

//
// Lese einen 16-Bit-Wert, Little Endian.
//
void LuxBase::memreadLE(BYTE*& buffer, UINT16& value)
{
        value = ((UINT16)(buffer[0])) + ((UINT16)(buffer[1]) << 8);
        buffer += 2;
}

//
// Lese einen 32-Bit-Wert, Little Endian.
//
void LuxBase::memreadLE(BYTE*& buffer, UINT32& value)
{
        value = ((UINT32)(buffer[0])) + ((UINT32)(buffer[1]) << 8)
                        + ((UINT32)(buffer[2]) << 16) + ((UINT32)(buffer[3]) << 24);
        buffer += 4;
}


bool LuxBase::decodeGetStatus()
{
        UINT32 pos = 24;        // 24 is the length of the data header
        UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
        pos += 2;
//      UINT16 firmwareVersion = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);

        BYTE* bufferPos = &(m_inputBuffer[pos]);

        ScopedLock lock(&m_updateMutex);
        UINT16 dummy;

        memreadLE(bufferPos, m_firmwareVersion);
        memreadLE(bufferPos, m_FPGAVersion);
        memreadLE(bufferPos, m_scannerStatus);
        memreadLE(bufferPos, dummy);                            // Reserved. This is the SVN repository index.
        memreadLE(bufferPos, dummy);                            // Reserved. This is the scanner type.
        memreadLE(bufferPos, m_temperature);
        memreadLE(bufferPos, m_serialNumber[0]);
        memreadLE(bufferPos, m_serialNumber[1]);
        memreadLE(bufferPos, m_serialNumber[2]);
        memreadLE(bufferPos, m_FPGATimestamp[0]);
        memreadLE(bufferPos, m_FPGATimestamp[1]);
        memreadLE(bufferPos, m_FPGATimestamp[2]);
        memreadLE(bufferPos, m_firmwareTimestamp[0]);
        memreadLE(bufferPos, m_firmwareTimestamp[1]);
        memreadLE(bufferPos, m_firmwareTimestamp[2]);

        // Debug
        printInfoMessage("LuxBase::decodeGetStatus: " + m_longName + " Received a GetStatus with CMD " +
                                         toHexString(cmd) + " and FirmwareVersion " + toHexString(m_firmwareVersion) + ".", m_beVerbose);

        // Here we could decode the rest of the message.

        return true;
}

bool LuxBase::decodeGetParameter(UINT32* value)
{
        UINT32 pos = 24;        // 24 is the length of the data header
        UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
        pos += 2;
        BYTE* bufferPos = &(m_inputBuffer[pos]);
    UINT16 index = 0;

    memreadLE(bufferPos, index);
        memreadLE(bufferPos, *value);
        // Debug
        printInfoMessage("LuxBase::decodeGetParameter: " + m_longName + " Received a GetParameter with CMD " +
                         toHexString(cmd) + " and parameter index 0x" + toHexString(index) + " and value " + toString(*value) + ".", m_beVerbose);

        return true;
}


UINT16 celsius2Int (double celsius)
{
        // Step 1, temperature to voltage
        // (Taken from the data sheet of the temperature sensor LM 20.)
        double voltage = 1.8663f - 0.01169f * celsius;

        // Step 2, voltage to integer
        // The MRS has a 10 bit ADC (Analog-Digital Converter).
        // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
        UINT16 value = (UINT16)(((1023 * voltage) / 3.3f) + 0.5);
        return value;
}


double int2Celsius (double intValue)
{
        // Step 1, integer value to voltage
        // The MRS has a 10 bit ADC (Analog/Digital Converter).
        // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
        double voltage = ((double)intValue * 3.3f) / 1023.0;

        // Step 2, voltage to temperature
        // (Taken from the data sheet of the temperature sensor LM 20.)
        return (1.8663f - voltage) / 0.01169f;
}

double LuxBase::getTemperature()
{
        double temperature = 0;

        ScopedLock lock(&m_updateMutex);

        static UINT16 maxRawTemperature = celsius2Int (-273.16f);       // = 1568 = theoretical raw value of 0 Kelvin
        if (m_temperature <= maxRawTemperature)
                temperature = int2Celsius (m_temperature);
        else
        {
                // In this case, the raw temperature is most probably equal to 0xFFFF.
                // This happens when the LUX firmware is currently unable to read the
                // temperature from the FPGA, e.g. in MEASURE mode.
                temperature = ::NaN_double;     // Not-a-Number
        }

        return temperature;
}

//
//
//
std::string LuxBase::getSerialNumber()
{
        ScopedLock lock(&m_updateMutex);

        if (m_serialNumber[0] == 0xFFFF)
        {
                return "<not set>";
        }
        else
        {
                std::ostringstream oss;

                // Low byte of m_serialNumber[2] indicates the style of the serial number:
                // 0 = old style with MAC address
                // 1 = new style with consecutive counter
                bool isNewStyle = ((m_serialNumber[2] & 0x00FF) == 1);

                if (isNewStyle)
                {
                        // Output format...
                        // Example: "0829 0123" = LUX #123 in week 29 of year '08
                        oss << std::setfill ('0')
                                << std::hex << std::setw(4) << m_serialNumber[0] << ' '
                                << std::dec << std::setw(4) << m_serialNumber[1];
                }
                else
                {
                        // Output format...
                        // Example: "0823-0A7E3F" = LUX with MAC address "**:**:**:0A:7E:3F"
                        // produced in week 23 of year '08
                        oss << std::setfill ('0') << std::hex
                                << std::setw(4) <<  m_serialNumber[0] << '-'
                                << std::setw(4) <<  m_serialNumber[1]
                                << std::setw(2) << (m_serialNumber[2] >> 8);
                }
                return oss.str();
        }
}

std::string LuxBase::int2Version (UINT16 val)
{
        if (val == 0xFFFF)
        {
                return "n/a";   // not available
        }
        else
        {
                std::ostringstream oss;
                oss << std::hex << std::uppercase
                        << ((val & 0xF000) >> 12) << '.'
                        << std::setfill('0') << std::setw(2) << ((val & 0x0FF0) >>  4) << '.'
                        << (val & 0x000F);
                return oss.str();
        }
}

std::string LuxBase::version2string (UINT16 version, const UINT16 timestamp[3])
{
        if (isValidVersion (version))
        {
                UINT16 year   = timestamp[0];                           // Debugging: Display the data as *hex*
                UINT16 month  = timestamp[1] >> 8;                      // to see the "correct" values, e.g.
                UINT16 day    = timestamp[1] & 0x00FF;          // 8200 = 0x2008 = year 2008
                UINT16 hour   = timestamp[2] >> 8;
                UINT16 minute = timestamp[2] & 0x00FF;

                std::ostringstream oss;
                oss << int2Version (version) << ' ' << std::setfill ('0') << std::hex
                        << std::setw (4) << year   << '-'
                        << std::setw (2) << month  << '-'
                        << std::setw (2) << day    << ' '
                        << std::setw (2) << hour   << ':'
                        << std::setw (2) << minute;

                return oss.str();
        }
        else
        {
                return "<unknown>";
        }
}



UINT32 LuxBase::readUValueLE(UINT8* buffer, UINT8 bytes)
{
        UINT32 value;

        switch (bytes)
        {
        case 1:
                value = buffer[0];
                break;
        case 2:
                value = buffer[0];
                value += ((UINT32)buffer[1]) << 8;
                break;
        case 4:
                value = buffer[0];
                value += ((UINT32)buffer[1]) << 8;
                value += ((UINT32)buffer[2]) << 16;
                value += ((UINT32)buffer[3]) << 24;
                break;
        default:
                printError("LuxBase::readUValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
                value = 0xFFFFFFFF;
        }

        return value;
}

UINT64 LuxBase::readUINT64ValueLE(UINT8* buffer)
{
        UINT64 value;

        value = buffer[0];
        value += ((UINT64)buffer[1]) << 8;
        value += ((UINT64)buffer[2]) << 16;
        value += ((UINT64)buffer[3]) << 24;
        value += ((UINT64)buffer[4]) << 32;
        value += ((UINT64)buffer[5]) << 40;
        value += ((UINT64)buffer[6]) << 48;
        value += ((UINT64)buffer[7]) << 56;

        return value;
}


//
// Little-Endian-Read (signed)
//
INT32 LuxBase::readValueLE(UINT8* buffer, UINT8 bytes)
{
//      UINT32 uValue;
        INT32 value;

        switch (bytes)
        {
        case 1:
                value = (INT32)(buffer[0]);
                if (value > 0x7F)
                {
//                              value = value - 0x100;
                        value |= 0xFFFFFF00;
                }
                break;
        case 2:
                value = (INT32)(buffer[0]);
                value += ((INT32)buffer[1]) << 8;
                if (value > 0x7FFF)
                {
                        value = value - 0x10000;
                        value |= 0xFFFF0000;
                }
                break;
        case 4:
                value = buffer[0];
                value += ((INT32)buffer[1]) << 8;
                value += ((INT32)buffer[2]) << 16;
                value += ((INT32)buffer[3]) << 24;
                break;
        default:
                printError("LuxBase::readValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
                value = 0xFFFFFFFF;
        }

        return value;
}



UINT32 LuxBase::readUValueBE(UINT8* buffer, UINT8 bytes)
{
        UINT32 value;

        switch (bytes)
        {
        case 1:
                value = buffer[0];
                break;
        case 2:
                value = buffer[1];
                value += ((UINT32)buffer[0]) << 8;
                break;
        case 4:
                value = buffer[3];
                value += ((UINT32)buffer[2]) << 8;
                value += ((UINT32)buffer[1]) << 16;
                value += ((UINT32)buffer[0]) << 24;
                break;
        default:
                printError("LuxBase::readUValueBE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
                value = 0xFFFFFFFF;
        }

        return value;
}



//
// Decodes the data in the input buffer:
// - Syncs to magic word.
// - Decodes data type and length
// - If dataset is complete,
//     - calls decoding of scan and object data directly
//     - transfers command replys to reply buffer
//
// Returns the datatype of the processed message. Note that when this
// function returns, the message is already decoded and removed from the buffer!
//
// Note: Access to input buffer (and reply buffer) must be mutex'ed.
//
UINT16 LuxBase::decodeAnswerInInputBuffer()
{
        bool beVerboseHere = false;     // = m_beVerbose;
        printInfoMessage("LuxBase::decodeAnswerInInputBuffer: Called.", beVerboseHere);

        const UINT32 headerLen = 24;    // Length of data header, in [bytes]
        UINT16 datatype = 0;

        // Enough data for a header?
        if (m_inBufferLevel <= headerLen)
        {
                // Not enough data
                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Not enough data in input buffer, just " +
                                                 toString(m_inBufferLevel) + " bytes available, done.", beVerboseHere);
                return 0;
        }

        // Sync to magic word. This should not happen too often, as data should come in structured - unless
        // we start to miss complete IP packages...
        UINT32 magicWord = 0;
        UINT32 end = m_inBufferLevel - 4 + 1;
        UINT32 i;
        for (i = 0; i < end; i++)
        {
                magicWord = readUValueBE(&(m_inputBuffer[i]), 4);
                if (magicWord == 0xAFFEC0C2)
                {
                        printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found at pos " + toString(i) + ".", beVerboseHere);
                        break;
                }
        }

        if ((i > 0) && (magicWord != 0xAFFEC0C2))
        {
                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, and no magic word found - clearing buffer.", beVerboseHere);
                m_inBufferLevel = 0;

                // That was it
                return 0;
        }
        else if ((i > 0) && (magicWord == 0xAFFEC0C2))
        {
                // Adjust buffer by i bytes
                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, adjusting the buffer by " + toString(i) + " bytes.", beVerboseHere);

                removeDataFromInputBuffer(i);
        }

        // Magic word found?
        if (magicWord != 0xAFFEC0C2)
        {
                // Not found.
                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word not found, aborting!", beVerboseHere);
                return 0;
        }
        else
        {
                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found, now decoding header.", beVerboseHere);
        }

        // Complete message header found?
        if (m_inBufferLevel >= headerLen)
        {
                // Yes, we have a data header. We now calculate the size of the complete message.
                UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);

                printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);

                // Is the message complete?
                if (m_inBufferLevel >= (payloadLen + headerLen))
                {
                        // The command is completely in the buffer, so now get its datatype
                        datatype = readUValueBE(&(m_inputBuffer[14]), 2);

                        // What is it?
                        switch (datatype)
                        {
                        case 0x2202:            // Scan data
                                if (m_weWantScanData == true)
                                {
                                        decodeScan();
                                }
                                removeDataFromInputBuffer(headerLen + payloadLen);
                                break;
                        case 0x2221:            // Object data
                                if (m_weWantObjectData == true)
                                {
                                        decodeObjects();
                                }
                                removeDataFromInputBuffer(headerLen + payloadLen);
                                break;
                        case 0x2020:            // Command reply
                                // It is a reply, transfer to cmd reply buffer
                                moveDataFromInputToCmdBuffer(headerLen + payloadLen);
                                break;
                        case 0x2030:            // Error message
                                decodeErrorMessage();
                                removeDataFromInputBuffer(headerLen + payloadLen);
                                break;
                        case 0x2805:            // VehicleStateBasic
                                removeDataFromInputBuffer(headerLen + payloadLen);
                                break;
                        case 0x7100:            // SensorInfo
                                decodeSensorInfo();
                                removeDataFromInputBuffer(headerLen + payloadLen);
                                break;
                        default:
                                // Unknown!
                                printWarning("LuxBase::decodeAnswerInInputBuffer(): Unknown datatype 0x" + toHexString(datatype) +
                                                                "(Length=" + ::toString(headerLen + payloadLen) +  " bytes) in buffer, removing this dataset.");
                                removeDataFromInputBuffer(headerLen + payloadLen);
                        }
                }
        }
        
        if (beVerboseHere == true)
        {
                if (datatype > 0)
                {
                        infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, datatype 0x" + toHexString(datatype) + ".");
                }
                else
                {
                        infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, but message is incomplete.");
                        dumpHeader();
                }
        }
        return datatype;
}



//
// Decodes an error message that is completely present as the first message in the input buffer.
//
void LuxBase::decodeErrorMessage()
{
//      printInfoMessage("decodeErrorMessage(): There is an error/warning message.", m_beVerbose);

        UINT8* errorBuffer = &(m_inputBuffer[24]);      // Skip the data header

        m_errorRegister1 = (UINT16)readUValueLE(&(errorBuffer[0]), 2);
        m_errorRegister2 = (UINT16)readUValueLE(&(errorBuffer[2]), 2);
        m_warnRegister1 = (UINT16)readUValueLE(&(errorBuffer[4]), 2);
        m_warnRegister2 = (UINT16)readUValueLE(&(errorBuffer[6]), 2);

        // Was there an error?
        if ((m_errorRegister1 != 0) || (m_errorRegister2 != 0))
        {
                // There is an error. Generate a readable error message.
                std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports an error: errReg1=0x" + toHexString(m_errorRegister1) +
                                          ", errReg2=0x" + toHexString(m_errorRegister2) + ". Meaning: ";
                
                // First, error register 1
                if ((m_errorRegister1 & 0x0004) != 0)
                {
                        text += "<Scan buffer transmitted incompletely> ";
                }
                if ((m_errorRegister1 & 0x0008) != 0)
                {
                        text += "<Scan buffer overflow> ";
                }
                if ((m_errorRegister1 & 0x0300) == 0x0300)
                {
                        text += "<APD temperature sensor defective> ";
                }
                else
                {
                        if ((m_errorRegister1 & 0x0100) != 0)
                        {
                                text += "<APD undertemperature> ";
                        }
                        if ((m_errorRegister1 & 0x0200) != 0)
                        {
                                text += "<APD overtemperature> ";
                        }
                }
        
                // Then, error register 2
                if ((m_errorRegister2 & 0x0001) != 0)
                {
                        text += "<No scan data received> ";
                }
                if ((m_errorRegister2 & 0x0010) != 0)
                {
                        text += "<Incorrect configuration data> ";
                }
                if ((m_errorRegister2 & 0x0020) != 0)
                {
                        text += "<Configuration contains incorrect parameters> ";
                }
                if ((m_errorRegister2 & 0x0040) != 0)
                {
                        text += "<Data processing timeout> ";
                }
                if ((m_errorRegister2 & Err_FlexResParameter) != 0)
                {
                        text += "<Incorrect flex. res. configurarion> ";
                }


                // Finally, all other internal errors
                if (((m_errorRegister1 & 0x3C13) != 0) ||
                        ((m_errorRegister2 & 0x008E) != 0))
                {
                        text += "<Some other error> ";
                }
                
                
                printWarning(text);
        }
        else
        {
                // There must have been a warning.
                std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports a warning: warnReg1=0x" + toHexString(m_warnRegister1) +
                                                  ", warnReg2=0x" + toHexString(m_warnRegister2) + ". Meaning: ";

                // First, warn register 1

                if ((m_warnRegister1 & 0x0001) != 0)
                {
                        text += "<Internal communication error> ";
                }
                if ((m_warnRegister1 & 0x0008) != 0)
                {
                        text += "<Warning of insufficient temperature> ";
                }
                if ((m_warnRegister1 & 0x0010) != 0)
                {
                        text += "<Warning of exceeding temperature> ";
                }
                if ((m_warnRegister1 & 0x0080) != 0)
                {
                        text += "<Check syncronisation- and scan frequency> ";
                }

                // Then, warn register 2
                if ((m_warnRegister2 & 0x0001) != 0)
                {
                        text += "<CAN interface blocked> ";
                }
                if ((m_warnRegister2 & 0x0002) != 0)
                {
                        text += "<Ethernet interface blocked> ";
                }
                if ((m_warnRegister2 & 0x0004) != 0)
                {
                        text += "<Incorrect CAN message received> ";
                }
                if ((m_warnRegister2 & 0x0010) != 0)
                {
                        text += "<Check ethernet data> ";
                }
                if ((m_warnRegister2 & 0x0020) != 0)
                {
                        text += "<Incorrect or forbidden command received> ";
                }
                if ((m_warnRegister2 & 0x0040) != 0)
                {
                        text += "<Memory access failure> ";
                }
                
                if ((m_warnRegister2 & 0x0008) != 0)
                {
                        text += "<Some other warning> ";
                }
                
                printInfoMessage(text, true);
        }

        // Send the data also to the manager for use in the application(s)
        std::ostringstream registers;
        registers << "FPGA Error: 0x" << std::hex << m_errorRegister1 << "; ";
        registers << "DSP Error: 0x" << std::hex << m_errorRegister2 << "; ";
        registers << "FPGA Warn: 0x" << std::hex << m_warnRegister1 << "; ";
        registers << "DSP Warn: 0x" << std::hex << m_warnRegister2;
        Msg* msg = new Msg(m_deviceId, registers.str());
        m_manager->setDeviceData(msg);
}



//
// Decodes a scan that is completely present as the first message in the input buffer.
void LuxBase::decodeScan()
{
//      printInfoMessage("LuxBase::decodeScan(): We have received a scan that is now being decoded.", m_beVerbose);

        // Sollen wir jemanden informieren?
        if (m_onScanReceiveCallback != NULL)
        {
                // Ja, machen.
                m_onScanReceiveCallback(m_onScanReceiveCallbackObjPtr);
        }

        // Scan decodieren
        Scan* scan = new Scan;
        UINT8* scanBuffer = &(m_inputBuffer[24]);       // Skip the data header

        // Decode the scan
        UINT16 scanNumber = (UINT16)readUValueLE(&(scanBuffer[0]), 2);
        scan->setScanNumber(scanNumber);

//      UINT16 scannerStatus = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
//      UINT16 syncPhaseOffset = (UINT16)readUValueLE(&(scanBuffer[4]), 2);

        UINT64 timestampStart = (UINT64)readUINT64ValueLE(&(scanBuffer[6]));
//      NTPTime startTime = NTPTime (timestamp);        // read NTPTime using Little Endian
        UINT64 timestampEnd = (UINT64)readUINT64ValueLE(&(scanBuffer[14]));
//      NTPTime endTime   = NTPTime (timestamp);

        INT16 startAngleTicks = (INT16)readValueLE(&(scanBuffer[24]), 2);
        double startAngle = convertTicktsToAngle(startAngleTicks);                              // startAngle is in [rad]
        INT16 endAngleTicks =   (INT16)readValueLE(&(scanBuffer[26]), 2);
        double endAngle = convertTicktsToAngle(endAngleTicks);                                  // endAngle is in [rad]
        UINT16 scanPoints =     (UINT16)readUValueLE(&(scanBuffer[28]), 2);

        // Scanner mounting position
        INT16 mountingPosYawTicks       = (INT16)readValueLE(&(scanBuffer[30]), 2);
        INT16 mountingPosPitchTicks = (INT16)readValueLE(&(scanBuffer[32]), 2);
        INT16 mountingPosRollTicks      = (INT16)readValueLE(&(scanBuffer[34]), 2);
        INT16 mountingPosX                      = (INT16)readValueLE(&(scanBuffer[36]), 2);
        INT16 mountingPosY                      = (INT16)readValueLE(&(scanBuffer[38]), 2);
        INT16 mountingPosZ                      = (INT16)readValueLE(&(scanBuffer[40]), 2);

        // Processing flags
        // Meaning:
        // Bit 0:  ground detection performed: 0 = false, 1 = true
        // Bit 1:  dirt detection performed: 0 = false, 1 = true
        // Bit 2:  rain detection performed: 0 = false, 1 = true
        // Bit 5:  transparency detection performed: 0 = false, 1 = true
        // Bit 6:  horizontal angle offset added: 0 = false, 1 = true
        // Bit 10: mirror side: 0=front (for 8-Layer, tilted downward), 1=rear (for 8-layer, tilted upward)
        // All other flags are reserved-internal and should not be evaluated.
        volatile bool isRearMirrorSide = true;
        INT16 processingFlags           = (INT16)readValueLE(&(scanBuffer[42]), 2);
        if ((processingFlags & 0x0400) == 0)
        {
//              printInfoMessage("LuxBase::decodeScan(): Mirror side 0.", true);
                isRearMirrorSide = false;
        }
        else
        {
//              printInfoMessage("LuxBase::decodeScan(): Mirror side 1.", true);
                isRearMirrorSide = true;
        }

        // Time per angle (rad). Used later to calculate the time of the beam inside the scan.
//      double angleTime;       // Time in the scan for 1 degree
//      if (m_scanFrequency > 0)
//      {
//              angleTime = (1.0 / (double)m_scanFrequency) / (360.0 * deg2rad); // in [s/rad]
//      }
//      else
//      {
//              angleTime = 0.001; // Default to avoid errors: 1 ms in [s]; for emergencies only.
//      }

        // Scan point list
        UINT8* scanPtBuffer;
        for (UINT16 s = 0; s < scanPoints; s++)
        {
                scanPtBuffer = &(scanBuffer[44 + s*10]);        // Start of the scanpoint
                UINT8 layerAndEcho = (UINT8)readUValueLE(&(scanPtBuffer[0]), 1);
                UINT8 layer = layerAndEcho & 0x0F;                                                                                      // One of the 4 layers
                UINT8 echo = (layerAndEcho >> 4) & 0x0F;                                                                        // Number of the echo of this pulse
                UINT16 flags = (UINT16)readUValueLE(&(scanPtBuffer[1]), 1);                                     // 0x01:transparent; 0x02:clutter; 0x04:ground; 0x08: dirt
                INT16 horzAngleTicks = (INT16)readValueLE(&(scanPtBuffer[2]), 2);                       // H-Angle of this shot
                UINT16 radialDistanceCm = (UINT16)readUValueLE(&(scanPtBuffer[4]), 2);          // Radial distance in scanner coords
                UINT16 echoPulseWidthCm = (UINT16)readUValueLE(&(scanPtBuffer[6]), 2);          // Echo pulse width; Indicator for energy of incoming beam
                // Bytes 8 and 9 are reserved.

                // Now add the data to the scan structure
                ScanPoint& newPoint = scan->addNewPoint();

                // Horizontal angle
                double hAngle = convertTicktsToAngle(horzAngleTicks);   // hAngle is in [rad]

                // Vertical angle
                double vAngle = 0.0;
                vAngle = getVAngleOfLayer(isRearMirrorSide, layer, hAngle);
                
                // Radial distance
                double dist = (double)radialDistanceCm / 100.0; // cm to m

                // Set the coordinates of the new point. Also automatically generates x-y-z coordinates.
                newPoint.setPolar (dist, hAngle, vAngle);

                // Copy data to new scan point
                newPoint.setEchoWidth ((float)echoPulseWidthCm / 100.0);
                newPoint.setFlags (flags);
                newPoint.setSourceId (m_deviceId);
                newPoint.setLayer (layer);
                newPoint.setEchoNum (echo); // 0 or 1 or ...
//              newPoint.setTimeOffset (boost::posix_time::microseconds ((UINT32)((startAngle - hAngle) * angleTime * 1000000.0))); // Time offset of scan point
//              newPoint.setSegmentId(0); // Not available
        }

//      if (m_enableCoordTransformation == true)
//      {
//              scanFlags = Scan::FlagVehicleCoordinates;
//      }
        scan->setFlags(processingFlags);

        //
        // Set some information about the scanner
        //
        // Create Scanner Info
        ScannerInfo si;
        si.setStartAngle(startAngle);
        si.setEndAngle(endAngle);

        si.setScanFrequency(m_scanFrequency);
        si.setBeamTilt(m_beamTiltAngle);
//      si.setScannerStatus(scannerStatus);
        si.setScanFlags(processingFlags);
        si.setScanNumber(scanNumber);
        si.setDeviceID(m_deviceId);
        si.setScannerType(Sourcetype_LDMRS); // for compatibility, if no value is set in the scanner's config.
        Time start, end;
        start.set(timestampStart);
        end.set(timestampEnd);
        si.setTimestamps(start, end);
        si.setProcessingFlags(processingFlags); // Mirror side etc.

        // Mounting position
        double yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ;
        yawAngle = convertTicktsToAngle(mountingPosYawTicks);
        pitchAngle = convertTicktsToAngle(mountingPosPitchTicks);
        rollAngle = convertTicktsToAngle(mountingPosRollTicks);
        offsetX = (double)mountingPosX / 100.0;
        offsetY = (double)mountingPosY / 100.0;
        offsetZ = (double)mountingPosZ / 100.0;
        Position3D mp(yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ);
        si.setMountingPosition(mp);
        scan->setScannerInfos(Scan::ScannerInfoVector(1, si));
//      scan.setStartTimestamp(startTime);
//      scan.setEndTimestamp(endTime);

        // Post this scan to the world above...
        scan->setSourceId(m_deviceId);
        m_manager->setDeviceData(scan);

//      printInfoMessage("decodeScan(): Decoded scan with " + toString(scanPoints) + " points.", m_beVerbose);
}

void LuxBase::decodeSensorInfo()
{
        UINT8* sensorInfoBuffer = &(m_inputBuffer[24]);   // Skip the data header

        // decode sensor info
//      UINT16 sensorInfoVersion = (UINT16)readUValueLE(&(scanBuffer[0]), 2);   // here: 1
//      UINT16 relatedScanNumber = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
//      UINT16 fpgaErrorRegister1 = (UINT16)readUValueLE(&(scanBuffer[4]), 2);
//      UINT16 fpgaErrorRegister2 = (UINT16)readUValueLE(&(scanBuffer[6]), 2);
//      UINT16 fpgaWarningRegister = (UINT16)readUValueLE(&(scanBuffer[8]), 2);
//      UINT16 dspWarningRegister = (UINT16)readUValueLE(&(scanBuffer[10]), 2);

        m_temperature = (UINT16)readUValueLE(&(sensorInfoBuffer[12]), 2);

//      ...
}


/*
 * Calculate the elevation angle of the scanpoint depending on scan layer, mirror side, horziontal angle and UpsideDown flag.
 */
double LuxBase::getVAngleOfLayer(bool isRearMirrorSide, UINT8 layerNumber, double hAngle)
{
        // Calculate the effective mirror tilt as a function of the beam tilt at 0 degree horizontally and the current horizontal angle
        const double effMirrorTilt = m_beamTiltAngle / 2.0 * M_SQRT2 * std::sin(0.5 * hAngle - M_PI_4);

        // Distinguish between the front and back side of the mirror.
        // Front mirror side: beam tilted/pitched down (positive pitch), rear mirror side: beam tilted/pitched up (negative pitch)
        double mirrorDirection = 1.0;
        if (isRearMirrorSide == false)
        {
                 mirrorDirection = -1.0;
        }

        // Take UpsideDown into account
        if (m_upsideDownActive == true)
        {
                mirrorDirection *= -1.0;
        }

        // Calculate the layer offset of each layer. This calculation defines 0 degree as a symetrical layer.
        const double numLayers = 4.0;
        const double verticalBeamDivergence = 0.8 * deg2rad;
        const double layerOffset = ((numLayers - 1) / 2 - layerNumber) * verticalBeamDivergence;
        const double vAngle = layerOffset + mirrorDirection * 2 * effMirrorTilt;

        return vAngle;
}



//
// Converts an angle from ticks (MRS-internal unit) to radians.
//
double LuxBase::convertTicktsToAngle(INT16 angleTicks)
{
        double angle = ((double)angleTicks / 32.0) * deg2rad;

        if ((angle > -1000.0) && (angle < 1000.0))
        {
                // Angle ist in "sinnvollem" Bereich
                while (angle > PI)
                {
                        angle -= 2.0 * PI;
                }
                while (angle <= -PI)
                {
                        angle += 2.0 * PI;
                }
        }
        else
        {
                // Winkel ist unsinning
                printError("convertTicktsToAngle(): The angle value " + toString(angle, 2) + " is widely out of the reasonable range, setting to 0.0.");
                angle = 0.0;
        }

        return angle;
}



//
// Decodes object data that is completely present as the first message in the input buffer.
//
// Note that not all LD-MRS versions can deliver object data.
//
void LuxBase::decodeObjects()
{
        printInfoMessage("decodeObjects(): We have received a dataset and are now decoding objects.", m_beVerbose);
//      printWarning("decodeObjects(): We have received a dataset, BUT THIS FUNCTION IS NOT IMPLEMENTED, ignoring the data!");
//      return;

        // Unlike the scan data decoder, we are using a bufferOffset pointer here. As the number of contour
        // points varies for each object, this is a simple way to keep track of our position in the buffer.
        UINT32 bufferOffset = 24;

        ObjectList* objectList = new ObjectList;        // The container for the output data

        // Decode the number of objects
        UINT16 numObjects = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset + 8]), 2);
        bufferOffset = 24 + 10;

        for (UINT16 i = 0; i < numObjects; i++)
        {
                Object newObject;

                // Offset 0: Object ID
                UINT16 objectId = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                newObject.setObjectId (objectId);

                // Offset 2: Object age
                UINT16 objectAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                newObject.setObjectAge (objectAge);

                // Offset 4: Object prediction age
                UINT16 objectPredictionAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                newObject.setHiddenStatusAge (objectPredictionAge);

                // Offset 6: Relative timestamp
                UINT16 relativeTimestamp = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                Time t;
                t.set((double)relativeTimestamp);
                newObject.setTimestamp (t);

                // Offset 8: Reference point
                Point2D referencePoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                newObject.setCenterPoint (referencePoint);

                // Offset 12: Reference point sigma
                Point2D referencePointSigma = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                newObject.setCenterPointSigma(referencePointSigma);

                Point2D closestPoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                newObject.setClosestPoint (closestPoint);

                Point2D boundingBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                newObject.setBoundingBoxCenter(boundingBoxCenter);

                Point2D boundingBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
                double tmp = boundingBoxSize.getX();
                // x and y are flipped on the wire
                boundingBoxSize.setX(boundingBoxSize.getY());
                boundingBoxSize.setY(tmp);
                bufferOffset += 4;
                newObject.setBoundingBox(boundingBoxSize);

                //Point2D objectBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;

                Point2D objectBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                newObject.setObjectBox (objectBoxSize);

                INT16 objectBoxOrientationTicks = (INT16)readValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                newObject.setCourseAngle (convertTicktsToAngle(objectBoxOrientationTicks));

                Point2D absoluteVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                Point2D absoluteVelocitySigma = readSize2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;
                Point2D relativeVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
                bufferOffset += 4;

                if (absoluteVelocity.getX() < -320.0)
                {
                        // Absolute velocity is invalid, use relative velocity instead
                        newObject.setAbsoluteVelocity (relativeVelocity);
                        newObject.setRelativeVelocitySigma (absoluteVelocitySigma);
                        newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
                }
                else
                {
                        // Absolute velocity is valid. This will only be the case if vehicle movement data is
                        // sent to the sensor via CAN.
                        newObject.setAbsoluteVelocity (absoluteVelocity);
                        newObject.setRelativeVelocitySigma (Point2D(NaN_double, NaN_double));
                        newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
                }
                newObject.setRelativeVelocity (relativeVelocity);

                UINT16 classification = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                switch (classification)
                {
                case ClassUnclassified:
                        newObject.setClassification (Object::Unclassified);
                        break;
                case ClassUnknownSmall:
                        newObject.setClassification (Object::UnknownSmall);
                        break;
                case ClassUnknownBig:
                        newObject.setClassification (Object::UnknownBig);
                        break;
                case ClassPedestrian:
                        newObject.setClassification (Object::Pedestrian);
                        break;
                case ClassBike:
                        newObject.setClassification (Object::Bike);
                        break;
                case ClassCar:
                        newObject.setClassification (Object::Car);
                        break;
                case ClassTruck:
                        newObject.setClassification (Object::Truck);
                        break;
                default:
                        newObject.setClassification(Object::Unknown);
                }

                UINT16 classificationAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                newObject.setClassificationAge (classificationAge);

                UINT16 classificationCertainty = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                if (classificationCertainty <= 100)
                {
                        newObject.setClassificationQuality(classificationCertainty / 100.f);
                }
                else
                {
                        // Invalid value, set to 0
                        newObject.setClassificationQuality(0);
                }

                // Contour points of this object
                UINT16 numContourPoints = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
                bufferOffset += 2;
                // Bugfix: If the scanner reports 0xFFFF, he means "1"...
                if (numContourPoints == 0xFFFF)
                {
                        numContourPoints = 1;
                }
                
                Point2D cp;
                for (UINT16 c = 0; c < numContourPoints; c++)
                {
                        cp = readPoint2D(&(m_inputBuffer[bufferOffset]));
                        bufferOffset += 4;
                        newObject.addContourPoint(cp);
                }

                // Transfer the object to the object list
                objectList->push_back (newObject);
        }

        // Set the remaining fields of the object list and send it
        objectList->setSourceId(m_deviceId);
        m_manager->setDeviceData(objectList);

        printInfoMessage("LuxBase::decodeObjects(): We have received " + toString(numObjects) + " objects. Now leaving.", m_beVerbose);
}

//
// Reads a Point2D structure, represented in the buffer by two INT16 values.
// Output unit is [m].
//
Point2D LuxBase::readPoint2D(UINT8* buffer)
{
        INT16 value1 = (INT16)readValueLE(buffer, 2);
        INT16 value2 = (INT16)readValueLE(&(buffer[2]), 2);

        return Point2D(((double)value1 / 100.0), ((double)value2 / 100.0));
}

//
// Reads a Size2D structure, represented in the buffer by two INT16 values.
// For simplicity, a Point2D container is used for the data.
// Output unit is [m].
//
Point2D LuxBase::readSize2D(UINT8* buffer)
{
        UINT16 value1 = (UINT16)readUValueLE(buffer, 2);
        UINT16 value2 = (UINT16)readUValueLE(&(buffer[2]), 2);
        Point2D s((double)value1 / 100.0, (double)value2 / 100.0);
        return s;
}

//
// Moves data from the input to the command reply buffer, and removes this data from the
// input buffer. Note that any data that may have been present in the reply buffer is
// overwritten, so be sure to mutex this section against cross-thread access.
//
void LuxBase::moveDataFromInputToCmdBuffer(UINT32 bytesToBeMoved)
{
        if (m_beVerbose == true)
        {
                infoMessage("moveDataFromInputToCmdBuffer(): Moving " + toString(bytesToBeMoved) + " bytes from input to command buffer.");
                if (m_cmdBufferLevel > 0)
                {
                        infoMessage("moveDataFromInputToCmdBuffer(): ...and " + toString(m_cmdBufferLevel) + " bytes in the command buffer were destroyed in the process!");
                }
        }

        memcpy(m_cmdReplyBuffer, m_inputBuffer, bytesToBeMoved);
        m_cmdBufferLevel = bytesToBeMoved;
        removeDataFromInputBuffer(bytesToBeMoved);
}


void LuxBase::removeDataFromInputBuffer(UINT32 bytesToBeRemoved)
{
        if (bytesToBeRemoved == m_inBufferLevel)
        {
                // All data should be removed
                m_inBufferLevel = 0;
                return;
        }
        else if (bytesToBeRemoved > m_inBufferLevel)
        {
                // Error: We do not have so much data
                printError("removeDataFromInputBuffer(): The buffer holds " + toString(m_inBufferLevel) + " bytes, but " +
                                           toString(bytesToBeRemoved) + " bytes should be removed - clearing buffer.");
                m_inBufferLevel = 0;
                return;
        }

        // Do the shift
        UINT32 bytesRemaining = m_inBufferLevel - bytesToBeRemoved;
        memmove(&(m_inputBuffer[0]), &(m_inputBuffer[bytesToBeRemoved]), bytesRemaining);
        m_inBufferLevel = bytesRemaining;
}


UINT16 LuxBase::decodeAnswerInCmdReplyBuffer()
{
        bool beVerboseHere = false;     // = m_beVerbose;
        printInfoMessage("Entering decodeAnswerInCmdReplyBuffer().", beVerboseHere);

        const UINT32 headerLen = 24;    // Length of data header, in [bytes]
        UINT16 commandId = 0;

        // Check to be sure the data in the buffer is complete
        if (m_cmdBufferLevel == 0)
        {
                // No data. Check is there is news from the input buffer.
                decodeAnswerInInputBuffer();

                if (m_cmdBufferLevel == 0)
                {
                        // Still no data
                        printInfoMessage("decodeAnswerInCmdReplyBuffer(): No data in input buffer.", beVerboseHere);
                        return 0;
                }
        }

        // There is data in the buffer. Now perform some consistency checks.
        if (m_cmdBufferLevel <= headerLen)
        {
                printError("decodeAnswerInCmdReplyBuffer(): ERROR: Not enough data in reply buffer for the data header, just " +
                                        toString(m_cmdBufferLevel) + " bytes available. Clearing buffer!");
                m_cmdBufferLevel = 0;
                return 0;
        }

        // Ensure that data starts with magic word
        UINT32 magicWord;
        magicWord = readUValueBE(&(m_cmdReplyBuffer[0]), 4);
        if (magicWord != 0xAFFEC0C2)
        {
                printError("decodeAnswerInCmdReplyBuffer(): Magic word not found, clearing cmd buffer.");
                m_cmdBufferLevel = 0;
                return 0;
        }
        else
        {
                // Magic word found
                printInfoMessage("decodeAnswerInCmdReplyBuffer(): Magic word found, now decoding header.", beVerboseHere);
        }

        // Yes, we have a data header. We now calculate the size of the complete message.
        UINT32 payloadLen = readUValueBE(&(m_cmdReplyBuffer[8]), 4);

        printInfoMessage("decodeAnswerInCmdReplyBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);

        // Is the message complete and length consistent?
        if (m_cmdBufferLevel == (payloadLen + headerLen))
        {
                // The command is completely in the buffer, so now return its command ID
                commandId = readUValueLE(&(m_cmdReplyBuffer[24]), 2);
                printInfoMessage("decodeAnswerInCmdReplyBuffer(): Header decoded successfully, command = " +
                                                toHexString(commandId) + ".", beVerboseHere);
        }
        else
        {
                printError("decodeAnswerInCmdReplyBuffer(): Inconsistent length of data in reply buffer, expected length was " +
                                          toString(payloadLen + headerLen) + ", but buffer length is " + toString(m_cmdBufferLevel) + " bytes - clearing buffer.");
                m_cmdBufferLevel = 0;
                return 0;
        }

        printInfoMessage("decodeAnswerInCmdReplyBuffer(): Leaving successfully, command = " + toHexString(commandId) + ".", beVerboseHere);
        return commandId;
}

//
// Debug helper.
//
void LuxBase::dumpHeader()
{
        std::ostringstream s;
        s << "Header:";
        for (UINT32 i = 0; i < 24; i++)
        {
                s << " " << toHexString(m_inputBuffer[i]);
        }

        infoMessage(s.str());
}



void LuxBase::dumpMessage()
{
        std::ostringstream s;
        s << "Message:";
        UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);

        for (UINT32 i = 0; i < payloadLen; i++)
        {
                s << " " << toHexString(m_inputBuffer[24+i]);
        }

        infoMessage(s.str());
}



void LuxBase::removeAnswerFromInputBuffer()
{
        const UINT32 headerLen = 24;    // Length of data header, in [bytes]

        // Check if a command can be complete. Any command has at least the header + 2 bytes command ID.
        if (m_inBufferLevel <= (headerLen + 1))
        {
                printWarning("LuxBase::removeAnswerFromInputBuffer: Not enough data in input buffer to remove a dataset, aborting!");
                return;
        }

        // Check magic word
        UINT32 magicWord;
        magicWord = readUValueBE(&(m_inputBuffer[0]), 4);
        if (magicWord != 0xAFFEC0C2)
        {
                printError("LuxBase::removeAnswerFromInputBuffer: Magic word does not match, aborting!");
                return;
        }

        // Complete message?
        UINT32 msgLen = headerLen + readUValueBE(&(m_inputBuffer[8]), 4);
        if (m_inBufferLevel < msgLen)
        {
                printError("LuxBase::removeAnswerFromInputBuffer: The buffer does not hold enough data for the message!");
                return;
        }

        // Ok, remove the data
        if (m_inBufferLevel == msgLen)
        {
                // There is only this message in the buffer, so just invalidate it
                m_inBufferLevel = 0;
                printInfoMessage("removeAnswerFromInputBuffer(): Clearing the whole buffer.", m_beVerbose);
        }
        else
        {
                // Remove the first message. Can be replaced by memmove().
                for (UINT32 i = msgLen; i < m_inBufferLevel; i++)
                {
                        m_inputBuffer[i-msgLen] = m_inputBuffer[i];
                }
                m_inBufferLevel -= msgLen;

                printInfoMessage("LuxBase::removeAnswerFromInputBuffer(): Removed " + toString(msgLen) + " bytes from buffer, new size is " +
                                                        toString(m_inBufferLevel) + " bytes.", m_beVerbose);
        }
}



//
//
//
bool LuxBase::receiveMrsReply(MrsCommandId cmd, UINT32 timeoutMs, UINT32* value)
{
        bool beVerboseHere = m_beVerbose;
        
        printInfoMessage("LuxBase::receiveMrsReply: Entering.", beVerboseHere);

        // For the timeout
        UINT32 maxLoops = timeoutMs;
        bool result = false;

        for (UINT32 i = 0; i < maxLoops; i++)
        {
                // Read the data, if any
                {
                        ScopedLock lock(&m_inputBufferMutex);           // Mutex for access to the input buffer
                        UINT16 cmdInBuffer = decodeAnswerInCmdReplyBuffer();    // Get the command ID, if any
                        if (cmdInBuffer == cmd)
                        {
                                // Ok, it is the wanted reply
                                printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " There is a valid message (cmd=0x" +
                                                                        toHexString(cmdInBuffer) + ").", beVerboseHere);
                                if (cmd == CmdMrsGetStatus)
                                {
                                        printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Decoding a status message.", beVerboseHere);
                                        decodeGetStatus();
                                }
                                if (cmd == CmdMrsStopMeasure)
                                {
                                        printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StopMeasure acknowledge received.", beVerboseHere);
                                }
                                if (cmd == CmdMrsStartMeasure)
                                {
                                        printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StartMeasure acknowledge received.", beVerboseHere);
                                }
                                if (cmd == CmdMrsGetParameter)
                                {
                                        printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " GetParameter acknowledge received.", beVerboseHere);
                                        if (value != NULL)
                                        {
                                                decodeGetParameter(value);
                                        }
                                }

                                result = true;
                        }

                        // Remove the received answer from the reply buffer
                        if (cmdInBuffer != 0)
                        {
                                printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Removing the command from the reply buffer.", beVerboseHere);
                                m_cmdBufferLevel = 0;
                        }

                        if (cmdInBuffer == 0x8000 + cmd)
                        {
                                printWarning("LuxBase::receiveMrsReply: " + m_longName + " Received that an error occurred.");
                                result = false;
                                break;
                        }
                }

                if (result == true)
                {
                        break;
                }

                // Schlafe eine ms
                usleep(1000);
        }

        printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Leaving.", beVerboseHere);
        return result;
}



bool LuxBase::sendMrsCommand(MrsCommandId cmd, UINT16 para, UINT32 value)       // , boost::posix_time::time_duration timeOut)
{
        UINT8 cmdBuffer[256];
        UINT32 sizeOfThisMsg = 0;

        // Determine the size of the message to be sent
        switch (cmd)
        {
        case CmdMrsGetStatus:
                sizeOfThisMsg = 2 + 2;  // 4 Bytes
                break;
        case CmdMrsStopMeasure:
                sizeOfThisMsg = 2 + 2;  // 4 Bytes
                break;
        case CmdMrsStartMeasure:
                sizeOfThisMsg = 2 + 2;  // 4 Bytes
                break;
        case CmdMrsGetParameter:
                sizeOfThisMsg = 2 + 2 + 2;      // 6 Bytes
                break;
        case CmdMrsSetParameter:
                sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
                break;
        case CmdMrsSetNTPTimestampSec:
                sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
                break;
        case CmdMrsSetNTPTimestampFracSec:
                sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
                break;
        default:
                printError("LuxBase::sendMrsCommand: " + m_longName + " ERROR: Unknown command to be sent (ID=0x" +
                                        toHexString((UINT16)cmd) + "), aborting!");
                return false;
        }

        // Build the header in the buffer
        // DataTypeCommand      = 0x2010,       ///< Command for a device
        // DataTypeReply        = 0x2020,       ///< Reply of a previous command
        UINT32 bufferPos;
        bufferPos = buildDataHeader(cmdBuffer, sizeOfThisMsg, m_deviceId, 0x2010);      // DataTypeCommand);

        // Add the payload
        bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), cmd, 2); // Note: Little Endian always!
        UINT16 version = 3;
        bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), version, 2); // Note: Little Endian!

        // For a setParameter command, add the parameter index and the value to be set
        if (cmd == CmdMrsSetParameter
                        || cmd == CmdMrsSetNTPTimestampSec
                        || cmd == CmdMrsSetNTPTimestampFracSec)
        {
                bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
                bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), value, 4);
        }
        else if (cmd == CmdMrsGetParameter)
        {
                bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
        }

        // Write the data to the interface
        UINT32 bytesToSend = bufferPos;
        bool result = m_tcp.write(cmdBuffer, bytesToSend);

        return result;
}



//
// Big-Endian-Write
//
UINT32 LuxBase::writeValueToBufferBE(UINT8* buffer, UINT32 value, UINT8 numBytes)
{
        switch (numBytes)
        {
        case 1:
                buffer[0] = value & 0xff;
                break;
        case 2:
                buffer[0] = (value & 0xff00) >> 8;
                buffer[1] = value & 0xff;
                break;
        case 4:
                buffer[0] = (value & 0xff000000) >> 24;
                buffer[1] = (value & 0xff0000) >> 16;
                buffer[2] = (value & 0xff00) >> 8;
                buffer[3] = value & 0xff;
                break;
        default:
                printError("LuxBase::writeValueToBufferBE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
        }

        return (UINT32)numBytes;
}



//
// Little-Endian-Write
//
UINT32 LuxBase::writeValueToBufferLE(UINT8* buffer, UINT32 value, UINT8 numBytes)
{
        switch (numBytes)
        {
        case 1:
                buffer[0] = value & 0xff;
                break;
        case 2:
                buffer[1] = (value & 0xff00) >> 8;
                buffer[0] = value & 0xff;
                break;
        case 4:
                buffer[3] = (value & 0xff000000) >> 24;
                buffer[2] = (value & 0xff0000) >> 16;
                buffer[1] = (value & 0xff00) >> 8;
                buffer[0] = value & 0xff;
                break;
        default:
                printError("LuxBase::writeValueToBufferLE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
        }

        return (UINT32)numBytes;
}



UINT32 LuxBase::buildDataHeader(UINT8* buffer, UINT32 sizeOfThisMsg, UINT8 deviceId, UINT16 dataType)
{
        UINT32 magicWord = 0xAFFEC0C2;
        UINT32 sizeOfPrevMsg = 0;
        UINT8 reserved = 0;
        
        // Status: AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0100 0000
        // Reset:  AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0000 0000
        //
        // Fill the buffer
        writeValueToBufferBE (&(buffer[0]), magicWord, 4);                      // 4
        writeValueToBufferBE (&(buffer[4]), sizeOfPrevMsg, 4);          // 4 (8)
        writeValueToBufferBE (&(buffer[8]), sizeOfThisMsg, 4);          // 4 (12)
        writeValueToBufferBE (&(buffer[12]), reserved, 1);                      // 1 (13)
        writeValueToBufferBE (&(buffer[13]), deviceId, 1);                      // 1 (14)
        writeValueToBufferBE (&(buffer[14]), dataType, 2);                      // 2 (16)

        // Timestamp
        UINT32 timeSec = 0;
        UINT32 timeSecFractionOffset = 0;
        writeValueToBufferBE (&(buffer[16]), timeSec, 4);                               // 4 (20)
        writeValueToBufferBE (&(buffer[20]), timeSecFractionOffset, 4); // 4 (24)

        return 24;      // Header is always 24 Bytes long
}



//
// RUN-Modus:
//
// Starte den Scanner, und abonniere Scans und/oder Objektdaten.
//
bool LuxBase::run(bool weWantScanData, bool weWantObjectData)
{
        bool result;

        if ((m_inputFileName == "") && (m_tcp.isOpen() == true))
        {
                // TCP connection was opened by the MRS::init() function 
//              printWarning("LuxBase::run(): TCP connection is alread open.");
        }
        else
        {
                // We are not connected, so connect now
                printInfoMessage("LuxBase::run(): We are called, but we are not connected. Calling init() now.", m_beVerbose);
                bool result = false;
                if (m_inputFileName == "")
                {
                        // TCP
                        result = initTcp(m_tcpDisconnectFunction, m_disconnectFunctionObjPtr);
                }
                else
                {
                        // TCP
                        result = initFile(m_fileDisconnectFunction, m_disconnectFunctionObjPtr);
                }

                if (result == false)
                {
                        // We failed to connect
                        printError("LuxBase::run(): Call to init() was not successful, aborting!");
                        return false;
                }
                else
                {
                        // We are connected
                        printInfoMessage("LuxBase::run(): Call to init() was successful, we are connected.", m_beVerbose);
                }
        }

        if (isRunning() == true)
        {
                // Error: Already running
                printInfoMessage("LuxBase::run(): Duplicate RUN command, " + m_longName + " is running already. Ignoring command.", true);
                return true;
        }

        printInfoMessage("LuxBase::run(): Beginning scanner start sequence for " + m_longName + ".", m_beVerbose);

        // Set the scanner parameters
        if ((m_readOnlyMode == false) && (m_inputFileName == ""))
        {
                // TCP
                // We may write parameters
                result = cmd_setScanFrequency(m_scanFrequency);
                if (result == false)
                {
                        printError("LuxBase::run(): Failed to set scan frequency, aborting.");
                        return false;
                }


                result = cmd_setScanAngles(m_scanStartAngle, m_scanEndAngle);
                if (result == false)
                {
                        printError("LuxBase::run(): Failed to set scan angles, aborting.");
                        return false;
                }

                // "MountingPosition" is a container for the 3 angles and 3 offsets.
                Position3D mp(m_yawAngle, m_pitchAngle, m_rollAngle, m_offsetX, m_offsetY, m_offsetZ);
                result = cmd_setMountingPos(mp);
                if (result == false)
                {
                        printError("LuxBase::run(): Failed to set mounting pos, aborting.");
                        return false;
                }
        }
        else
        {
                // We may not write parameters.
                infoMessage("LuxBase::run(): Running in read-only mode, so we're just using current sensor parameters.");
        }

        //
        // Start measuring. Note that here, also the output flags are set.
        //
        if (m_inputFileName == "")
        {
                // TCP
                result = cmd_startMeasure(weWantScanData, weWantObjectData);
                if (result == true)
                {
                        // The scanner is running (= is delivering scans)
                        m_isRunning = true;
                }
                else
                {
                        // We failed to set the scan mode
                        printError("LuxBase::run(): The StartMeasure command failed!");
                }
        }
        else
        {
                // File
                m_isRunning = true;     // The scanner is "running"
        }

        return result;
}



//
// The TCP read callback.
//
void LuxBase::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
{
        bool beVerboseHere = false;     // = m_beVerbose;
        printInfoMessage("LuxBase::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.", beVerboseHere);

        ScopedLock lock(&m_inputBufferMutex);           // Mutex for access to the input buffer
        UINT32 remainingSpace = MRS_INPUTBUFFERSIZE - m_inBufferLevel;
        UINT32 bytesToBeTransferred = numOfBytes;
        if (remainingSpace < numOfBytes)
        {
                bytesToBeTransferred = remainingSpace;
        }

        printInfoMessage("LuxBase::readCallbackFunction(): Transferring " + toString(bytesToBeTransferred) +
                                                " bytes from TCP to input buffer.", beVerboseHere);

        if (bytesToBeTransferred > 0)
        {
                // Data can be transferred into our input buffer
                memcpy(&(m_inputBuffer[m_inBufferLevel]), buffer, bytesToBeTransferred);
                m_inBufferLevel += bytesToBeTransferred;

                // Now work on the input buffer until all received datasets are processed
                UINT16 datatype;
                do
                {
                        datatype = decodeAnswerInInputBuffer();
                }
                while (datatype != 0);
        }
        else
        {
                // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
                // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
                // we clear the input buffer here.
                m_inBufferLevel = 0;
        }

        printInfoMessage("LuxBase::readCallbackFunction(): Added " + toString(bytesToBeTransferred) +
                                                " bytes to input buffer, new fill level is now " + toString(m_inBufferLevel) + " bytes.", beVerboseHere);
}



//
// Stop: Stop scanning, but keep the interface open.
//
bool LuxBase::stop()
{
        if (m_isRunning == false)
        {
                printWarning("stop(): Device is not running, nothing to do.");
                return true;
        }

        printInfoMessage("LuxBase::stop: Stopping LD-MRS device.", m_beVerbose);

        // Stop
        bool result = cmd_stopMeasure();
        if (result == true)
        {
                // Erfolg
                printInfoMessage("LuxBase::stop: LD-MRS device stopped scanning.", m_beVerbose);
                m_isRunning = false;
        }
        else
        {
                // Fehler
                printError("LuxBase::stop: StopMeasure command to LD-MRS was not successful!");
        }

        return true;
}

}       // namespace devices