#include "profinet.hpp"

#include "profinet_cb_state_ind.hpp"
#include "profinet_cb_connect_ind.hpp" 
#include "profinet_cb_release_ind.hpp" 
#include "profinet_cb_dcontrol_ind.hpp" 
#include "profinet_cb_ccontrol_ind.hpp" 
#include "profinet_cb_read_ind.hpp" 
#include "profinet_cb_write_ind.hpp" 
#include "profinet_cb_exp_module_ind.hpp" 
#include "profinet_cb_exp_submodule_ind.hpp" 
#include "profinet_cb_new_data_status_ind.hpp" 
#include "profinet_cb_alarm_ind.hpp" 
#include "profinet_cb_alarm_cnf.hpp" 
#include "profinet_cb_alarm_ack_cnf.hpp" 
#include "profinet_cb_reset_ind.hpp" 
#include "profinet_cb_signal_led_ind.hpp" 

#include "utils.h"
#include <cstring>

#include <cstdio>
#include <cstring>
#include <iostream>

#include <chrono>
#include <thread>


using namespace std;


#define GET_HIGH_BYTE(id) ((id >> 8) & 0xFF)
#define GET_LOW_BYTE(id)  (id & 0xFF)

#define APP_SNMP_THREAD_PRIORITY       1
#define APP_SNMP_THREAD_STACKSIZE      256 * 1024 /* bytes */
#define APP_ETH_THREAD_PRIORITY        10
#define APP_ETH_THREAD_STACKSIZE       4096 /* bytes */
#define APP_BG_WORKER_THREAD_PRIORITY  5
#define APP_BG_WORKER_THREAD_STACKSIZE 4096 /* bytes */

#define FLAGS_AREP_APP_READY 0x00000001

#ifdef LOG_ENABLE
    #define LOG(STR) log_.put(STR)
#else   
    #define LOG(STR)
#endif

static void pnetConfigure(pnet_cfg_t& cfg, ProfinetSettings& Settings, ProfinetDeviceSettings& DevSettings)
{   
    cfg.tick_us = Settings.ticks_us;
    cfg.im_0_data.im_vendor_id_hi = GET_HIGH_BYTE(DevSettings.im_0.vendor_id);
    cfg.im_0_data.im_vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
    cfg.im_0_data.im_hardware_revision = DevSettings.im_0.hw_revision;
    cfg.im_0_data.im_sw_revision_prefix = DevSettings.im_0.sw_revision.prefix;
    cfg.im_0_data.im_sw_revision_functional_enhancement = DevSettings.im_0.sw_revision.functional_enhancement;
    cfg.im_0_data.im_sw_revision_bug_fix = DevSettings.im_0.sw_revision.bug_fix;
    cfg.im_0_data.im_sw_revision_internal_change = DevSettings.im_0.sw_revision.internal_change;
    cfg.im_0_data.im_revision_counter = DevSettings.im_0.revision_counter;
    cfg.im_0_data.im_profile_id = DevSettings.im_0.profile_id;
    cfg.im_0_data.im_profile_specific_type = DevSettings.im_0.profile_specific_type;
    cfg.im_0_data.im_version_major = 1;
    cfg.im_0_data.im_version_minor = 1;
    cfg.im_0_data.im_supported = DevSettings.im_0.supported;

    std::snprintf (
      cfg.im_0_data.im_order_id,
      sizeof (cfg.im_0_data.im_order_id),
      "%s",
      DevSettings.im_0.order_id.c_str());
    std::snprintf (
        cfg.im_0_data.im_serial_number,
        sizeof (cfg.im_0_data.im_serial_number),
        "%s",
        DevSettings.im_0.serial_number.c_str());
    std::snprintf (
        cfg.im_1_data.im_tag_function,
        sizeof (cfg.im_1_data.im_tag_function),
        "%s",
        DevSettings.im_1.tag_function.c_str());
    std::snprintf (
        cfg.im_1_data.im_tag_location,
        sizeof (cfg.im_1_data.im_tag_location),
        "%s",
        DevSettings.im_1.tag_location.c_str());
    std::snprintf (
        cfg.im_2_data.im_date,
        sizeof (cfg.im_2_data.im_date),
        "%s",
        DevSettings.im_2.date.c_str());
    std::snprintf (
        cfg.im_3_data.im_descriptor,
        sizeof (cfg.im_3_data.im_descriptor),
        "%s",
        DevSettings.im_3.descriptor.c_str());
    std::snprintf (
        cfg.im_4_data.im_signature,
        sizeof (cfg.im_4_data.im_signature),
        "%s",
        DevSettings.im_4.signature.c_str());

    cfg.state_cb = profinet_cb_state_ind;
    cfg.connect_cb = profinet_cb_connect_ind;
    cfg.release_cb = profinet_cb_release_ind;
    cfg.dcontrol_cb = profinet_cb_dcontrol_ind;
    cfg.ccontrol_cb = profinet_cb_ccontrol_ind;
    cfg.read_cb = profinet_cb_read_ind;
    cfg.write_cb = profinet_cb_write_ind;
    cfg.exp_module_cb = profinet_cb_exp_module_ind;
    cfg.exp_submodule_cb = profinet_cb_exp_submodule_ind;
    cfg.new_data_status_cb = profinet_cb_new_data_status_ind;
    cfg.alarm_ind_cb = profinet_cb_alarm_ind;
    cfg.alarm_cnf_cb = profinet_cb_alarm_cnf;
    cfg.alarm_ack_cnf_cb = profinet_cb_alarm_ack_cnf;
    cfg.reset_cb = profinet_cb_reset_ind;
    cfg.signal_led_cb = profinet_cb_signal_led_ind;
    
    cfg.device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.im_0.vendor_id);
    cfg.device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.im_0.vendor_id);
    cfg.device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.device_id);
    cfg.device_id.device_id_lo = GET_LOW_BYTE (DevSettings.device_id);
    cfg.oem_device_id.vendor_id_hi = GET_HIGH_BYTE (DevSettings.oem_vendor_id);
    cfg.oem_device_id.vendor_id_lo = GET_LOW_BYTE (DevSettings.oem_vendor_id);
    cfg.oem_device_id.device_id_hi = GET_HIGH_BYTE (DevSettings.oem_device_id);
    cfg.oem_device_id.device_id_lo = GET_LOW_BYTE (DevSettings.oem_device_id);


    snprintf (
      cfg.product_name,
      sizeof (cfg.product_name),
      "%s",
      DevSettings.product_name.c_str());

    cfg.send_hello = true;

    /* Timing */
    cfg.min_device_interval = DevSettings.min_device_interval;

    /* Should be set by application as part of network configuration. */
    cfg.num_physical_ports = 1;

    snprintf (
        cfg.station_name,
        sizeof (cfg.station_name),
        "%s",
        DevSettings.station_name.c_str());

    /* Diagnosis mechanism */
    /* We prefer using "Extended channel diagnosis" instead of
        * "Qualified channel diagnosis" format on the wire,
        * as this is better supported by Wireshark.
        */
    cfg.use_qualified_diagnosis = false;
}

