/*
 * eth_ecat.cpp
 *
 *  Created on: 16 ���. 2023 �.
 *      Author: sychev
 */

#include "ethernet_industry/eth_ecat.hpp"

#include <cstring>
#include <kernel/dpl/ClockP.h>

namespace free_rtos {

EthEcat::EthEcat(Eth& eth)
    : p_pkt_{&pkt_[e_pktFirst]}
    , p_pkt_next_{&pkt_[e_pktSecond]}
    , eth_{eth}
    , tx_flow_{*eth.getTxFlowPtr()}
    , telegram_{eth}
    , eeprom_{telegram_} { }

void EthEcat::Init(TEthMacPorts port_id) {
    port_id_ = port_id;
    telegram_.init(port_id);
}

int32_t EthEcat::Process(uint8_t * p_data, uint32_t len) {
    p_pkt_next_->length = len + sizeof(TEthFrameHeader);

    memcpy(p_pkt_next_->data, p_data - sizeof(TEthFrameHeader), p_pkt_next_->length);

    ++stat_.rx_pkt;

    rx_sem_.post();

    return 0;
}

std::vector<uint8_t> EthEcat::receive_datagram() {
    rx_sem_.pend();

    return std::vector<uint8_t>(p_pkt_next_->data + sizeof(TEthFrameHeader), p_pkt_next_->data + p_pkt_next_->length);
}

void EthEcat::send_datagram(const std::vector<uint8_t>& datagram) {
    TEthFrameHeader *p_eth_hdr = reinterpret_cast<TEthFrameHeader*>(p_pkt_->data);
    uint8_t *p_eth_data = reinterpret_cast<uint8_t*>(p_pkt_->data + sizeof(TEthFrameHeader));

    uint8_t mac_dest[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
    uint8_t mac_src[ETH_FRAME_MAC_ADDR_LEN_BYTES] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01};

    memcpy(p_eth_hdr->mac_dest, mac_dest, sizeof(mac_dest));
    memcpy(p_eth_hdr->mac_src, mac_src, sizeof(mac_src));
    p_eth_hdr->prot_id = ETH_PROT_ECAT_LE;

    memcpy(p_eth_data, datagram.data(), datagram.size());

    p_pkt_->length = sizeof(TEthFrameHeader) + datagram.size();

    bool stat = tx_flow_.send(port_id_, p_pkt_->data, p_pkt_->length);

    if (stat)
    {
        ++stat_.tx_pkt;
    }
}

void EthEcat::set_slaves_to_default() {
/* deact loop manual */
    uint8_t a_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> a{ {{0x0000, ECT_REG_DLPORT}}, a_data_out };

/* set IRQ mask */
    uint16_t b_data_out{0x0C04};
    datagram::EcatDatagram<command::BWR, uint16_t> b{ {{0x0000, ECT_REG_IRQMASK}}, b_data_out };

/* reset CRC counters */
    std::array<uint8_t, 8> c_data_out;
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{0x0000, ECT_REG_RXERR}}, c_data_out };

/* reset FMMU's */
    std::array<uint8_t, 16*3> d_data_out;
    std::array<uint8_t, 16*3> d_data_in;
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{0x0000, ECT_REG_FMMU0}}, d_data_out };

/* reset SyncM */
    std::array<uint8_t, 8*4> e_data_out;
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{0x0000, ECT_REG_SM0}}, e_data_out };

/* reset activation register */
    uint8_t f_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> f{ {{0x0000, ECT_REG_DCSYNCACT}}, f_data_out };

/* reset system time+ofs */
    std::array<uint8_t, 4> g_data_out;
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{0x0000, ECT_REG_DCSYSTIME}}, g_data_out };

/* DC speedstart */
    uint16_t h_data_out{0x1000};
    datagram::EcatDatagram<command::BWR, uint16_t> h{ {{0x0000, ECT_REG_DCSPEEDCNT}}, h_data_out };

/* DC filt expr */
    uint16_t i_data_out{0x0C00};
    datagram::EcatDatagram<command::BWR, uint16_t> i{ {{0x0000, ECT_REG_DCTIMEFILT}}, i_data_out };

/* Ignore Alias register */
    uint8_t j_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> j{ {{0x0000, ECT_REG_DLALIAS}}, j_data_out };

/* Reset all slaves to Init */
    uint8_t k_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> k{ {{0x0000, ECT_REG_ALCTL}}, k_data_out };

