RealSenseServer.h

#pragma once
#include "RealSenseInterface/Headers/RealSenseScanner.h"
#include "RealSenseInterface/Headers/RealSenseScannerFactory.h"
#include "Design/Headers/DesignObjectLookup.h"
#include <NDEVR/Connection.h>
#include <NDEVR/OrientationSensor.h>
#include "Widgets/Headers/QTTools.h"
#include "Base/Headers/Translator.h"
#include "Base/Headers/BasicThread.h"
#include <QTimer>
namespace NDEVR
{
    union RealsensePacketChecksum
    {
        uint08 index_value = 0x8c15f09407357894;    
        char char_value[8];                         
    };
    static_assert(sizeof(RealsensePacketChecksum) == 8, "Bad checksum");
    enum PacketType : uint01
    {
        e_command_packet = 0        
        , e_probe_info_packet = 1   
        , e_cloud_data = 2          
    };
#pragma pack(push,1)
    struct CommandPacket
    {
        char packet_info[255];  
    };
#pragma pack(pop)
#pragma pack(push,1)
    struct InfoPacketHeader
    {
        UUID scanner_id;        
        Matrix<fltp08> matrix;  
        sint02 gyro_status = 0; 
        Time frame_time;        
        RealSenseDeviceType device_type = RealSenseDeviceType::e_unknown;   
    };
#pragma pack(pop)
#pragma pack(push,1)
    struct DataPacketHeader
    {
        uint02 data_size = 0;                   
        uint02 compressed_point_data_size = 0;  
        uint02 compressed_color_data_size = 0;  
    };
#pragma pack(pop)
 
#pragma pack(push,1)
    struct RealsensePacketHeader
    {
        RealsensePacketChecksum check_sum;  
        PacketType packet_type = PacketType::e_command_packet;  
        uint01 packet_num = 0;              
    };
#pragma pack(pop)
    static_assert(sizeof(RealsensePacketHeader) == 10, "Bad header");
 
 
    static_assert(sizeof(RealsensePacketHeader) == 10, "Bad header");
    class RealSenseServer
    {
    public:
        static constexpr uint04 rx_port_number = 31458; 
        static constexpr uint04 tx_port_number = 31459; 
        static void CreateCommandPacket(String& packet, const String& command, uint04 packet_num)
        {
            packet.setSize(sizeof(RealsensePacketHeader) + sizeof(CommandPacket));
 
            RealsensePacketHeader header;
            header.packet_type = PacketType::e_probe_info_packet;
            header.packet_num = cast<uint01>(packet_num % 256);
            memmove(packet.begin(), &header, sizeof(RealsensePacketHeader));
 
            CommandPacket info;
            memmove(info.packet_info, command.begin(), command.size());
            info.packet_info[command.size()] = '\0';
            memmove(packet.begin(sizeof(RealsensePacketHeader)), &info, sizeof(CommandPacket));
        }

        static void CreateProbeInfoPacket(String& packet, const RasterPacketInfo& packet_info, uint04 packet_num)
        {
            packet.setSize(sizeof(RealsensePacketHeader) + sizeof(InfoPacketHeader));
 
            RealsensePacketHeader header;
            header.packet_type = PacketType::e_probe_info_packet;
            header.packet_num = cast<uint01>(packet_num % 256);
            InfoPacketHeader info;
            info.matrix = packet_info.mat;
            //info.device_type = packet_info.device_type;
            info.scanner_id = packet_info.scanner_id;
            info.frame_time = packet_info.frame_time;
            info.gyro_status = packet_info.gyro_status;
            memmove(packet.begin(), &header, sizeof(RealsensePacketHeader));
            memmove(packet.begin(sizeof(RealsensePacketHeader)), &info, sizeof(InfoPacketHeader));
        }