Profinet::Profinet() : m_pnet_data({nullptr, 0, UINT32_MAX, 0}),
                       m_cyclic_io_cnt{0},
                       m_periodic_stop{false},
                       log_{1}
{
    
}

void Profinet::createInternalStucture(ProfinetData_Map * const p_data_map)
{
    for (int i = 0; i < p_data_map->mods_nbr; ++i)
    {
        auto& mod = p_data_map->p_mods[i];

        auto module_ptr = ProfinetModule::Create(mod.id, mod.name);

        for (int j = 0; j < mod.submods_nbr; ++j)
        {
            auto& submod = mod.p_submods[j];
            pnet_data_cfg_t dap_data_cfg = { static_cast<pnet_submodule_dir_t>(submod.cyc_data_dir), 
                                             submod.cyc_indata_len, submod.cyc_outdata_len };
            auto submodule_ptr = ProfinetSubmodule::Create(submod.id, submod.name, dap_data_cfg, &submod.inp_data, &submod.out_data);

            for (int k = 0; k < submod.params_nbr; ++k)
            {
                auto& param = submod.p_params[k];
                auto Parameter = ProfinetParameter::Create(param.index, param.name, &param.data);
                submodule_ptr->addParameter(Parameter);
            }

            module_ptr->addSubmodule(submodule_ptr);
        }
        addModule(module_ptr);
    }
   
}

