imageprotocol.cpp

/*******************************************************************************
 * Copyright (c) 2018 Nerian Vision Technologies
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *******************************************************************************/

#include <cstring>
#include <iostream>
#include <limits>
#include <vector>
#include <memory>
#include <algorithm>
#include "visiontransfer/imageprotocol.h"
#include "visiontransfer/alignedallocator.h"
#include "visiontransfer/datablockprotocol.h"
#include "visiontransfer/exceptions.h"
#include "visiontransfer/bitconversions.h"

// Network headers
#ifdef _WIN32
    #ifndef NOMINMAX
        #define NOMINMAX
    #endif
    #include <winsock2.h>
#else
    #include <arpa/inet.h>
#endif

using namespace std;

/*************** Pimpl class containing all private members ***********/

class ImageProtocol::Pimpl {
public:
    Pimpl(ProtocolType protType, int maxUdpPacketSize);

    // Redeclaration of public members
    void setTransferImagePair(const ImagePair& imagePair);
    void setRawTransferData(const ImagePair& metaData, unsigned char* rawData,
        int firstTileWidth = 0, int secondTileWidth = 0, int validBytes = 0x7FFFFFFF);
    void setRawValidBytes(int validBytes);
    const unsigned char* getTransferMessage(int& length);
    bool transferComplete();
    void resetTransfer();
    bool getReceivedImagePair(ImagePair& imagePair);
    bool getPartiallyReceivedImagePair(ImagePair& imagePair,
        int& validRows, bool& complete);
    bool imagesReceived() const;

    unsigned char* getNextReceiveBuffer(int& maxLength);

    bool processReceivedMessage(int length);
    int getProspectiveMessageSize();
    void resetReception();

private:
    enum HeaderPixelFormat {
        HDR_FMT_8_BIT,
        HDR_FMT_12_BIT_SPLIT, // For backwards compatibility
        HDR_FMT_12_BIT_PACKED,
    };

    // Header data transferred in the first packet
#pragma pack(push,1)
    struct HeaderData{
        unsigned char protocolVersion;
        unsigned char isRawImagePair;

        unsigned short width;
        unsigned short height;

        unsigned short firstTileWidth;
        unsigned short secondTileWidth;

        unsigned char format0;
        unsigned char format1;
        unsigned char minDisparity;
        unsigned char maxDisparity;

        unsigned int seqNum;
        int timeSec;
        int timeMicrosec;

        float q[16];

        unsigned char padding1[6]; // Pad to 32 bytes
    };
#pragma pack(pop)

    static const unsigned char CURRENT_VERSION = 0x04;

    // Underlying protocol for data transfers
    DataBlockProtocol dataProt;
    ProtocolType protType;

    // Transfer related variables
    bool headerTransferred;
    std::vector<unsigned char> headerBuffer;
    std::vector<unsigned char> rawBuffer;
    unsigned char* rawData;
    int rawValidBytes;
    int rawDataLength;

    // Reception related variables
    std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
    bool receiveHeaderParsed;
    HeaderData receiveHeader;
    int lastReceivedPayloadBytes[2];
    int receiveTotalSize;
    bool receptionDone;

    // Copies the transmission header to the given buffer
    void copyHeaderToBuffer(const ImagePair& imagePair, int firstTileWidth,
        int secondTileWidth, unsigned char* buffer);

    // Decodes header information from the received data
    void tryDecodeHeader(const unsigned char* receivedData, int receivedBytes);

    // Decodes a received image from an interleaved buffer
    unsigned char* decodeInterleaved(int imageNumber, int receivedBytes,
        unsigned char* data, int& validRows, int& rowStride);

    int getFrameSize(int width, int height, int firstTileWidth, int secondTileWidth,
        ImagePair::ImageFormat format0, ImagePair::ImageFormat format1, int headerSize);

    int getFormatNibbles(HeaderPixelFormat format);