        static void CreateCloudPacket(String& packet, const RasterPacketInfo& packet_info, uint04 data_offset, uint02 data_size, uint04 packet_num)
        {
            packet.setSize(sizeof(RealsensePacketHeader) + sizeof(DataPacketHeader) + data_size * sizeof(Vertex<3, sint02>) + data_size * sizeof(RGBColor));
 
            RealsensePacketHeader header;
            header.packet_type = PacketType::e_cloud_data;
            header.packet_num = cast<uint01>(packet_num % 256);
            DataPacketHeader data;
            data.data_size = data_size;
            uint08 packet_offset = sizeof(RealsensePacketHeader) + sizeof(DataPacketHeader);
 
            size_t compressed_point_data_size = cast<size_t>(data_size) * sizeof(Vertex<3, sint02>);
            Compressor::ZLibCompress((uint01*)packet.begin(packet_offset), (uint01*)packet_info.active_point_data.begin(data_offset)
                , compressed_point_data_size, compressed_point_data_size);
            data.compressed_point_data_size = cast<uint02>(compressed_point_data_size);
            packet_offset += compressed_point_data_size;
 
 
            compressed_point_data_size = cast<size_t>(data_size) * sizeof(RGBColor);
            Compressor::ZLibCompress((uint01*)packet.begin(packet_offset), (uint01*)packet_info.active_color_data.begin(data_offset)
                , compressed_point_data_size, compressed_point_data_size);
            data.compressed_color_data_size = cast<uint02>(compressed_point_data_size);
            packet_offset += compressed_point_data_size;
 
 
            memmove(packet.begin(), &header, sizeof(RealsensePacketHeader));
            memmove(packet.begin(sizeof(RealsensePacketHeader)), &data, sizeof(DataPacketHeader));
            packet.setSize(sizeof(RealsensePacketHeader) + sizeof(DataPacketHeader) + data.compressed_point_data_size + data.compressed_color_data_size);
        }