bool Profinet::Config(ProfinetSettings& Settings, 
                      ProfinetDeviceSettings& DevSettings, 
                      ProfinetData_Map * const p_data_map)
{
    bool ret = true;
    pnet_cfg_t pnet_cfg = {0};
    utils_netif_namelist_t netif_name_list;
    uint16_t number_of_ports = 1;
    pnet_if_cfg_t netif_cfg = {0};

    std::memset(&pnet_cfg, 0, sizeof(pnet_cfg));

    pnetConfigure(pnet_cfg, Settings, DevSettings);

    m_run_period_us = Settings.ticks_us;
    m_cyclic_io_period_ms = Settings.cyclic_ms;

    ///В колбэки pnet передается этот объект
    pnet_cfg.cb_arg = static_cast<void*>(this);
    
    utils_pnet_cfg_init_netifs(Settings.eth_dev_name.c_str(),
        &netif_name_list,
        &number_of_ports,
        &netif_cfg,
        0x10 /* Copper 100 Mbit/s Full duplex */
    );

    pnet_cfg.if_cfg = netif_cfg;
    pnet_cfg.num_physical_ports = number_of_ports;

    pnet_cfg.pnal_cfg.snmp_thread.prio = APP_SNMP_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.snmp_thread.stack_size = APP_SNMP_THREAD_STACKSIZE;
    pnet_cfg.pnal_cfg.eth_recv_thread.prio = APP_ETH_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.eth_recv_thread.stack_size = APP_ETH_THREAD_STACKSIZE;
    pnet_cfg.pnal_cfg.bg_worker_thread.prio = APP_BG_WORKER_THREAD_PRIORITY;
    pnet_cfg.pnal_cfg.bg_worker_thread.stack_size = APP_BG_WORKER_THREAD_STACKSIZE;
    
    /// Инициализация библиотеки pnet
    m_pnet_data.pnet_ptr = pnet_init (&pnet_cfg);

    p_events_ = &p_data_map->Events;

    if (m_pnet_data.pnet_ptr == nullptr)
        return false;
    
    ///
    pnet_data_cfg_t dap_data_cfg = { PNET_DIR_NO_IO, 0, 0 };
    /// Создаем обязательный модуль DAP
    auto dap_module = ProfinetModule::Create(PNET_MOD_DAP_IDENT, "DAP 1");
    /// Создаем обязательный подмодуль DAP Identity 1
    auto dap_submodule_indent_1 = ProfinetSubmodule::Create(PNET_SUBMOD_DAP_IDENT, "DAP Identity 1", dap_data_cfg, nullptr, nullptr);
    /// Создаем обязательный подмодуль DAP IFACE1 IDENT
    auto dap_submodule_iface_1_indent_1 = ProfinetSubmodule::Create(PNET_SUBMOD_DAP_INTERFACE_1_IDENT, "DAP IFACE1 IDENT", dap_data_cfg, nullptr, nullptr);
    /// Создаем обязательный подмодуль DAP Port 1
    auto dap_subslot_iface_1_port1_ident = ProfinetSubmodule::Create(PNET_SUBMOD_DAP_INTERFACE_1_PORT_1_IDENT, "DAP Port 1", dap_data_cfg, nullptr, nullptr);
    /// Добавляем подмодули к модулю
    ret&= dap_module->addSubmodule(dap_submodule_indent_1);
    ret&= dap_module->addSubmodule(dap_submodule_iface_1_indent_1);
    ret&= dap_module->addSubmodule(dap_subslot_iface_1_port1_ident);
    /// Добавляем модуль в конфигурацию
    addModule(dap_module);

    /// Создаем слот для модуля DAP и доключаем к нему модуль dap_module
    ret&= addSlotAndPlugModule(PNET_SLOT_DAP_IDENT, dap_module);
    /// Добавляем подслоты к слоту PNET_SLOT_DAP_IDENT и подключаем к подслотам подмодули
    /// 1. Подмодуль "DAP Identity 1"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_IDENT, dap_submodule_indent_1);
    /// 2. Подмодуль "DAP IFACE1 IDENT"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_IDENT, dap_submodule_iface_1_indent_1);
    /// 3. Подмодуль "DAP Port 1"
    ret&= addSubslotAndPlugSubmodule(PNET_SLOT_DAP_IDENT, PNET_SUBSLOT_DAP_INTERFACE_1_PORT_1_IDENT, dap_subslot_iface_1_port1_ident);
    
    ///Создаем внутреннию структуру модулей
    createInternalStucture(p_data_map);

    return ret;
}

bool Profinet::addSlot(std::shared_ptr<ProfinetSlot>& slot_ptr)
{
    if (slot_ptr == nullptr)
        return false;

    m_slots[slot_ptr->m_slot_nbr] = slot_ptr;

    return true;
}

std::shared_ptr<ProfinetSlot> Profinet::addSlot(uint16_t slot_nbr)
{
    std::shared_ptr<ProfinetSlot> slot_ptr(nullptr);

    if (m_slots.count(slot_nbr))
    {
        slot_ptr = m_slots[slot_nbr];
    }
    else
    {
        slot_ptr = make_shared<ProfinetSlot>(slot_nbr);
    }

    if (slot_ptr != nullptr)
    {
        m_slots[slot_nbr] = slot_ptr;
    }

    return slot_ptr;
}