    void decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
        const unsigned char* data, int firstTileStride, int secondTileStride, int& validRows,
        HeaderPixelFormat format);

    void decodeRowsFromTile(int startRow, int stopRow, unsigned const char* src,
        unsigned char* dst, int srcStride, int dstStride, int tileWidth);

    void allocateDecodeBuffer(int imageNumber);
};


/******************** Stubs for all public members ********************/

ImageProtocol::ImageProtocol(ProtocolType protType, int maxUdpPacketSize)
    : pimpl(new Pimpl(protType, maxUdpPacketSize)) {
    // All initializations are done by the Pimpl class
}

ImageProtocol::~ImageProtocol() {
    delete pimpl;
}

void ImageProtocol::setTransferImagePair(const ImagePair& imagePair) {
    pimpl->setTransferImagePair(imagePair);
}

void ImageProtocol::setRawTransferData(const ImagePair& metaData,
        unsigned char* imageData, int firstTileWidth, int secondTileWidth, int validBytes) {
    pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
}

void ImageProtocol::setRawValidBytes(int validBytes) {
    pimpl->setRawValidBytes(validBytes);
}

const unsigned char* ImageProtocol::getTransferMessage(int& length) {
    return pimpl->getTransferMessage(length);
}

bool ImageProtocol::transferComplete() {
    return pimpl->transferComplete();
}

void ImageProtocol::resetTransfer() {
    pimpl->resetTransfer();
}

bool ImageProtocol::getReceivedImagePair(ImagePair& imagePair) {
    return pimpl->getReceivedImagePair(imagePair);
}

bool ImageProtocol::getPartiallyReceivedImagePair(
        ImagePair& imagePair, int& validRows, bool& complete) {
    return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
}

bool ImageProtocol::imagesReceived() const {
    return pimpl->imagesReceived();
}

unsigned char* ImageProtocol::getNextReceiveBuffer(int& maxLength) {
    return pimpl->getNextReceiveBuffer(maxLength);
}

bool ImageProtocol::processReceivedMessage(int length) {
    return pimpl->processReceivedMessage(length);
}

void ImageProtocol::resetReception() {
    pimpl->resetReception();
}

/******************** Implementation in pimpl class *******************/

ImageProtocol::Pimpl::Pimpl(ProtocolType protType, int maxUdpPacketSize)
        :dataProt(static_cast<DataBlockProtocol::ProtocolType>(protType), maxUdpPacketSize),
        protType(protType), headerTransferred(false),
        rawData(nullptr), rawValidBytes(0), rawDataLength(0), receiveHeaderParsed(false),
        lastReceivedPayloadBytes{0, 0}, receiveTotalSize(0), receptionDone(false) {
    headerBuffer.resize(sizeof(HeaderData) + sizeof(unsigned short));
    memset(&headerBuffer[0], 0, sizeof(headerBuffer.size()));
    memset(&receiveHeader, 0, sizeof(receiveHeader));

    // Just after start-up we don't yet know the expected data size. So lets
    // just allocate enough memory for one UDP packet
    dataProt.setReceiveDataSize(maxUdpPacketSize);
}

