Move solution and projects to src

8 files changed, 0 insertions, 844 deletions

diff --git a/Ryujinx.Input/Motion/CemuHook/Client.cs b/Ryujinx.Input/Motion/CemuHook/Client.cs
deleted file mode 100644
index 4498b8ca..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Client.cs
+++ /dev/null
@@ -1,475 +0,0 @@
-using Ryujinx.Common;
-using Ryujinx.Common.Configuration.Hid;
-using Ryujinx.Common.Configuration.Hid.Controller;
-using Ryujinx.Common.Configuration.Hid.Controller.Motion;
-using Ryujinx.Common.Logging;
-using Ryujinx.Common.Memory;
-using Ryujinx.Input.HLE;
-using Ryujinx.Input.Motion.CemuHook.Protocol;
-using System;
-using System.Collections.Generic;
-using System.IO;
-using System.IO.Hashing;
-using System.Net;
-using System.Net.Sockets;
-using System.Numerics;
-using System.Threading.Tasks;
-
-namespace Ryujinx.Input.Motion.CemuHook
-{
-    public class Client : IDisposable
-    {
-        public const uint   Magic   = 0x43555344; // DSUC
-        public const ushort Version = 1001;
-
-        private bool _active;
-
-        private readonly Dictionary<int, IPEndPoint> _hosts;
-        private readonly Dictionary<int, Dictionary<int, MotionInput>> _motionData;
-        private readonly Dictionary<int, UdpClient> _clients;
-
-        private readonly bool[] _clientErrorStatus = new bool[Enum.GetValues<PlayerIndex>().Length];
-        private readonly long[] _clientRetryTimer  = new long[Enum.GetValues<PlayerIndex>().Length];
-        private NpadManager _npadManager;
-
-        public Client(NpadManager npadManager)
-        {
-            _npadManager = npadManager;
-            _hosts       = new Dictionary<int, IPEndPoint>();
-            _motionData  = new Dictionary<int, Dictionary<int, MotionInput>>();
-            _clients     = new Dictionary<int, UdpClient>();
-
-            CloseClients();
-        }
-
-        public void CloseClients()
-        {
-            _active = false;
-
-            lock (_clients)
-            {
-                foreach (var client in _clients)
-                {
-                    try
-                    {
-                        client.Value?.Dispose();
-                    }
-                    catch (SocketException socketException)
-                    {
-                        Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to dispose motion client. Error: {socketException.ErrorCode}");
-                    }
-                }
-
-                _hosts.Clear();
-                _clients.Clear();
-                _motionData.Clear();
-            }
-        }
-
-        public void RegisterClient(int player, string host, int port)
-        {
-            if (_clients.ContainsKey(player) || !CanConnect(player))
-            {
-                return;
-            }
-
-            lock (_clients)
-            {
-                if (_clients.ContainsKey(player) || !CanConnect(player))
-                {
-                    return;
-                }
-
-                UdpClient client = null;
-
-                try
-                {
-                    IPEndPoint endPoint = new IPEndPoint(IPAddress.Parse(host), port);
-
-                    client = new UdpClient(host, port);
-
-                    _clients.Add(player, client);
-                    _hosts.Add(player, endPoint);
-
-                    _active = true;
-
-                    Task.Run(() =>
-                    {
-                        ReceiveLoop(player);
-                    });
-                }
-                catch (FormatException formatException)
-                {
-                    if (!_clientErrorStatus[player])
-                    {
-                        Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error: {formatException.Message}");
-
-                        _clientErrorStatus[player] = true;
-                    }
-                }
-                catch (SocketException socketException)
-                {
-                    if (!_clientErrorStatus[player])
-                    {
-                        Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to connect to motion source at {host}:{port}. Error: {socketException.ErrorCode}");
-
-                        _clientErrorStatus[player] = true;
-                    }
-
-                    RemoveClient(player);
-
-                    client?.Dispose();
-
-                    SetRetryTimer(player);
-                }
-                catch (Exception exception)
-                {
-                    Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to register motion client. Error: {exception.Message}");
-
-                    _clientErrorStatus[player] = true;
-
-                    RemoveClient(player);
-
-                    client?.Dispose();
-
-                    SetRetryTimer(player);
-                }
-            }
-        }
-
-        public bool TryGetData(int player, int slot, out MotionInput input)
-        {
-            lock (_motionData)
-            {
-                if (_motionData.ContainsKey(player))
-                {
-                    if (_motionData[player].TryGetValue(slot, out input))
-                    {
-                        return true;
-                    }
-                }
-            }
-
-            input = null;
-
-            return false;
-        }
-
-        private void RemoveClient(int clientId)
-        {
-            _clients?.Remove(clientId);
-
-            _hosts?.Remove(clientId);
-        }
-
-        private void Send(byte[] data, int clientId)
-        {
-            if (_clients.TryGetValue(clientId, out UdpClient _client))
-            {
-                if (_client != null && _client.Client != null && _client.Client.Connected)
-                {
-                    try
-                    {
-                        _client?.Send(data, data.Length);
-                    }
-                    catch (SocketException socketException)
-                    {
-                        if (!_clientErrorStatus[clientId])
-                        {
-                            Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to send data request to motion source at {_client.Client.RemoteEndPoint}. Error: {socketException.ErrorCode}");
-                        }
-
-                        _clientErrorStatus[clientId] = true;
-
-                        RemoveClient(clientId);
-
-                        _client?.Dispose();
-
-                        SetRetryTimer(clientId);
-                    }
-                    catch (ObjectDisposedException)
-                    {
-                        _clientErrorStatus[clientId] = true;
-
-                        RemoveClient(clientId);
-
-                        _client?.Dispose();
-
-                        SetRetryTimer(clientId);
-                    }
-                }
-            }
-        }
-
-        private byte[] Receive(int clientId, int timeout = 0)
-        {
-            if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client))
-            {
-                if (_client != null && _client.Client != null && _client.Client.Connected)
-                {
-                    _client.Client.ReceiveTimeout = timeout;
-
-                    var result = _client?.Receive(ref endPoint);
-
-                    if (result.Length > 0)
-                    {
-                        _clientErrorStatus[clientId] = false;
-                    }
-
-                    return result;
-                }
-            }
-
-            throw new Exception($"Client {clientId} is not registered.");
-        }
-
-        private void SetRetryTimer(int clientId)
-        {
-            var elapsedMs = PerformanceCounter.ElapsedMilliseconds;
-
-            _clientRetryTimer[clientId] = elapsedMs;
-        }
-
-        private void ResetRetryTimer(int clientId)
-        {
-            _clientRetryTimer[clientId] = 0;
-        }
-
-        private bool CanConnect(int clientId)
-        {
-            return _clientRetryTimer[clientId] == 0 || PerformanceCounter.ElapsedMilliseconds - 5000 > _clientRetryTimer[clientId];
-        }
-
-        public void ReceiveLoop(int clientId)
-        {
-            if (_hosts.TryGetValue(clientId, out IPEndPoint endPoint) && _clients.TryGetValue(clientId, out UdpClient _client))
-            {
-                if (_client != null && _client.Client != null && _client.Client.Connected)
-                {
-                    try
-                    {
-                        while (_active)
-                        {
-                            byte[] data = Receive(clientId);
-
-                            if (data.Length == 0)
-                            {
-                                continue;
-                            }
-
-                            Task.Run(() => HandleResponse(data, clientId));
-                        }
-                    }
-                    catch (SocketException socketException)
-                    {
-                        if (!_clientErrorStatus[clientId])
-                        {
-                            Logger.Warning?.PrintMsg(LogClass.Hid, $"Unable to receive data from motion source at {endPoint}. Error: {socketException.ErrorCode}");
-                        }
-
-                        _clientErrorStatus[clientId] = true;
-
-                        RemoveClient(clientId);
-
-                        _client?.Dispose();
-
-                        SetRetryTimer(clientId);
-                    }
-                    catch (ObjectDisposedException)
-                    {
-                        _clientErrorStatus[clientId] = true;
-
-                        RemoveClient(clientId);
-
-                        _client?.Dispose();
-
-                        SetRetryTimer(clientId);
-                    }
-                }
-            }
-        }
-
-        public void HandleResponse(byte[] data, int clientId)
-        {
-            ResetRetryTimer(clientId);
-
-            MessageType type = (MessageType)BitConverter.ToUInt32(data.AsSpan().Slice(16, 4));
-
-            data = data.AsSpan()[16..].ToArray();
-
-            using MemoryStream stream = new MemoryStream(data);
-            using BinaryReader reader = new BinaryReader(stream);
-
-            switch (type)
-            {
-                case MessageType.Protocol:
-                    break;
-                case MessageType.Info:
-                    ControllerInfoResponse contollerInfo = reader.ReadStruct<ControllerInfoResponse>();
-                    break;
-                case MessageType.Data:
-                    ControllerDataResponse inputData = reader.ReadStruct<ControllerDataResponse>();
-
-                    Vector3 accelerometer = new Vector3()
-                    {
-                        X = -inputData.AccelerometerX,
-                        Y = inputData.AccelerometerZ,
-                        Z = -inputData.AccelerometerY
-                    };
-
-                    Vector3 gyroscrope = new Vector3()
-                    {
-                        X = inputData.GyroscopePitch,
-                        Y = inputData.GyroscopeRoll,
-                        Z = -inputData.GyroscopeYaw
-                    };
-
-                    ulong timestamp = inputData.MotionTimestamp;
-
-                    InputConfig config = _npadManager.GetPlayerInputConfigByIndex(clientId);
-
-                    lock (_motionData)
-                    {
-                        // Sanity check the configuration state and remove client if needed if needed.
-                        if (config is StandardControllerInputConfig controllerConfig &&
-                            controllerConfig.Motion.EnableMotion &&
-                            controllerConfig.Motion.MotionBackend == MotionInputBackendType.CemuHook &&
-                            controllerConfig.Motion is CemuHookMotionConfigController cemuHookConfig)
-                        {
-                            int slot = inputData.Shared.Slot;
-
-                            if (_motionData.ContainsKey(clientId))
-                            {
-                                if (_motionData[clientId].ContainsKey(slot))
-                                {
-                                    MotionInput previousData = _motionData[clientId][slot];
-
-                                    previousData.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone);
-                                }
-                                else
-                                {
-                                    MotionInput input = new MotionInput();
-
-                                    input.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone);
-
-                                    _motionData[clientId].Add(slot, input);
-                                }
-                            }
-                            else
-                            {
-                                MotionInput input = new MotionInput();
-
-                                input.Update(accelerometer, gyroscrope, timestamp, cemuHookConfig.Sensitivity, (float)cemuHookConfig.GyroDeadzone);
-
-                                _motionData.Add(clientId, new Dictionary<int, MotionInput>() { { slot, input } });
-                            }
-                        }
-                        else
-                        {
-                            RemoveClient(clientId);
-                        }
-                    }
-                    break;
-            }
-        }
-
-        public void RequestInfo(int clientId, int slot)
-        {
-            if (!_active)
-            {
-                return;
-            }
-
-            Header header = GenerateHeader(clientId);
-
-            using (MemoryStream stream = MemoryStreamManager.Shared.GetStream())
-            using (BinaryWriter writer = new BinaryWriter(stream))
-            {
-                writer.WriteStruct(header);
-
-                ControllerInfoRequest request = new ControllerInfoRequest()
-                {
-                    Type       = MessageType.Info,
-                    PortsCount = 4
-                };
-
-                request.PortIndices[0] = (byte)slot;
-
-                writer.WriteStruct(request);
-
-                header.Length = (ushort)(stream.Length - 16);
-
-                writer.Seek(6, SeekOrigin.Begin);
-                writer.Write(header.Length);
-
-                Crc32.Hash(stream.ToArray(), header.Crc32.AsSpan());
-
-                writer.Seek(8, SeekOrigin.Begin);
-                writer.Write(header.Crc32.AsSpan());
-
-                byte[] data = stream.ToArray();
-
-                Send(data, clientId);
-            }
-        }
-
-        public unsafe void RequestData(int clientId, int slot)
-        {
-            if (!_active)
-            {
-                return;
-            }
-
-            Header header = GenerateHeader(clientId);
-
-            using (MemoryStream stream = MemoryStreamManager.Shared.GetStream())
-            using (BinaryWriter writer = new BinaryWriter(stream))
-            {
-                writer.WriteStruct(header);
-
-                ControllerDataRequest request = new ControllerDataRequest()
-                {
-                    Type           = MessageType.Data,
-                    Slot           = (byte)slot,
-                    SubscriberType = SubscriberType.Slot
-                };
-
-                writer.WriteStruct(request);
-
-                header.Length = (ushort)(stream.Length - 16);
-
-                writer.Seek(6, SeekOrigin.Begin);
-                writer.Write(header.Length);
-
-                Crc32.Hash(stream.ToArray(), header.Crc32.AsSpan());
-
-                writer.Seek(8, SeekOrigin.Begin);
-                writer.Write(header.Crc32.AsSpan());
-
-                byte[] data = stream.ToArray();
-
-                Send(data, clientId);
-            }
-        }
-
-        private Header GenerateHeader(int clientId)
-        {
-            Header header = new Header()
-            {
-                Id          = (uint)clientId,
-                MagicString = Magic,
-                Version     = Version,
-                Length      = 0
-            };
-
-            return header;
-        }
-
-        public void Dispose()
-        {
-            _active = false;
-
-            CloseClients();
-        }
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
deleted file mode 100644
index 7fb72344..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
+++ /dev/null
@@ -1,47 +0,0 @@
-using Ryujinx.Common.Memory;
-using System.Runtime.InteropServices;
-
-namespace Ryujinx.Input.Motion.CemuHook.Protocol
-{
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    struct ControllerDataRequest
-    {
-        public MessageType Type;
-        public SubscriberType SubscriberType;
-        public byte Slot;
-        public Array6<byte> MacAddress;
-    }
-
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    public struct ControllerDataResponse
-    {
-        public SharedResponse Shared;
-        public byte           Connected;
-        public uint           PacketId;
-        public byte           ExtraButtons;
-        public byte           MainButtons;
-        public ushort         PSExtraInput;
-        public ushort         LeftStickXY;
-        public ushort         RightStickXY;
-        public uint           DPadAnalog;
-        public ulong          MainButtonsAnalog;
-
-        public Array6<byte> Touch1;
-        public Array6<byte> Touch2;
-
-        public ulong MotionTimestamp;
-        public float AccelerometerX;
-        public float AccelerometerY;
-        public float AccelerometerZ;
-        public float GyroscopePitch;
-        public float GyroscopeYaw;
-        public float GyroscopeRoll;
-    }
-
-    enum SubscriberType : byte
-    {
-        All,
-        Slot,
-        Mac
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
deleted file mode 100644
index 63d4524a..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
+++ /dev/null
@@ -1,20 +0,0 @@
-using Ryujinx.Common.Memory;
-using System.Runtime.InteropServices;
-
-namespace Ryujinx.Input.Motion.CemuHook.Protocol
-{
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    public struct ControllerInfoResponse
-    {
-        public SharedResponse Shared;
-        private byte _zero;
-    }
-
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    public struct ControllerInfoRequest
-    {
-        public MessageType Type;
-        public int PortsCount;
-        public Array4<byte> PortIndices;
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
deleted file mode 100644
index 57f58ff0..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
+++ /dev/null
@@ -1,15 +0,0 @@
-using Ryujinx.Common.Memory;
-using System.Runtime.InteropServices;
-
-namespace Ryujinx.Input.Motion.CemuHook.Protocol
-{
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    public struct Header
-    {
-        public uint MagicString;
-        public ushort Version;
-        public ushort Length;
-        public Array4<byte> Crc32;
-        public uint Id;
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
deleted file mode 100644
index de1e5e90..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
+++ /dev/null
@@ -1,9 +0,0 @@
-namespace Ryujinx.Input.Motion.CemuHook.Protocol
-{
-    public enum MessageType : uint
-    {
-        Protocol = 0x100000,
-        Info,
-        Data
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs b/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
deleted file mode 100644
index e2e1ee9b..00000000
--- a/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
+++ /dev/null
@@ -1,51 +0,0 @@
-using Ryujinx.Common.Memory;
-using System.Runtime.InteropServices;
-
-namespace Ryujinx.Input.Motion.CemuHook.Protocol
-{
-    [StructLayout(LayoutKind.Sequential, Pack = 1)]
-    public struct SharedResponse
-    {
-        public MessageType     Type;
-        public byte            Slot;
-        public SlotState       State;
-        public DeviceModelType ModelType;
-        public ConnectionType  ConnectionType;
-
-        public Array6<byte> MacAddress;
-        public BatteryStatus BatteryStatus;
-    }
-
-    public enum SlotState : byte
-    {
-        Disconnected,
-        Reserved,
-        Connected
-    }
-
-    public enum DeviceModelType : byte
-    {
-        None,
-        PartialGyro,
-        FullGyro
-    }
-
-    public enum ConnectionType : byte
-    {
-        None,
-        USB,
-        Bluetooth
-    }
-
-    public enum BatteryStatus : byte
-    {
-        NA,
-        Dying,
-        Low,
-        Medium,
-        High,
-        Full,
-        Charging,
-        Charged
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/MotionInput.cs b/Ryujinx.Input/Motion/MotionInput.cs
deleted file mode 100644
index 1923d9cb..00000000
--- a/Ryujinx.Input/Motion/MotionInput.cs
+++ /dev/null
@@ -1,65 +0,0 @@
-using Ryujinx.Input.Motion;
-using System;
-using System.Numerics;
-
-namespace Ryujinx.Input
-{
-    public class MotionInput
-    {
-        public ulong   TimeStamp     { get; set; }
-        public Vector3 Accelerometer { get; set; }
-        public Vector3 Gyroscrope    { get; set; }
-        public Vector3 Rotation      { get; set; }
-
-        private readonly MotionSensorFilter _filter;
-
-        public MotionInput()
-        {
-            TimeStamp     = 0;
-            Accelerometer = new Vector3();
-            Gyroscrope    = new Vector3();
-            Rotation      = new Vector3();
-
-            // TODO: RE the correct filter.
-            _filter = new MotionSensorFilter(0f);
-        }
-
-        public void Update(Vector3 accel, Vector3 gyro, ulong timestamp, int sensitivity, float deadzone)
-        {
-            if (TimeStamp != 0)
-            {
-                Accelerometer = -accel;
-
-                if (gyro.Length() < deadzone)
-                {
-                    gyro = Vector3.Zero;
-                }
-
-                gyro *= (sensitivity / 100f);
-
-                Gyroscrope = gyro;
-
-                float deltaTime = MathF.Abs((long)(timestamp - TimeStamp) / 1000000f);
-
-                Vector3 deltaGyro = gyro * deltaTime;
-
-                Rotation += deltaGyro;
-
-                _filter.SamplePeriod = deltaTime;
-                _filter.Update(accel, DegreeToRad(gyro));
-            }
-
-            TimeStamp = timestamp;
-        }
-
-        public Matrix4x4 GetOrientation()
-        {
-            return Matrix4x4.CreateFromQuaternion(_filter.Quaternion);
-        }
-
-        private static Vector3 DegreeToRad(Vector3 degree)
-        {
-            return degree * (MathF.PI / 180);
-        }
-    }
-}
\ No newline at end of file
diff --git a/Ryujinx.Input/Motion/MotionSensorFilter.cs b/Ryujinx.Input/Motion/MotionSensorFilter.cs
deleted file mode 100644
index 440fa7ac..00000000
--- a/Ryujinx.Input/Motion/MotionSensorFilter.cs
+++ /dev/null
@@ -1,162 +0,0 @@
-using System.Numerics;
-
-namespace Ryujinx.Input.Motion
-{
-    // MahonyAHRS class. Madgwick's implementation of Mayhony's AHRS algorithm.
-    // See: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-    // Based on: https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs
-    class MotionSensorFilter
-    {
-        /// <summary>
-        /// Sample rate coefficient.
-        /// </summary>
-        public const float SampleRateCoefficient = 0.45f;
-
-        /// <summary>
-        /// Gets or sets the sample period.
-        /// </summary>
-        public float SamplePeriod { get; set; }
-
-        /// <summary>
-        /// Gets or sets the algorithm proportional gain.
-        /// </summary>
-        public float Kp { get; set; }
-
-        /// <summary>
-        /// Gets or sets the algorithm integral gain.
-        /// </summary>
-        public float Ki { get; set; }
-
-        /// <summary>
-        /// Gets the Quaternion output.
-        /// </summary>
-        public Quaternion Quaternion { get; private set; }
-
-        /// <summary>
-        /// Integral error.
-        /// </summary>
-        private Vector3 _intergralError;
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
-        /// </summary>
-        /// <param name="samplePeriod">
-        /// Sample period.
-        /// </param>
-        public MotionSensorFilter(float samplePeriod) : this(samplePeriod, 1f, 0f) { }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
-        /// </summary>
-        /// <param name="samplePeriod">
-        /// Sample period.
-        /// </param>
-        /// <param name="kp">
-        /// Algorithm proportional gain.
-        /// </param>
-        public MotionSensorFilter(float samplePeriod, float kp) : this(samplePeriod, kp, 0f) { }
-
-        /// <summary>
-        /// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
-        /// </summary>
-        /// <param name="samplePeriod">
-        /// Sample period.
-        /// </param>
-        /// <param name="kp">
-        /// Algorithm proportional gain.
-        /// </param>
-        /// <param name="ki">
-        /// Algorithm integral gain.
-        /// </param>
-        public MotionSensorFilter(float samplePeriod, float kp, float ki)
-        {
-            SamplePeriod = samplePeriod;
-            Kp = kp;
-            Ki = ki;
-
-            Reset();
-
-            _intergralError = new Vector3();
-        }
-
-        /// <summary>
-        /// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
-        /// </summary>
-        /// <param name="accel">
-        /// Accelerometer measurement in any calibrated units.
-        /// </param>
-        /// <param name="gyro">
-        /// Gyroscope measurement in radians.
-        /// </param>
-        public void Update(Vector3 accel, Vector3 gyro)
-        {
-            // Normalise accelerometer measurement.
-            float norm = 1f / accel.Length();
-
-            if (!float.IsFinite(norm))
-            {
-                return;
-            }
-
-            accel *= norm;
-
-            float q2 = Quaternion.X;
-            float q3 = Quaternion.Y;
-            float q4 = Quaternion.Z;
-            float q1 = Quaternion.W;
-
-            // Estimated direction of gravity.
-            Vector3 gravity = new Vector3()
-            {
-                X = 2f * (q2 * q4 - q1 * q3),
-                Y = 2f * (q1 * q2 + q3 * q4),
-                Z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4
-            };
-
-            // Error is cross product between estimated direction and measured direction of gravity.
-            Vector3 error = new Vector3()
-            {
-                X = accel.Y * gravity.Z - accel.Z * gravity.Y,
-                Y = accel.Z * gravity.X - accel.X * gravity.Z,
-                Z = accel.X * gravity.Y - accel.Y * gravity.X
-            };
-
-            if (Ki > 0f)
-            {
-                _intergralError += error; // Accumulate integral error.
-            }
-            else
-            {
-                _intergralError = Vector3.Zero; // Prevent integral wind up.
-            }
-
-            // Apply feedback terms.
-            gyro += (Kp * error) + (Ki * _intergralError);
-
-            // Integrate rate of change of quaternion.
-            Vector3 delta = new Vector3(q2, q3, q4);
-
-            q1 += (-q2 * gyro.X - q3 * gyro.Y - q4 * gyro.Z) * (SampleRateCoefficient * SamplePeriod);
-            q2 += (q1 * gyro.X + delta.Y * gyro.Z - delta.Z * gyro.Y) * (SampleRateCoefficient * SamplePeriod);
-            q3 += (q1 * gyro.Y - delta.X * gyro.Z + delta.Z * gyro.X) * (SampleRateCoefficient * SamplePeriod);
-            q4 += (q1 * gyro.Z + delta.X * gyro.Y - delta.Y * gyro.X) * (SampleRateCoefficient * SamplePeriod);
-
-            // Normalise quaternion.
-            Quaternion quaternion = new Quaternion(q2, q3, q4, q1);
-
-            norm = 1f / quaternion.Length();
-
-            if (!float.IsFinite(norm))
-            {
-                return;
-            }
-
-            Quaternion = quaternion * norm;
-        }
-
-        public void Reset()
-        {
-            Quaternion = Quaternion.Identity;
-        }
-    }
-}
\ No newline at end of file