bool Profinet::addModule(std::shared_ptr<ProfinetModule>& module_ptr)
{
    if (module_ptr == nullptr)
        return false;
    
    m_modules[module_ptr->m_id] = module_ptr;

    return true;
}

std::shared_ptr<ProfinetModule> Profinet::getModule(uint32_t module_id)
{
    if (!m_modules.count(module_id))
    {
        return nullptr;
    }

    return m_modules[module_id];
}


bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, std::shared_ptr<ProfinetModule>& module_ptr)
{
    auto slot = addSlot(slot_nbr);

    if (slot == nullptr)
    {
        return false;
    }

    return slot->plugModule(m_pnet_data, module_ptr);
}

bool Profinet::addSlotAndPlugModule(uint16_t slot_nbr, uint32_t module_id)
{
    /// 1. Проверяем, что module_id поддерживается
    auto module_ptr = getModule(module_id);

    if (module_ptr == nullptr)
    {
        return false;
    }
    ///2. Добавляет слот 
    auto slot = addSlot(slot_nbr);

    if (slot == nullptr)
    {
        return false;
    }
    /// 3. Подключаем модуль к слоту
    return slot->plugModule(m_pnet_data, module_ptr);
}


bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr, uint16_t subslot_nbr, std::shared_ptr<ProfinetSubmodule>& submodule_ptr)
{
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return false;
    }

    auto subslot_ptr = slot_ptr->addSubslot(subslot_nbr);

    if (subslot_ptr == nullptr)
    {
        return false;
    }

    return subslot_ptr->plugSubmodule(m_pnet_data, submodule_ptr);
}

bool Profinet::addSubslotAndPlugSubmodule(uint16_t slot_nbr, 
                                uint16_t subslot_nbr, 
                                uint32_t module_id,
                                uint32_t submodule_id,
                                const pnet_data_cfg_t * p_exp_data)
{
    ///1. Проверяем наличие слота
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return false;
    }

    ///2. Проверяем, что к слоту подключен модуль с идентификатором module_id
    auto module_ptr = slot_ptr->getModulePtr();
    if (module_ptr == nullptr)
    {
        return false;
    }

    if (module_ptr->m_id != module_id)
    {
        return false;
    }

    ///3. Проверяем, что в модуль входит подмодуль с идентфикатором submodule_id
    auto submodule_ptr = module_ptr->getSubmodulePtr(submodule_id);
    if (submodule_ptr == nullptr)
    {
        return false;
    }

    if (submodule_ptr->m_id != submodule_id)
    {
        return false;
    }

    ///4. Проверяем, что конфигурация данных подмодуля совпадает
    if ((submodule_ptr->m_data_cfg.data_dir != p_exp_data->data_dir) ||
        (submodule_ptr->m_data_cfg.insize != p_exp_data->insize) ||
        (submodule_ptr->m_data_cfg.outsize != p_exp_data->outsize))
    {
        return false;
    }

    return addSubslotAndPlugSubmodule(slot_nbr, subslot_nbr, submodule_ptr);
}


std::shared_ptr<ProfinetSlot> Profinet::getSlotPtr(uint16_t slot_nbr)
{
    /// 1. Проверить, что слот slot добавлен в конфигурацию
    if (!m_slots.count(slot_nbr))
    {
       return nullptr;
    }

    return m_slots[slot_nbr];
}

std::shared_ptr<ProfinetSubslot> Profinet::getSubslotPtr(uint16_t slot_nbr, uint16_t subslot_nbr)
{
    auto slot_ptr = getSlotPtr(slot_nbr);

    if (slot_ptr == nullptr)
    {
        return nullptr;
    }

    return slot_ptr->getSubslotPtr(subslot_nbr);
}

std::shared_ptr<ProfinetParameter> Profinet::getSubmoduleParameter(uint16_t slot_nbr, uint16_t subslot_nbr, uint32_t param_index)
{
    ///1. Проверяем что существует подслот \a subslot_nbr входящий к \a slot_nbr
    auto subslot_ptr = getSubslotPtr(slot_nbr, subslot_nbr);

    if (subslot_ptr == nullptr)
    {
        return nullptr;
    }

    ///2. Проверяем что к подслоту \a subslot_nbr подключен подмодуль
    auto submodule_ptr = subslot_ptr->getSubmodulePtr();

    if (submodule_ptr == nullptr)
    {
        return nullptr; 
    }
    ///3. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = submodule_ptr->getParameterPtr(param_index);

    if (param_ptr == nullptr)
    {
        return nullptr;
    }

    return param_ptr;
}