void ImageProtocol::Pimpl::setTransferImagePair(const ImagePair& imagePair) {
    if(imagePair.getPixelData(0) == nullptr || imagePair.getPixelData(1) == nullptr) {
        throw ProtocolException("Image data is null pointer!");
    }

    headerTransferred = false;

    // Set header as first piece of data
    copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[0]);
    dataProt.startTransfer();
    dataProt.setTransferData(&headerBuffer[0], sizeof(HeaderData));

    // Perform 12 bit packed encoding if neccessary
    int nibbles[2] = {0, 0};
    int rowSize[2] = {0, 0};
    int rowStride[2] = {0, 0};
    const unsigned char* pixelData[2] = {nullptr, nullptr};
    std::vector<unsigned char> encodingBuffer[2];

    for(int i = 0; i<2; i++) {
        nibbles[i] = imagePair.getPixelFormat(i) == ImagePair::FORMAT_8_BIT ? 2 : 3;
        rowSize[i] = imagePair.getWidth()*nibbles[i]/2;

        if(imagePair.getPixelFormat(i) == ImagePair::FORMAT_8_BIT) {
            pixelData[i] = imagePair.getPixelData(i);
            rowStride[i] = imagePair.getRowStride(i);
        } else {
            encodingBuffer[i].resize(rowSize[i] * imagePair.getHeight());
            BitConversions::encode12BitPacked(0, imagePair.getHeight(), imagePair.getPixelData(i),
                &encodingBuffer[i][0], imagePair.getRowStride(i), rowSize[i], imagePair.getWidth());
            pixelData[i] = &encodingBuffer[i][0];
            rowStride[i] = rowSize[i];
        }
    }

    // Make a interleaved copy
    rawBuffer.resize(imagePair.getWidth()*imagePair.getHeight()*(nibbles[0] + nibbles[1])/2 + sizeof(short));
    int bufferOffset = 0;

    for(int y = 0; y<imagePair.getHeight(); y++) {
        memcpy(&rawBuffer[bufferOffset], &pixelData[0][y*rowStride[0]], rowSize[0]);
        bufferOffset += rowSize[0];

        memcpy(&rawBuffer[bufferOffset], &pixelData[1][y*rowStride[1]], rowSize[1]);
        bufferOffset += rowSize[1];
    }

    rawData = &rawBuffer[0];
    rawValidBytes = static_cast<int>(rawBuffer.size() - sizeof(short));

    rawDataLength = getFrameSize(imagePair.getWidth(), imagePair.getHeight(), 0, 0,
        imagePair.getPixelFormat(0), imagePair.getPixelFormat(1), 0);
}

void ImageProtocol::Pimpl::setRawTransferData(const ImagePair& metaData, unsigned char* rawData,
        int firstTileWidth, int secondTileWidth, int validBytes) {
    if(rawData == nullptr) {
        throw ProtocolException("Image data is null pointer!");
    }

    headerTransferred = false;

    // Set header as first piece of data
    copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[0]);
    dataProt.startTransfer();
    dataProt.setTransferData(&headerBuffer[0], sizeof(HeaderData));

    this->rawData = rawData;
    rawValidBytes = validBytes;

    rawDataLength = getFrameSize(metaData.getWidth(), metaData.getHeight(),
        firstTileWidth, secondTileWidth, metaData.getPixelFormat(0),
        metaData.getPixelFormat(1), 0);
}

void ImageProtocol::Pimpl::setRawValidBytes(int validBytes) {
    rawValidBytes = validBytes;
    if(headerTransferred) {
        dataProt.setTransferValidBytes(validBytes);
    }
}

const unsigned char* ImageProtocol::Pimpl::getTransferMessage(int& length) {
    const unsigned char* msg = dataProt.getTransferMessage(length);

    if(msg == nullptr && !headerTransferred && rawValidBytes > 0) {
        // Transmitting the header is complete. Lets transfer the actual
        // payload.
        headerTransferred = true;
        dataProt.setTransferData(rawData, rawDataLength, rawValidBytes);
        msg = dataProt.getTransferMessage(length);
    }

    return msg;
}

bool ImageProtocol::Pimpl::transferComplete() {
    return dataProt.transferComplete() && headerTransferred;
}

int ImageProtocol::Pimpl::getFrameSize(int width, int height, int firstTileWidth,
        int secondTileWidth, ImagePair::ImageFormat format0,
        ImagePair::ImageFormat format1, int headerSize) {
    int nibbles0 = format0 == ImagePair::FORMAT_8_BIT ? 2 : 3;
    int nibbles1 = format1 == ImagePair::FORMAT_8_BIT ? 2 : 3;

    int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;

    return (effectiveWidth * height * (nibbles0 + nibbles1)) /2 + headerSize;
}

