/*
 * 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 "handler_store/handler.hpp"
#include "ethernet/eth_frame.h"
#include "timer/timer.hpp"
#include "mutex/mutex.hpp"
#include "semaphore/semaphore.hpp"

#include "ethernet/eth.hpp"
#include "ethernet_industry/ethercattype.hpp"
#include "ethernet_industry/eth_ecat_types.h"
#include "ethernet_industry/eth_ecat_command.hpp"
#include "ethernet_industry/eth_ecat_datagram.hpp"
#include "ethernet_industry/eth_ecat_telegram.hpp"
#include "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>
    void enable_PDI(telegram::EcatTelegram& telegram) {
        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}}, data };

        do {
            telegram.transfer(datagram);
        } while(datagram.get_wkc() < 0x0001);
    }

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

        {
            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}}, data };

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        ClockP_usleep(5000000ul);

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

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        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}}, stat_code};

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        return (stat.state == EC_STATE_PRE_OP) && (stat.fault == 0);
    }

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

        {
            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}}, data };

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        ClockP_usleep(1000000ul);

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

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        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}}, stat_code};

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);

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

        return (stat.state == EC_STATE_SAFE_OP) && (stat.fault == 0);
    }

    template<typename TypeT>
    bool safeop_to_op(telegram::EcatTelegram& telegram) {
        auto slave_address = get_slave_address<TypeT>();
        ALSTAT stat{0x0000, 0x0000};
        uint16_t zero{0x00000000};

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

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        //ClockP_usleep(3000000ul);

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

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);
        }

        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}}, stat_code};

            do {
                telegram.transfer(datagram);
            } while(datagram.get_wkc() < 0x0001);

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

        return (stat.state == EC_STATE_OPERATIONAL) && (stat.fault == 0);
    }

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);

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

    uint16_t config_init(uint16_t address_base);

    void 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_;
    }

    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
    };

    //Mutex mut_;

    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_ */