// Copyright 2021 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.

/**

 * @file RTPSStatisticsMessages.hpp

 */

#ifndef _STATISTICS_RTPS_MESSAGES_RTPSSTATISTICSMESSAGES_HPP_

#define _STATISTICS_RTPS_MESSAGES_RTPSSTATISTICSMESSAGES_HPP_

#include <cstddef>

#include <cstdint>

#include <cstring>

#include <fastdds/rtps/common/CDRMessage_t.hpp>

#include <fastdds/rtps/common/Types.hpp>

#include <rtps/messages/CDRMessage.hpp>

#include <rtps/messages/RTPSMessageCreator.hpp>

#define FASTDDS_STATISTICS_NETWORK_SUBMESSAGE 0x80

namespace eprosima {

namespace fastdds {

namespace statistics {

namespace rtps {

struct StatisticsSubmessageData

{

    struct TimeStamp

    {

        int32_t seconds = 0;

        uint32_t fraction = 0;

    };

    struct Sequence

    {

        uint64_t sequence = 0;

        uint64_t bytes = 0;

        uint16_t bytes_high = 0;

        void add_message(

                uint32_t message_size)

        {

            sequence++;

            auto new_bytes = bytes + message_size;

            bytes_high += (new_bytes < bytes);

            bytes = new_bytes;

        }

        static Sequence distance(

                const Sequence& from,

                const Sequence& to)

        {

            // Check to >= from

            assert(to.sequence >= from.sequence);

            assert(to.bytes_high >= from.bytes_high);

            assert((to.bytes_high > from.bytes_high) || (to.bytes >= from.bytes));

            Sequence ret;

            ret.sequence = to.sequence - from.sequence;

            ret.bytes_high = to.bytes_high - from.bytes_high;

            ret.bytes = to.bytes - from.bytes;

            ret.bytes_high -= (ret.bytes > to.bytes);

            return ret;

        }

    };

    eprosima::fastdds::rtps::Locator_t destination;

    TimeStamp ts{};

    Sequence seq{};

};

constexpr uint16_t statistics_submessage_data_length = sizeof(StatisticsSubmessageData);

constexpr uint16_t statistics_submessage_length =

        RTPSMESSAGE_SUBMESSAGEHEADER_SIZE + // submessage header

