// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).

//

// Licensed under the Apache License, Version 2.0 (the "License");

// you may not use this file except in compliance with the License.

// You may obtain a copy of the License at

//

//     http://www.apache.org/licenses/LICENSE-2.0

//

// Unless required by applicable law or agreed to in writing, software

// distributed under the License is distributed on an "AS IS" BASIS,

// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

// See the License for the specific language governing permissions and

// limitations under the License.

/**

 * DataMsg.hpp

 *

 */

#include <fastdds/dds/core/policy/ParameterTypes.hpp>

#include <fastdds/core/policy/ParameterSerializer.hpp>

#include <fastdds/core/policy/ParameterList.hpp>

#include <fastdds/rtps/transport/NetworkBuffer.hpp>

using NetworkBuffer = eprosima::fastdds::rtps::NetworkBuffer;

namespace eprosima {

namespace fastdds {

namespace rtps {

namespace {

struct DataMsgUtils

{

    static void prepare_submessage_flags(

            const CacheChange_t* change,

            TopicKind_t topicKind,

            bool expectsInlineQos,

            InlineQosWriter* inlineQos,

            bool& dataFlag,

            bool& keyFlag,

            bool& inlineQosFlag,

            octet& status)

    {

        inlineQosFlag =

                (nullptr != inlineQos) ||

                ((WITH_KEY == topicKind) &&

                (!change->writerGUID.is_builtin() || expectsInlineQos || change->kind != ALIVE)) ||

                (change->write_params.related_sample_identity() != SampleIdentity::unknown()) ||

                (change->write_params.has_more_replies());

        dataFlag = ALIVE == change->kind &&

                change->serializedPayload.length > 0 && nullptr != change->serializedPayload.data;

        keyFlag = !dataFlag && !inlineQosFlag && (WITH_KEY == topicKind);

        status = 0;

        if (change->kind == NOT_ALIVE_DISPOSED)

        {

            status = status | BIT(0);

        }

        if (change->kind == NOT_ALIVE_UNREGISTERED)

        {

            status = status | BIT(1);

        }

        if (change->kind == NOT_ALIVE_DISPOSED_UNREGISTERED)

        {

            status = status | BIT(0);

            status = status | BIT(1);

        }

    }

    static bool serialize_header(

            uint32_t fragment_number,

            CDRMessage_t* msg,

            const CacheChange_t* change,

            const EntityId_t& readerId,

            octet flags,

            uint32_t& submessage_size_pos,

            uint32_t& position_size_count_size)

    {

        bool ok = true;

        bool is_fragment = fragment_number > 0;

        CDRMessage::addOctet(msg, is_fragment ? DATA_FRAG : DATA);

        CDRMessage::addOctet(msg, flags);

        submessage_size_pos = msg->pos;

        CDRMessage::addUInt16(msg, 0);

        position_size_count_size = msg->pos;

        //extra flags. not in this version.

        ok &= CDRMessage::addUInt16(msg, 0);

        //octet to inline Qos is 12 or 28, may change in future versions

        ok &= is_fragment ?

                CDRMessage::addUInt16(msg, RTPSMESSAGE_OCTETSTOINLINEQOS_DATAFRAGSUBMSG) :

                CDRMessage::addUInt16(msg, RTPSMESSAGE_OCTETSTOINLINEQOS_DATASUBMSG);

        //Entity ids

        ok &= CDRMessage::addEntityId(msg, &readerId);

        ok &= CDRMessage::addEntityId(msg, &change->writerGUID.entityId);

        //Add Sequence Number

        ok &= CDRMessage::addSequenceNumber(msg, &change->sequenceNumber);

        if (is_fragment)

        {

            // Add fragment starting number

            ok &= CDRMessage::addUInt32(msg, fragment_number); // fragments start in 1

            // Add fragments in submessage

            ok &= CDRMessage::addUInt16(msg, 1); // we are sending one fragment

            // Add fragment size

            ok &= CDRMessage::addUInt16(msg, change->getFragmentSize());

            // Add total sample size

            ok &= CDRMessage::addUInt32(msg, change->serializedPayload.length);

        }

        return ok;

    }

