Add EventLoop based classes

src/wpilibsharp/Event/BooleanEvent.cs

@@ -0,0 +1,94 @@
+using CommunityToolkit.Diagnostics;
+using UnitsNet;
+using WPIMath.Filter;
+using WPIUtil;
+using WPIUtil.Atomic;
+
+namespace WPILib.Event;
+
+public class BooleanEvent
+{
+    protected EventLoop Loop { get; }
+
+    private readonly Func<bool> m_signal;
+    private readonly AtomicBool m_state = new(false);
+
+    public BooleanEvent(EventLoop loop, Func<bool> signal)
+    {
+        Loop = WpiGuard.RequireNotNull(loop);
+        m_signal = WpiGuard.RequireNotNull(signal);
+        m_state.Set(m_signal());
+        loop.Bind(() => m_state.Set(m_signal()));
+    }
+
+    public bool Get()
+    {
+        return m_state.Get();
+    }
+
+    public void IfHigh(Action action)
+    {
+        Loop.Bind(() =>
+        {
+            if (m_state.Get())
+            {
+                action();
+            }
+        });
+    }
+
+    public BooleanEvent Rising()
+    {
+        bool previous = m_state.Get();
+        return new BooleanEvent(Loop, () =>
+        {
+            bool present = m_state.Get();
+            bool ret = !previous && present;
+            previous = present;
+            return ret;
+        });
+    }
+
+    public BooleanEvent Falling()
+    {
+        bool previous = m_state.Get();
+        return new BooleanEvent(Loop, () =>
+        {
+            bool present = m_state.Get();
+            bool ret = previous && !present;
+            previous = present;
+            return ret;
+        });
+    }
+
+    public BooleanEvent Debounce(Duration duration, Debouncer.DebounceType type = Debouncer.DebounceType.Rising)
+    {
+        Debouncer debouncer = new(duration, type);
+        return new BooleanEvent(Loop, () =>
+        {
+            return debouncer.Calculate(m_state.Get());
+        });
+    }
+
+    public BooleanEvent Negate()
+    {
+        return new BooleanEvent(Loop, () => !m_state.Get());
+    }
+
+    public BooleanEvent And(Func<bool> other)
+    {
+        Guard.IsNotNull(other);
+        return new BooleanEvent(Loop, () => m_state.Get() && other());
+    }
+
+    public BooleanEvent Or(Func<bool> other)
+    {
+        Guard.IsNotNull(other);
+        return new BooleanEvent(Loop, () => m_state.Get() || other());
+    }
+
+    public T CastTo<T>(Func<EventLoop, Func<bool>, T> ctor)
+    {
+        return ctor(Loop, m_state.Get);
+    }
+}

src/wpilibsharp/Event/EventLoop.cs

@@ -0,0 +1,46 @@
+using CommunityToolkit.Diagnostics;
+
+namespace WPILib.Event;
+
+public sealed class EventLoop
+{
+    private readonly List<Action> m_bindings = [];
+
+    private bool m_running;
+
+    public EventLoop() { }
+
+    public void Bind(Action action)
+    {
+        if (m_running)
+        {
+            ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running");
+        }
+        m_bindings.Add(action);
+    }
+
+    public void Pool()
+    {
+        try
+        {
+            m_running = true;
+            foreach (var action in m_bindings)
+            {
+                action();
+            }
+        }
+        finally
+        {
+            m_running = false;
+        }
+    }
+
+    public void Clear()
+    {
+        if (m_running)
+        {
+            ThrowHelper.ThrowInvalidOperationException("Cannot bind EventLoop while it is running");
+        }
+        m_bindings.Clear();
+    }
+}

src/wpilibsharp/Event/NetworkBooleanEvent.cs

@@ -0,0 +1,32 @@
+using CommunityToolkit.Diagnostics;
+using NetworkTables;
+
+namespace WPILib.Event;
+
+public class NetworkBooleanEvent : BooleanEvent
+{
+    public NetworkBooleanEvent(EventLoop loop, BooleanTopic topic) : this(loop, topic.Subscribe(false))
+    {
+
+    }
+
+    public NetworkBooleanEvent(EventLoop loop, IBooleanSubscriber sub) : base(loop, () => sub.Topic.Instance.Connected && sub.Get())
+    {
+        Guard.IsNotNull(sub);
+    }
+
+    public NetworkBooleanEvent(EventLoop loop, NetworkTable table, string topicName) : this(loop, table.GetBooleanTopic(topicName))
+    {
+
+    }
+
+    public NetworkBooleanEvent(EventLoop loop, string tableName, string topicName) : this(loop, NetworkTableInstance.Default, tableName, topicName)
+    {
+
+    }
+
+    public NetworkBooleanEvent(EventLoop loop, NetworkTableInstance inst, string tableName, string topicName) : this(loop, inst.GetTable(tableName), topicName)
+    {
+
+    }
+}

src/wpilibsharp/TimedRobot.cs