void Profinet::initDataAndIoxs()
{
    uint8_t indata_iops;
    /**
     * Итерируемся по всем слотам. 
     * Если слоты есть, то и модули к ним уже подключены, т.к. это делает контроллер.
     */
    for (auto slot : m_slots)
    {   
        shared_ptr<ProfinetSlot>& slot_ptr = slot.second;

        auto& subslots = slot_ptr->getSubslotsPtr();
        /// Итерируемся по всем подслотам
        for (auto subslot : subslots)
        {
            shared_ptr<ProfinetSubslot>& subslot_ptr = subslot.second;

            auto submodule_ptr = subslot_ptr->getSubmodulePtr();
            
            /// На всякий случай. Вообще если подслоты есть, то и подмодули уже к ним подключены.
            if (submodule_ptr == nullptr)
                continue;

            indata_iops = PNET_IOXS_BAD;
            /**
             * @brief Если для подмодуля существуют входные данные (данные от устройства к контроллеру) или
             * циклических данных нет(DAP), то нужно установить статус IOPS.
             * submodule_ptr->m_data_cfg.data_dir == PNET_DIR_NO_IO - вот это условие нужно для установки статуса для DAP
             */
            if ( submodule_ptr->m_data_cfg.insize > 0 ||
                 submodule_ptr->m_data_cfg.data_dir == PNET_DIR_NO_IO )
            {
                /// Для данных DAP статус всегда ОК
                if (slot_ptr->m_slot_nbr == PNET_SLOT_DAP_IDENT)
                {
                    /// Для данных DAP статус ОК
                    indata_iops = PNET_IOXS_GOOD;
                }
                else if (submodule_ptr->m_data_cfg.insize > 0)
                {

                }

                submodule_ptr->inputSetData( m_pnet_data.pnet_ptr,
                                             m_pnet_data.api,
                                             slot_ptr->m_slot_nbr,
                                             subslot_ptr->m_subslot_nbr,
                                             indata_iops);

                /*
                * If a submodule is still plugged but not used in current AR,
                * setting the data and IOPS will fail.
                * This is not a problem.
                * Log message below will only be printed for active submodules.
                */
            }

            /**
             * @brief Если данные подмодуля включаются в себя выходные данные(данные от контроллера к устройству)
             * То нужно установить для них статус IOCS в PNET_IOXS_GOOD.
             * 
             */
            if (submodule_ptr->m_data_cfg.outsize > 0)
            {
               int ret = pnet_output_set_iocs ( m_pnet_data.pnet_ptr,
                                                m_pnet_data.api,
                                                slot_ptr->m_slot_nbr,
                                                subslot_ptr->m_subslot_nbr,
                                                PNET_IOXS_GOOD);

               if (ret == 0)
               {
                  /// Успешно
               }
            }
        }
    }
}

void Profinet::PeriodicOperations()
{
    while(!m_periodic_stop.load(std::memory_order_relaxed))
    {
        /// Ждем время в мкс: ticks_period_us, по умолчанию 1мс
        std::this_thread::sleep_for(std::chrono::microseconds(m_run_period_us));
        
        /// Вызов cyclic io
        if (++m_cyclic_io_cnt >= m_cyclic_io_period_ms)
        {
            m_cyclic_io_cnt = 0;
            cyclicIoData();
        }

        /// запускаем pnet
        pnet_handle_periodic (m_pnet_data.pnet_ptr);

        /// Нужно делать после pnet_handle_periodic(следующий тик).
        if (flags_.get_flag(FLAGS_AREP_APP_READY))
        {
            pnet_application_ready (m_pnet_data.pnet_ptr, m_pnet_data.arep_for_appl_ready);
            flags_.clear_flags(FLAGS_AREP_APP_READY);
            LOG("PeriodicOperations: FLAGS_AREP_APP_READY");
        }
    }
}