int ImageProtocol::Pimpl::getFormatNibbles(HeaderPixelFormat format) {
    // A nibble is 4 bits
    if(format == HDR_FMT_12_BIT_SPLIT || format == HDR_FMT_12_BIT_PACKED) {
        return 3;
    } else {
        return 2;
    }
}

void ImageProtocol::Pimpl::copyHeaderToBuffer(const ImagePair& imagePair,
        int firstTileWidth, int secondTileWidth, unsigned char* buffer) {
    HeaderData* transferHeader = reinterpret_cast<HeaderData*>(buffer);
    memset(transferHeader, 0, sizeof(*transferHeader));
    transferHeader->protocolVersion = CURRENT_VERSION;
    transferHeader->isRawImagePair = imagePair.isImageDisparityPair() ? 0 : 1;
    transferHeader->width = htons(imagePair.getWidth());
    transferHeader->height = htons(imagePair.getHeight());
    transferHeader->firstTileWidth = htons(firstTileWidth);
    transferHeader->secondTileWidth = htons(secondTileWidth);
    transferHeader->format0 = imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT ? HDR_FMT_8_BIT : HDR_FMT_12_BIT_PACKED;
    transferHeader->format1 = imagePair.getPixelFormat(1) == ImagePair::FORMAT_8_BIT ? HDR_FMT_8_BIT : HDR_FMT_12_BIT_PACKED;
    transferHeader->seqNum = static_cast<unsigned int>(htonl(imagePair.getSequenceNumber()));

    int minDisp = 0, maxDisp = 0;
    imagePair.getDisparityRange(minDisp, maxDisp);
    transferHeader->minDisparity = minDisp;
    transferHeader->maxDisparity = maxDisp;

    int timeSec = 0, timeMicrosec = 0;
    imagePair.getTimestamp(timeSec, timeMicrosec);
    transferHeader->timeSec = static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
    transferHeader->timeMicrosec = static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));

    if(imagePair.getQMatrix() != nullptr) {
        memcpy(transferHeader->q, imagePair.getQMatrix(), sizeof(float)*16);
    }
}

void ImageProtocol::Pimpl::resetTransfer() {
    dataProt.resetTransfer();
}

unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(int& maxLength) {
    maxLength = dataProt.getMaxReceptionSize();
    return dataProt.getNextReceiveBuffer(maxLength);
}

bool ImageProtocol::Pimpl::processReceivedMessage(int length) {
    receptionDone = false;

    // Add the received message
    if(!dataProt.processReceivedMessage(length)) {
        resetReception();
        return false;
    }

    int receivedBytes = 0;
    const unsigned char* receivedData = dataProt.getReceivedData(receivedBytes);

    // Immediately try to decode the header
    if(!receiveHeaderParsed && receivedBytes > 0) {
        tryDecodeHeader(receivedData, receivedBytes);
    }

    // Check if we have received a complete frame
    if(receivedBytes == receiveTotalSize) {
        receptionDone = true;
        dataProt.finishReception();
    } else if(receivedBytes > receiveTotalSize) {
        // This is a corrupted frame
        dataProt.resetReception();
        return false;
    }

    return true;
}

