Simulator/DroneClientCpp/Drone.h

#pragma once

#include "DroneData.h"
#include <Windows.h>
#include <vcclr.h>

#using <System.dll>
#using <mscorlib.dll>

using namespace System;
using namespace System::Collections::Generic;
using namespace System::Runtime::InteropServices;

namespace DroneClient {

    public ref class Drone
    {
    public:
        float AccX, AccY, AccZ;
        float GyrX, GyrY, GyrZ;
        float PosX, PosY;
        float LaserRange;

        float MotorUL, MotorUR, MotorDL, MotorDR;

        static array<Byte>^ GetBytes(Object^ data)
        {
            int size = Marshal::SizeOf(data);
            array<Byte>^ arr = gcnew array<Byte>(size);

            IntPtr ptr = IntPtr::Zero;
            try
            {
                ptr = Marshal::AllocHGlobal(size);
                Marshal::StructureToPtr(data, ptr, true);
                Marshal::Copy(ptr, arr, 0, size);
            }
            finally
            {
                Marshal::FreeHGlobal(ptr);
            }
            return arr;
        }

        static Object^ FromBytes(array<Byte>^ arr, Type^ type)
        {
            Object^ mem = gcnew Object();

            int size = Marshal::SizeOf(type);
            IntPtr ptr = IntPtr::Zero;
            try
            {
                ptr = Marshal::AllocHGlobal(size);
                Marshal::Copy(arr, 0, ptr, size);
                mem = Marshal::PtrToStructure(ptr, type);
            }
            finally
            {
                Marshal::FreeHGlobal(ptr);
            }
            return mem;
        }

    private:
        array<Byte>^ SendDataMotor4()
        {
            DroneData::DataMotor4 mot4;

            mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
            mot4.Head.Mode = DroneData::DataMode::Response;
            mot4.Head.Type = DroneData::DataType::DataMotor4;

            mot4.UL = MotorUL;
            mot4.UR = MotorUR;
            mot4.DL = MotorDL;
            mot4.DR = MotorDR;

            return GetBytes(mot4);
        }

        array<Byte>^ RecvDataIMU(array<Byte>^ data)
        {
            DroneData::DataIMU imu = (DroneData::DataIMU)FromBytes(data, DroneData::DataIMU::typeid);

            AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
            GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;

            return gcnew array<Byte>(0);
        }

        array<Byte>^ RecvDataPos(array<Byte>^ data)
        {
            DroneData::DataPos pos = (DroneData::DataPos)FromBytes(data, DroneData::DataPos::typeid);

            PosX = pos.Local.X; PosY = pos.Local.Y;
            LaserRange = pos.LiDAR;

            return gcnew array<Byte>(0);
        }

        array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
        {
            array<Byte>^ zero = gcnew array<Byte>(0);

            switch (head.Type)
            {
            case DroneData::DataType::DataIMU:
                if (head.Mode == DroneData::DataMode::Request)
                {
                    return zero; // Запрос данных (не реализовано)
                }
                else
                {
                    return RecvDataIMU(body); // Пришли данные
                }

            case DroneData::DataType::DataPos:
                if (head.Mode == DroneData::DataMode::Request)
                {
                    return zero; // Запрос данных (не реализовано)
                }
                else
                {
                    return RecvDataPos(body); // Пришли данные
                }

            case DroneData::DataType::DataMotor4:
                if (head.Mode == DroneData::DataMode::Request)
                {
                    return SendDataMotor4(); // Запрос данных
                }
                else
                {
                    return zero; // Пришли данные (не реализовано)
                }
            }

            return zero;
        }

        literal int DroneStreamCount = 512;
        array<Byte>^ DroneStreamData = gcnew array<Byte>(DroneStreamCount);
        int DroneStreamIndex = 0;
        DroneData::DataHead DroneStreamHead; // Исправлено: без явной инициализации

    public:
        // Конструктор для явной инициализации
        Drone()
        {
            DroneStreamHead.Mode = DroneData::DataMode::None;
            DroneStreamHead.Size = 0;
            DroneStreamHead.Type = DroneData::DataType::None;
        }

        System::Collections::Generic::List<array<Byte>^>^ DataStream(array<Byte>^ data, int size)
        {
            System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();

            if (data == nullptr) return ret; // Последовательность не сформирована

            if (size + DroneStreamIndex > DroneStreamCount) return nullptr; // Ошибка переполнения

            Array::Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
            DroneStreamIndex += size;

            while (true)
            {
                if (DroneStreamHead.Size == 0) // Заголовок еще не получен
                {
                    if (DroneStreamIndex < DroneData::DataHead::StrLen) return ret;
                    DroneStreamHead = (DroneData::DataHead)FromBytes(DroneStreamData, DroneData::DataHead::typeid);
                }

                if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет еще не полный

                array<Byte>^ body = gcnew array<Byte>(DroneStreamHead.Size);
                Array::Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);

                int shift = DroneStreamHead.Size;

                DroneStreamIndex -= shift;
                Array::Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвиг массива

                DroneStreamHead.Size = 0; // Сброс заголовка

                ret->Add(ClientRequestResponse(DroneStreamHead, body));
            }

            return ret;
        }

        array<Byte>^ SendRequest()
        {
            DroneData::DataHead imu;
            imu.Size = DroneData::DataHead::StrLen;
            imu.Mode = DroneData::DataMode::Request;
            imu.Type = DroneData::DataType::DataIMU;

            DroneData::DataHead pos;
            pos.Size = DroneData::DataHead::StrLen;
            pos.Mode = DroneData::DataMode::Request;
            pos.Type = DroneData::DataType::DataPos;

            array<Byte>^ si = GetBytes(imu);
            array<Byte>^ sp = GetBytes(pos);
            array<Byte>^ sm = SendDataMotor4();

            array<Byte>^ send = gcnew array<Byte>(si->Length + sp->Length + sm->Length);
            Array::Copy(si, 0, send, 0, si->Length);
            Array::Copy(sp, 0, send, si->Length, sp->Length);
            Array::Copy(sm, 0, send, si->Length + sp->Length, sm->Length);

            return send;
        }
    };
}