        static uint04 ReadNetworkPacket(const String& packet, RasterPacketInfo& packet_info, uint04 offset, String& command_response)
        {
            RealsensePacketHeader header;
            if (offset + sizeof(RealsensePacketHeader) > packet.size())
                return offset;
            memcpy(&header, packet.begin(offset), sizeof(RealsensePacketHeader));
            if (header.check_sum.index_value != 0x8c15f09407357894)
            {
                char search[9];
                memcpy(search, RealsensePacketChecksum().char_value, 8);
                search[8] = 0;
                uint04 index_value = packet.indexOf(search, false, offset);
                if (IsInvalid(index_value))
                    return getMax(packet.size(), 7U) - 7U;//throw out 'invalid' data
                offset = index_value;
                if (offset + sizeof(RealsensePacketHeader) > packet.size())
                    return offset;
                memcpy(&header, packet.begin(offset), sizeof(RealsensePacketHeader));
            }
            if (header.packet_num != packet_info.packet_num)
            {
                if (packet_info.active_color_data.size() > 0)
                    return Constant<uint04>::Invalid;//signal for new frame
 
            }
            packet_info.packet_num = header.packet_num;
            switch (header.packet_type)
            {
            case PacketType::e_command_packet:
            {
                if (offset + sizeof(RealsensePacketHeader) + sizeof(CommandPacket) > packet.size())
                    return offset;//not all data recieved
                offset += sizeof(RealsensePacketHeader);
                CommandPacket info;
                memcpy(&info, packet.begin(offset), sizeof(CommandPacket));
                offset += sizeof(CommandPacket);
                command_response = String(info.packet_info);
            } break;
            case PacketType::e_probe_info_packet:
            {
                if (offset + sizeof(RealsensePacketHeader) + sizeof(InfoPacketHeader) > packet.size())
                    return offset;//not all data recieved
                offset += sizeof(RealsensePacketHeader);
                InfoPacketHeader info;
                memcpy(&info, packet.begin(offset), sizeof(InfoPacketHeader));
                offset += sizeof(InfoPacketHeader);
                packet_info.scanner_id = info.scanner_id;
                packet_info.mat = info.matrix;
                //packet_info.device_type = info.device_type;
                packet_info.frame_time = info.frame_time;
                packet_info.gyro_status = info.gyro_status;
            } break;
            case PacketType::e_cloud_data:
            {
                if (offset + sizeof(RealsensePacketHeader) + sizeof(DataPacketHeader) > packet.size())
                    return offset;//not all data recieved
                DataPacketHeader data;
                memcpy(&data, packet.begin(offset + sizeof(RealsensePacketHeader)), sizeof(DataPacketHeader));
 
                if (offset + sizeof(RealsensePacketHeader) + sizeof(DataPacketHeader)
                    + data.compressed_point_data_size
                    + data.compressed_color_data_size
                > packet.size())
                    return offset;//not all data recieved
                offset += sizeof(RealsensePacketHeader);
                offset += sizeof(DataPacketHeader);
 
                uint04 point_data_offset = packet_info.active_point_data.size();
                packet_info.active_point_data.addSpace<false>(data.data_size);
                packet_info.active_color_data.addSpace<false>(data.data_size);
 
                size_t decompressed_size = data.data_size * sizeof(Vector<3, sint02>);
 
                if (decompressed_size == data.compressed_point_data_size)
                {
                    memcpy(packet_info.active_point_data.begin(point_data_offset), packet.begin(offset), decompressed_size);
                }
                else
                {
                    Compressor::ZLibDecompress((uint01*)packet.begin(offset)
                        , (uint01*)packet_info.active_point_data.begin(point_data_offset)
                        , data.compressed_point_data_size, decompressed_size);
                }
                //lib_assert(decompressed_size == data.data_size * sizeof(Vector<3, sint02>), "bad decompression of rs point data");
                offset += data.compressed_point_data_size;
 
                decompressed_size = data.data_size * sizeof(RGBColor);
 
                if (decompressed_size == data.compressed_point_data_size)
                {
                    memcpy(packet_info.active_color_data.begin(point_data_offset), packet.begin(offset), decompressed_size);
                }
                else
                {
                    Compressor::ZLibDecompress((uint01*)packet.begin(offset)
                        , (uint01*)packet_info.active_color_data.begin(point_data_offset)
                        , data.compressed_color_data_size, decompressed_size);
                }
                offset += data.compressed_color_data_size;
                //lib_assert(decompressed_size == data.data_size * sizeof(RGBColor), "bad decompression of rs color data");
            } break;
 
            }
            return offset;
        }
    public:
        class WorkerThread : public QThread
        {
        public:
            WorkerThread(RealSenseServer* server)
                : m_server(server)
            {}

            void run() override
            {
                for (;;)
                {
                    QTTools::ServiceQT();
                    //Thread::sleep(100);
                    ThreadFunctions::RequestSleep(TimeSpan(0.25));
                    if (!m_server->updateServer())
                    {
                        ThreadFunctions::RequestSleep(TimeSpan(.01));
                    }
                }
            }
            RealSenseServer* m_server;  
        };

        RealSenseServer(RealSenseScannerFactory* scanner_factory)
            : m_scanner_factory(scanner_factory)
        {
            m_thread = new WorkerThread(this);
        }

        void sendPointData(const RasterPacketInfo& packet_info)
        {
            {
                CreateProbeInfoPacket(m_tx_packet, packet_info, m_tx_packet_count);
                m_tx_connection->tx(m_tx_packet);
            }
            for (uint04 i = 0; i < packet_info.active_color_data.size(); i += 150U)
            {
                CreateCloudPacket(m_tx_packet, packet_info, i, cast<uint02>(getMin(150U, packet_info.active_color_data.size() - i)), m_tx_packet_count);
                m_tx_connection->tx(m_tx_packet);
                //QTTools::ServiceQT();
                //QThread::sleep(1);
            }
            m_tx_packet_count++;
        }