void Profinet::cyclicIoData()
{
    ///Циклический обмен только когда установлено соединение
    if (!isConnectedToController())
    {
        return;
    }

    for (auto slot : m_slots)
    {   
        shared_ptr<ProfinetSlot>& slot_ptr = slot.second;

        /// DAP слот игнорируем при циклическом обмене
        if (slot_ptr->m_slot_nbr == PNET_SLOT_DAP_IDENT)
            continue;

        auto& subslots = slot_ptr->getSubslotsPtr();
        /// Итерируемся по всем подслотам
        for (auto subslot : subslots)
        {
            shared_ptr<ProfinetSubslot>& subslot_ptr = subslot.second;

            auto submodule_ptr = subslot_ptr->getSubmodulePtr();
            
            /// На всякий случай. Вообще если подслоты есть, то и подмодули уже к ним подключены.
            if (submodule_ptr == nullptr)
                continue;
            
            if (submodule_ptr->m_data_cfg.outsize > 0)
            {
                bool data_updated;
                /// Копируем данные от контроллера в m_out_data_ptr
                submodule_ptr->outputGetData( m_pnet_data.pnet_ptr,
                                              m_pnet_data.api,
                                              slot_ptr->m_slot_nbr,
                                              subslot_ptr->m_subslot_nbr, data_updated);

                if (data_updated)
                {
                    p_events_->set_flag(EVENT_NEW_CYCLIC_DATA);
                }        
            }
            
            if (submodule_ptr->m_data_cfg.insize > 0)
            {
                uint8_t indata_iocs;
                uint8_t indata_iops = PNET_IOXS_GOOD;
                /**
                 * @brief Отправляем данные подмодуля в ПЛК
                 * 
                 */
                submodule_ptr->inputSetData(
                    m_pnet_data.pnet_ptr,
                    m_pnet_data.api,
                    slot_ptr->m_slot_nbr,
                    subslot_ptr->m_subslot_nbr,
                    indata_iops
                );
                
                (void)pnet_input_get_iocs (
                    m_pnet_data.pnet_ptr,
                    m_pnet_data.api,
                    slot_ptr->m_slot_nbr,
                    subslot_ptr->m_subslot_nbr,
                    &indata_iocs);

                submodule_ptr->m_indata_iocs = indata_iocs;
            }
        }
    }
}

void Profinet::Start()
{
    LOG("Waiting PLC connection...");
    m_periodic_thread = std::move(std::thread(&Profinet::PeriodicOperations, this));
}

int Profinet::getOutputCyclicData(uint32_t module_id, 
                                  uint32_t submodule_id, 
                                  std::vector<uint8_t>& data)
{
    if (!m_modules.contains(module_id))
        return ErrorCode::ERR_MODULE_DOES_NOT_EXIST;
    
    auto submodule = m_modules[module_id]->getSubmodulePtr(submodule_id);
    
    if (submodule == nullptr)
        return ErrorCode::ERR_SUBMODULE_DOES_NOT_EXIST;

    submodule->readOutputData(data);

    return ErrorCode::ERR_SUCCESS;
}
    
int Profinet::putInputCyclicData(uint32_t module_id, 
                                 uint32_t submodule_id, 
                                 std::vector<uint8_t>& data)
{
    if (!m_modules.contains(module_id))
        return ErrorCode::ERR_MODULE_DOES_NOT_EXIST;
    
    auto submodule = m_modules[module_id]->getSubmodulePtr(submodule_id);
    
    if (submodule == nullptr)
        return ErrorCode::ERR_SUBMODULE_DOES_NOT_EXIST;

    if (data.size() != submodule->m_data_cfg.insize)
        return ErrorCode::ERR_WRONG_DATA_LENGTH;
    
    submodule->writeInputData(data);

    return ErrorCode::ERR_SUCCESS;
}

int Profinet::getSubmoduleParam(uint32_t module_id, 
                                  uint32_t submodule_id, 
                                  uint32_t param_id, 
                                  std::vector<uint8_t>& data)
{
    if (!m_modules.contains(module_id))
        return ErrorCode::ERR_MODULE_DOES_NOT_EXIST;
    
    auto submodule = m_modules[module_id]->getSubmodulePtr(submodule_id);
    
    if (submodule == nullptr)
        return ErrorCode::ERR_SUBMODULE_DOES_NOT_EXIST;

    auto param = submodule->getParameterPtr(param_id);

    if (param == nullptr)
        return ErrorCode::ERR_PARAM_DOES_NOT_EXIST;
    
    uint8_t buff[param->length];

    param->readParameter(buff);

    data.insert(data.end(), &buff[0], &buff[param->length]);

    return ErrorCode::ERR_SUCCESS; 
}

Profinet::~Profinet()
{
    ///Остановка потока
    m_periodic_stop.store(true, std::memory_order_relaxed);
    
    if (m_periodic_thread.joinable())
    {
        m_periodic_thread.join(); 
    }
}


/**\
 * =========================================================================================
 * Callbacks
 * =========================================================================================
 */
