using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Numerics;
using System.Reflection;

namespace DroneSimulator
{
  internal class RealMode
  {
    public static bool RealSimulation;

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

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private uint 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 = (uint)(Lateness * 1000);

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

        value = laten[index];

        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;

      private uint last = 0;

      private Random rand = new Random();

      private const int count = 1000;
      private Vector3[] laten = new Vector3[count];
      private uint 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 = (uint)(Lateness * 1000);

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

        value = laten[index];

        uint freq = 1000 / Freq;

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

    internal class Magnetometer
    {

    }

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

      private uint last = 0;

      private Random rand = new Random();

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

      public uint timer = 0;
      public Vector3 result;

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

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

        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 = (uint)(Lateness * 1000);

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

        value = laten[index];

        uint freq = 1000 / Freq;

        if (timer + freq < time)
        {
          result = value;
          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;

      private uint last = 0;

      private Random rand = new Random();

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

      public uint timer = 0;
      public float result;

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

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

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

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

        uint clock = (uint)(Lateness * 1000);

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

        value = laten[index];

        uint freq = 1000 / Freq;

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

    internal class OpticalFlow
    {

    }

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

      private uint last = 0;

      private Random rand = new Random();

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

      public uint timer = 0;
      public float result;

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

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

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

        uint clock = (uint)(Lateness * 1000);

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

        value = laten[index];

        uint freq = 1000 / Freq;

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