#include "profinet_shared_data_client.hpp"
#include <iostream>
#include <thread>

#define ECHO_MODULE_ID 0x00000040 /// Идентификатор тестового модуля
#define ECHO_SUBMOD_ID 0x00000140 /// Идентификатор тестового подмодуля
#define ECHO_INPUT_DATA_SIZE 8
#define ECHO_OUTPUT_DATA_SIZE 8
#define ECHO_PARAMETER_GAIN_IDX 125 /// Индекс параметра Gain для подмодуля ECHO

using namespace std;

uint32_t Echo_Gain = 0;

struct EchoData {
    float  data_f;
    uint32_t data_i;
};

union EchoDataMem
{
    EchoData data;
    uint8_t mem[sizeof(EchoData)];
};

EchoDataMem Echo_inpCycData;
EchoDataMem Echo_outCycData;

uint8_t be_data_inp[sizeof(EchoData)];

void endian_convert_32(uint8_t * p_data)
{
    uint8_t tmp = p_data[1];
    p_data[1] = p_data[2];
    p_data[2] = tmp;
    tmp = p_data[0];
    p_data[0] = p_data[3];
    p_data[3] = tmp;
}

ProfinetSharedDataClient shared_data;
ProfinetData_Map * p_profinet_data;

uint32_t event_mask = ( ProfinetEvent::EVENT_CONNECTION_ESTABLISHED | 
                        ProfinetEvent::EVENT_NEW_CYCLIC_DATA        | 
                        ProfinetEvent::EVENT_NEW_PARAM_DATA         |
                        ProfinetEvent::EVENT_CONNECTION_ABORT );



int main(int argc, char * argv[])
{
    p_profinet_data = shared_data.Connect("profinet_shared_data");

    if (p_profinet_data == nullptr)
    {
        return 0;
    }

    /// Берем указатель на данные подмодуля в разделяемой памяти
    auto echo_submod_ptr = shared_data.getSubmodule(ECHO_MODULE_ID, ECHO_SUBMOD_ID);
    
    if (echo_submod_ptr->id != ECHO_SUBMOD_ID)
    {
        return 0;
    }
    /// Берем указатель на данные параметра подмодуля в разделяемолй памяти
    auto echo_param_gain_ptr = shared_data.getSubmoduleParameter(ECHO_MODULE_ID, ECHO_SUBMOD_ID, ECHO_PARAMETER_GAIN_IDX);
    
    if (echo_param_gain_ptr->index != ECHO_PARAMETER_GAIN_IDX)
    {
        return 0;
    }

    std::cout << "Strarting..." << std::endl;

    for(;;)
    {
        uint32_t events = p_profinet_data->Events.wait_flags(event_mask);

        if (events & ProfinetEvent::EVENT_CONNECTION_ESTABLISHED)
        {
            p_profinet_data->Events.clear_flags(ProfinetEvent::EVENT_CONNECTION_ESTABLISHED);

            std::cout << "Event: PLC connection established" << std::endl;
        }
        
        if (events & ProfinetEvent::EVENT_NEW_CYCLIC_DATA)
        {
            static uint32_t value_i = 0;
            static float value_f = 0.0;

            p_profinet_data->Events.clear_flags(ProfinetEvent::EVENT_NEW_CYCLIC_DATA);

            ///Читаем данные от ПЛК
            echo_submod_ptr->out_data.Read(0, Echo_outCycData.mem, echo_submod_ptr->cyc_outdata_len);
            /// Конвертируем в литл эндиан
            endian_convert_32((uint8_t*)&Echo_outCycData.data.data_i);
            endian_convert_32((uint8_t*)&Echo_outCycData.data.data_f);

            if (value_i != Echo_outCycData.data.data_i)
            {
                value_i = Echo_outCycData.data.data_i;

                std::cout << "New data_i from PLC: " << value_i << std::endl;
            }

            if (value_f != Echo_outCycData.data.data_f)
            {
                value_f = Echo_outCycData.data.data_f;

                std::cout << "New data_f from PLC: " << value_f << std::endl;
            }

            ///Подготавливаем данные для ПЛК
            Echo_inpCycData.data.data_i = Echo_Gain * Echo_outCycData.data.data_i;
            Echo_inpCycData.data.data_f = Echo_Gain * Echo_outCycData.data.data_f;
            ///Конвертируем в биг эндиан
            endian_convert_32((uint8_t*)&Echo_inpCycData.data.data_i);
            endian_convert_32((uint8_t*)&Echo_inpCycData.data.data_f);
            ///Отправляем данные для ПЛК
            echo_submod_ptr->inp_data.Write(0, Echo_inpCycData.mem, echo_submod_ptr->cyc_indata_len);
        }
        
        if (events & ProfinetEvent::EVENT_NEW_PARAM_DATA)
        {
            p_profinet_data->Events.clear_flags(ProfinetEvent::EVENT_NEW_PARAM_DATA);

            std::cout << "Event: New parameter data: " << std::endl;
            //Читаем параметр записанный ПЛК при установлении связи
            echo_param_gain_ptr->data.Read(0, (uint8_t*)&Echo_Gain, echo_param_gain_ptr->length);
            /// Конвертируем в литл эндиан
            endian_convert_32((uint8_t*)&Echo_Gain);
            std::cout << "Echo_Gain = " << std::to_string(Echo_Gain) << endl;
        }

        if (events & ProfinetEvent::EVENT_CONNECTION_ABORT)
        {
            p_profinet_data->Events.clear_flags(ProfinetEvent::EVENT_CONNECTION_ABORT);
            std::cout << "Connection Aborted" << std::endl;
        }
    }
}