        statistics_submessage_data_length;  // submessage data

/**

 * @brief Adds an empty statistics submessage to a message.

 * @param msg Message where the statistics submessage will be added.

 * @pre There should be room in the message for the statistics submessage.

 */

inline void add_statistics_submessage(

        eprosima::fastdds::rtps::CDRMessage_t* msg)

{

    static_cast<void>(msg);

#ifdef FASTDDS_STATISTICS

    assert(msg->max_size >= msg->length + statistics_submessage_length);

    using namespace eprosima::fastdds::rtps;

    RTPSMessageCreator::addSubmessageHeader(

        msg, FASTDDS_STATISTICS_NETWORK_SUBMESSAGE, 0x00, statistics_submessage_data_length);

    memset(msg->buffer + msg->pos, 0, statistics_submessage_data_length);

    msg->length += statistics_submessage_data_length;

    msg->pos += statistics_submessage_data_length;

#endif // FASTDDS_STATISTICS

}

/**

 * @brief Read a statistics submessage from a message.

 * @param [in,out] msg Message from where to extract the submessage.

 * @param [out] data Data read from the statistics submessage.

 * @pre msg->pos should point to the beginning of the statistics submessage payload

 * @post msg->length will be decremented by @c statistics_submessage_length (i.e. the

 *       submessage will be consumed, and it won't be available anymore)

 */

inline void read_statistics_submessage(

        eprosima::fastdds::rtps::CDRMessage_t* msg,

        StatisticsSubmessageData& data)

{

    static_cast<void>(msg);

    static_cast<void>(data);

#ifdef FASTDDS_STATISTICS

    // Should be exactly at the end

    assert(msg->pos + statistics_submessage_data_length == msg->length);

    // Read all fields

    using namespace eprosima::fastdds::rtps;

    CDRMessage::read_locator(msg, &data.destination);

    CDRMessage::readInt32(msg, &data.ts.seconds);

    CDRMessage::readUInt32(msg, &data.ts.fraction);

    CDRMessage::readUInt64(msg, &data.seq.sequence);

    CDRMessage::readUInt64(msg, &data.seq.bytes);

    CDRMessage::readUInt16(msg, &data.seq.bytes_high);

    // Consume submessage

    msg->length -= statistics_submessage_length;

    msg->pos = msg->length;

#endif // FASTDDS_STATISTICS

}

#ifdef FASTDDS_STATISTICS

inline uint32_t get_statistics_message_pos(

        const eprosima::fastdds::rtps::octet* send_buffer,

        uint32_t send_buffer_size)

{

    // zero can be use as an error value because the minimum valid value

    // is RTPSMESSAGE_HEADER_SIZE

    uint32_t statistics_pos = 0;

    // Message should contain RTPS header and statistic submessage

    if (statistics_submessage_length + RTPSMESSAGE_HEADER_SIZE <= send_buffer_size)

    {

        // The last submessage should be the statistics submessage

        uint32_t pos = send_buffer_size - statistics_submessage_length;

        if (FASTDDS_STATISTICS_NETWORK_SUBMESSAGE == send_buffer[pos])

        {

            // only succeed if the message is properly formatted

            statistics_pos = pos;

        }

    }

    return statistics_pos;

}

inline bool is_statistics_buffer(

        const eprosima::fastdds::rtps::NetworkBuffer& stat_buffer)

{

    return statistics_submessage_length == stat_buffer.size &&

           FASTDDS_STATISTICS_NETWORK_SUBMESSAGE ==

           static_cast<const eprosima::fastdds::rtps::octet*>(stat_buffer.buffer)[0];

}

#endif // FASTDDS_STATISTICS

inline void set_statistics_submessage_from_transport(

        const eprosima::fastdds::rtps::Locator_t& destination,

        const eprosima::fastdds::rtps::NetworkBuffer& send_buffer,

        const uint32_t& total_bytes,

        StatisticsSubmessageData::Sequence& sequence)

{

    static_cast<void>(destination);

    static_cast<void>(send_buffer);

    static_cast<void>(total_bytes);

    static_cast<void>(sequence);

#ifdef FASTDDS_STATISTICS

    using namespace eprosima::fastdds::rtps;

    if (is_statistics_buffer(send_buffer))

    {

        uint32_t statistics_pos = 0;

        // Accumulate bytes on sequence

        sequence.add_message(total_bytes);

        // Skip the submessage header

        statistics_pos += RTPSMESSAGE_SUBMESSAGEHEADER_SIZE;

        // Set current timestamp and sequence

        auto current_pos = &static_cast<const octet*>(send_buffer.buffer)[statistics_pos];

        eprosima::fastdds::rtps::Time_t ts;

        eprosima::fastdds::rtps::Time_t::now(ts);

        /*

         * This set of memcpy blocks is intended to prevent an undefined behavior caused when casting from an octet* to a StatisticsSubmessageData*

         * since these classes have different alignment.

         */

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, destination), &destination, sizeof(destination));

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, ts.seconds), &ts.seconds(),

                sizeof(StatisticsSubmessageData::ts.seconds));

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, ts.fraction), &ts.fraction(),

                sizeof(StatisticsSubmessageData::ts.fraction));

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, seq.sequence), &sequence.sequence,

                sizeof(sequence.sequence));

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, seq.bytes), &sequence.bytes,

                sizeof(sequence.bytes));

        memcpy((char*)current_pos + offsetof(StatisticsSubmessageData, seq.bytes_high), &sequence.bytes_high,

                sizeof(sequence.bytes_high));

    }

#endif // FASTDDS_STATISTICS

}

inline bool remove_statistics_buffer(

        const eprosima::fastdds::rtps::NetworkBuffer& stat_buffer,

        uint32_t& total_bytes)

{

    static_cast<void>(stat_buffer);

    static_cast<void>(total_bytes);

#ifdef FASTDDS_STATISTICS

    if (is_statistics_buffer(stat_buffer))

    {

        total_bytes -= statistics_submessage_length;

        return true;

    }

#endif // FASTDDS_STATISTICS

    return false;

}

} // namespace rtps

} // namespace statistics

} // namespace fastdds

} // namespace eprosima

#endif  // _STATISTICS_RTPS_MESSAGES_RTPSSTATISTICSMESSAGES_HPP_