/*
 * eth_ecat.hpp
 *
 *  Created on: 16 ���. 2023 �.
 *      Author: sychev, algin
 */

#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_

#include <cstdint>
#include <utility>
#include <kernel/dpl/ClockP.h>

#include "free_rtos/handler_store/handler.hpp"
#include "free_rtos/ethernet/eth_frame.h"
#include "free_rtos/timer/timer.hpp"
#include "free_rtos/mutex/mutex.hpp"
#include "free_rtos/semaphore/semaphore.hpp"

#include "free_rtos/ethernet/eth.hpp"
#include "free_rtos/ethernet_industry/ethercattype.hpp"
#include "free_rtos/ethernet_industry/eth_ecat_types.h"
#include "free_rtos/ethernet_industry/eth_ecat_command.hpp"
#include "free_rtos/ethernet_industry/eth_ecat_datagram.hpp"
#include "free_rtos/ethernet_industry/eth_ecat_telegram.hpp"
#include "free_rtos/ethernet_industry/eth_ecat_eeprom.hpp"

namespace free_rtos {

struct ALSTAT {
    uint16_t state;
    uint16_t fault;
};

class EcatSlave {
public:
    EcatSlave(address::SlaveAddresses&& slave_addresses)
    : slave_addresses_{slave_addresses} {

    }

    EcatSlave() { }

    template<typename TypeT>
    void set_slave_address(typename TypeT::TSlaveAddress&& slave_address) {
        free_rtos::get<static_cast<size_t>(TypeT::type)>(slave_addresses_) = slave_address;
    }

    template<typename TypeT>
    typename TypeT::TSlaveAddress& get_slave_address() {
        return free_rtos::get<static_cast<size_t>(TypeT::type)>(slave_addresses_);
    }

    template<typename TypeT>
    bool enable_PDI(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
        using TCommand = command::EcatCommand<TypeT, command::WR>;
        auto slave_address = get_slave_address<TypeT>();
        uint8_t data{0x01};
        datagram::EcatDatagram<TCommand, uint8_t> datagram{ {{slave_address, ECT_REG_EEPCFG}}, expected_wkc, data };

        return telegram.transfer(datagram);
    }

    template<typename TypeT>
    bool init_to_preop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
        auto slave_address = get_slave_address<TypeT>();
        ALSTAT stat{0x0000, 0x0000};
        bool status;

        {
            using TCommand = command::EcatCommand<TypeT, command::WR>;
            uint16_t data{EC_STATE_PRE_OP|EC_STATE_ACK};
            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }
        }

        ClockP_usleep(5000000ul);

        {
            using TCommand = command::EcatCommand<TypeT, command::RD>;
            datagram::EcatDatagram<TCommand, ALSTAT> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat };

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }
        }

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

        if((stat.state & 0x0010) != 0) {
            using TCommand = command::EcatCommand<TypeT, command::RD>;

            uint16_t stat_code{0x0000};
            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }

            DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
        }

        status = ((stat.state == EC_STATE_PRE_OP) && (stat.fault == 0));

        return status;
    }

    template<typename TypeT>
    bool preop_to_safeop(telegram::EcatTelegram& telegram, const datagram::TEcatWkc expected_wkc) {
        auto slave_address = get_slave_address<TypeT>();
        ALSTAT stat{0x0000, 0x0000};
        uint32_t zero{0x00000000};
        bool status;

        {
            using TCommand = command::EcatCommand<TypeT, command::WR>;
            uint16_t data{EC_STATE_SAFE_OP};
            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALCTL}}, expected_wkc, data };

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }
        }

        ClockP_usleep(1500000ul);

        {
            using TCommand = command::EcatCommand<TypeT, command::RD>;
            datagram::EcatDatagram<TCommand, ALSTAT, uint32_t> datagram{ {{slave_address, ECT_REG_ALSTAT}}, expected_wkc, stat, zero };

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }
        }

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

        if((stat.state & 0x0010) != 0) {
            using TCommand = command::EcatCommand<TypeT, command::RD>;

            uint16_t stat_code{0x0000};
            datagram::EcatDatagram<TCommand, uint16_t> datagram{ {{slave_address, ECT_REG_ALSTATCODE}}, expected_wkc, stat_code};

            status = telegram.transfer(datagram);

            if(status != true) {
                return status;
            }

            DebugP_log((char*)"stat_code = 0x%02x\r\n", stat_code);
        }

        status = ((stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0));

        return status;
    }

private:
    address::SlaveAddresses slave_addresses_;
};

class EthEcat {
public:
    struct Statistic {
        uint32_t rx_pkt;
        uint32_t tx_pkt;
    };

    EthEcat(Eth& eth);

    void Init(TEthMacPorts port_id, uint32_t period_microsec);

    bool set_slaves_to_default();
    uint16_t slaves_detecting();
    bool set_addresses_of_slaves(uint16_t number_of_slaves, uint16_t address_base);
    bool get_addresses_of_slaves();

    bool config_init(uint16_t address_base);

    bool enable_PDI();

    bool init_to_preop();
    bool preop_to_safeop();
    bool safeop_to_op();

    /*
     * Тип адресации slave_address зависит от типа команды CommandT
     * Может иметь тип Position, Broadcast, Node
     */
    template<typename TypeT, typename DirT, typename EcatDgDataT>
    void simple_send_datagram(typename TypeT::TSlaveAddress& slave_address, uint16_t offset, EcatDgDataT& data) {
        using TCommand = command::EcatCommand<TypeT, DirT>;
        datagram::EcatDatagram<TCommand, EcatDgDataT> datagram{ {{slave_address, offset}}, data };

        telegram_.transfer(datagram);
    }

    telegram::EcatTelegram& get_telegram() {
        return telegram_;
    }

    const telegram::EcatTelegramStatus& get_telegram_status() {
        return telegram_.get_status();
    }

    eeprom::EEPROM& get_eeprom() {
        return eeprom_;
    }

    std::vector<EcatSlave>& get_slaves() {
        return slaves_;
    }

    free_rtos::Timer& get_ecat_timer() {
        return ecat_timer_;
    }

    free_rtos::Semaphore& get_init_sem() {
        return init_sem_;
    }

    free_rtos::Semaphore& get_process_sem() {
        return process_sem_;
    }

    bool is_ready() { return ready_; }

    Statistic getStat() { return stat_;}

private:
    enum {
        e_pktFirst,
        e_pktSecond,
        e_pktTotal
    };

    static constexpr uint32_t process_timeout_ticks_ = 5000;

    free_rtos::Timer ecat_timer_;

    free_rtos::Semaphore rx_sem_;

    free_rtos::Semaphore init_sem_;
    free_rtos::Semaphore process_sem_;

    Eth& eth_;
    EthTxFlowIface& tx_flow_;

    telegram::EcatTelegram telegram_;
    eeprom::EEPROM eeprom_;

    TEthMacPorts port_id_; /// ���� ���� ����� ���������� ������

    bool ready_;

    Statistic stat_;

    std::vector<EcatSlave> slaves_;
};

}

#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_HPP_ */