/* force Eeprom from PDI */
    uint8_t l_data_out{0x02};
    datagram::EcatDatagram<command::BWR, uint8_t> l{ {{0x0000, ECT_REG_EEPCFG}}, l_data_out };

/* set Eeprom to master */
    uint8_t m_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> m{ {{0x0000, ECT_REG_EEPCFG}}, m_data_out };

    a + b + c + d + e + f + g + h + i + j + k + l + m;
    telegram_.transfer(a);

    DebugP_log("a.get_wkc() = %d\r\n", a.get_wkc());
    DebugP_log("b.get_wkc() = %d\r\n", b.get_wkc());
    DebugP_log("c.get_wkc() = %d\r\n", c.get_wkc());
    DebugP_log("d.get_wkc() = %d\r\n", d.get_wkc());
    DebugP_log("e.get_wkc() = %d\r\n", e.get_wkc());
    DebugP_log("f.get_wkc() = %d\r\n", f.get_wkc());
    DebugP_log("g.get_wkc() = %d\r\n", g.get_wkc());
    DebugP_log("h.get_wkc() = %d\r\n", h.get_wkc());
    DebugP_log("i.get_wkc() = %d\r\n", i.get_wkc());
    DebugP_log("j.get_wkc() = %d\r\n", j.get_wkc());
    DebugP_log("k.get_wkc() = %d\r\n", k.get_wkc());
    DebugP_log("l.get_wkc() = %d\r\n", l.get_wkc());
    DebugP_log("m.get_wkc() = %d\r\n", m.get_wkc());
}

uint16_t EthEcat::slaves_detecting() {
    uint16_t data_out{0x0000};
    datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{0x0000, ECT_REG_TYPE}}, data_out };

    telegram_.transfer(datagram);

    return datagram.get_wkc();
}

void EthEcat::set_addresses_of_slaves(uint16_t address_base) {
    std::array<datagram::EcatDatagram<command::APWR, address::Node>, max_number_of_slaves> datagrams;

    for(uint16_t i = 0; i < number_of_slaves_; i++) {
        slaves_[i].set_slave_address<command::FP>(static_cast<address::Node>(address_base + i));
        slaves_[i].set_slave_address<command::AP>(static_cast<address::Position>(-i));

        datagrams[i] = { {{address::Position{static_cast<int16_t>(-i)}, ECT_REG_STADR}},  slaves_[i].get_slave_address<command::FP>() };
    }

    for(uint16_t i = 1; i < number_of_slaves_; i++) {
        datagrams[i - 1] + datagrams[i];
    }

    telegram_.transfer(datagrams[0]);
}

void EthEcat::get_addresses_of_slaves() {
    std::array<datagram::EcatDatagram<command::APRD, address::Node>, max_number_of_slaves> datagrams;

    for(uint16_t i = 0; i < number_of_slaves_; i++) {
        slaves_[i].set_slave_address<command::AP>(static_cast<address::Position>(-i));

        datagrams[i] = { {{address::Position{static_cast<int16_t>(-i)}, ECT_REG_STADR}}, slaves_[i].get_slave_address<command::FP>() };
    }

    for(uint16_t i = 1; i < number_of_slaves_; i++) {
        datagrams[i - 1] + datagrams[i];
    }

    telegram_.transfer(datagrams[0]);

    for(uint16_t i = 0; i < number_of_slaves_; i++) {
        DebugP_log("Slave %d address = %d\r\n", i, slaves_[i].get_slave_address<command::FP>());
    }
}

uint16_t EthEcat::config_init() {
    DebugP_log("Initializing slaves...\r\n");

    set_slaves_to_default();
    number_of_slaves_ = slaves_detecting();
    DebugP_log("number_of_slaves = %d\r\n", number_of_slaves_);

    set_addresses_of_slaves(0x1000);
    get_addresses_of_slaves();

    address::Node node = 4096;

    BufferProperties mbx_write_adr_len;
    eeprom_.read<command::FP>(node, ECT_SII_RXMBXADR, mbx_write_adr_len);

    BufferProperties mbx_read_adr_len;
    eeprom_.read<command::FP>(node, ECT_SII_TXMBXADR, mbx_read_adr_len);

    DebugP_log("response = 0x%04x\r\n", mbx_write_adr_len);
    DebugP_log("response = 0x%04x\r\n", mbx_read_adr_len);

    return number_of_slaves_;
}

}