using Microsoft.VisualBasic.Devices;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Reflection;
using System.Text;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;

namespace DroneSimulator
{
  internal class RealMode
  {


    internal class Accelerometer
    {
      public static uint Freq;
      public static float Noise;
      public static float ScaleLeft;
      public static float ScaleRight;
      public static float Lateness;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private int index = 0;

      public uint timer = 0;
      public Vector3 result;

      public void Update(Vector3 value, uint time)
      {
        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        float scale = (ScaleRight - ScaleLeft) / 2;
        float shift = scale + ScaleLeft;

        value.X = (value.X * scale) + shift;
        value.Y = (value.Y * scale) + shift;
        value.Z = (value.Z * scale) + shift;

        int noise = (int)(Noise * 1000);
        value.X += ((float)rand.Next(-noise, noise)) / 1000;
        value.Y += ((float)rand.Next(-noise, noise)) / 1000;
        value.Z += ((float)rand.Next(-noise, noise)) / 1000;

        uint clock = time - last;
        while (true)
        {
          laten[index] = value;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        value = laten[move];

        uint freq = 1000 / Freq;

        if (timer + freq <= time)
        {
          result = value;
          timer = time;
        }
      }
    }

    internal class Gyroscope
    {
      public static uint Freq;
      public static float Noise;
      public static Vector3 Shift;
      public static float Lateness;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private int index = 0;

      public uint timer = 0;
      public Vector3 result;

      public void Update(Vector3 value, uint time)
      {
        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        value.X += Shift.X;
        value.Y += Shift.Y;
        value.Z += Shift.Z;

        int noise = (int)(Noise * 1000);
        value.X += ((float)rand.Next(-noise, noise)) / 1000;
        value.Y += ((float)rand.Next(-noise, noise)) / 1000;
        value.Z += ((float)rand.Next(-noise, noise)) / 1000;

        uint clock = time - last;
        while (true)
        {
          laten[index] = value;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        value = laten[move];

        uint freq = 1000 / Freq;

        if (timer + freq <= time)
        {
          result = value;
          timer = time;
        }
      }
    }

    internal class Magnetometer
    {
      /**
       * The model is produced by the United States’ National Geospatial-Intelligence Agency (NGA)
       * and the United Kingdom’s Defence Geographic Centre (DGC)
       * NCEI and the British Geological Survey (BGS) jointly developed the WMM.
       */
      /* Taganrog
       * 47° 12' 32" N
       * 38° 56' 10" E
       * Declination: 8° 32' 28"
       * Inclination: 65° 34' 9"
       * Total Field: 51,120.8 nT
       */
      public static bool Enable;
      public static uint Freq;
      public static float Noise;
      public static Vector3 Shift;
      public static float Lateness;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      public static float fieldStrength = 51.1208F; // uT
      public static float fieldDeclination = (8 + 32 / 60 + 28 / 3600) * (MathF.PI / 180);
      public static float fieldInclination = (65 + 34 / 60 + 9 / 3600) * (MathF.PI / 180);

      private static Vector3 InitializeMagneticField()
      {
        float horizontalComponent = fieldStrength * MathF.Cos(fieldInclination);

        float northComponent = horizontalComponent * MathF.Cos(fieldDeclination); // X
        float eastComponent = horizontalComponent * MathF.Sin(fieldDeclination);  // Y
        float downComponent = fieldStrength * MathF.Sin(fieldInclination);        // Z

        return new Vector3(northComponent, eastComponent, downComponent);
      }

      private static Vector3 magneticField = InitializeMagneticField();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private int index = 0;

      public uint timer = 0;
      public Vector3 result;

      public void Update(Quaternion oreintantion, uint time)
      {
        Vector3 value = Vector3.Transform(magneticField, oreintantion);

        Vector3 v = value;

        v.X += Shift.X;
        v.Y += Shift.Y;
        v.Z += Shift.Z;

        int noise = (int)(Noise * 1000);
        v.X += ((float)rand.Next(-noise, noise)) / 1000;
        v.Y += ((float)rand.Next(-noise, noise)) / 1000;
        v.Z += ((float)rand.Next(-noise, noise)) / 1000;

        uint clock = time - last;
        while (true)
        {
          laten[index] = v;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        if (!Enable)
        {
          result = Vector3.NaN;
          timer = time;
          return;
        }

        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        v = laten[move];

        uint freq = 1000 / Freq;

        if (timer + freq <= time)
        {
          result = v;
          timer = time;
        }
      }
    }