    static void serialize_inline_qos(

            CDRMessage_t* msg,

            const CacheChange_t* change,

            TopicKind_t topicKind,

            bool expectsInlineQos,

            InlineQosWriter* inlineQos,

            octet status)

    {

        if (change->write_params.related_sample_identity() != SampleIdentity::unknown())

        {

            fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_sample_identity(msg,

                    change->write_params.related_sample_identity());

            fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_custom_related_sample_identity(

                msg,

                change->write_params.related_sample_identity());

        }

        if (change->write_params.has_more_replies())

        {

            fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_more_replies(msg);

        }

        if (WITH_KEY == topicKind && (!change->writerGUID.is_builtin() || expectsInlineQos || ALIVE != change->kind))

        {

            fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_key(msg,

                    change->instanceHandle);

            if (ALIVE != change->kind)

            {

                fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_status(msg, status);

            }

        }

        if (inlineQos != nullptr)

        {

            inlineQos->writeQosToCDRMessage(msg);

        }

        fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_sentinel(msg);

    }

};

}  // empty namespace

bool RTPSMessageCreator::addMessageData(

        CDRMessage_t* msg,

        GuidPrefix_t& guidprefix,

        const CacheChange_t* change,

        TopicKind_t topicKind,

        const EntityId_t& readerId,

        bool expectsInlineQos,

        InlineQosWriter* inlineQos)

{

    RTPSMessageCreator::addHeader(msg, guidprefix);

    RTPSMessageCreator::addSubmessageInfoTS_Now(msg, false);

    NetworkBuffer pending_buffer;

    uint8_t pending_padding = 0;

    bool is_big_submessage;

    RTPSMessageCreator::addSubmessageData(msg, change, topicKind, readerId, expectsInlineQos, inlineQos,

            is_big_submessage, true, pending_buffer, pending_padding);

    msg->length = msg->pos;

    return true;

}

bool RTPSMessageCreator::addSubmessageData(

        CDRMessage_t* msg,

        const CacheChange_t* change,

        TopicKind_t topicKind,

        const EntityId_t& readerId,

        bool expectsInlineQos,

        InlineQosWriter* inlineQos,

        bool& is_big_submessage,

        bool copy_data,

        NetworkBuffer& pending_buffer,

        uint8_t& pending_padding)

{

    octet status = 0;

    octet flags = 0;

    // Initialize output parameters

    is_big_submessage = false;

    pending_buffer = NetworkBuffer();

    pending_padding = 0;

    //Find out flags

    bool dataFlag = false;

    bool keyFlag = false;

    bool inlineQosFlag = false;

    Endianness_t old_endianess = msg->msg_endian;

#if FASTDDS_IS_BIG_ENDIAN_TARGET

    msg->msg_endian = BIGEND;

#else

    flags = flags | BIT(0);

    msg->msg_endian = LITTLEEND;

#endif // if FASTDDS_IS_BIG_ENDIAN_TARGET

    DataMsgUtils::prepare_submessage_flags(change, topicKind, expectsInlineQos, inlineQos,

            dataFlag, keyFlag, inlineQosFlag, status);

    if (inlineQosFlag)

    {

        flags = flags | BIT(1);

    }

    if (dataFlag)

    {

        flags = flags | BIT(2);

    }

    if (keyFlag)

    {

        flags = flags | BIT(3);

    }

    // Submessage header.

    uint32_t submessage_size_pos = 0;

    uint16_t submessage_size = 0;

    uint32_t position_size_count_size = 0;

    bool added_no_error = DataMsgUtils::serialize_header(0, msg, change, readerId, flags,

                    submessage_size_pos, position_size_count_size);

    //Add INLINE QOS AND SERIALIZED PAYLOAD DEPENDING ON FLAGS:

    if (inlineQosFlag) //inlineQoS

    {

        DataMsgUtils::serialize_inline_qos(msg, change, topicKind, expectsInlineQos, inlineQos, status);

    }

    //Add Serialized Payload

    if (dataFlag)

    {

        if (copy_data)

        {

            added_no_error &=

                    CDRMessage::addData(msg, change->serializedPayload.data, change->serializedPayload.length);

        }

        else if (msg->pos + change->serializedPayload.length > msg->max_size)

        {

            return false;

        }

        else

        {

            pending_buffer = NetworkBuffer(change->serializedPayload.data, change->serializedPayload.length);

            msg->pos += pending_buffer.size;

        }

    }

    if (keyFlag)

    {

        added_no_error &= CDRMessage::addOctet(msg, 0); //ENCAPSULATION

        if (msg->msg_endian == BIGEND)

        {

            added_no_error &= CDRMessage::addOctet(msg, PL_CDR_BE); //ENCAPSULATION

        }

        else

        {

            added_no_error &= CDRMessage::addOctet(msg, PL_CDR_LE); //ENCAPSULATION

        }

        added_no_error &= CDRMessage::addUInt16(msg, 0); //ENCAPSULATION OPTIONS

        added_no_error &=

                fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_key(msg,

                        change->instanceHandle);

        added_no_error &=

                fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_status(msg,

                        status);

        added_no_error &= fastdds::dds::ParameterSerializer<fastdds::dds::Parameter_t>::add_parameter_sentinel(msg);

    }

    // Align submessage to rtps alignment (4).

    uint8_t align = (4 - msg->pos % 4) & 3;

    if (copy_data)

    {

        for (uint32_t count = 0; count < align; ++count)

        {

            added_no_error &= CDRMessage::addOctet(msg, 0);

        }

    }

    else

    {

        pending_padding = align;

        msg->pos += align;

    }

    uint32_t size32 = msg->pos - position_size_count_size;

    if (size32 <= std::numeric_limits<uint16_t>::max())

    {

        submessage_size = static_cast<uint16_t>(size32);

        octet* o = reinterpret_cast<octet*>(&submessage_size);

        if (msg->msg_endian == DEFAULT_ENDIAN)

        {

            msg->buffer[submessage_size_pos] = *(o);

            msg->buffer[submessage_size_pos + 1] = *(o + 1);

        }

        else

        {

            msg->buffer[submessage_size_pos] = *(o + 1);

            msg->buffer[submessage_size_pos + 1] = *(o);

        }

    }

    else

    {

        // Submessage > 64 KB

        is_big_submessage = true;

    }

    // Rewind position when not copying data. Needed for size checks.

    if (!copy_data)

    {

        msg->pos -= pending_padding;

        msg->pos -= pending_buffer.size;

    }

    msg->msg_endian = old_endianess;

    return added_no_error;

}

bool RTPSMessageCreator::addMessageDataFrag(

        CDRMessage_t* msg,

        GuidPrefix_t& guidprefix,

        const CacheChange_t* change,

        uint32_t fragment_number,

        TopicKind_t topicKind,

        const EntityId_t& readerId,

        bool expectsInlineQos,

        InlineQosWriter* inlineQos)

{

    RTPSMessageCreator::addHeader(msg, guidprefix);

    RTPSMessageCreator::addSubmessageInfoTS_Now(msg, false);

    // Calculate fragment start

    uint32_t fragment_start = change->getFragmentSize() * (fragment_number - 1);

    // Calculate fragment size. If last fragment, size may be smaller

    uint32_t fragment_size = fragment_number < change->getFragmentCount() ? change->getFragmentSize() :

            change->serializedPayload.length - fragment_start;

    // TODO (Ricardo). Check to create special wrapper.

    SerializedPayload_t payload;

    payload.data = change->serializedPayload.data + fragment_start;

    payload.length = fragment_size;

    NetworkBuffer pending_buffer;

    uint8_t pending_padding = 0;

    RTPSMessageCreator::addSubmessageDataFrag(msg, change, fragment_number, payload,

            topicKind, readerId, expectsInlineQos, inlineQos, true, pending_buffer, pending_padding);

    payload.data = NULL;

    msg->length = msg->pos;

    return true;

}

bool RTPSMessageCreator::addSubmessageDataFrag(

        CDRMessage_t* msg,

        const CacheChange_t* change,

        uint32_t fragment_number,

        const SerializedPayload_t& payload,

        TopicKind_t topicKind,

        const EntityId_t& readerId,

        bool expectsInlineQos,

        InlineQosWriter* inlineQos,

        bool copy_data,

        NetworkBuffer& pending_buffer,

        uint8_t& pending_padding)

{

    octet status = 0;

    octet flags = 0;

    //Find out flags

    bool dataFlag = false;

    bool keyFlag = false;

    bool inlineQosFlag = false;

    // Initialize output parameters

    pending_buffer = NetworkBuffer();

    pending_padding = 0;

    Endianness_t old_endianess = msg->msg_endian;

#if FASTDDS_IS_BIG_ENDIAN_TARGET

    msg->msg_endian = BIGEND;

#else

    flags = flags | BIT(0);

    msg->msg_endian = LITTLEEND;

#endif // if FASTDDS_IS_BIG_ENDIAN_TARGET

    DataMsgUtils::prepare_submessage_flags(change, topicKind, expectsInlineQos, inlineQos,

            dataFlag, keyFlag, inlineQosFlag, status);

    if (inlineQosFlag)

    {

        flags = flags | BIT(1);

    }

    if (keyFlag)

    {

        flags = flags | BIT(2);

    }

    // Submessage header.

    uint32_t submessage_size_pos = 0;

    uint16_t submessage_size = 0;

    uint32_t position_size_count_size = 0;

    bool added_no_error = DataMsgUtils::serialize_header(fragment_number, msg, change, readerId, flags,

                    submessage_size_pos, position_size_count_size);

    //Add INLINE QOS AND SERIALIZED PAYLOAD DEPENDING ON FLAGS:

    if (inlineQosFlag) //inlineQoS

    {

        DataMsgUtils::serialize_inline_qos(msg, change, topicKind, expectsInlineQos, inlineQos, status);

    }

    //Add Serialized Payload XXX TODO

    if (!keyFlag) // keyflag = 0 means that the serializedPayload SubmessageElement contains the serialized Data

    {

        if (copy_data)

        {

            added_no_error &= CDRMessage::addData(msg, payload.data, payload.length);

        }

        else if (msg->pos + payload.length > msg->max_size)

        {

            return false;

        }

        else

        {

            pending_buffer = NetworkBuffer(payload.data, payload.length);

            msg->pos += pending_buffer.size;

        }

    }

    else

    {

        // keyflag = 1 means that the serializedPayload SubmessageElement contains the serialized Key

        /*

            added_no_error &= CDRMessage::addOctet(&submsgElem, 0); //ENCAPSULATION

            if (submsgElem.msg_endian == BIGEND)

            added_no_error &= CDRMessage::addOctet(&submsgElem, PL_CDR_BE); //ENCAPSULATION

            else

            added_no_error &= CDRMessage::addOctet(&submsgElem, PL_CDR_LE); //ENCAPSULATION

            added_no_error &= CDRMessage::addUInt16(&submsgElem, 0); //ENCAPSULATION OPTIONS

            added_no_error &= Parameter_t::addParameterKey(&submsgElem, &change->instanceHandle);

            added_no_error &= Parameter_t::addParameterStatus(&submsgElem, status);

            added_no_error &= Parameter_t::addParameterSentinel(&submsgElem);

         */

        msg->msg_endian = old_endianess;

        return false;

    }

    // TODO(Ricardo) This should be on cachechange.

    // Align submessage to rtps alignment (4).

    submessage_size = uint16_t(msg->pos - position_size_count_size);

    for (; 0 != (submessage_size & 3); ++submessage_size)

    {

        if (copy_data)

        {

            added_no_error &= CDRMessage::addOctet(msg, 0);

        }

        else

        {

            ++pending_padding;

        }

    }

    if (!copy_data)

    {

        msg->pos -= pending_buffer.size;

    }

    //TODO(Ricardo) Improve.

    octet* o = reinterpret_cast<octet*>(&submessage_size);

    if (msg->msg_endian == DEFAULT_ENDIAN)

    {

        msg->buffer[submessage_size_pos] = *(o);

        msg->buffer[submessage_size_pos + 1] = *(o + 1);

    }

    else

    {

        msg->buffer[submessage_size_pos] = *(o + 1);

        msg->buffer[submessage_size_pos + 1] = *(o);

    }

    msg->msg_endian = old_endianess;

    return added_no_error;

}

} // namespace rtps

} // namespace fastdds

} // namespace eprosima