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

#include "free_rtos/ethernet_industry/eth_ecat.hpp"

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

namespace free_rtos {

EthEcat::EthEcat(Eth& eth)
    : eth_{eth}
    , tx_flow_{*eth.getTxFlowPtr()}
    , telegram_{eth}
    , eeprom_{telegram_} { }

void EthEcat::Init(TEthMacPorts port_id, uint32_t period_microsec = 250) {
    port_id_ = port_id;
    telegram_.init(port_id);

    Timer::Settings ecat_tmr_sett = {
                 .input_clk_Hz = 25000000, // 25MHz
                 .base_address = 0x2400000u, // memory mapping,
                 .clock_src_mux_addr = 0x430081B0u, // sysconfig
                 .int_num = 152u,                   // sysconfig
                 .int_priority = 4,                 // sysconfig
                 .period_us = period_microsec /// microsec
              };

    ecat_timer_.Init(ecat_tmr_sett);
    ecat_timer_.Stop();
}

void EthEcat::set_slaves_to_default() {
    address::Broadcast broadcast{0x0000};

/* deact loop manual */
    uint8_t a_data_out{0x00};
    datagram::EcatDatagram<command::BWR, uint8_t> a{ {{broadcast, ECT_REG_DLPORT}}, a_data_out };

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

/* reset CRC counters */
    std::array<uint8_t, 8> c_data_out;
    c_data_out.fill(0x00);
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8>> c{ {{broadcast, ECT_REG_RXERR}}, c_data_out };

/* reset FMMU's */
    std::array<uint8_t, 16*3> d_data_out;
    d_data_out.fill(0x00);
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 16*3>> d{ {{broadcast, ECT_REG_FMMU0}}, d_data_out };

/* reset SyncM */
    std::array<uint8_t, 8*4> e_data_out;
    e_data_out.fill(0x00);
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 8*4>> e{ {{broadcast, ECT_REG_SM0}}, e_data_out };

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

/* reset system time+ofs */
    std::array<uint8_t, 4> g_data_out;
    g_data_out.fill(0x00);
    datagram::EcatDatagram<command::BWR, std::array<uint8_t, 4>> g{ {{broadcast, ECT_REG_DCSYSTIME}}, g_data_out };

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

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

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

/* Reset all slaves to Init */
    uint16_t k_data_out{EC_STATE_INIT|EC_STATE_ACK};
    datagram::EcatDatagram<command::BWR, uint16_t> k{ {{broadcast, ECT_REG_ALCTL}}, k_data_out };

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

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

    auto queue = a + b + c + d + e + f + g + h + i + j + k + l + m;

    do {
        telegram_.transfer(queue);
    } while(a.get_all_wkc() < 0x0001);

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

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

uint16_t EthEcat::slaves_detecting() {
    address::Broadcast broadcast{0x0000};
    uint16_t data{0x0000};
    datagram::EcatDatagram<command::BRD, uint16_t> datagram{ {{broadcast, ECT_REG_TYPE}}, data };

    telegram_.transfer(datagram);

    return datagram.get_wkc();
}

// Setting Station address (FP) of slave via Position addressing (AP)
// Station address is datagram data
void EthEcat::set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base) {
    using TDatagram = datagram::EcatDatagram<command::APWR, address::Station>;

    std::vector<TDatagram> datagrams;

    slaves_.reserve(number_of_slaves);
    datagrams.reserve(number_of_slaves);

    for(uint16_t i = 0; i < number_of_slaves; i++) {
        address::Position position{static_cast<int16_t>(-i)};
        address::Station station{static_cast<uint16_t>(address_base + i)};
        address::SlaveAddresses slave_addresses{position, 0x0000, station, 0x00000000};

        slaves_.emplace_back(EcatSlave{std::move(slave_addresses)});
        datagrams.emplace_back(TDatagram{ {{position, ECT_REG_STADR}},  slaves_.back().get_slave_address<command::FP>() });
    }

    queue::Queue<datagram::IEcatDatagram> queue;

    for(uint16_t i = 0; i < number_of_slaves; i++) {
        queue + datagrams[i];
    }

    do {
        telegram_.transfer(queue);
    } while(datagrams[0].get_all_wkc() < number_of_slaves);

    DebugP_log((char*)"Slave addresses set\r\n");
}

void EthEcat::get_addresses_of_slaves() {
    using TDatagram = datagram::EcatDatagram<command::APRD, address::Station>;

    std::vector<TDatagram> datagrams;
    uint16_t number_of_slaves = slaves_.size();

    datagrams.reserve(number_of_slaves);

    for(EcatSlave& slave : slaves_) {
        datagrams.emplace_back(TDatagram{ {{slave.get_slave_address<command::AP>(), ECT_REG_STADR}}, slave.get_slave_address<command::FP>() });
    }

    queue::Queue<datagram::IEcatDatagram> queue;

    for(uint16_t i = 0; i < number_of_slaves; i++) {
        queue + datagrams[i];
    }

    do {
        telegram_.transfer(queue);
    } while(datagrams[0].get_all_wkc() < number_of_slaves);

    for(EcatSlave& slave : slaves_) {
        DebugP_log((char*)"Slave %d address = %d\r\n", -slave.get_slave_address<command::AP>(), slave.get_slave_address<command::FP>());
    }
}

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

    set_slaves_to_default();
    uint16_t number_of_slaves = slaves_detecting();
    DebugP_log((char*)"number_of_slaves = %d\r\n", number_of_slaves);

    set_addresses_of_slaves(number_of_slaves, address_base);
    get_addresses_of_slaves();

    return number_of_slaves;
}

void EthEcat::enable_PDI() {
    for(EcatSlave& slave : slaves_) {
        slave.enable_PDI<command::FP>(telegram_);
    }
}

bool EthEcat::init_to_preop() {
    bool success = true;

    for(EcatSlave& slave : slaves_) {
        success &= slave.init_to_preop<command::FP>(telegram_);
    }

    DebugP_log((char*)"success = %d\r\n", success);

    return success;
}

bool EthEcat::preop_to_safeop() {
    bool success = true;

    for(EcatSlave& slave : slaves_) {
        success &= slave.preop_to_safeop<command::FP>(telegram_);
    }

    DebugP_log((char*)"success = %d\r\n", success);

    return success;
}

bool EthEcat::safeop_to_op() {
    uint16_t number_of_slaves = slaves_.size();
    bool success;

    address::Broadcast broadcast{0x0000};
    ALSTAT stat{0x0000, 0x0000};
    uint16_t zero{0x00000000};

    process_sem_.post();
    init_sem_.pend();

    {
        uint16_t data{EC_STATE_OPERATIONAL};
        datagram::EcatDatagram<command::BWR, uint16_t> datagram{ {{broadcast, ECT_REG_ALCTL}}, data };

        do {
            telegram_.transfer(datagram);
        } while(datagram.get_wkc() < number_of_slaves);
    }

    process_sem_.post();
    init_sem_.pend();

    {
        datagram::EcatDatagram<command::BRD, ALSTAT, uint16_t> datagram{ {{broadcast, ECT_REG_ALSTAT}}, stat, zero };

        do {
            telegram_.transfer(datagram);
        } while(datagram.get_wkc() < number_of_slaves);
    }

    success = (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);

    if(success == true) {
        ecat_timer_.Start();
    }

    //DebugP_log((char*)"stat.state = %d, stat.fault = %d\r\n", stat.state, stat.fault);
    //DebugP_log((char*)"success = %d\r\n", success);

    return success;
}

}