/*
 * eth_ecat_telegram.cpp
 *
 *  Created on: Jun 5, 2023
 *      Author: algin
 */

#include "ethernet_industry/eth_ecat_telegram.hpp"

namespace free_rtos {

namespace telegram {

int32_t EcatTelegram::Process(uint8_t *p_data, uint32_t len) {
    //buffer_in_.length = len + sizeof(TEthFrameHeader);

    //memcpy(buffer_in_.data, p_data - sizeof(TEthFrameHeader), buffer_in_.length);

    if(datagram_queue_ == nullptr) {
        return 0;
    }

    unpack(p_data);

    datagram_queue_ = nullptr;

    rx_sem_.post();

    return 0;
}

void EcatTelegram::pack() {
    TEthFrameHeader *p_eth_hdr = new(buffer_out_.data) TEthFrameHeader{ {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
                                                                        {0x01, 0x01, 0x01, 0x01, 0x01, 0x01},
                                                                        ETH_PROT_ECAT_LE};
    TEcatFrameHeader *p_hdr = new(buffer_out_.data + sizeof(TEthFrameHeader)) TEcatFrameHeader{ .bits{
                                                                                                    .length = 0,
                                                                                                    .type = static_cast<uint16_t>(ec_network::PROTOCOL_TYPE)}};
    uint8_t *p_datagram_first = buffer_out_.data + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
    uint8_t *p_datagram_last = p_datagram_first;
    queue::QueueEntity<datagram::IEcatDatagram> *next = datagram_queue_;

    (void)p_eth_hdr;
    (void)p_hdr;

    while(next != nullptr) {
        p_datagram_last = next->get_data()->pack(p_datagram_last);
        next = next->get_next();
    }

    p_hdr->bits.length = p_datagram_last - p_datagram_first;
    buffer_out_.length = sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader) + p_hdr->bits.length;
}

void EcatTelegram::unpack(uint8_t *raw) {
    TEthFrameHeader *p_eth_hdr = reinterpret_cast<TEthFrameHeader*>(raw);
    TEcatFrameHeader *p_hdr = reinterpret_cast<TEcatFrameHeader*>(raw + sizeof(TEthFrameHeader));
    uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
    uint8_t *p_datagram_last = p_datagram_first;
    queue::QueueEntity<datagram::IEcatDatagram> *next = datagram_queue_;

    (void)p_eth_hdr;
    (void)p_hdr;

    while(next != nullptr) {
        p_datagram_last = next->get_data()->unpack(p_datagram_last);
        next = next->get_next();
    }
}

void EcatTelegram::transfer(datagram::IEcatDatagram& first) {
    datagram_queue_ = &first.get_queue_entity(); // TODO: Доделать добавление в очередь более одного элемента

    pack();
    bool stat = tx_flow_.send(port_id_, buffer_out_.data, buffer_out_.length);

    if(stat == false) {
        return;
    }

    rx_sem_.pend();
    //unpack(first);
}

}

}