lowfield_mri_programs/pico-tcp-service/pico-tcp/main.cpp

#include <memory>
#include <tuple>
#include "src/pico_server_dispatcher.hpp"

using namespace std;

extern std::ostream out(cout.rdbuf());
extern SimpleLogger logger(out, "pico", "picologs");

void createDirectoryRecursively(const std::string &directory) {
  static const std::string separators("\\/");

  // If the specified directory name doesn't exist, do our thing
  DWORD fileAttributes = ::GetFileAttributesA(directory.c_str());
  if(fileAttributes == INVALID_FILE_ATTRIBUTES) {

    // Recursively do it all again for the parent directory, if any
    std::size_t slashIndex = directory.find_last_of(separators);
    if(slashIndex != std::wstring::npos) {
      createDirectoryRecursively(directory.substr(0, slashIndex));
    }

    // Create the last directory on the path (the recursive calls will have taken
    // care of the parent directories by now)
    BOOL result = ::CreateDirectoryA(directory.c_str(), nullptr);
    if(result == FALSE) {
      throw std::runtime_error("Could not create directory");
    }

  } else { // Specified directory name already exists as a file or directory

    bool isDirectoryOrJunction =
      ((fileAttributes & FILE_ATTRIBUTE_DIRECTORY) != 0) ||
      ((fileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) != 0);

    if(!isDirectoryOrJunction) {
      throw std::runtime_error(
        "Could not create directory because a file with the same name exists"
      );
    }

  }
}

class PicoLocalService
{
private:
    CActiveSocket* SocketActive; // *
    const static uint8_t magic_number = 0xAA; // *
    int16_t handle = 0; // *
    uint8_t last_command = 0x00; // *
    uint32_t error_code = 0x00000000; // *
    int32_t points_value = 0;
    uint32_t sample_rate = 0;
    std::vector<uint32_t> times;
    uint32_t num_triggers = 0;
    uint32_t number_channels = 0;
    std::vector<PS4000A_CHANNEL> channels;
    std::vector<uint32_t> points_vector;
    uint32_t premeasurement_percentage = 0;
    PS4000A_CONDITION* conditions;
    PS4000A_CHANNEL trig_channel = PS4000A_CHANNEL_A;
    PS4000A_THRESHOLD_DIRECTION th_direction = PS4000A_ABOVE;
    int16_t threshold = 0;
    int16_t trig_delay = 0;
    int16_t trig_autoTrigger_ms = 0;
    uint32_t nbytes_sent = 0; // *
    std::vector<std::vector<int16_t*>> data_buffer_vec;

    uint8_t rcbuf[MAX_PACKET]{0};
    uint8_t sndbuf[MAX_PACKET]{0};

public:
    PicoLocalService(CActiveSocket* socket) : SocketActive(socket) {}
    ~PicoLocalService() {}



