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

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


#define MAX_PACKET 4096
using namespace std;

std::vector<std::string> split(const std::string& s, const std::string& delimiter) {
    std::vector<std::string> tokens;
    size_t pos = 0;
    std::string token;
    while ((pos = s.find(delimiter)) != std::string::npos) {
        token = s.substr(0, pos);
        tokens.push_back(token);
        s.erase(0, pos + delimiter.length());
    }
    tokens.push_back(s);

    return tokens;
}

class PicoLocalClient
{
    private:
        CActiveSocket *socket;
        uint8_t sndbuf[MAX_PACKET]{0};
        uint8_t rcvbuf[MAX_PACKET]{0};
        std::string line_edit;
        std::vector<std::string> tokens;
        uint8_t error_state = 0x00;
        uint32_t error_code = 0x00000000;
    public:
        PicoLocalClient(CActiveSocket *socket) : socket(socket) {}
        ~PicoLocalClient() { delete socket; }

        int pico_open()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x01;
            if(socket->Send(sndbuf, 2) == 0)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();

            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC1)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device opened" << endl;
            return 0;
        }

        int pico_xml_config(const std::string& file_name)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x02;
            uint32_t size = file_name.size();
            memcpy(sndbuf + 2, &size, sizeof(uint32_t));
            memcpy(sndbuf + 6, file_name.c_str(), size);

            if(socket->Send(sndbuf, size + 6) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();

            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC2)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device configured" << endl;
            return 0;            
        }

        int pico_set_params(uint32_t* apoints, uint32_t* times, uint32_t sample_rate, uint32_t number_channels, uint32_t size)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x0C;
            memcpy(sndbuf + 2, &size, sizeof(uint32_t));
            memcpy(sndbuf + 6, apoints, size * sizeof(uint32_t));
            memcpy(sndbuf + 6 + size * sizeof(uint32_t), times, size * sizeof(uint32_t));
            memcpy(sndbuf + 6 + 2 * size * sizeof(uint32_t), &sample_rate, sizeof(uint32_t));
            memcpy(sndbuf + 10 + 2 * size * sizeof(uint32_t), &number_channels, sizeof(uint32_t));

            if(socket->Send(sndbuf, size * 2 * sizeof(uint32_t) + 14) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }
            
            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;
            }
            else if(rcvbuf[1] != 0xCC)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device parameters set" << endl;
            return 0;
        }

        int pico_close()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x03;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC3)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device closed" << endl;
            return 0;
        }

        int pico_get_current_params()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x04;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC4)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            if(bytes_received < 18)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }

            uint32_t size;
            memcpy(&size, rcvbuf + 2, sizeof(uint32_t));
            if(size < 1)
            {
                cerr << "Invalid data size" << endl;
                return -1;
            }

            uint32_t* apoints = new uint32_t[size];
            memcpy(apoints, rcvbuf + 6, size * sizeof(uint32_t));
            for(uint32_t i = 0; i < size; ++i)
            {
                cout << "apoints[" << i << "] = " << std::dec << apoints[i] << endl;
            }
            uint32_t* atimes = new uint32_t[size];
            memcpy(atimes, rcvbuf + 6 + size * sizeof(uint32_t), size * sizeof(uint32_t));
            for(uint32_t i = 0; i < size; ++i)
            {
                cout << "atimes[" << i << "] = " << std::dec << atimes[i] << endl;
            }

            uint32_t sample_rate = 0;
            memcpy(&sample_rate, rcvbuf + 6 + 2 * size * sizeof(uint32_t), sizeof(uint32_t));
            cout << "sample_rate = " << std::dec << sample_rate << endl;

            uint32_t number_channels = 0;
            memcpy(&number_channels, rcvbuf + 10 + 2 * size * sizeof(uint32_t), sizeof(uint32_t));
            cout << "number_channels = " << std::dec << number_channels << endl;

            cout << "Pico device parameters retrieved" << endl;
            return 0;
        }

        int pico_probe()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x05;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC5)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device probe completed" << endl;
            return 0;            
        }

        int pico_set_points(uint32_t* points, uint32_t size)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x06;
            memcpy(sndbuf + 2, &size, sizeof(uint32_t));
            memcpy(sndbuf + 6, points, size * sizeof(uint32_t));

            if(socket->Send(sndbuf, size * sizeof(uint32_t) + 6) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC6)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device points set" << endl;
            return 0;
        }

        int pico_set_sample_rate(uint32_t sample_rate)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x07;
            memcpy(sndbuf + 2, &sample_rate, sizeof(uint32_t));

            if(socket->Send(sndbuf, sizeof(uint32_t) + 6) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;
return 0;                
            }
            else if(rcvbuf[1] != 0xC7)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device sample rate set" << endl;
            return 0;            
        }

        int pico_set_times(uint32_t* times, uint32_t size)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x08;
            memcpy(sndbuf + 2, &size, sizeof(uint32_t));
            memcpy(sndbuf + 6, times, size * sizeof(uint32_t));

            if(socket->Send(sndbuf, size * sizeof(uint32_t) + 6) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC8)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device times set" << endl;
            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)
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x09;
            memcpy(sndbuf + 2, &number_channels, sizeof(uint32_t));
            memcpy(sndbuf + 6, &trigger_channel, sizeof(uint8_t));
            memcpy(sndbuf + 7, &direction, sizeof(int32_t));
            memcpy(sndbuf + 11, &threshold, sizeof(uint16_t));
            memcpy(sndbuf + 13, &autoTrigger_ms, sizeof(int16_t));

            if(socket->Send(sndbuf, 15) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xC9)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device channels configured" << endl;
            return 0;            
        }

        int pico_begin_measurement()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x0A;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }

            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }
            
            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xCA)
            {
                cout << "Invalid command callback" << endl;
            }

            cout << "Pico device measurement started" << endl;
            cout << "Measurement in progress..." << endl;

            while(rcvbuf[1] != 0xCB)
            {
                if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
                {
                    cerr << "Failed to receive data from socket" << endl;
                    return -1;
                }
                bytes_received = socket->GetBytesReceived();
                if(bytes_received == 0)
                {  
                    cerr << "Server disconnected" << endl;
                    return -1;
                }
                if(bytes_received < 2)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                if(rcvbuf[0] != 0xAA)
                {
                    cerr << "Invalid magic number" << endl;
                    return -1;
                }
                if(rcvbuf[1] == 0xFF)
                {
                    if(bytes_received < 7)
                    {
                        cerr << "Invalid buffer size" << endl;
                        return -1;
                    }

                    cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                    memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                    memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                    cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
                }
                else if(rcvbuf[1] != 0xCA)
                {
                    cerr << "Invalid command callback" << endl;
                    return -1;
                }

                cout << "Measurement in progress..." << endl;
            }

            cout << "Pico device measurement completed" << endl;
            return 0;
        }

        int pico_stop()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x0E;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }
            
            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xCE)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device measurement stopped" << endl;
            return 0;
        }

        int pico_exit()
        {
            sndbuf[0] = 0xAA;
            sndbuf[1] = 0x0D;
            if(socket->Send(sndbuf, 2) == -1)
            {
                cerr << "Failed to send data to socket" << endl;
                return -1;
            }
            if(socket->Receive(rcvbuf, sizeof(rcvbuf)) == -1)
            {
                cerr << "Failed to receive data from socket" << endl;
                return -1;
            }

            uint32_t bytes_received = socket->GetBytesReceived();
            if(bytes_received == 0)
            {  
                cerr << "Server disconnected" << endl;
                return -1;
            }
            if(bytes_received < 2)
            {
                cerr << "Invalid buffer size" << endl;
                return -1;
            }
            if(rcvbuf[0] != 0xAA)
            {
                cerr << "Invalid magic number" << endl;
                return -1;
            }

            if(rcvbuf[1] == 0xFF)
            {
                if(bytes_received < 7)
                {
                    cerr << "Invalid buffer size" << endl;
                    return -1;
                }

                cerr << "Error at ADC: " << std::hex << rcvbuf[2] << endl;
                memcpy(&error_state, rcvbuf + 2, sizeof(uint8_t));
                memcpy(&error_code, rcvbuf + 3, sizeof(uint32_t));
                cerr << "Error code: " << std::hex << error_code << " " << return_fun(error_code) << endl;                
            }
            else if(rcvbuf[1] != 0xCD)
            {
                cerr << "Invalid command callback" << endl;
                return -1;
            }

            cout << "Pico device exited" << endl;
            return 0;
        }

        int get_command()
        {
            if(socket->Receive(sndbuf, sizeof(sndbuf)) != 0)
            {
                std::getline(std::cin, line_edit);
                tokens = split(line_edit, " ");

                if(tokens.size() < 1)
                {
                    cerr << "Invalid command" << endl;
                    return -1;
                }

                if(tokens[0] = "open")
                {
                    if(pico_open() != 0)
                    {
                        cerr << "Failed to open pico device" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "xml_config")
                {
                    if(tokens.size() < 2)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }
                    if(pico_xml_config(tokens[1]) != 0)
                    {
                        cerr << "Failed to configure pico device" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "set_params")
                {
                    if(tokens.size() < 5)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }

                    std::vector<uint32_t> points;
                    std::vector<uint32_t> times;
                    uint32_t sample_rate = std::stoi(tokens[2]);
                    uint32_t number_channels = std::stoi(tokens[3]);
                    uint32_t size = std::stoi(tokens[4]);
                    points = string_to_vector(points);
                    times = string_to_vector(times);
                    if(points.size() != size || times.size() != size)
                    {
                        cerr << "Invalid data size" << endl;
                        return -1;
                    }
                    if(pico_set_params(points.data(), times.data(), sample_rate, number_channels, size) != 0)
                    {
                        cerr << "Failed to set pico device parameters" << endl;
                        return -1;
                    }
                    return 0;
                }
                else if(tokens[0] == "close")
                {
                    if(pico_close() != 0)
                    {
                        cerr << "Failed to close pico device" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "exit")
                {
                    if(pico_exit() != 0)
                    {
                        cerr << "Failed to exit pico device" << endl;
                        return -1;
                    }

                    return 1;
                }
                else if(tokens[0] == "get_current_params")
                {
                    if(pico_get_current_params() != 0)
                    {
                        cerr << "Failed to get pico device parameters" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "probe")
                {
                    if(pico_probe() != 0)
                    {
                        cerr << "Failed to probe pico device" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "set_points")
                {
                    if(tokens.size() < 3)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }

                    uint32_t size = std::stoi(tokens[2]);
                    std::vector<uint32_t> points = string_to_vector(points);
                    if(points.size() != size)
                    {
                        cerr << "Invalid data size" << endl;
                        return -1;
                    }
                    if(pico_set_points(points.data(), size) != 0)
                    {
                        cerr << "Failed to set pico device points" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "set_sample_rate")
                {
                    if(tokens.size() < 2)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }

                    uint32_t sample_rate = std::stoi(tokens[1]);
                    if(pico_set_sample_rate(sample_rate) != 0)
                    {
                        cerr << "Failed to set pico device sample rate" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "set_times")
                {
                    if(tokens.size() < 3)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }

                    uint32_t size = std::stoi(tokens[2]);
                    std::vector<uint32_t> times = string_to_vector(times);
                    if(times.size() != size)
                    {
                        cerr << "Invalid data size" << endl;
                        return -1;
                    }
                    if(pico_set_times(times.data(), size) != 0)
                    {
                        cerr << "Failed to set pico device times" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "configure_channels")
                {
                    if(tokens.size() < 6)
                    {
                        cerr << "Invalid command" << endl;
                        return -1;
                    }
                    uint32_t number_channels = std::stoi(tokens[1]); // number of channels (1-8)
                    uint8_t trigger_channel = std::stoi(tokens[2]); // trigger channel (0-7, 0 = channel A, 1 = channel B, ...)
                    int32_t direction = std::stoi(tokens[3]); // trigger direction (0 = rising, 1 = falling)
                    uint16_t threshold = std::stoi(tokens[4]); // trigger threshold (0-65535)
                    int16_t autoTrigger_ms = std::stoi(tokens[5]); // auto trigger time (0-65535 ms)
                    if(pico_configure_channels(number_channels, trigger_channel, direction, threshold, autoTrigger_ms) != 0)
                    {
                        cerr << "Failed to configure pico device channels" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "begin_measurement")
                {
                    if(pico_begin_measurement() != 0)
                    {
                        cerr << "Failed to begin pico device measurement" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "stop")
                {
                    if(pico_stop() != 0)
                    {
                        cerr << "Failed to stop pico device measurement" << endl;
                        return -1;
                    }
                }
                else if(tokens[0] == "help")
                {
                    cout << "Available commands:" << endl;
                    cout << "open" << endl;
                    cout << "xml_config <filename>" << endl;
                    cout << "set_params <points> <times> <sample_rate> <number_channels> <size>" << endl;
                    cout << "close" << endl;
                    cout << "exit" << endl;
                    cout << "get_current_params" << endl;
                    cout << "probe" << endl;
                    cout << "set_points <points> <size>" << endl;
                    cout << "set_sample_rate <sample_rate>" << endl;
                    cout << "set_times <times> <size>" << endl;
                    cout << "configure_channels <number_channels> <trigger_channel> <direction> <threshold> <autoTrigger_ms>" << endl;
                    cout << "begin_measurement" << endl;
                    cout << "stop" << endl;
                }
                else if(tokens[0] == "test")
                {
                    cout << "Test command received" << endl;
                }
                else
                {
                    cerr << "Unknown command" << endl;
                    return -1;
                }
            }

            return 0;
        }
}

int main()
{
    cout << "Open socket" << endl;
    CPassiveSocket SocketPassive(CSimpleSocket::CSocketType::SocketTypeTcp);
    if (!SocketPassive.Initialize())
    {
        cerr << "Socket initialization failed" << endl;
        return -1;
    }
    if(!SocketPassive.Listen("localhost", 5002))
    {
        cerr << "Socket listening failed" << endl;
        return -1;
    }

    CActiveSocket* ClientSocket;

    while(true)
    {
        if ((ClientSocket = SocketPassive.Accept()) != nullptr)
        {
            cerr << "Socket accept failed" << endl;
            return -1;
        }

        cout << LogPref::Flag(INFO) << "Client connected" << endl;
        pico_service = new PicoLocalService(ClientSocket);

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

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

        delete ClientSocket;
        ClientSocket = nullptr;
    }

    return 0;
}