void ImageProtocol::Pimpl::tryDecodeHeader(const
unsigned char* receivedData, int receivedBytes) {
    if(receivedBytes >= static_cast<int>(sizeof(HeaderData))) {
        receiveHeader =  *reinterpret_cast<const HeaderData*>(receivedData);

        if(receiveHeader.protocolVersion > CURRENT_VERSION ||
                receiveHeader.protocolVersion < 4) {
            throw ProtocolException("Protocol version mismatch!");
        }

        // Convert byte order
        receiveHeader.width = ntohs(receiveHeader.width);
        receiveHeader.height = ntohs(receiveHeader.height);
        receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
        receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
        receiveHeader.timeSec = static_cast<int>(
            htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
        receiveHeader.timeMicrosec = static_cast<int>(
            htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
        receiveHeader.seqNum = htonl(receiveHeader.seqNum);

        // Make sure that the receive buffer is large enough
        receiveTotalSize = getFrameSize(
            receiveHeader.width,
            receiveHeader.height,
            receiveHeader.firstTileWidth,
            receiveHeader.secondTileWidth,
            receiveHeader.format0 == HDR_FMT_8_BIT ? ImagePair::FORMAT_8_BIT : ImagePair::FORMAT_12_BIT,
            receiveHeader.format1 == HDR_FMT_8_BIT ? ImagePair::FORMAT_8_BIT : ImagePair::FORMAT_12_BIT,
            sizeof(HeaderData));

        dataProt.setReceiveDataSize(receiveTotalSize);
        receiveHeaderParsed = true;
    }
}

bool ImageProtocol::Pimpl::imagesReceived() const {
    return receptionDone && receiveHeaderParsed;
}

bool ImageProtocol::Pimpl::getReceivedImagePair(ImagePair& imagePair) {
    bool complete = false;
    int validRows;
    bool ok = getPartiallyReceivedImagePair(imagePair, validRows, complete);

    return (ok && complete);
}

bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(ImagePair& imagePair, int& validRows, bool& complete) {
    imagePair.setWidth(0);
    imagePair.setHeight(0);

    complete = false;

    if(!receiveHeaderParsed) {
        // We haven't even received the image header yet
        return false;
    } else {
        // We received at least some pixel data
        int receivedBytes = 0;
        unsigned char* data = dataProt.getReceivedData(receivedBytes);
        if(receivedBytes == receiveTotalSize) {
            // Receiving this frame has completed
            dataProt.finishReception();
        }

        imagePair.setImageDisparityPair(receiveHeader.isRawImagePair == 0);

        validRows = 0;
        imagePair.setWidth(receiveHeader.width);
        imagePair.setHeight(receiveHeader.height);
        imagePair.setPixelFormat(0, receiveHeader.format0  == HDR_FMT_8_BIT ? ImagePair::FORMAT_8_BIT : ImagePair::FORMAT_12_BIT);
        imagePair.setPixelFormat(1, receiveHeader.format1  == HDR_FMT_8_BIT ? ImagePair::FORMAT_8_BIT : ImagePair::FORMAT_12_BIT);

        int rowStride0 = 0, rowStride1 = 0;
        int validRows0 = 0, validRows1 = 0;
        unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
        unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);

        imagePair.setRowStride(0, rowStride0);
        imagePair.setRowStride(1, rowStride1);
        imagePair.setPixelData(0, pixel0);
        imagePair.setPixelData(1, pixel1);
        imagePair.setQMatrix(receiveHeader.q);

        imagePair.setSequenceNumber(receiveHeader.seqNum);
        imagePair.setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
        imagePair.setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);

        validRows = min(validRows0, validRows1);

        if(validRows == receiveHeader.height) {
            complete = true;
        }

        if(receptionDone) {
            // Reset everything for receiving the next image
            resetReception();
        }

        return true;
    }
}

