A bunch more NT functionality

dev/desktopDev/Program.cs

@@ -53,7 +53,8 @@ static void Main(string[] args)
             Source = camera
         };
 
-        RawSink sink = new RawSink("Sink") {
+        RawSink sink = new RawSink("Sink")
+        {
             Source = camera
         };
 
@@ -68,15 +69,18 @@ static void Main(string[] args)
             ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen
         };
 
-        MJpegServer sinkServer = new MJpegServer("SinkServer", 1182) {
+        MJpegServer sinkServer = new MJpegServer("SinkServer", 1182)
+        {
             Source = source
         };
 
         RawFrameReader reader = new RawFrameReader();
 
-        while (true) {
+        while (true)
+        {
             long ts = sink.GrabFrame(reader);
-            if (ts <= 0) {
+            if (ts <= 0)
+            {
                 continue;
             }
 

src/cscore/VideoEvent.cs

@@ -3,6 +3,7 @@
 using System.Runtime.InteropServices.Marshalling;
 using CsCore.Handles;
 using CsCore.Natives;
+using WPIUtil;
 using WPIUtil.Marshal;
 
 namespace CsCore;
@@ -11,28 +12,54 @@ namespace CsCore;
 [StructLayout(LayoutKind.Auto)]
 public readonly struct VideoEvent : INativeArrayFree<VideoEventMarshaller.NativeCsEvent>
 {
-    public required EventKind Kind { get; init; }
+    public EventKind Kind { get; }
+    public CsListener Listener { get; }
 
-    public required CsListener Listener { get; init; }
+    public VideoEvent(in VideoEventMarshaller.NativeCsEvent csEvent)
+    {
+        Kind = csEvent.kind;
+        Listener = new CsListener(csEvent.listener);
+    }
 
     public static unsafe void FreeArray(VideoEventMarshaller.NativeCsEvent* ptr, int len)
     {
         CsNative.FreeEvents(ptr, len);
     }
 }
 
+[StructLayout(LayoutKind.Explicit)]
+internal struct VideoEventUnion
+{
+    [FieldOffset(0)]
+    public CsSource source;
+    [FieldOffset(0)]
+    public CsSink sink;
+    [FieldOffset(0)]
+    public VideoMode mode;
+    [FieldOffset(0)]
+    public PropertyTuple property;
+}
+
+[StructLayout(LayoutKind.Auto)]
+internal struct PropertyTuple
+{
+    public CsProperty property;
+    public PropertyKind kind;
+    public int value;
+}
+
 [CustomMarshaller(typeof(VideoEvent), MarshalMode.ElementOut, typeof(VideoEventMarshaller))]
 public static unsafe class VideoEventMarshaller
 {
 
     public static NativeCsEvent ConvertToUnmanaged(in VideoEvent managed)
     {
-        throw new System.NotSupportedException();
+        throw new NotSupportedException();
     }
 
     public static VideoEvent ConvertToManaged(in NativeCsEvent unmanaged)
     {
-        throw new NotImplementedException();
+        return new(unmanaged);
     }
 
     [StructLayout(LayoutKind.Sequential)]
@@ -41,12 +68,12 @@ public struct NativeCsEvent
         public EventKind kind;
         public int source;
         public int sink;
-        public byte* name;
+        public WpiStringMarshaller.WpiStringNative name;
         public VideoMode mode;
         public int property;
         public PropertyKind propertyKind;
         public int value;
-        public byte* valueStr;
+        public WpiStringMarshaller.WpiStringNative valueStr;
         public int listener;
     }
 }

src/hal/Natives/HalBase.cs

@@ -9,6 +9,11 @@ namespace WPIHal.Natives;
 
 public static partial class HalBase
 {
+    [LibraryImport("wpiHal", EntryPoint = "HAL_Initialize")]
+    [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
+    [return: MarshalAs(UnmanagedType.I4)]
+    public static partial bool Initialize(int timeout, int mode);
+
     [LibraryImport("wpiHal", EntryPoint = "HAL_ExpandFPGATime")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
     internal static partial ulong ExpandFPGATimeRefShim(uint unexpandedLower, ref HalStatus status);

src/ntcore/MultiSubscriber.cs

@@ -4,29 +4,20 @@
 
 namespace NetworkTables;
 
-public sealed class MultiSubscriber : IDisposable
+public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options) : IDisposable
 {
-    public MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options)
-    {
-        Inst = inst;
-        Handle = NtCore.SubscribeMultiple(inst.Handle, prefixes, (nuint)prefixes.Length, options);
-    }
-
     public void Dispose()
     {
-        lock (this)
+        if (Handle.Handle != 0)
         {
-            if (Handle.Handle != 0)
-            {
-                NtCore.UnsubscribeMultiple(Handle);
-                Handle = default;
-            }
+            NtCore.UnsubscribeMultiple(Handle);
+            Handle = default;
         }
     }
 
-    public NtMultiSubscriber Handle { get; private set; }
+    public NtMultiSubscriber Handle { get; private set; } = NtCore.SubscribeMultiple(inst.Handle, prefixes, (nuint)prefixes.Length, options);
 
-    public NetworkTableInstance Inst { get; }
+    public NetworkTableInstance Instance { get; } = inst;
 
     public bool IsValid => Handle.Handle != 0;
 }

src/ntcore/Natives/NtCore.cs

@@ -59,6 +59,16 @@ public static NetworkTableValue GetEntryValue<T>(T entry) where T : struct, INtE
         return value;
     }
 
+    [LibraryImport("ntcore", EntryPoint = "NT_GetEntryValueType")]
+    [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
+    internal static partial void GetEntryValue(int entry, NetworkTableType types, out NetworkTableValue value);
+
+    public static NetworkTableValue GetEntryValue<T>(T entry, NetworkTableType types) where T : struct, INtEntryHandle
+    {
+        GetEntryValue(entry.Handle, types, out var value);
+        return value;
+    }
+
     [LibraryImport("ntcore", EntryPoint = "NT_SetDefaultEntryValue")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
     [return: MarshalAs(UnmanagedType.I4)]
@@ -107,6 +117,16 @@ public static unsafe NetworkTableValue[] ReadQueueValue<T>(T subentry) where T :
         return ReadQueueValue(subentry.Handle, out var _);
     }
 
+    [LibraryImport("ntcore", EntryPoint = "NT_ReadQueueValueType")]
+    [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
+    [return: MarshalUsing(typeof(ManagedFreeArrayMarshaller<,>), CountElementName = nameof(count))]
+    internal static unsafe partial NetworkTableValue[] ReadQueueValue(int subentry, NetworkTableType types, out nuint count);
+
+    public static unsafe NetworkTableValue[] ReadQueueValue<T>(T subentry, NetworkTableType types) where T : struct, INtEntryHandle
+    {
+        return ReadQueueValue(subentry.Handle, types, out var _);
+    }
+
     [LibraryImport("ntcore", EntryPoint = "NT_GetTopics")]
     [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])]
     [return: MarshalUsing(typeof(ManagedFreeArrayMarshaller<,>), CountElementName = "count")]

src/ntcore/NetworkTable.cs

@@ -1,8 +1,10 @@
 using System;
 using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.IO;
 using NetworkTables.Handles;
 using NetworkTables.Natives;
+using WPIUtil.Concurrent;
 
 namespace NetworkTables;
 
@@ -21,12 +23,26 @@ public static ReadOnlySpan<char> BasenameKey(ReadOnlySpan<char> key)
         return key[(slash + 1)..];
     }
 
-    public static string NormalizeKey(ReadOnlySpan<char> key, bool withLeadingSlash = true)
+    public static string NormalizeKey(string key, bool withLeadingSlash = true)
     {
-        throw new NotImplementedException();
+        string normalized;
+        if (withLeadingSlash)
+        {
+            normalized = $"{PATH_SEPARATOR}{key}";
+        }
+        else
+        {
+            normalized = key;
+        }
+        normalized = normalized.Replace($"{PATH_SEPARATOR}{PATH_SEPARATOR}", $"{PATH_SEPARATOR}");
+        if (!withLeadingSlash && normalized[0] == PATH_SEPARATOR)
+        {
+            normalized = normalized[1..];
+        }
+        return normalized;
     }
 
-    public static List<string> GetHierarchy(ReadOnlySpan<char> key)
+    public static List<string> GetHierarchy(string key)
     {
         string normal = NormalizeKey(key);
         List<string> hierarchy = [];
@@ -180,17 +196,82 @@ public NetworkTableValue GetValue(string key)
 
     public string Path { get; }
 
-    // public NtListener AddListener(EventFlags kinds, Action<NetworkTable, string, NetworkTableEvent> listener) {
-    //     int prefixLex = Path.Length + 1;
-    //     return Instance.Add
-    // }
+    public NtListener AddListener(EventFlags eventKinds, Action<NetworkTable, string, NetworkTableEvent> listener)
+    {
+        int prefixLex = Path.Length + 1;
+        return Instance.AddListener([m_pathWithSep], eventKinds, ntEvent =>
+        {
+            string? topicName = null;
+            if (ntEvent.TopicInfo != null)
+            {
+                topicName = ntEvent.TopicInfo.Value.Name;
+            }
+            else if (ntEvent.ValueData != null)
+            {
+                // Don't fully construct the lazy object
+                topicName = ntEvent.ValueData.Value.GetTopicName();
+            }
+            if (topicName == null)
+            {
+                return;
+            }
+            string relativeKey = topicName[prefixLex..];
+            if (relativeKey.Contains(PATH_SEPARATOR))
+            {
+                // part of a subtable
+                return;
+            }
+            listener(this, relativeKey, ntEvent);
+        });
+    }
+
+    public NtListener AddListener(string key, EventFlags eventKinds, Action<NetworkTable, string, NetworkTableEvent> listener)
+    {
+        var entry = GetEntry(key);
+        return Instance.AddListener(entry, eventKinds, ntEvent => listener(this, key, ntEvent));
+    }
+
+    private class SubTableListenerHolder(int prefixLen, NetworkTable parent, Action<NetworkTable, string, NetworkTable> listener)
+    {
+        private readonly int m_prefixLen = prefixLen;
+        private readonly NetworkTable m_parent = parent;
+        private readonly Action<NetworkTable, string, NetworkTable> m_listener = listener;
+        private readonly HashSet<string> m_notifiedTables = [];
+
+        public void OnEvent(NetworkTableEvent ntEvent)
+        {
+            if (ntEvent.TopicInfo == null)
+            {
+                return;
+            }
+            var relativeKey = ntEvent.TopicInfo.Value.Name.AsSpan()[m_prefixLen..];
+            int endSubTable = relativeKey.IndexOf(PATH_SEPARATOR);
+            if (endSubTable == -1)
+            {
+                return;
+            }
+            string subTableKey = relativeKey[..endSubTable].ToString();
+            if (m_notifiedTables.Contains(subTableKey))
+            {
+                return;
+            }
+            m_notifiedTables.Add(subTableKey);
+            m_listener(m_parent, subTableKey, m_parent.GetSubTable(subTableKey));
+        }
+    }
+
+    public NtListener AddSubTableListener(Action<NetworkTable, string, NetworkTable> listener)
+    {
+        int prefixLen = Path.Length + 1;
+        SubTableListenerHolder holder = new(prefixLen, this, listener);
+        return Instance.AddListener([m_pathWithSep], EventFlags.Publish | EventFlags.Immediate, holder.OnEvent);
+    }
 
     public void RemoveListener(NtListener listener)
     {
         Instance.RemoveListener(listener);
     }
 
-
     public override bool Equals(object? obj)
     {
         return Equals(obj as NetworkTable);