/*
 * eth_ecat_packer.hpp
 *
 *  Created on: Jun 5, 2023
 *      Author: algin
 */

#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_PACKER_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_PACKER_HPP_

#include <cstddef>

namespace free_rtos {

struct RawData {
    void *data{nullptr};
    size_t size{0};
};

struct Padding {
    size_t size;
};

struct PackFunctorBase {
    uint8_t *raw;
    const bool skip;

    template<typename DataT>
    void operator()(DataT& data) {
        if(skip == false) {
            DataT *data_p = new(raw) DataT{data};
            (void)data_p;
        }

        raw += sizeof(DataT);

        //DebugP_log((char*)"Data packed: %d\r\n", sizeof(DataT));
    }
};

// Функтор для обхода и упаковки элементов custom_tuple
struct PackFunctor : public PackFunctorBase {
    PackFunctor(uint8_t *raw, bool skip = false)
    : PackFunctorBase{raw, skip} { }

    using PackFunctorBase::operator ();

    template<typename DataT>
    void operator()(std::vector<DataT>& data) {
        size_t size = data.size() * sizeof(DataT);

        if(skip == false) {
            memcpy(raw, data.data(), size);
        }

        raw += size;
    }

    void operator()(RawData& data) {
        if(data.data == nullptr) {
            return;
        }

        if(skip == false) {
            memcpy(raw, data.data, data.size);
        }

        raw += data.size;
    }

    void operator()(Padding& padding) {
        raw += padding.size;
    }
};

struct UnpackFunctorBase {
    uint8_t *raw;
    const bool skip;

    template<typename DataT>
    void operator()(DataT& data) {
        if(skip == false) {
            DataT *p_data = reinterpret_cast<DataT*>(raw);
            data = *p_data;
        }

        raw += sizeof(DataT);

        //DebugP_log((char*)"Data unpacked: %d\r\n", sizeof(DataT));
    }

    void operator()() { }
};

// Функтор для обхода и распаковки элементов custom_tuple
struct UnpackFunctor : public UnpackFunctorBase {
    UnpackFunctor(uint8_t *raw, bool skip)
    : UnpackFunctorBase{raw, skip} { }

    using UnpackFunctorBase::operator ();

    template<typename DataT>
    void operator()(std::vector<DataT>& data) {
        size_t size = data.size() * sizeof(DataT);

        if(skip == false) {
            memcpy(data.data(), raw, size);
        }

        raw += size;
    }

    void operator()(RawData& data) {
        if(data.data == nullptr) {
            return;
        }

        if(skip == false) {
            memcpy(data.data, raw, data.size);
        }

        raw += data.size;
    }

    void operator()(Padding& padding) {
        raw += padding.size;
    }
};

}

#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_PACKER_HPP_ */