#include "RwHandler.h"
#include "OBSWConfig.h"

#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>

RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
		LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) :
		DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio),
		temperatureSet(this), statusSet(this), lastResetStatusSet(this), tmDataset(this) {
	if (comCookie == NULL) {
		sif::error << "RwHandler: Invalid com cookie" << std::endl;
	}
	if (gpioComIF == NULL) {
		sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
	}
}

RwHandler::~RwHandler() {
}

void RwHandler::doStartUp() {

    internalState  = InternalState::GET_RESET_STATUS;

    if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
        sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
    }

#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
    setMode(MODE_NORMAL);
#else
    setMode(_MODE_TO_ON);
#endif
}

void RwHandler::doShutDown() {
	if(gpioComIF->pullLow(enableGpio) != RETURN_OK) {
		sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
	}
	setMode(_MODE_POWER_DOWN);
}

ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
    switch (internalState) {
    case InternalState::GET_RESET_STATUS:
        *id = RwDefinitions::GET_LAST_RESET_STATUS;
        internalState = InternalState::READ_TEMPERATURE;
        break;
    case InternalState::CLEAR_RESET_STATUS:
        *id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
        /** After reset status is cleared, reset status will be polled again for verification */
        internalState = InternalState::GET_RESET_STATUS;
        break;
    case InternalState::READ_TEMPERATURE:
        *id = RwDefinitions::GET_TEMPERATURE;
        internalState = InternalState::GET_RW_SATUS;
        break;
    case InternalState::GET_RW_SATUS:
        *id = RwDefinitions::GET_RW_STATUS;
        internalState = InternalState::GET_RESET_STATUS;
        break;
    default:
        sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step"
                << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
    return NOTHING_TO_SEND;
}

ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t * commandData, size_t commandDataLen) {
    ReturnValue_t result = RETURN_OK;

    switch (deviceCommand) {
    case (RwDefinitions::RESET_MCU): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::GET_LAST_RESET_STATUS): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::GET_RW_STATUS): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::INIT_RW_CONTROLLER): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::SET_SPEED): {
        if (commandDataLen != 6) {
            sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                    << " invalid length" << std::endl;
            return SET_SPEED_COMMAND_INVALID_LENGTH;
        }
        result = checkSpeedAndRampTime(commandData, commandDataLen);
        if (result != RETURN_OK) {
            return result;
        }
        prepareSetSpeedCmd(commandData, commandDataLen);
        return result;
    }
    case (RwDefinitions::GET_TEMPERATURE): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    case (RwDefinitions::GET_TM): {
        prepareSimpleCommand(deviceCommand);
        return RETURN_OK;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_FAILED;
}

void RwHandler::fillCommandAndReplyMap() {
	this->insertInCommandMap(RwDefinitions::RESET_MCU);
	this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
			RwDefinitions::SIZE_GET_RESET_STATUS);
	this->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr,
			RwDefinitions::SIZE_CLEAR_RESET_STATUS);
	this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
			RwDefinitions::SIZE_GET_RW_STATUS);
	this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr,
			RwDefinitions::SIZE_INIT_RW);
	this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
			RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
	this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
			RwDefinitions::SIZE_SET_SPEED_REPLY);
	this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset,
	            RwDefinitions::SIZE_GET_TELEMETRY_REPLY);
}

ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
        DeviceCommandId_t *foundId, size_t *foundLen) {

    switch (*(start)) {
    case (static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_STATUS)): {
        *foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
        *foundId = RwDefinitions::GET_LAST_RESET_STATUS;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::CLEAR_LAST_RESET_STATUS)): {
        *foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS;
        *foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
        *foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
        *foundId = RwDefinitions::GET_RW_STATUS;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::INIT_RW_CONTROLLER)): {
        *foundLen = RwDefinitions::SIZE_INIT_RW;
        *foundId = RwDefinitions::INIT_RW_CONTROLLER;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::SET_SPEED)): {
        *foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
        *foundId = RwDefinitions::SET_SPEED;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE)): {
        *foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
        *foundId = RwDefinitions::GET_TEMPERATURE;
        break;
    }
    case (static_cast<uint8_t>(RwDefinitions::GET_TM)): {
        *foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY;
        *foundId = RwDefinitions::GET_TM;
        break;
    }
    default: {
        sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
        return RETURN_FAILED;
        break;
    }
    }

    sizeOfReply = *foundLen;

    return RETURN_OK;
}

ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {

    /** Check result code */
    if (*(packet + 1) == RwDefinitions::ERROR) {
		sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: "
				<< id << std::endl;
        return EXECUTION_FAILED;
    }

    /** Received in little endian byte order */
    uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ;

    if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
        sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
        return CRC_ERROR;
    }

    switch (id) {
    case (RwDefinitions::GET_LAST_RESET_STATUS): {
        handleResetStatusReply(packet);
        break;
    }
    case (RwDefinitions::GET_RW_STATUS): {
        handleGetRwStatusReply(packet);
        break;
    }
    case (RwDefinitions::CLEAR_LAST_RESET_STATUS):
    case (RwDefinitions::INIT_RW_CONTROLLER):
    case (RwDefinitions::SET_SPEED):
        // no reply data expected
        break;
    case (RwDefinitions::GET_TEMPERATURE): {
        handleTemperatureReply(packet);
        break;
    }
    case (RwDefinitions::GET_TM): {
        handleGetTelemetryReply(packet);
        break;
    }
    default: {
        sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
    }

    return RETURN_OK;
}

void RwHandler::setNormalDatapoolEntriesInvalid() {

}

uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
    return 5000;
}

ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {

    localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));

    localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));

    return RETURN_OK;
}

