/*
 * 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) {
    // TODO: Не забывать вычитать из указателя sizeof(TEthFrameHeader) !
    unpack(p_data - sizeof(TEthFrameHeader));

/*
    DebugP_log((char*)"Process started\r\n");

    for(uint8_t *data = p_data; data < p_data + len; data++) {
        DebugP_log((char*)"0x%01x ", *data);
    }

    DebugP_log((char*)"\r\n");
*/

    rx_sem_.post();

    return 0;
}

uint32_t EcatTelegram::Sender(uint8_t *p_data, size_t scatter_segment) {
    uint8_t *raw = pack(p_data);

    //DebugP_log((char*)"Sender started\r\n");

    return raw - p_data;
}

uint8_t* EcatTelegram::pack(uint8_t *raw) {
    TEthFrameHeader *p_eth_hdr = new(raw) TEthFrameHeader{  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
                                                            {0x01, 0x01, 0x01, 0x01, 0x01, 0x01},
                                                            ETH_PROT_ECAT_LE };
    TEcatFrameHeader *p_hdr = new(raw + sizeof(TEthFrameHeader)) TEcatFrameHeader{  .bits{  .length = 0,
                                                                                            .type = static_cast<uint16_t>(ec_network::PROTOCOL_TYPE) } };
    uint8_t *p_datagram_first = raw + sizeof(TEthFrameHeader) + sizeof(TEcatFrameHeader);
    uint8_t *p_datagram_last = p_datagram_first;

    (void)p_eth_hdr;
    (void)p_hdr;

    auto queue = datagram_queue_;  // Копия очереди
    auto next = queue.dequeue();

    while(next != nullptr) {
        //DebugP_log((char*)"Packet packed\r\n");

        p_datagram_last = next->pack(p_datagram_last);
        next = queue.dequeue();
    }

    p_hdr->bits.length = p_datagram_last - p_datagram_first;

    return p_datagram_last;
}

uint8_t* 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;

    if(p_eth_hdr->prot_id != ETH_PROT_ECAT_LE) {
        DebugP_log((char*)"Error: wrong protocol ID\r\n");

        return raw;
    }

    if(p_hdr->bits.type != static_cast<uint16_t>(ec_network::PROTOCOL_TYPE)) {
        DebugP_log((char*)"Error: wrong ethercat protocol type\r\n");

        return raw;
    }

    auto queue = datagram_queue_; // Копия очереди
    auto next = queue.dequeue();

    while(next != nullptr) {
        //DebugP_log((char*)"Packet unpacked\r\n");

        p_datagram_last = next->unpack(p_datagram_last);
        next = queue.dequeue();
    }

    return p_datagram_last;
}

void EcatTelegram::transfer() {
    bool stat = eth_stack_.send_pkt(port_id_, ETH_PROT_ECAT_LE, 1);

    if(stat == false) {
        DebugP_log((char*)"telegram transfer error !\r\n");

        return;
    }

    rx_sem_.pend();

    datagram_queue_.clear();
}

void EcatTelegram::transfer(datagram::IEcatDatagram& next) {
    datagram_queue_ + next;

    transfer();
}

void EcatTelegram::transfer(queue::Queue<datagram::IEcatDatagram>& next) {
    datagram_queue_ + next;

    transfer();
}

}

}