        void sendCommandData(const String& data)
        {
            {
                CreateCommandPacket(m_tx_packet, data, m_tx_packet_count);
                m_tx_connection->tx(m_tx_packet);
            }
            m_tx_packet_count++;
        }

        void checkRecieve()
        {
            String s = m_rx_connection->rx();
            if (s.hasSubString("ping"))
            {
                return sendCommandData(s);
            }
            else if (s.hasSubString("get_time"))
            {
                return sendCommandData("get_time|" + String(Time::SystemTime()));
            }
            else if (s.hasSubString("reset_server"))
            {
                stopServer();
                startServer();
            }
            else if (s.hasSubString("request_gyro_calibration"))
            {
                for (RealSenseScanner* scanner : m_connected_scanners)
                    scanner->orientationSensor()->requestGyroAlignment(false);
            }
            else if (s.hasSubString("request_stream"))
            {
                StringView stream = s.split('|')[1];
 
                m_tx_connection = new Connection(Scene(_t("Tx Connection")));
                ConnectionInfo tx_connection_info;
                tx_connection_info.type = "UDP";
                tx_connection_info.address = stream + ":" + String(tx_port_number);
                tx_connection_info.connect_mode = ConnectionOpenType::e_write_only;
                m_tx_connection->open(tx_connection_info);
            }
            else if (s.hasSubString("set_system_time"))
            {
                StringView stream = s.split('|')[1];
                Time::SetSystemTime(stream.getAs<Time>());
                for (RealSenseScanner* scanner : m_connected_scanners)
                    scanner->invalidateTime();
            }
        }

        void initConnections()
        {
            m_rx_connection = new Connection(Scene(_t("Rx Connection")));
            ConnectionInfo rx_connection_info;
            rx_connection_info.type = "UDPLH";
            rx_connection_info.address = String(rx_port_number);
            rx_connection_info.connect_mode = ConnectionOpenType::e_read_only;
            m_rx_connection->open(rx_connection_info);
            QObject::connect(m_rx_connection, &Connection::dataAvailableSignal, m_rx_connection, [this]
            {
                checkRecieve();
            });
        }

        bool updateServer()
        {
            if (m_rx_connection == nullptr)
                initConnections();
            bool has_update = false;
            if (m_tx_connection)
            {
                for (RealSenseScanner* scanner : m_connected_scanners)
                {
                    if (scanner->getCurrentPacket(m_packet_info))
                    {
                        sendPointData(m_packet_info);
                        has_update = true;
                    }
                }
            }
 
            QTTools::log().addMessage("Scanners: "+String(m_connected_scanners.size()));
            checkRecieve();
            return has_update;
        }

        void stopServer()
        {
            for (RealSenseScanner* scanner : m_connected_scanners)
                delete scanner;
            m_connected_scanners.clear();
        }

        void startServer()
        {
            HardwareSearchPathParameters params;
            Buffer<QPointer<Connection>> connections = m_scanner_factory->possibleLocalConnections(params);
            for (QPointer<Connection> connection : connections)
            {
                if (connection == nullptr)
                    return;
                RealSenseScanner* controller = new RealSenseScanner(dynamic_cast<RealSenseScannerConnection*>(connection.data()), nullptr);
                if (controller->isValid())
                {
                    controller->setIsNetworkServer(true);
                    QTimer::singleShot(2000, [controller]
                    {
                        controller->orientationSensor()->requestGyroAlignment(false);
                    });
                    m_connected_scanners.add(controller);
                }
                else
                {
                    delete controller;
                }
            }
            m_thread->start();
        }
        uint04 m_tx_packet_count = 0;                       
        RealSenseScannerFactory* m_scanner_factory = nullptr;   
        String m_tx_packet;                                 
        Connection* m_rx_connection = nullptr;              
        Connection* m_tx_connection = nullptr;              
        RasterPacketInfo m_packet_info;                     
        Buffer<RealSenseScanner*> m_connected_scanners;     
        WorkerThread* m_thread;                             
    };
    
}