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

    array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
    {
        DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);

        AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
        TimeAcc = imu.Time;

        return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
    {
      DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);

      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
      TimeGyr = imu.Time;

      return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
    {
      DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);

      LaserRange = pos.LiDAR;
      TimeRange = pos.Time;

      return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
    {
        DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);

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

        return gcnew array<Byte>(0);
    }

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

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

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

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

        case DroneData::DataType::DataLocal:
          if (head.Mode == DroneData::DataMode::Request)
          {
            return zero; // Запрос данных (не реализовано)
          }
          else
          {
            return RecvDataLocal(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
    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^ acc = gcnew DroneData::DataHead();
      acc->Size = DroneData::DataHead::StrLen;
      acc->Mode = DroneData::DataMode::Request;
      acc->Type = DroneData::DataType::DataAcc;

      DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
      gyr->Size = DroneData::DataHead::StrLen;
      gyr->Mode = DroneData::DataMode::Request;
      gyr->Type = DroneData::DataType::DataGyr;

      DroneData::DataHead^ range = gcnew DroneData::DataHead();
      range->Size = DroneData::DataHead::StrLen;
      range->Mode = DroneData::DataMode::Request;
      range->Type = DroneData::DataType::DataRange;

      DroneData::DataHead^ local = gcnew DroneData::DataHead();
      local->Size = DroneData::DataHead::StrLen;
      local->Mode = DroneData::DataMode::Request;
      local->Type = DroneData::DataType::DataLocal;

      List<array<byte>^>^ list = gcnew List<array<byte>^>();

      list->Add(GetBytes(acc));
      list->Add(GetBytes(gyr));
      list->Add(GetBytes(range));
      list->Add(GetBytes(local));
      list->Add(SendDataMotor4());

      int count = 0;

      for each(array<byte>^ d in list) count += d->Length;

      array<byte>^ send = gcnew array<byte>(count);

      count = 0;
      for each (array<byte> ^ d in list)
      {
        d->CopyTo(send, count);
        count += d->Length;
      }

      return send;
    }
}