int Profinet::callbackStateInd ( uint32_t arep, pnet_event_values_t event)
{
    LOG("callbackStateInd: event = " + std::to_string(event));

    uint16_t err_cls = 0;  /* Error code 1 */
    uint16_t err_code = 0; /* Error code 2 */
    const char * error_class_description = "";
    const char * error_code_description = "";

    if (event == PNET_EVENT_ABORT)
    {
        LOG("callbackStateInd: PNET_EVENT_ABORT: The AR has been closed.");
        if (pnet_get_ar_error_codes (m_pnet_data.pnet_ptr, arep, &err_cls, &err_code) == 0)
        {
            LOG("err_cls= " + std::to_string(err_cls));
            LOG("err_code= " + std::to_string(err_code));
            /**
             * @brief Коды ошибок в err_cls и err_code
             * TODO: Нужно передавать в управляющее приложение
             */
            
        }
        else
        {
            /// Ошибок нет
        }

        p_events_->set_flag(EVENT_CONNECTION_ABORT);

        m_pnet_data.arep = UINT32_MAX;

        /* Only abort AR with correct session key */
        ///os_event_set (app->main_events, APP_EVENT_ABORT);
    }
    else if (event == PNET_EVENT_PRMEND)
    {
        LOG("callbackStateInd: PNET_EVENT_PRMEND: Controller has sent all write records.");
        if (isConnectedToController())
        {
            /// Если уже были подключены и пришел запрос на еще одно подключение
        }

        m_pnet_data.arep = arep;

        initDataAndIoxs();

        (void)pnet_set_provider_state (m_pnet_data.pnet_ptr, true);
        /**
         * @brief Костыль библиотеки pnet:
         * По правилам Profinet нужно ответить Application ready.
         * Однако тут нельзя вызвать pnet_application_ready, т.к. это испортит
         * внутреннее состояние стэка pnet.
         * Это нужно делать на следующем "тике".
         * См. описание pnet_application_ready:
         * This function must be called after the application has received the pnet_state_ind() 
         * user callback with PNET_EVENT_PRMEND, in order for a connection to be established.
         * 
         */
        /* Send application ready at next tick
            Do not call pnet_application_ready() here as it will affect
            the internal stack states */
        m_pnet_data.arep_for_appl_ready = arep;
        flags_.set_flag(FLAGS_AREP_APP_READY);
    }
    else if (event == PNET_EVENT_DATA)
    {
        LOG("callbackStateInd: PNET_EVENT_DATA: The cyclic data transmission is now properly set up");
        /// Стартовал обмен cyclic io
    }
    else if (event == PNET_EVENT_STARTUP)
    {
        LOG("callbackStateInd: PNET_EVENT_STARTUP: A connection has been initiated.");
        p_events_->set_flag(EVENT_CONNECTION_ESTABLISHED);
    }
    else if (event == PNET_EVENT_APPLRDY)
    {
        LOG("callbackStateInd: PNET_EVENT_APPLRDY: Controller has acknowledged the APPL_RDY message.");
    }
    return 0;
}

int Profinet::callbackConnectInd ( uint32_t arep, pnet_result_t * p_result)
{
    LOG("callbackConnectInd");
    /**
     * @brief 
     * Если будет возвращено 0, то соединение будет установлено. 
     * Если будет возвращено значение отличное от 0, то не будет, в этом случае нужно указать p_result.
     */

    /// Сообщить приложению об установлении связи с контроллером.

    return 0;
}

int Profinet::callbackReleaseInd ( uint32_t arep, pnet_result_t * p_result)
{
    LOG("callbackReleaseInd");
    /**
     * @brief 
     * Cоединение будет разорвано при любом возвращаемом значении
     */

    /// Сообщить приложению об разрыве связи с контроллером.

    return 0;
}

int Profinet::callbackDcontrolInd ( uint32_t arep, 
                                    pnet_control_command_t control_command,
                                    pnet_result_t * p_result)
{
    LOG("callbackDcontrolInd: command= " + to_string(control_command));
    /**
     * @brief 
     * Можно оставить пустым
     */
    return 0;
}

int Profinet::callbackCcontrolInd ( uint32_t arep, pnet_result_t * p_result)
{
    LOG("callbackCcontrolInd");

    return 0;
}

