using System.CodeDom;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
using static DroneSimulator.Drone;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;

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 uint DataTimer;

    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 Random MainRandom = new Random();

    public struct DataBarometer
    {
      public int Value, Pressure; // Значение давления в Паскальях
      public float Accuracy;
      public int Frequency;
      public uint Time;
    }

    public DataBarometer dataBarometer;

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

      if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer)
      {
        float bar = dataBarometer.Pressure - PosXYZ.Z * 11;
        int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10;
        dataBarometer.Value = (int)(bar + rnd);
        dataBarometer.Time = DataTimer;
      }

      DataTimer = (uint)tick;
    }

    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[] SendDataAcc()
    {
      DroneData.DataAcc acc = new DroneData.DataAcc();

      acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
      acc.Head.Mode = DroneData.DataMode.Response;
      acc.Head.Type = DroneData.DataType.DataAcc;
      acc.Head.Time = (uint)Environment.TickCount;

      acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
      acc.Time = DataTimer;

      return getBytes(acc);
    }

    private byte[] SendDataGyr()
    {
      DroneData.DataGyr gyr = new DroneData.DataGyr();

      gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
      gyr.Head.Mode = DroneData.DataMode.Response;
      gyr.Head.Type = DroneData.DataType.DataGyr;
      gyr.Head.Time = (uint)Environment.TickCount;

      gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
      gyr.Time = DataTimer;

      return getBytes(gyr);
    }

    private byte[] SendDataMag()
    {
      DroneData.DataMag mag = new DroneData.DataMag();

      mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
      mag.Head.Mode = DroneData.DataMode.Response;
      mag.Head.Type = DroneData.DataType.DataMag;
      mag.Head.Time = (uint)Environment.TickCount;

      mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
      mag.Time = DataTimer;

      return getBytes(mag);
    }

    private byte[] SendDataRange()
    {
      DroneData.DataRange range = new DroneData.DataRange();

      range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
      range.Head.Mode = DroneData.DataMode.Response;
      range.Head.Type = DroneData.DataType.DataRange;
      range.Head.Time = (uint)Environment.TickCount;

      range.LiDAR = LaserRange;
      range.Time = DataTimer;

      return getBytes(range);
    }

    private byte[] SendDataLocal()
    {
      DroneData.DataLocal local = new DroneData.DataLocal();

      local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
      local.Head.Mode = DroneData.DataMode.Response;
      local.Head.Type = DroneData.DataType.DataLocal;
      local.Head.Time = (uint)Environment.TickCount;

      local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
      local.Time = DataTimer;

      return getBytes(local);
    }

    private byte[] SendDataBarometer()
    {
      DroneData.DataBar bar = new DroneData.DataBar();

      bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
      bar.Head.Mode = DroneData.DataMode.Response;
      bar.Head.Type = DroneData.DataType.DataBar;
      bar.Head.Time = (uint)Environment.TickCount;

      bar.Pressure = dataBarometer.Value;
      bar.Time = dataBarometer.Time;

      return getBytes(bar);
    }

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

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

        case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;

        case DroneData.DataType.DataMag: if (head.Mode == DroneData.DataMode.Request) return SendDataMag(); else return zero;

        case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero;

        case DroneData.DataType.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else return zero;

        case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;

        case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) 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;
    }
  }
}