#include <iostream>
#include <memory>
#include <windows.h>
#include <tuple>
#include "picofunctions.h"
#include "pugiconfig.hpp"
#include "pugixml.hpp"
#include "simplelogger.hpp"
#include "parser.hpp"
#include "src/PassiveSocket.h"


#define MAX_PACKET 4096
using namespace std;

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

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 number_channels = 0;
    std::vector<PS4000A_CHANNEL> channels;
    std::vector<uint32_t> points_vector;
    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 < 15) {
                    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));
                uint8_t trigger_channel = 0;
                memcpy(&trigger_channel, rcbuf + 6, sizeof(uint8_t));
                int32_t direction = 0;
                memcpy(&direction, rcbuf + 7, sizeof(int32_t));
                uint16_t threshold = 0;
                memcpy(&threshold, rcbuf + 11, sizeof(uint16_t));
                int16_t autoTrigger_ms = 0;
                memcpy(&autoTrigger_ms, rcbuf + 13, sizeof(int16_t));
                pico_configure_channels(channels, trigger_channel, direction, threshold, autoTrigger_ms);
                logger << LogPref::Flag(INFO) << "Pico device channels configured" << endl;
                break;
            }
            case 0x0A:
            {
                this->last_command = 0x0A;
                logger << LogPref::Flag(INFO) << "Command 0x0A received" << endl;
                pico_begin_measurement();
                logger << LogPref::Flag(INFO) << "Pico device measurement started" << endl;
                break;
            }
            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;
            }
            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;
    }

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

    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_configure_channels(const uint32_t number_channels, 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;

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

        if(auto retval = ps4000aSetChannel(handle, channels[trigger_channel], true, PS4000A_DC, PICO_X1_PROBE_5V, 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 {
                auto retval = ps4000aSetChannel(handle, channels[i], true, PS4000A_AC, PICO_X1_PROBE_5V, 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;
            }
        }

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

    int pico_begin_measurement() {
        // Begin measurement on Pico device

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

        for(size_t i = 0; i < times.size(); ++i) {


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

            for(size_t j = 0; j < number_channels; ++j) {
                data_buffer_vec[i][j] = new int16_t[points_vector[i]];
            }

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


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

            int32_t retval = ps4000aGetTimebase(handle, timebase, points_vector[i], &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;
            }

            logger << LogPref::Flag(INFO) << "Run block:" << endl;

            int32_t noOfPreTriggerSamples = 0;
            int32_t noOfPostTriggerSamples = points_value;

            retval = ps4000aRunBlock(handle, noOfPreTriggerSamples, noOfPostTriggerSamples, timebase,
                                      nullptr, segmentIndex, nullptr, nullptr);
            logger << "retval: " << retval << endl;
            if(retval != 0) {
                logger << LogPref::Flag(ERROR) << "Failed to run block. Code: " << retval << endl;
                sndbuf[0] = magic_number;
                sndbuf[1] = 0xFF;
                sndbuf[2] = 0x14;
                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);
            }

            for(uint8_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],
                                              segmentIndex, 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_vector[i]);
            int16_t overflow = 0;
            uint32_t DownSampleRatio = 1;

            retval = ps4000aGetValues(handle, StartIndex, &noOfSamples, DownSampleRatio, PS4000A_RATIO_MODE_NONE,
                                        segmentIndex, &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;
            sndbuf[0] = magic_number;
            sndbuf[1] = 0xCA;
            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;

            logger << LogPref::Flag(INFO) << "Data confirmed" << endl;
            logger << LogPref::Flag(INFO) << "Sleeping for " << times[i+1] - times[i] << " ms" << endl;
            if(i < times.size() - 1) {
                std::chrono::duration<int64, std::milli> duration(times[i+1] - times[i]);
                std::this_thread::sleep_for(duration);
            }
        }

        for(int i = 0; i < data_buffer_vec.size(); ++i) {
            logger << LogPref::Flag(INFO) << "Writing data " << i << " to file" << endl;
            std::string filename = "data" + std::to_string(i) + ".csv";
            writing_data_fixed_name(filename, data_buffer_vec[i], points_vector[i], number_channels);
            logger << LogPref::Flag(INFO) << "Data collection done" << endl;
            logger << LogPref::Flag(INFO) << "Free buffers? Why?" << endl;
            free_buffers(data_buffer_vec[i]);
            logger << LogPref::Flag(INFO) << "Buffers freed" << endl;
        }

        logger << LogPref::Flag(INFO) << "Measurement done" << endl;
        sndbuf[0] = magic_number;
        sndbuf[1] = 0xCB;

        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_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 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("127.0.0.1", 5003))
    {
        logger << LogPref::Flag(ERROR) << "Socket listening failed" << endl;
        return -1;
    }

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

    CActiveSocket* ClientSocket;
    PicoLocalService* pico_service;

    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;
        pico_service = new PicoLocalService(ClientSocket);

        while(pico_service->get_request() != 0)
        {
            logger << LogPref::Flag(INFO) << "Request received" << endl;
        }

        logger << LogPref::Flag(INFO) << "Request processing finished" << endl;
        delete pico_service;
        pico_service = nullptr;

        delete ClientSocket;
        ClientSocket = nullptr;
    }

    return 0;
}