void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
    commandBuffer[0] = static_cast<uint8_t>(id);
    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
    commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
    rawPacket = commandBuffer;
    rawPacketLen = 3;
}

ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
    int32_t speed = *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8
            | *(commandData + 3);

    if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
        sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
        return INVALID_SPEED;
    }

    uint16_t rampTime = *(commandData + 4) << 8 | *(commandData + 5);

    if (rampTime < 10 || rampTime > 10000) {
        sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
                << std::endl;
        return INVALID_RAMP_TIME;
    }

    return RETURN_OK;
}

void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);

    /** Speed (0.1 RPM) */
    commandBuffer[1] = *(commandData + 3);
    commandBuffer[2] = *(commandData + 2);
    commandBuffer[3] = *(commandData + 1);
    commandBuffer[4] = *commandData;
    /** Ramp time (ms) */
    commandBuffer[5] = *(commandData + 5);
    commandBuffer[6] = *(commandData + 4);

    uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
    commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
    commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
    rawPacket = commandBuffer;
    rawPacketLen = 9;
}

void RwHandler::handleResetStatusReply(const uint8_t* packet) {
    PoolReadGuard rg(&lastResetStatusSet);
    uint8_t offset = 2;
    uint8_t resetStatus = *(packet + offset);
    if (resetStatus != RwDefinitions::CLEARED) {
        internalState = InternalState::CLEAR_RESET_STATUS;
        lastResetStatusSet.lastResetStatus = resetStatus;
    }
    lastResetStatusSet.currentResetStatus = resetStatus;
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_RW == 1
    sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
            << static_cast<unsigned int>(lastResetStatusSet.lastResetStatus.value) << std::endl;
    sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
            << static_cast<unsigned int>(lastResetStatusSet.currentResetStatus.value) << std::endl;
#endif
}

void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
    PoolReadGuard rg(&statusSet);
    uint8_t offset = 2;
    statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    statusSet.state = *(packet + offset);
    offset += 1;
    statusSet.clcMode = *(packet + offset);

    if (statusSet.state == RwDefinitions::ERROR) {
        /**
         * This requires the commanding of the init reaction wheel controller command to recover
         * form error state which must be handled by the FDIR instance.
         */
        triggerEvent(ERROR_STATE);
        sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state"
                << std::endl;
    }

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_RW == 1
    sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
            << " * 0.1 RPM" << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: "
            << statusSet.referenceSpeed << " * 0.1 RPM" << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: State is: "
            << (unsigned int) statusSet.state.value << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: "
            << (unsigned int) statusSet.clcMode.value << std::endl;
#endif
}

void RwHandler::handleTemperatureReply(const uint8_t* packet) {
    PoolReadGuard rg(&temperatureSet);
    uint8_t offset = 2;
    temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_RW == 1
    sif::info << "RwHandler::handleTemperatureReply: Temperature: "
            << temperatureSet.temperatureCelcius << " °C" << std::endl;
#endif
}

void RwHandler::handleGetTelemetryReply(const uint8_t* packet) {
    PoolReadGuard rg(&tmDataset);
    uint8_t offset = 2;
    tmDataset.lastResetStatus = *(packet + offset);
    offset += 1;
    tmDataset.mcuTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.pressureSensorTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.pressure = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.rwState = *(packet + offset);
    offset += 1;
    tmDataset.rwClcMode = *(packet + offset);
    offset += 1;
    tmDataset.rwCurrSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.rwRefSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.numOfInvalidCrcPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.numOfInvalidLenPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.numOfInvalidCmdPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.numOfCmdExecutedReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.numOfCmdReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfParityErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfNoiseErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfFrameErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.uartTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.spiNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.spiNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.spiNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    tmDataset.spiTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
                | *(packet + offset + 1) << 8 | *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_RW == 1
    sif::info << "RwHandler::handleTemperatureReply: Last reset status: "
            << static_cast<unsigned int>(tmDataset.lastResetStatus.value) << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: MCU temperature: " << tmDataset.mcuTemperature
            << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Pressure sensor temperature: "
            << tmDataset.pressureSensorTemperature << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Pressure "
            << tmDataset.pressure << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: State: "
            << static_cast<unsigned int>(tmDataset.rwState.value) << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: CLC mode: "
            << static_cast<unsigned int>(tmDataset.rwClcMode.value) << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Current speed: " << tmDataset.rwCurrSpeed
            << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Reference speed: " << tmDataset.rwRefSpeed
            << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Number of invalid CRC packets: "
            << tmDataset.numOfInvalidCrcPackets << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Number of invalid length packets: "
            << tmDataset.numOfInvalidLenPackets << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Number of invalid command packets: "
            << tmDataset.numOfInvalidCmdPackets << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Number of command executed replies: "
            << tmDataset.numOfCmdExecutedReplies << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: Number of command replies: "
            << tmDataset.numOfCmdReplies << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of bytes written: "
            << tmDataset.uartNumOfBytesWritten << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of bytes read: "
            << tmDataset.uartNumOfBytesRead << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of parity errors: "
            << tmDataset.uartNumOfParityErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of noise errors: "
            << tmDataset.uartNumOfNoiseErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of frame errors: "
            << tmDataset.uartNumOfFrameErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of register overrun errors: "
            << tmDataset.uartNumOfRegisterOverrunErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: UART number of total errors: "
            << tmDataset.uartTotalNumOfErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes written: "
            << tmDataset.spiNumOfBytesWritten << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes read: "
            << tmDataset.spiNumOfBytesRead << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: SPI number of register overrun errors: "
            << tmDataset.spiNumOfRegisterOverrunErrors << std::endl;
    sif::info << "RwHandler::handleTemperatureReply: SPI number of register total errors: "
            << tmDataset.spiTotalNumOfErrors << std::endl;
#endif
}