    internal class Position
    {
      public static bool Enable;
      public static uint Freq;
      public static float Noise;
      public static float Lateness;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private int index = 0;

      public uint timer = 0;
      public Vector3 result;

      public void Update(Vector3 value, uint time)
      {
        Vector3 v = value;

        int noise = (int)(Noise * 1000);
        v.X += ((float)rand.Next(-noise, noise)) / 1000;
        v.Y += ((float)rand.Next(-noise, noise)) / 1000;
        v.Z += ((float)rand.Next(-noise, noise)) / 1000;

        uint clock = time - last;
        while (true)
        {
          laten[index] = v;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        if (!Enable)
        {
          result = Vector3.NaN;
          timer = time;
          return;
        }

        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        v = laten[move];

        uint freq = 1000 / Freq;
        if (timer + freq <= time)
        {
          result = v;
          timer = time;
        }
      }
    }

    internal class Barometer
    {
      public static bool Enable;
      public static float Pressure;
      public static uint Freq;
      public static float Noise;
      public static float Lateness;
      public static bool RealSimulation;
      public static float Temperature = 25.0f;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private float[] laten = new float[count];
      private int index = 0;

      public uint timer = 0;
      public float result;

      public void Update(float value, uint time)
      {
        value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));

        float v = value;

        int noise = (int)(Noise * 1000);
        v += ((float)rand.Next(-noise, noise)) / 1000;

        uint clock = time - last;
        while (true)
        {
          laten[index] = v;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        if (!Enable)
        {
          result = float.NaN;
          timer = time;
          return;
        }

        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        v = laten[move];

        uint freq = 1000 / Freq;
        if (timer + freq <= time)
        {
          result = v;
          timer = time;
        }
      }
    }

    internal class OpticalFlow
    {
      public static bool Enable;
      public static float MaxHeight;
      public static uint Freq;
      public static float Noise;
      public static float Lateness;
      public static float Lens;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector2[] laten = new Vector2[count];
      private int index = 0;

      public uint delay = 0;

      public uint timer = 0;
      public Vector2 result;
      public bool Update(Vector2 value, float Range, uint time)
      {
        value *= Lens;

        Vector2 v = value;

        if (Range > MaxHeight) v = Vector2.Zero;
        else
        {
          int noise = (int)(Noise * 1000);
          v.X += ((float)rand.Next(-noise, noise)) / 1000;
          v.Y += ((float)rand.Next(-noise, noise)) / 1000;
        }

        uint clock = time - last;
        while (true)
        {
          laten[index] = v;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        if (!Enable)
        {
          result = Vector2.NaN;
          timer = time;
          return true;
        }

        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return true;
        }

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        result = laten[move];

        uint freq = count / Freq;
        if (timer + freq <= time)
        {
          timer = time;
          return true;
        }

        return false;
      }
    }

    internal class Range
    {
      public static bool Enable;
      public static float MaxHeight;
      public static uint Freq;
      public static float Noise;
      public static float Lateness;
      public static bool RealSimulation;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private float[] laten = new float[count];
      private int index = 0;

      public uint timer = 0;
      public float result;

      public void Update(float value, uint time)
      {
        float v = value;

        if (v > MaxHeight) v = -1;
        else
        {
          int noise = (int)(Noise * 1000);
          v += ((float)rand.Next(-noise, noise)) / 1000;
        }

        uint clock = time - last;
        while (true)
        {
          laten[index] = v;
          clock--;
          if (clock == 0) break;
          index++;
          if (index >= count) index = 0;
        }
        last = time;

        if (!Enable)
        {
          result = float.NaN;
          timer = time;
          return;
        }

        if (!RealSimulation)
        {
          result = value;
          timer = time;
          return;
        }

        int move = (int)(Lateness * count);
        move = index - move;
        while (move < 0) move += count;
        v = laten[move];

        uint freq = 1000 / Freq;
        if (timer + freq <= time)
        {
          result = v;
          timer = time;
        }
      }
    }
  }
}