@@ -7,7 +7,7 @@ namespace WPILib;
 
 public class TimedRobot : IterativeRobotBase
 {
-    private class Callback(Action func, Duration startTime, Duration period, Duration offset)
+    private sealed class Callback(Action func, Duration startTime, Duration period, Duration offset)
     {
         public Action CB { get; } = func;
         public Duration Period { get; } = period;

src/wpimath/Filter/Debouncer.cs

@@ -0,0 +1,71 @@
+using CommunityToolkit.Diagnostics;
+using UnitsNet;
+
+namespace WPIMath.Filter;
+
+public class Debouncer
+{
+    public enum DebounceType
+    {
+        Rising,
+        Falling,
+        Both
+    }
+
+    private readonly Duration m_debounceTime;
+    private readonly DebounceType m_debounceType;
+    private bool m_baseline;
+
+    private Duration m_prevTime;
+
+    public Debouncer(Duration debounceTime, DebounceType type = DebounceType.Rising)
+    {
+        m_debounceTime = debounceTime;
+        m_debounceType = type;
+
+        ResetTimer();
+
+        switch (m_debounceType)
+        {
+            case DebounceType.Both:
+            case DebounceType.Rising:
+                m_baseline = false;
+                break;
+            case DebounceType.Falling:
+                m_baseline = true;
+                break;
+            default:
+                ThrowHelper.ThrowArgumentOutOfRangeException(nameof(type));
+                break;
+        }
+    }
+
+    private void ResetTimer()
+    {
+        m_prevTime = MathSharedStore.Timestamp;
+    }
+
+    private bool Elapsed => MathSharedStore.Timestamp - m_prevTime >= m_debounceTime;
+
+    public bool Calculate(bool input)
+    {
+        if (input == m_baseline)
+        {
+            ResetTimer();
+        }
+
+        if (Elapsed)
+        {
+            if (m_debounceType == DebounceType.Both)
+            {
+                m_baseline = input;
+                ResetTimer();
+            }
+            return input;
+        }
+        else
+        {
+            return m_baseline;
+        }
+    }
+}

src/wpimath/IMathShared.cs

@@ -0,0 +1,10 @@
+using UnitsNet;
+
+namespace WPIMath;
+
+public interface IMathShared
+{
+    void ReportError(string error, string stackTrace);
+    void ReportUsage(MathUsageId id, int count);
+    Duration Timestamp { get; }
+}

src/wpimath/MathSharedStore.cs

@@ -0,0 +1,58 @@
+using UnitsNet;
+using UnitsNet.NumberExtensions.NumberToDuration;
+using WPIUtil.Natives;
+
+namespace WPIMath;
+
+public static class MathSharedStore
+{
+    private static IMathShared? m_mathShared;
+    private static readonly object m_lockObject = new();
+
+    private sealed class DefaultMathShared : IMathShared
+    {
+        public Duration Timestamp => TimestampNative.Now().Microseconds();
+
+        public void ReportError(string error, string stackTrace)
+        {
+        }
+
+        public void ReportUsage(MathUsageId id, int count)
+        {
+        }
+    }
+
+    public static IMathShared MathShared
+    {
+        get
+        {
+            lock (m_lockObject)
+            {
+                if (m_mathShared == null)
+                {
+                    m_mathShared = new DefaultMathShared();
+                }
+                return m_mathShared;
+            }
+        }
+        set
+        {
+            lock (m_lockObject)
+            {
+                m_mathShared = value;
+            }
+        }
+    }
+
+    public static void ReportError(string error, string stackTrace)
+    {
+        MathShared.ReportError(error, stackTrace);
+    }
+
+    public static void ReportUsage(MathUsageId id, int count)
+    {
+        MathShared.ReportUsage(id, count);
+    }
+
+    public static Duration Timestamp => MathShared.Timestamp;
+}

src/wpimath/MathUsageId.cs

@@ -0,0 +1,34 @@
+namespace WPIMath;
+
+public enum MathUsageId
+{
+    /** DifferentialDriveKinematics. */
+    KinematicsDifferentialDrive,
+
+    /** MecanumDriveKinematics. */
+    KinematicsMecanumDrive,
+
+    /** SwerveDriveKinematics. */
+    KinematicsSwerveDrive,
+
+    /** TrapezoidProfile. */
+    TrajectoryTrapezoidProfile,
+
+    /** LinearFilter. */
+    FilterLinear,
+
+    /** DifferentialDriveOdometry. */
+    OdometryDifferentialDrive,
+
+    /** SwerveDriveOdometry. */
+    OdometrySwerveDrive,
+
+    /** MecanumDriveOdometry. */
+    OdometryMecanumDrive,
+
+    /** PIDController. */
+    ControllerPIDController2,
+
+    /** ProfiledPIDController. */
+    ControllerProfiledPIDController,
+}

src/wpiutil/Atomic/AtomicBool.cs

@@ -0,0 +1,36 @@
+using System.Runtime.CompilerServices;
+
+namespace WPIUtil.Atomic;
+
+public sealed class AtomicBool
+{
+    private int m_value;
+
+    public AtomicBool()
+    {
+        m_value = 0;
+    }
+
+    public AtomicBool(bool initialValue)
+    {
+        m_value = initialValue ? 1 : 0;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool Get()
+    {
+        return Interlocked.CompareExchange(ref m_value, 0, 0) != 0;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public bool GetAndSet(bool newValue)
+    {
+        return Interlocked.Exchange(ref m_value, newValue ? 1 : 0) != 0;
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public void Set(bool newValue)
+    {
+        Interlocked.Exchange(ref m_value, newValue ? 1 : 0);
+    }
+}