Move solution and projects to src

8 files changed, 844 insertions, 0 deletions

diff --git a/src/Ryujinx.Input/Motion/CemuHook/Client.cs b/src/Ryujinx.Input/Motion/CemuHook/Client.cs
new file mode 100644
index 00000000..4498b8ca
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Client.cs
@@ -0,0 +1,475 @@
+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/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs b/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
new file mode 100644
index 00000000..7fb72344
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerData.cs
@@ -0,0 +1,47 @@
+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/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs b/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
new file mode 100644
index 00000000..63d4524a
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Protocol/ControllerInfo.cs
@@ -0,0 +1,20 @@
+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/src/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs b/src/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
new file mode 100644
index 00000000..57f58ff0
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Protocol/Header.cs
@@ -0,0 +1,15 @@
+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/src/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs b/src/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
new file mode 100644
index 00000000..de1e5e90
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Protocol/MessageType.cs
@@ -0,0 +1,9 @@
+namespace Ryujinx.Input.Motion.CemuHook.Protocol
+{
+    public enum MessageType : uint
+    {
+        Protocol = 0x100000,
+        Info,
+        Data
+    }
+}
\ No newline at end of file
diff --git a/src/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs b/src/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
new file mode 100644
index 00000000..e2e1ee9b
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/CemuHook/Protocol/SharedResponse.cs
@@ -0,0 +1,51 @@
+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/src/Ryujinx.Input/Motion/MotionInput.cs b/src/Ryujinx.Input/Motion/MotionInput.cs
new file mode 100644
index 00000000..1923d9cb
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/MotionInput.cs
@@ -0,0 +1,65 @@
+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/src/Ryujinx.Input/Motion/MotionSensorFilter.cs b/src/Ryujinx.Input/Motion/MotionSensorFilter.cs
new file mode 100644
index 00000000..440fa7ac
--- /dev/null
+++ b/src/Ryujinx.Input/Motion/MotionSensorFilter.cs
@@ -0,0 +1,162 @@
+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