int Profinet::callbackReadInd ( uint32_t arep,
                                uint16_t slot_nbr,
                                uint16_t subslot_nbr,
                                uint16_t idx,
                                uint16_t sequence_number,
                                uint8_t ** pp_read_data,
                                uint16_t * p_read_length,
                                pnet_result_t * p_result)
{   
    LOG("callbackReadInd");
    ///1. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);

    if (param_ptr == nullptr)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }

    ///2. Проверяем корректность длины данных (меньше быть не должно, если больше - это норм)
    if (*p_read_length < param_ptr->length)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_READ;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_READ_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        return -1;
    }

    ///3. Передаем данные параметра
    /**
     * В функции getReadBufPtr данные копируются во временный буфер и уже затем передаются в стек pnet.
     * Это нужно из-за отсутствия события которое бы сообщало об окончании процесса чтения данных стеком pnet.
     *
     */
    *pp_read_data = param_ptr->getReadBufPtr();
    *p_read_length = param_ptr->length;

    return 0;
}

int Profinet::callbackWriteInd ( uint32_t arep,
        uint16_t slot_nbr,
        uint16_t subslot_nbr,
        uint16_t idx,
        uint16_t sequence_number,
        uint16_t write_length,
        const uint8_t * p_write_data,
        pnet_result_t * p_result)
{
    LOG("callbackWriteInd");
    ///1. Проверяем, что у подмодуля есть параметр с индексом idx
    auto param_ptr = getSubmoduleParameter(slot_nbr, subslot_nbr, idx);

    if (param_ptr == nullptr)
    {   
        p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        LOG("callbackWriteInd: No such parameter with index " + std::to_string(idx));
        return -1;
    }
    ///2. Проверяем корректность длины данных
    if (write_length != param_ptr->length)
    {
        p_result->pnio_status.error_code = PNET_ERROR_CODE_WRITE;
        p_result->pnio_status.error_decode = PNET_ERROR_DECODE_PNIORW;
        p_result->pnio_status.error_code_1 = PNET_ERROR_CODE_1_APP_WRITE_ERROR;
        p_result->pnio_status.error_code_2 = 0; /* User specific */
        LOG("callbackWriteInd: Wrong data length " + std::to_string(write_length) + ", will be " + std::to_string(param_ptr->length));
        return -1;
    }

    ///3. Копируем данные
    if (!param_ptr->writeParameter(p_write_data))
    {
        return -1;
    }

    /// Устанавливаем событие
    p_events_->set_flag(EVENT_NEW_PARAM_DATA);

    return 0;
}

int Profinet::callbackExpModuleInd ( uint16_t slot, uint32_t module_ident)
{
    LOG("callbackExpModuleInd: plug module id=" + std::to_string(module_ident) + " -> slot num=" + std::to_string(slot));

    bool ret = addSlotAndPlugModule(slot, module_ident);
    
    if (ret == false)
    {
        return -1;
        LOG("Error!");
    }
    
    return 0;
}

int Profinet::callbackExpSubmoduleInd ( uint16_t slot,
                              uint16_t subslot,
                              uint32_t module_id,
                              uint32_t submodule_id,
                              const pnet_data_cfg_t * p_exp_data)
{
    LOG("callbackExpSubmoduleInd: plug submodule " + std::to_string(submodule_id) + " -> subslot " + std::to_string(subslot));

    bool ret = addSubslotAndPlugSubmodule(slot, subslot, module_id, submodule_id, p_exp_data);
    
    if (ret == false)
    {
        LOG("Error!");
        return -1;
    }
    
    return 0;
}

int Profinet::callbackNewDataStatusInd ( uint32_t arep,
                               uint32_t crep,
                               uint8_t changes,
                               uint8_t data_status)
{
    LOG("callbackNewDataStatusInd");
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackAlarmInd ( uint32_t arep,
                       const pnet_alarm_argument_t * p_alarm_arg,
                       uint16_t data_len,
                       uint16_t data_usi,
                       const uint8_t * p_data)
{
    LOG("callbackAlarmInd");
   /**
    * @brief Должен быть ответ pnet_alarm_send_ack
    * Пока заглушка
    * 
    */
    pnet_pnio_status_t pnio_status = {0, 0, 0, 0};

    pnet_alarm_send_ack (m_pnet_data.pnet_ptr, arep, p_alarm_arg, &pnio_status);

    return 0;
}

int Profinet::callbackAlarmCnf ( uint32_t arep, const pnet_pnio_status_t * p_pnio_status)
{
    LOG("callbackAlarmCnf");
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackAlarmAckCnf ( uint32_t arep, int res)
{
    LOG("callbackAlarmAckCnf");
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackResetInd ( bool should_reset_application, uint16_t reset_mode)
{
    LOG("callbackResetInd");
    /// Пока ничего не делаем
    return 0;
}

int Profinet::callbackSignalLedInd (bool led_state)
{
    LOG("callbackSignalLedInd");
    /// Пока ничего не делаем
    return 0;
}