#ifndef __MILSTD1553B_BSRT_MESSAGE_H__
#define __MILSTD1553B_BSRT_MESSAGE_H__

#include <vector>
#include <cstring>

#include "MILSTD1553B/Limits.h"
#include "MILSTD1553B/StatusWord.h"
#include "MILSTD1553B/BusControllerCommandWord.h"

namespace MILSTD1553B {
    class BSRTMessage {
    public:
        constexpr static const std::size_t MaxPayloadSize {
            DataSizeLimits<WordType>::max() - (CommandWordSize + StatusWordSize)
        };
    private:
        BusControllerCommandWord _commandWord;
        std::vector<WordType> _payload;
        StatusWord _statusWord;
    public:
        BSRTMessage();
        BSRTMessage(const char *data, const std::size_t dataSize);
        bool isNull() const;
        BusControllerCommandWord getCommandWord() const;
        const WordType *getPayload() const;
        StatusWord getStatusWord() const;

        void setCommandWord(const BusControllerCommandWord &value);
        template<class InputIterator>
        void setPayload(InputIterator first, InputIterator last) {
            typedef typename std::iterator_traits<InputIterator>::value_type ValueType;
            static_assert(std::is_same<ValueType, std::uint16_t>::value,
                          "InputIterator value_type is not support!");

            auto distance = std::distance(first, last);
            if (static_cast<std::size_t>(distance) > MaxPayloadSize) {
                return;
            }

            if (!_payload.empty()) {
                _payload.clear();
            }
            _payload.reserve(distance);

            std::copy(first, last, std::back_inserter(_payload));
        }

        void setPayload(const char *data, const std::size_t dataSize);
        void setPayload(const std::initializer_list<std::uint16_t> &value);
        void setStatusWord(const StatusWord &value);
        bool fromRawData(const char *data, const std::size_t dataSize);
        void toRawData(char *data);
        void zeroing();
        std::size_t getSize() const;
    private:
        std::uint16_t readWord(const char *data);
        void writeWord(const std::uint16_t value, char *data);
    }; // BSRTMessage
} // MILSTD1553B

#endif // __MILSTD1553B_BSRT_MESSAGE_H__

#include "BSRTMessage.h"

using namespace MILSTD1553B;

BSRTMessage::BSRTMessage() {
    _commandWord.zeroing();
    _payload.clear();
    _statusWord.zeroing();
}

BSRTMessage::BSRTMessage(const char *data, const std::size_t dataSize) {
    if (!fromRawData(data, dataSize)) {
        zeroing();
    }
}

bool BSRTMessage::isNull() const {
    return (!_commandWord.isValid() &&
            _payload.empty() &&
            !_statusWord.isValid());
}

BusControllerCommandWord BSRTMessage::getCommandWord() const { return _commandWord; }
const WordType *BSRTMessage::getPayload() const { return _payload.data(); }
StatusWord BSRTMessage::getStatusWord() const { return _statusWord; }

void BSRTMessage::setCommandWord(const BusControllerCommandWord &value) {
    if (!value.isValid()) {
        return;
    }

    _commandWord = value;
}

void BSRTMessage::setPayload(const char *data, const std::size_t dataSize) {
    if (data == nullptr) {
        return;
    }
    if (dataSize > MaxPayloadSize) {
        return;
    }
    if ((dataSize % WordSize) != 0) {
        return;
    }

    if (!_payload.empty()) {
        _payload.clear();
    }
    _payload.reserve(dataSize / WordSize);

    const char *first = data;
    const char *last  = data + dataSize;
    std::uint16_t temp { 0 };
    for ( ; first != last; first += WordSize) {
        std::memcpy(std::addressof(temp), data, WordSize);
        _payload.push_back(temp);
    }
}

void BSRTMessage::setPayload(const std::initializer_list<uint16_t> &value) {
    setPayload(value.begin(), value.end());
}

void BSRTMessage::setStatusWord(const StatusWord &value) {
    if (!value.isValid()) {
        return;
    }

    _statusWord = value;
}

bool BSRTMessage::fromRawData(const char *data, const std::size_t dataSize) {
    if (data == nullptr) {
        return false;
    }
    if (dataSize > DataSizeLimits<char>::max()) {
        return false;
    }
    if ((dataSize % WordSize) != 0) {
        return false;
    }

    const char *pointer = data;
    _commandWord = readWord(pointer);
    if (!_commandWord.isValid()) {
        return false;
    }

    pointer += WordSize;

    if (!_payload.empty()) {
        _payload.clear();
    }
    _payload.reserve(_commandWord.getDataWordCount());

    for (std::size_t counter = 0; counter < _commandWord.getDataWordCount(); ++counter) {
        _payload.push_back(readWord(pointer));
        pointer += WordSize;
    }

    _statusWord = readWord(pointer);
    if (!_statusWord.isValid()) {
        return false;
    }

    return true;
}

void BSRTMessage::toRawData(char *data) {
    if (data == nullptr) {
        return;
    }

    char *pointer = data;
    writeWord(_commandWord, data);
    pointer += WordSize;

    for (auto value : _payload) {
        writeWord(value, pointer);
        pointer += WordSize;
    }

    writeWord(_statusWord, pointer);
}

void BSRTMessage::zeroing() {
    _commandWord.zeroing();
    _payload.clear();
    _statusWord.zeroing();
}

std::size_t BSRTMessage::getSize() const {
    return (CommandWordSize + StatusWordSize + _payload.size());
}

uint16_t BSRTMessage::readWord(const char *data) {
    if (data == nullptr) {
        return 0;
    }

    std::uint16_t result { 0 };
    std::memcpy(std::addressof(result), data, WordSize);
    return result;
}

void BSRTMessage::writeWord(const uint16_t value, char *data) {
    if (data == nullptr) {
        return;
    }

    std::memcpy(data, std::addressof(value), WordSize);
}