    int get_request() {
        // Check if buffer has at least 2 bytes for magic number and command
        // Receive data from socket
        int bytes_recieved = SocketActive->Receive(MAX_PACKET, rcbuf);

        if(bytes_recieved == 0)
        {
            logger << LogPref::Flag(ERROR) << "Client disconnected" << endl;
            pico_stop();
            pico_close();
            return 0;
        }


        if(bytes_recieved < 2)
        {
            logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF; // Error flag
            sndbuf[2] = 0x01; // Error type
            this->error_code = 0x00000000; // No pico error
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        // Extract magic number from buffer
        uint8_t magic;
        memcpy(&magic, rcbuf, sizeof(uint8_t));

        // Check if magic number is correct
        if (magic != this->magic_number) {
            logger << LogPref::Flag(ERROR) << "Invalid magic number: " << std::hex << magic << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x03;
            this->error_code = 0x00000000;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        // Extract command from buffer
        uint8_t cmd;
        memcpy(&cmd, rcbuf + 1, sizeof(uint8_t));

        // Check if command is correct
        switch(cmd)
        {
            case 0x01:
            {
                this->last_command = 0x01;
                logger << LogPref::Flag(INFO) << "Command 0x01 received" << endl;
                pico_open();
                logger << LogPref::Flag(INFO) << "Pico device opened" << endl;
                break;
            }
            case 0x02:
            {
                this->last_command = 0x02;
                logger << LogPref::Flag(INFO) << "Command 0x02 received" << endl;
                if(bytes_recieved < 7) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t size;
                memcpy(&size, rcbuf + 2, sizeof(uint32_t));
                if(size < 1) {
                    logger << LogPref::Flag(ERROR) << "Invalid data size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                char* file_name = new char[size + 1];
                memcpy(file_name, rcbuf + 6, size * sizeof(char));
                file_name[size] = '\0'; // Null-terminate the string
                std::string file_name_str(file_name);
                logger << LogPref::Flag(INFO) << "File name: " << file_name_str << endl;
                pico_xml_config(file_name_str);
                logger << LogPref::Flag(INFO) << "Pico device configured" << endl;
                delete[] file_name;
                break;
            }
            case 0x0C:
            {
                this->last_command = 0x0C;
                logger << LogPref::Flag(INFO) << "Command 0x0C received" << endl;
                if(bytes_recieved < 22) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    return -1;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t size;
                memcpy(&size, rcbuf + 2, sizeof(uint32_t));
                if(size < 1) {
                    logger << LogPref::Flag(ERROR) << "Invalid data size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t* apoints = new uint32_t[size];
                memcpy(apoints, rcbuf + 6, size * sizeof(uint32_t));
                uint32_t* atimes = new uint32_t[size];
                memcpy(atimes, rcbuf + 6 + size * sizeof(uint32_t), size * sizeof(uint32_t));
                uint32_t sample_rate = 0;
                memcpy(&sample_rate, rcbuf + 6 + 2 * size * sizeof(uint32_t), sizeof(uint32_t));
                uint32_t number_channels = 0;
                memcpy(&number_channels, rcbuf + 10 + 2 * size * sizeof(uint32_t), sizeof(uint32_t));
                pico_set_params(apoints, atimes, sample_rate, number_channels, size);
                logger << LogPref::Flag(INFO) << "Pico device parameters set" << endl;
                delete[] apoints;
                delete[] atimes;
                break;
            }
            case 0x03:
            {
                this->last_command = 0x03;
                logger << LogPref::Flag(INFO) << "Command 0x03 received" << endl;
                pico_close();
                logger << LogPref::Flag(INFO) << "Pico device closed" << endl;
                break;
            }
            case 0x04:
            {
                // Check if buffer has enough data for version numbers
                this->last_command = 0x04;
                logger << LogPref::Flag(INFO) << "Command 0x04 received" << endl;
                pico_get_current_params();
                logger << LogPref::Flag(INFO) << "Pico device parameters retrieved" << endl;
                break;
            }
            case 0x05:
            {
                this->last_command = 0x05;
                logger << LogPref::Flag(INFO) << "Command 0x05 received" << endl;
                pico_probe();
                logger << LogPref::Flag(INFO) << "Pico device probed" << endl;
                break;
            }
            case 0x06:
            {
                this->last_command = 0x06;
                logger << LogPref::Flag(INFO) << "Command 0x06 received" << endl;
                if(bytes_recieved < 10) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t size;
                memcpy(&size, rcbuf + 2, sizeof(uint32_t));
                if(size < 1) {
                    logger << LogPref::Flag(ERROR) << "Invalid data size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t* apoints = new uint32_t[size];
                memcpy(apoints, rcbuf + 6, size * sizeof(uint32_t));
                pico_set_points(apoints, size);
                logger << LogPref::Flag(INFO) << "Pico device points set" << endl;
                delete[] apoints;
                break;
            }
            case 0x07:
            {
                this->last_command = 0x07;
                logger << LogPref::Flag(INFO) << "Command 0x07 received" << endl;
                if(bytes_recieved < 6) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t sample_rate = 0;
                memcpy(&sample_rate, rcbuf + 2, sizeof(uint32_t));
                pico_set_sample_rate(sample_rate);
                logger << LogPref::Flag(INFO) << "Pico device sample rate set" << endl;
                break;
            }
            case 0x08:
            {
                this->last_command = 0x08;
                logger << LogPref::Flag(INFO) << "Command 0x08 received" << endl;
                if(bytes_recieved < 10) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t size;
                memcpy(&size, rcbuf + 2, sizeof(uint32_t));
                if(size < 1) {
                    logger << LogPref::Flag(ERROR) << "Invalid data size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t* atimes = new uint32_t[size];
                memcpy(atimes, rcbuf + 6, size * sizeof(uint32_t));
                pico_set_times(atimes, size);
                logger << LogPref::Flag(INFO) << "Pico device times set" << endl;
                delete[] atimes;
                break;
            }
            case 0x09:
            {
                this->last_command = 0x09;
                logger << LogPref::Flag(INFO) << "Command 0x09 received" << endl;
                if(bytes_recieved < 16) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t channels = 0;
                memcpy(&channels, rcbuf + 2, sizeof(uint32_t));
                logger << "CHANNELS: " << channels << endl;
                if(channels < 1 || channels > 16) {
                    logger << LogPref::Flag(ERROR) << "Invalid number of channels" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                enPicoConnectProbeRange* channels_range = new enPicoConnectProbeRange[channels];
                int8_t* raw_ranges = new int8_t[channels];

                memset(raw_ranges, PICO_X1_PROBE_5V, sizeof(int8_t) * channels);
                memcpy(raw_ranges, rcbuf + 6, sizeof(int8_t) * channels);
                logger << "RAW" << (int) raw_ranges[0] << endl;

                for(int i = 0; i < channels; ++i)
                {
                    channels_range[i] = static_cast<enPicoConnectProbeRange>(raw_ranges[i]);
                }

                uint8_t trigger_channel = 0;
                memcpy(&trigger_channel, rcbuf + 6 + channels, sizeof(uint8_t));
                int32_t direction = 0;
                memcpy(&direction, rcbuf + 7 + channels, sizeof(int32_t));
                uint16_t threshold = 0;
                memcpy(&threshold, rcbuf + 11 + channels, sizeof(uint16_t));
                int16_t autoTrigger_ms = 0;
                memcpy(&autoTrigger_ms, rcbuf + 13 + channels, sizeof(int16_t));
                pico_configure_channels_t(channels, channels_range, trigger_channel, direction, threshold, autoTrigger_ms);
                logger << LogPref::Flag(INFO) << "Pico device channels configured" << endl;
                break;
            }
            case 0x29:
            {

            }
            case 0x0D:
            {
                this->last_command = 0x0D;
                logger << LogPref::Flag(INFO) << "Command 0x0B received" << endl;
                int ret = pico_exit();
                if(ret == 0) {
                    logger << LogPref::Flag(INFO) << "Pico device exited successfully" << endl;
                    return 0;
                } else {
                    logger << LogPref::Flag(ERROR) << "Failed to exit Pico device" << endl;
                }
                break;
            }
            case 0x0E:
            {
                this->last_command = 0x0E;
                logger << LogPref::Flag(INFO) << "Command 0x0E received" << endl;
                pico_stop();
                logger << LogPref::Flag(INFO) << "Pico device stopped" << endl;
                break;
            }
            case 0x20:
            {
                this->last_command = 0x20;
                logger << LogPref::Flag(INFO) << "Command 0x0F received" << endl;
                if(bytes_recieved < 12) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t trigger_channel = 0;
                memcpy(&trigger_channel, rcbuf + 2, sizeof(uint32_t));
                int32_t direction = 0;
                memcpy(&direction, rcbuf + 6, sizeof(int32_t));
                uint16_t threshold = 0;
                memcpy(&threshold, rcbuf + 10, sizeof(uint16_t));
                int16_t autoTrigger_ms = 0;
                memcpy(&autoTrigger_ms, rcbuf + 12, sizeof(int16_t));

                pico_configure_trigger(trigger_channel, direction, threshold, autoTrigger_ms);
                break;
            }
            case 0x28:
            {
                this->last_command = 0x28;
                logger << LogPref::Flag(INFO) << "Command 0x28 received" << endl;
                if(bytes_recieved < 6) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t percentage = 0;
                memcpy(&percentage, rcbuf + 2, sizeof(uint32_t));
                if(percentage < 0 || percentage > 100) {
                    logger << LogPref::Flag(ERROR) << "Invalid premeasurement percentage" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x02;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                pico_set_premeasurement(percentage);
                break;
            }
            case 0x19:
            {
                this->last_command = 0x19;
                logger << LogPref::Flag(INFO) << "Command 0x19 received" << endl;
                if(bytes_recieved < 6) {
                    logger << LogPref::Flag(ERROR) << "Invalid buffer size" << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x01;
                    this->error_code = 0x00000000;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                uint32_t num_trigger = 0;
                memcpy(&num_trigger, rcbuf + 2, sizeof(uint32_t));
                pico_set_num_trigger(num_trigger);
                break;
            }
            case 0x3B:
            {
                this->last_command;
                logger << LogPref::Flag(INFO) << "Command 0x3B received" << endl;
                pico_tcp_measurement_s();
                break;
            }
            default:
            {
                this->last_command = 0xFF;
                logger << LogPref::Flag(ERROR) << "Invalid command: " << std::hex << cmd << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x00;
                this->error_code = 0x00000000;
                memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                if(SocketActive->Send(sndbuf, 7) == -1) {
                    logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                }
                return -1;
            }
        }

        return 1;
    }

    int pico_open() {
        // Open socket and initialize Pico device
        auto retval = ps4000aOpenUnit(&handle, NULL);
        logger << LogPref::Flag(INFO) << "Pico device opened with handle: " << handle << endl;
        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to open Pico device. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x03;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        int16_t start{10};
        retval = ps4000aFlashLed(handle, start);
        logger << LogPref::Flag(INFO) << "Pico device LED flashed." << endl;
        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to flash Pico device LED. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x04;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        logger << LogPref::Flag(INFO) << "Pico device LED flashed." << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC1;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;

        return 0;
    }

    int pico_stop()
    {
        // Stop measurement and close Pico device
        auto retval = ps4000aStop(handle);
        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to stop Pico device. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x06;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Pico device stopped" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xCE;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;

        return 0;
    }

    // To be deprecated in future versions
    int pico_xml_config(const string& file_name) {
        // Load configuration from XML file
        auto data_set = parse_xml_function(file_name.c_str());
        points_vector = string_to_vector(std::get<0>(data_set));
        times = string_to_vector(std::get<2>(data_set));
        points_value = *(std::max_element(points_vector.begin(), points_vector.end()));
        number_channels = std::get<1>(data_set);
        sample_rate = std::get<3>(data_set);

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC2;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;

        return 0;
    }

    // Deprecated function, use other functions instead
    int pico_set_params(const uint32_t* points, const uint32_t* times, const uint32_t sample_rate, const uint32_t number_channels, const uint32_t size) {
        // Set parameters for measurement
        this->points_vector.assign(points, points + size);
        this->times.assign(times, times + size);
        this->sample_rate = sample_rate;
        this->number_channels = number_channels;
        points_value = *(std::max_element(points_vector.begin(), points_vector.end()));
        logger << LogPref::Flag(INFO) << "Parameters set" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xCC;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_close() {
        // Close Pico device and socket
        auto retval = ps4000aCloseUnit(handle);
        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to close Pico device. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x05;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Pico device closed" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC3;
        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;

        return 0;
    }

    int pico_get_current_params() {
        // Get current parameters from Pico device
        logger << LogPref::Flag(INFO) << "Current parameters:" << endl;

        logger << "Points: " << points_value << endl;
        logger << "Times size: " << times.size() << endl;
        logger << "Sample rate: " << sample_rate << endl;
        logger << "Number of channels: " << number_channels << endl;
        logger << "Trigger channel: " << trig_channel << endl;
        logger << "Trigger direction: " << th_direction << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC4;
        uint32_t bsize = points_vector.size() * sizeof(uint32_t);
        uint32_t test = 0;

        memcpy(sndbuf + 2, &bsize, sizeof(uint32_t));
        memcpy(sndbuf + 6, points_vector.data(), bsize);
        memcpy(&test, points_vector.data(), bsize);
        logger << LogPref::Flag(INFO) << test << endl;
        memcpy(sndbuf + 6 + bsize, times.data(), bsize);
        logger << LogPref::Flag(INFO) << times.data()[0] << endl;
        memcpy(sndbuf + 6 + 2 * bsize, &sample_rate, sizeof(sample_rate));
        memcpy(sndbuf + 10 + 2 * bsize, &number_channels, sizeof(number_channels));

        if(SocketActive->Send(sndbuf, 10 + sizeof(points_vector.data()) + sizeof(times.data())) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_probe() {
        // Probe signal from Pico device
        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC5;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;

        return 0;
    }

    int pico_set_points(const uint32_t* points, const uint32_t size) {
        // Set points for measurement
        this->points_vector.assign(points, points + size);
        points_value = *(std::max_element(points_vector.begin(), points_vector.end()));
        logger << LogPref::Flag(INFO) << "Points set" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC6;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_set_sample_rate(const uint32_t sample_rate) {
        // Set sample rate for measurement
        this->sample_rate = sample_rate;
        logger << LogPref::Flag(INFO) << "Sample rate set to: " << sample_rate << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC7;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_set_times(const uint32_t* times, const uint32_t size) {
        // Set times for measurement
        this->times.assign(times, times + size);
        logger << LogPref::Flag(INFO) << "Times set" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC8;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_set_num_trigger(const uint32_t num_trigger) {
        // Set number of channels for trigger
        this->num_triggers = num_trigger;
        logger << LogPref::Flag(INFO) << "Number of triggers set to: " << num_trigger << endl;
        sndbuf[0] = magic_number;
        sndbuf[1] = 0xD9;
        memcpy(sndbuf + 2, &num_triggers, sizeof(uint32_t));
        if(SocketActive->Send(sndbuf, 6) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Data sent to socket" << endl;
        return 0;
    }

    int pico_configure_channels_s(const uint32_t number_channels, enPicoConnectProbeRange* channel_ranges, uint8_t trigger_channel)
    {
        this->number_channels = number_channels;
        logger << LogPref::Flag(INFO) << "Number of channels set to: " << number_channels << endl;

        this->channels = create_channel(number_channels);
        conditions = new PS4000A_CONDITION[number_channels];
        for (uint32_t i = 0; i < number_channels; ++i) {
            conditions[i].source = channels[i];
            conditions[i].condition = PS4000A_CONDITION_FALSE;
        }

        logger << LogPref::Flag(INFO) << "DEBUG RANGE" << trigger_channel << " " << channel_ranges[trigger_channel] << endl;
        auto retval = ps4000aSetChannel(handle, channels[trigger_channel], true, PS4000A_DC, channel_ranges[trigger_channel], 0);

        if(retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to set trigger channel. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x06;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Trigger channel set" << endl;

        for(uint8_t i = 0; i < number_channels; ++i) {
            if(i == trigger_channel) continue;
            else {
                logger << LogPref::Flag(INFO) << "DEBUG RANGE" << i << " " << channel_ranges[i] << endl;
                auto retval = ps4000aSetChannel(handle, channels[i], true, PS4000A_AC, channel_ranges[i], 0);
                if (retval != 0) {
                    logger << LogPref::Flag(ERROR) << "Failed to set channel " << i << ". Code: " << retval << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x07 + i;
                    this->error_code = retval;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                logger << LogPref::Flag(INFO) << "Channel " << i << " set" << endl;
            }
        }

        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to set trigger. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x12;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xE9;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
    }

    int pico_configure_channels_t(const uint32_t number_channels, enPicoConnectProbeRange* channel_ranges, uint8_t trigger_channel, int32_t direction, uint16_t threshold, int16_t autoTrigger_ms) {
        // Set number of channels for measurement
        this->number_channels = number_channels;
        logger << LogPref::Flag(INFO) << "Number of channels set to: " << number_channels << endl;
        logger << LogPref::Flag(INFO) << "trigger_channel set to: " << (int) trigger_channel << endl;
        logger << LogPref::Flag(INFO) << "direction set to: " << direction << endl;
        logger << LogPref::Flag(INFO) << "threshold set to: " << threshold << endl;
        logger << LogPref::Flag(INFO) << "autoTrigger_ms set to: " << autoTrigger_ms << endl;

        this->channels = create_channel(number_channels);
        conditions = new PS4000A_CONDITION[number_channels];
        for (uint32_t i = 0; i < number_channels; ++i) {
            conditions[i].source = channels[i];
            conditions[i].condition = PS4000A_CONDITION_FALSE;
        }

        logger << LogPref::Flag(INFO) << "DEBUG RANGE" << trigger_channel << " " << channel_ranges[trigger_channel] << endl;
        if(auto retval = ps4000aSetChannel(handle, channels[trigger_channel], true, PS4000A_DC, channel_ranges[trigger_channel], 0) != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to set trigger channel. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x06;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Trigger channel set" << endl;

        this->trig_channel = channels[trigger_channel];
        this->th_direction = static_cast<PS4000A_THRESHOLD_DIRECTION>(direction);
        this->threshold = threshold;
        this->trig_delay = 10;
        this->trig_autoTrigger_ms = autoTrigger_ms;

        for(uint8_t i = 0; i < number_channels; ++i) {
            if(i == trigger_channel) continue;
            else {
                logger << LogPref::Flag(INFO) << "DEBUG RANGE" << i << " " << channel_ranges[i] << endl;
                auto retval = ps4000aSetChannel(handle, channels[i], true, PS4000A_AC, channel_ranges[i], 0);
                if (retval != 0) {
                    logger << LogPref::Flag(ERROR) << "Failed to set channel " << i << ". Code: " << retval << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x07 + i;
                    this->error_code = retval;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
                logger << LogPref::Flag(INFO) << "Channel " << i << " set" << endl;
            }
        }

        logger << LogPref::Flag(INFO) << "DEBUG TRIGGER" << endl;
        logger << LogPref::Flag(INFO) << this->handle << endl;
        logger << LogPref::Flag(INFO) << this->trig_channel << endl;
        logger << LogPref::Flag(INFO) << this->threshold << endl;
        logger << LogPref::Flag(INFO) << this->th_direction << endl;
        logger << LogPref::Flag(INFO) << this->trig_delay << endl;
        logger << LogPref::Flag(INFO) << this->trig_autoTrigger_ms << endl;

        auto retval = ps4000aSetSimpleTrigger
        (
         handle,
         true,
         conditions[trigger_channel].source,
         threshold,
         th_direction,
         trig_delay,
         trig_autoTrigger_ms
        );

        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to set trigger. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x12;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC9;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    // To be deprecated in future versions

    int pico_exit() {
        // Exit Pico device and free resources
        if (handle != NULL) {
            auto ret = ps4000aCloseUnit(handle);
            if (ret != 0) {
                logger << LogPref::Flag(ERROR) << "Failed to close Pico device. Code: " << ret << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x23;
                this->error_code = ret;
                memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                if(SocketActive->Send(sndbuf, 7) == -1) {
                    logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                }
                return -1;
            }

            sndbuf[0] = magic_number;
            sndbuf[1] = 0xCD;
            if(SocketActive->Send(sndbuf, 2) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                return -1;
            }

            logger << LogPref::Flag(INFO) << "Pico device closed" << endl;
            handle = NULL;
        }

        return 0;
    }

    int pico_configure_trigger(int8_t trigger_channel, int32_t direction, uint16_t threshold, int16_t autoTrigger_ms) {
        // Configure trigger for Pico device
        this->trig_channel = channels[trigger_channel];
        this->th_direction = static_cast<PS4000A_THRESHOLD_DIRECTION>(direction);
        this->trig_delay = 10;
        this->threshold = threshold;
        this->trig_autoTrigger_ms = autoTrigger_ms;

        logger << LogPref::Flag(INFO) << "Trigger configured" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xC9;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        auto retval = ps4000aSetSimpleTrigger
        (
         handle,
         true,
         channels[trigger_channel],
         this->threshold,
         this->th_direction,
         this->trig_delay,
         this->trig_autoTrigger_ms
        );

        if (retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to set trigger. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x16;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        logger << LogPref::Flag(INFO) << "Trigger set successfully" << endl;\
        sndbuf[0] = magic_number;
        sndbuf[1] = 0xE0;
        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        return 0;
    }

    int pico_set_premeasurement(uint32_t percentage)
    {
        // Set pre-measurement percentage
        this->premeasurement_percentage = percentage;
        logger << LogPref::Flag(INFO) << "Pre-measurement percentage set to: " << percentage << "%" << endl;

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xE8;

        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        return 0;
    }

    int pico_tcp_measurement_s()
    {
        logger << LogPref::Flag(INFO) << "Multi-trigger measurement started" << endl;

        data_buffer_vec.assign(num_triggers, std::vector<int16_t*>(number_channels, nullptr));

        logger << LogPref::Flag(INFO) << "Get timebase:" << endl;
        uint32_t timebase = timebase_choice(sample_rate);
        logger << "Timebase = " << timebase << endl;

        #define FAST_BLOCK

        #ifdef FAST_BLOCK
        logger << LogPref::Flag(INFO) << "Set data buffer:" << endl;
        for(size_t i = 0; i < num_triggers; ++i) {
            for(size_t j = 0; j < number_channels; ++j) {
                data_buffer_vec[i][j] = new int16_t[points_vector[i]];
                memset(data_buffer_vec[i][j], 0, points_vector[i]);
            }
        }

        int32_t* nSamples = new int32_t[num_triggers];
        int32_t retval = ps4000aMemorySegments(handle, num_triggers, nSamples);
        if(retval != 0) {
                logger << LogPref::Flag(ERROR) << "Failed to get values. Code: " << retval << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x21;
                this->error_code = retval;
                memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                if(SocketActive->Send(sndbuf, 7) == -1) {
                    logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                }
                return -1;
        }

        retval = ps4000aSetNoOfCaptures(handle, num_triggers);
        if(retval != 0) {
                logger << LogPref::Flag(ERROR) << "Failed to get values. Code: " << retval << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x21;
                this->error_code = retval;
                memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                if(SocketActive->Send(sndbuf, 7) == -1) {
                    logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                }
                return -1;
        }

        int32_t timeIntervalNanoseconds = 0;
        int32_t maxSamples = 0;
        int32_t segmentIndex = 0;

        retval = ps4000aGetTimebase(handle, timebase, points_value, &timeIntervalNanoseconds,
                                                    &maxSamples, segmentIndex);
        logger << "retval: " << retval << endl;
        if(retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to get timebase. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x13;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }

        int32_t noOfPreTriggerSamples = 0;
        int32_t noOfPostTriggerSamples = points_value;

        logger << LogPref::Flag(INFO) << "Run block" << endl;
        retval = ps4000aRunBlock(handle, noOfPreTriggerSamples, noOfPostTriggerSamples, timebase,
                                      nullptr, segmentIndex, nullptr, nullptr);
        if(retval != 0) {
                logger << LogPref::Flag(ERROR) << "Failed to get values. Code: " << retval << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x21;
                this->error_code = retval;
                memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                if(SocketActive->Send(sndbuf, 7) == -1) {
                    logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                }
                return -1;
        }
        logger << LogPref::Flag(INFO) << "Run block done" << endl;

        int16_t ready = 0;
        while (ready == 0) {
                retval = ps4000aIsReady(handle, &ready);
        }

        logger << LogPref::Flag(INFO) << "Data ready" << endl;

        for(size_t i = 0; i < num_triggers; ++i) {
            for(size_t j = 0; j < number_channels; ++j) {
                logger << LogPref::Flag(INFO) << "Set data buffer for channel " << j << endl;
                PS4000A_RATIO_MODE mode = PS4000A_RATIO_MODE_NONE;
                retval = ps4000aSetDataBuffer(handle, channels[j], data_buffer_vec[i][j], points_vector[i],
                                              i, mode);
                logger << "retval: " << retval << endl;
                if(retval != 0) {
                    logger << LogPref::Flag(ERROR) << "Failed to set data buffer for channel " << j << ". Code: " << retval << endl;
                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFF;
                    sndbuf[2] = 0x15 + j;
                    this->error_code = retval;
                    memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
                    if(SocketActive->Send(sndbuf, 7) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                    }
                    return -1;
                }
            }
        }

        logger << LogPref::Flag(INFO) << "Set data buffer done" << endl;

        logger << LogPref::Flag(INFO) << "Data collection start" << endl;
        uint32_t StartIndex = 0;
        uint32_t noOfSamples = static_cast<uint32_t>(points_value);
        int16_t overflow = 0;
        uint32_t DownSampleRatio = 1;

        retval = ps4000aGetValuesBulk(handle, &noOfSamples, 0, num_triggers-1, DownSampleRatio, PS4000A_RATIO_MODE_NONE, &overflow);
        logger << "retval: " << retval << endl;
        if(retval != 0) {
            logger << LogPref::Flag(ERROR) << "Failed to get values. Code: " << retval << endl;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xFF;
            sndbuf[2] = 0x21;
            this->error_code = retval;
            memcpy(sndbuf + 3, &this->error_code, sizeof(uint32_t));
            if(SocketActive->Send(sndbuf, 7) == -1) {
                logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            }
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Data collection done" << endl;

        logger << LogPref::Flag(INFO) << "Confirm data to socket" << endl;
        #endif // FAST_BLOCK

        logger << LogPref::Flag(INFO) << "Sending confirmation code" << endl;
        sndbuf[0] = magic_number;
        sndbuf[1] = 0xFB;
        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }
        logger << LogPref::Flag(INFO) << "Confirmation code sended" << endl;
        int bytes_recieved = SocketActive->Receive(MAX_PACKET, rcbuf);
        if(rcbuf[1] == 0xCB)
        {
            logger << LogPref::Flag(INFO) << "Data confirmed" << endl;
        }
        else{
            return -1;
        }

        logger << LogPref::Flag(INFO) << "Sending data" << endl;

        std::chrono::time_point<std::chrono::system_clock> time_now = std::chrono::system_clock::now();
        std::time_t t_n = std::chrono::system_clock::to_time_t(time_now);

        for(uint32_t i = 0; i < num_triggers; ++i)
        {
            for(uint32_t j = 0; j < number_channels; ++j)
            {
                uint32_t data_seek = 0;
                uint32_t packet = 0;
                uint32_t total_data = sizeof(int16_t) * points_vector[i];
                uint32_t total_packets = 1 + ((uint32_t) ((total_data * 1.0f) / MAX_PACKET));
                do
                {
                    uint32_t ndata_sent;
                    if(total_data - data_seek < MAX_PACKET - 22)
                    {
                        ndata_sent = total_data - data_seek;
                    }
                    else
                    {
                        ndata_sent = MAX_PACKET - 22;
                    }

                    sndbuf[0] = magic_number;
                    sndbuf[1] = 0xFD;
                    memcpy(sndbuf + 2, &i, sizeof(uint32_t));
                    memcpy(sndbuf + 6, &j, sizeof(uint32_t));
                    memcpy(sndbuf + 10, &total_packets, sizeof(uint32_t));
                    memcpy(sndbuf + 14, &packet, sizeof(uint32_t));
                    memcpy(sndbuf + 18, &ndata_sent, sizeof(uint32_t));
                    int16_t test_data = data_buffer_vec[i][j][0];
                    memcpy(sndbuf + 22, data_buffer_vec[i][j] + (data_seek / 2), ndata_sent);

                    logger << LogPref::Flag(INFO) << "Total data: " << total_data << ". Data seek: " << data_seek << ". Test data:" << test_data << endl;
                    logger << LogPref::Flag(INFO) << "Sending packet " << packet << " of " << total_packets << " (sig" << i << "ch" << j << "). Data to send: " << ndata_sent << endl;

                    if(SocketActive->Send(sndbuf, 22 + ndata_sent) == -1) {
                        logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
                        return -1;
                    }
                    data_seek += ndata_sent;
                    ++packet;

                    int bytes_recieved = SocketActive->Receive(MAX_PACKET, rcbuf);
                    if(rcbuf[1] == 0x3D)
                    {
                        logger << LogPref::Flag(INFO) << "Data confirmed" << endl;
                    }
                    else{
                        return -1;
                    }
                } while (packet < total_packets);
            }
        }

        sndbuf[0] = magic_number;
        sndbuf[1] = 0xFC;
        if(SocketActive->Send(sndbuf, 2) == -1) {
            logger << LogPref::Flag(ERROR) << "Failed to send data to socket" << endl;
            return -1;
        }

        bytes_recieved = SocketActive->Receive(MAX_PACKET, rcbuf);
        if(rcbuf[1] == 0x3C)
        {
            logger << LogPref::Flag(INFO) << "Data confirmed" << endl;
        }
        else{
            return -1;
        }


        return 0;

    }
};

int main()
{
    logger.enableConsoleOutput(true);

    logger << "Open socket" << endl;
    CPassiveSocket SocketPassive(CSimpleSocket::CSocketType::SocketTypeTcp);
    if (!SocketPassive.Initialize())
    {
        logger << LogPref::Flag(ERROR) << "Socket initialization failed" << endl;
        return -1;
    }
    if(!SocketPassive.Listen("localhost", 5003))
    {
        logger << LogPref::Flag(ERROR) << "Socket listening failed" << endl;
        return -1;
    }

    logger << "Socket initialized!" << endl;

    CActiveSocket* ClientSocket = nullptr;
    pico_server_dispatcher pico_server(ClientSocket);

    while(true)
    {
        logger << LogPref::Flag(INFO) << "Wait for connection..." << endl;
        logger.closeLogger();

        if ((ClientSocket = SocketPassive.Accept()) == nullptr)
        {
            logger.closeLogger();
            logger.initializeLogger("picologs", "pico");
            logger.enableConsoleOutput(true);
            logger << LogPref::Flag(ERROR) << "Socket accept failed" << endl;
            logger.closeLogger();
            return -1;
        }

        logger.initializeLogger("picologs", "pico");
        logger.enableConsoleOutput(true);
        logger << LogPref::Flag(INFO) << "Client connected" << endl;

        while(pico_server.acceptRequest() == true)
        {
            logger << LogPref::Flag(INFO) << "Request received" << endl;
        }

        logger << LogPref::Flag(INFO) << "Request processing finished" << endl;

        delete ClientSocket;
        ClientSocket = nullptr;
    }

    return 0;
}