using System.CodeDom;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;

namespace DroneSimulator
{
  internal class Drone
  {
    public int ID;
    public float Mass; // Масса
    public bool Active; // Живой?
    public float Length; // Длинна лучей
    public const float Dynamic = 10; // Динамика вращения
    public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
    public Quaternion Quat; // Основной кватернион
    public float Power = 0; // Тяга всех двигателей (0-1)
    public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
    public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw

    public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
    public float LaserRange; // Имитация: Дальномер

    private const float Gravity = 9.8f;

    private const float TO_GRAD = 180 / MathF.PI;
    private const float TO_RADI = MathF.PI / 180;

    private Thread DroneThread;
    private int Timer;

    private static int CounterID = 0;

    public static byte[] getBytes(object data)
    {
      int size = Marshal.SizeOf(data);
      byte[] arr = new 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;
    }

    public static object fromBytes(byte[] arr, Type type)
    {
      object mem = new 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;
    }

    /*public struct DataOut
    {
      public float AccX, AccY, AccZ;
      public float GyrX, GyrY, GyrZ;
      public float PosX, PosY;
      public float LaserRange;
    }

    public DataOut GetDataOut()
    {
      DataOut data = new DataOut();

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

      data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;

      data.LaserRange = LaserRange;

      return data;
    }

    public struct DataIn
    {
      public float MotorUL, MotorUR, MotorDL, MotorDR;
    }*/

    public struct DataVisual
    {
      public int ID; // Идентификатор
      public float W, X, Y, Z; // Кватернион вращения
      public float PosX, PosY, PosZ; // Координаты в пространстве
    }

    public DataVisual GetVisual()
    {
      return new DataVisual() { ID = this.ID, W = this.Quat.W, X = this.Quat.X, Y = this.Quat.Y, Z = this.Quat.Z, PosX = this.PosXYZ.X, PosY = this.PosXYZ.Y, PosZ = this.PosXYZ.Z };
    }

    private void ThreadFunction()
    {
      while (DroneThread != null)
      {
        Action(Environment.TickCount);
        Thread.Sleep(1);
      }
    }

    public Drone(int id)
    {
      ID = id;
      Active = false;
      PosXYZ = Vector3.Zero;
      SpdXYZ = Vector3.Zero;
      AccXYZ = Vector3.Zero;
      Quat = Quaternion.Identity;

      DroneThread = new Thread(new ThreadStart(ThreadFunction));
      Timer = Environment.TickCount;
      DroneThread.Start();
    }

    public int Create(float power, float mass, float len)
    {
      Mass = mass;
      Length = len;
      MaxPower = power;

      Active = true;

      return ID;
    }

    public void Close()
    {
      DroneThread = null;
    }

    private float GetAngle(float a1, float a2, float az)
    {
      if (a2 == 0.0f && az == 0.0f)
      {
        if (a1 > 0) return 90;
        if (a1 < 0) return -90;
        return 0;
      }
      return MathF.Atan(a1 / MathF.Sqrt(a2 * a2 + az * az)) * TO_GRAD;
    }

    public void Rotate(float x, float y, float z)
    {
      x = x * MathF.PI / 180;
      y = y * MathF.PI / 180;
      z = -z * MathF.PI / 180;

      Quaternion map = Quat;
      Quaternion spd = new Quaternion(x, y, z, 0);
      Quaternion aq = spd * map;

      map.W -= 0.5f * aq.W;
      map.X -= 0.5f * aq.X;
      map.Y -= 0.5f * aq.Y;
      map.Z -= 0.5f * aq.Z;

      Quat = Quaternion.Normalize(map);
    }

    public Vector4 GetOrientation()
    {
      Quaternion grav = new Quaternion(0, 0, 1, 0);

      grav = (Quat * grav) * Quaternion.Inverse(Quat);

      float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
      if (yaw < 0.0f) yaw = 360.0f + yaw;

      return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
    }

    public void Action(int tick)
    {
      float time = (tick - Timer) / 1000.0f;
      Timer = tick;

      if (!Active) return;

      float flow = Power;

      if (PosXYZ.Z < 0.3f)
      {
        flow += flow * 0.1f; // Воздушная подушка
      }

      SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));

      Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
      AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);

      SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
      PosXYZ += SpdXYZ * time;

      AccXYZ /= Gravity; // Вернуть измерения в G

      if (PosXYZ.Z < 0)
      {
        SpdPRY = Vector3.Zero;
        SpdXYZ.X = 0;
        SpdXYZ.Y = 0;
        Quat = Quaternion.Identity;
      }
      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);

      Vector4 ori = GetOrientation();

      if (PosXYZ.Z < 0)
      {
        PosXYZ.Z = 0;

        /*if (SpdXYZ.Z < -5)
        {
          Active = false; // Сильно ударился о землю
        }*/

        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
        {
          Active = false; // Повредил винты при посадке
        }*/

        SpdXYZ.Z = 0;

        Acc = new Vector3(0, 0, 1);
        Gyr = Vector3.Zero;
        LaserRange = 0;
      }
      else
      {
        if (ori.W < 0)
        {
          //Active = false; // Перевернулся вверх ногами
        }

        Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
        //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
        Acc = new Vector3(grav.X, grav.Y, grav.Z);

        Gyr = SpdPRY;

        float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;

        if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
        else LaserRange = float.MaxValue;
      }
    }

    private float Range(float pow)
    {
      if (pow > 1) pow = 1;
      if (pow < 0) pow = 0;

      return pow * MaxPower;
    }

    public void SetQadroPow(float ul, float ur, float dl, float dr)
    {
      ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);

      Power = (ul + ur + dl + dr) / 4;

      AccPRY.Y = ((ul + dl) - (ur + dr));
      AccPRY.X = ((ul + ur) - (dl + dr));
      AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
    }

    private void RecvDataMotor4(byte[] data)
    {
      DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
      /* обработка */
      SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
    }

    private byte[] SendDataIMU()
    {
      DroneData.DataIMU imu = new DroneData.DataIMU();

      imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
      imu.Head.Mode = DroneData.DataMode.Response;
      imu.Head.Type = DroneData.DataType.DataIMU;

      imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
      imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
      imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;

      return getBytes(imu);
    }

    private byte[] SendDataPos()
    {
      DroneData.DataPos pos = new DroneData.DataPos();

      pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
      pos.Head.Mode = DroneData.DataMode.Response;
      pos.Head.Type = DroneData.DataType.DataPos;

      pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
      pos.LiDAR = LaserRange;

      return getBytes(pos);
    }

    private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
    {
      byte[] zero = new byte[0];

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

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

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

      return zero;
    }

    private const int DroneStreamCount = 512;
    private byte[] DroneStreamData = new byte[DroneStreamCount];
    private int DroneStreamIndex = 0;
    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };

    public List<byte[]?>? DataStream(byte[]? data, int size)
    {
      List<byte[]?> ret = new List<byte[]?>();

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

      if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец)

      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, typeof(DroneData.DataHead));
        }

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

        byte[] body = new 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(ServerRequestResponse(DroneStreamHead, body));
      }

      return ret;
    }
  }
}