#include "Drone.h"

namespace DroneClient {

    // Реализация статического метода GetBytes
    array<Byte>^ Drone::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;
    }

    // Реализация статического метода FromBytes
    Object^ Drone::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;
    }

    // Реализация приватного метода SendDataMotor4
    array<Byte>^ Drone::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);
    }

    // Реализация приватного метода RecvDataIMU
    array<Byte>^ Drone::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);
    }

    // Реализация приватного метода RecvDataPos
    array<Byte>^ Drone::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);
    }

    // Реализация приватного метода ClientRequestResponse
    array<Byte>^ Drone::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;
    }

    // Реализация конструктора
    Drone::Drone()
    {
        DroneStreamData = gcnew array<Byte>(DroneStreamCount); // Инициализация массива
        DroneStreamIndex = 0;
        DroneStreamHead.Mode = DroneData::DataMode::None;
        DroneStreamHead.Size = 0;
        DroneStreamHead.Type = DroneData::DataType::None;
    }

    // Реализация метода DataStream
    System::Collections::Generic::List<array<Byte>^>^ Drone::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;
    }

    // Реализация метода SendRequest
    array<Byte>^ Drone::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;
    }
}