unsigned char* ImageProtocol::Pimpl::decodeInterleaved(int imageNumber, int receivedBytes,
        unsigned char* data, int& validRows, int& rowStride) {
    if(receivedBytes <= static_cast<int>(sizeof(HeaderData))) {
        // We haven't yet received any data for the requested image
        return nullptr;
    }

    HeaderPixelFormat format = static_cast<HeaderPixelFormat>(
        imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
    int nibbles0 = getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format0));
    int nibbles1 = getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format1));

    unsigned char* ret = nullptr;
    int payloadBytes = receivedBytes - sizeof(HeaderData);

    if(receiveHeader.secondTileWidth == 0) {
        int bufferOffset = sizeof(HeaderData) + imageNumber*receiveHeader.width * nibbles0/2;
        int bufferRowStride = receiveHeader.width*(nibbles0 + nibbles1) / 2;

        if(format == HDR_FMT_8_BIT) {
            // No decoding is neccessary. We can just pass through the
            // data pointer
            ret = &data[bufferOffset];
            rowStride = bufferRowStride;
            validRows = payloadBytes / bufferRowStride;
        } else {
            // Perform 12-bit => 16 bit decoding
            allocateDecodeBuffer(imageNumber);
            validRows = payloadBytes / bufferRowStride;
            rowStride = 2*receiveHeader.width;
            int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;

            if(format == HDR_FMT_12_BIT_SPLIT) {
                BitConversions::decode12BitSplit(lastRow, validRows, &data[bufferOffset],
                    &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
            } else {
                BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
                    &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
            }

            ret = &decodeBuffer[imageNumber][0];
        }
    } else {
        // Decode the tiled transfer
        decodeTiledImage(imageNumber,
            lastReceivedPayloadBytes[imageNumber], payloadBytes,
            data, receiveHeader.firstTileWidth * (nibbles0 + nibbles1) / 2,
            receiveHeader.secondTileWidth * (nibbles0 + nibbles1) / 2,
            validRows, format);
        ret = &decodeBuffer[imageNumber][0];

        if(format == HDR_FMT_8_BIT) {
            rowStride = receiveHeader.width;
        } else {
            rowStride = 2*receiveHeader.width;
        }
    }

    lastReceivedPayloadBytes[imageNumber] = payloadBytes;
    return ret;
}

void ImageProtocol::Pimpl::allocateDecodeBuffer(int imageNumber) {
    HeaderPixelFormat format = static_cast<HeaderPixelFormat>(
        imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
    int bytesPerPixel = (format == HDR_FMT_8_BIT ? 1 : 2);
    int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;

    if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
        decodeBuffer[imageNumber].resize(bufferSize);
    }
}

void ImageProtocol::Pimpl::decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
        const unsigned char* data, int firstTileStride, int secondTileStride, int& validRows,
        HeaderPixelFormat format) {

    // Allocate a decoding buffer
    allocateDecodeBuffer(imageNumber);

    // Get beginning and end of first tile
    int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
    int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
        static_cast<int>(receiveHeader.height));

    // Get beginning and end of second tile
    int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
    int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
    int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
    int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
    int firstTileOffset = sizeof(HeaderData) + imageNumber * getFormatNibbles(
        static_cast<HeaderPixelFormat>(receiveHeader.format0)) * receiveHeader.firstTileWidth / 2;

    // Decode first tile
    if(format == HDR_FMT_12_BIT_SPLIT) {
        BitConversions::decode12BitSplit(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
            firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
    } else if(format == HDR_FMT_12_BIT_PACKED) {
        BitConversions::decode12BitPacked(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
            firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
    } else {
        decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
            &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width,
            receiveHeader.firstTileWidth);
    }

    // Decode second tile
    int secondTileOffset = sizeof(HeaderData) + receiveHeader.height*firstTileStride +
        imageNumber * getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format0)) * receiveHeader.secondTileWidth / 2;

    if(format == HDR_FMT_12_BIT_SPLIT) {
        BitConversions::decode12BitSplit(startSecondTile, stopSecondTile,
            &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
            secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
    } else if(format == HDR_FMT_12_BIT_PACKED) {
        BitConversions::decode12BitPacked(startSecondTile, stopSecondTile,
            &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
            secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
    } else {
        decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
            &decodeBuffer[imageNumber][receiveHeader.firstTileWidth],
            secondTileStride, receiveHeader.width, receiveHeader.secondTileWidth);
    }

    validRows = stopSecondTile;
}

void ImageProtocol::Pimpl::decodeRowsFromTile(int startRow, int stopRow, unsigned const char* src,
        unsigned char* dst, int srcStride, int dstStride, int tileWidth) {
    for(int y = startRow; y < stopRow; y++) {
        memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
    }
}

void ImageProtocol::Pimpl::resetReception() {
    receiveHeaderParsed = false;
    lastReceivedPayloadBytes[0] = 0;
    lastReceivedPayloadBytes[1] = 0;
    receiveTotalSize = 0;
    dataProt.resetReception();
    receptionDone = false;
}