Feature: log PICO data

PicoStreamingAssistantFTUDP/Pico4SAFTExtTrackingModule.cs

@@ -25,6 +25,10 @@ public sealed class Pico4SAFTExtTrackingModule : ExtTrackingModule, IDisposable
     private IPEndPoint? endPoint;
     private PxrFTInfo data;
 
+    private const bool FILE_LOG = true;
+    public static readonly string LOGGER_PATH = Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.ApplicationData), "PICOLogs.csv");
+    private Logger<PxrFTInfo> logger;
+
     public override (bool SupportsEye, bool SupportsExpression) Supported { get; } = (true, true);
 
     private bool StreamerValidity()
@@ -64,6 +68,12 @@ public override (bool eyeSuccess, bool expressionSuccess) Initialize(bool eyeAva
                     ReceivePxrData(pData);
             }
             Logger.LogDebug("Streaming Assistant handshake success.");
+
+            if (FILE_LOG)
+            {
+                this.logger = PicoDataLoggerFactory.build(LOGGER_PATH);
+                Logger.LogDebug("Using {} path for PICO logs.", LOGGER_PATH);
+            }
         }
         catch (SocketException ex) when (ex.ErrorCode is 10048)
         {
@@ -230,6 +240,9 @@ public override void Update()
                 fixed (UnifiedSingleEyeData* pRight = &UnifiedTracking.Data.Eye.Right)
                 {
                     ReceivePxrData(pData);
+
+                    if (this.logger != null) this.logger.UpdateValue(pData);
+
                     float* pxrShape = pData->blendShapeWeight;
                     UpdateEye(pxrShape, pLeft, pRight);
                     UpdateEyeExpression(pxrShape, unifiedShape);
@@ -261,6 +274,12 @@ private void Dispose(bool disposing)
                     udpClient.Client.Blocking = false;
                 Logger.LogInformation("Disposing of PxrFaceTracking UDP Client.");
                 udpClient?.Dispose();
+
+                if (this.logger != null)
+                {
+                    this.logger.Dispose();
+                    this.logger = null;
+                }
             }
 
             disposedValue = true;

PicoStreamingAssistantFTUDP/logger/DataExtractor.cs

@@ -0,0 +1,14 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Pico4SAFTExtTrackingModule;
+
+public interface DataExtractor<T>
+{
+    unsafe void Clone(T *obj, T *ret);
+    unsafe string ToCSV(T *obj, char delimiter);
+    string GetCSVHeader(char delimiter);
+}

PicoStreamingAssistantFTUDP/logger/Logger.cs

@@ -0,0 +1,85 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Pico4SAFTExtTrackingModule;
+
+public sealed class Logger<T> : IDisposable
+{
+    private const char CSV_DELIMITER = ';';
+
+    private Thread thread;
+    private bool threadEnabled;
+
+    private string filePath;
+
+    private DataExtractor<T> dataExtractor;
+    private bool waiting;
+    private T current;
+    private object waitingCurrentLock = new object();
+
+    public Logger(string filePath, DataExtractor<T> dataExtractor)
+    {
+        this.dataExtractor = dataExtractor;
+        this.waiting = true; // request first data
+
+        this.filePath = filePath;
+
+        this.threadEnabled = true;
+
+        this.thread = new Thread(new ThreadStart(ThreadMethod));
+        this.thread.Start();
+    }
+
+    public unsafe void UpdateValue(T* obj)
+    {
+        if (!this.IsWaiting()) return; // already got something
+
+        fixed (T* ret = &this.current) {
+            dataExtractor.Clone(obj, ret);
+        }
+        lock (this.waitingCurrentLock)
+        {
+            this.waiting = false; // got the new data
+        }
+    }
+
+    protected unsafe void ThreadMethod()
+    {
+        using (StreamWriter writer = new StreamWriter(this.filePath))
+        {
+            writer.WriteLine(this.dataExtractor.GetCSVHeader(CSV_DELIMITER)); // TODO add timestamp
+
+            while (this.threadEnabled)
+            {
+                if (this.IsWaiting()) continue; // no data; try again later
+
+                fixed (T *curr = &this.current) {
+                    writer.WriteLine(this.dataExtractor.ToCSV(curr, CSV_DELIMITER));
+                }
+                lock (this.waitingCurrentLock)
+                {
+                    this.waiting = true; // request a new data
+                }
+            }
+        }
+    }
+
+    public void Dispose()
+    {
+        this.threadEnabled = false;
+        this.thread.Join();
+    }
+
+    protected bool IsWaiting()
+    {
+        bool waiting;
+        lock(this.waitingCurrentLock)
+        {
+            waiting = this.waiting;
+        }
+        return waiting;
+    }
+}

PicoStreamingAssistantFTUDP/logger/PicoDataLoggerFactory.cs

@@ -0,0 +1,58 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Runtime.InteropServices;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace Pico4SAFTExtTrackingModule;
+
+public sealed class PicoDataLoggerFactory
+{
+    protected sealed class PicoDataExtractor : DataExtractor<PxrFTInfo>
+    {
+        public unsafe void Clone(PxrFTInfo *obj, PxrFTInfo* ret)
+        {
+            ret->timestamp = obj->timestamp;
+            ret->laughingProb = obj->laughingProb;
+
+            // TODO I'm sure there's an equivalent of `memcpy` for C# but I couldn't find it
+            for (int n = 0; n < Pxr.BLEND_SHAPE_NUMS; n++) ret->blendShapeWeight[n] = obj->blendShapeWeight[n];
+            for (int n = 0; n < 10; n++) ret->videoInputValid[n] = obj->videoInputValid[n];
+            for (int n = 0; n < 10; n++) ret->emotionProb[n] = obj->emotionProb[n];
+        }
+
+        public string GetCSVHeader(char delimiter)
+        {
+            StringBuilder sb = new StringBuilder();
+
+            foreach (BlendShapeIndex shape in Enum.GetValues(typeof(BlendShapeIndex)))
+            {
+                sb.Append(Enum.GetName(typeof(BlendShapeIndex), shape));
+                sb.Append(delimiter);
+            }
+
+            sb.Length--; // remove the last delimiter
+            return sb.ToString();
+        }
+
+        public unsafe string ToCSV(PxrFTInfo *obj, char delimiter)
+        {
+            StringBuilder sb = new StringBuilder();
+
+            for (int n = 0; n < Pxr.BLEND_SHAPE_NUMS; n++)
+            {
+                sb.Append(obj->blendShapeWeight[n]);
+                sb.Append(delimiter);
+            }
+
+            sb.Length--; // remove the last delimiter
+            return sb.ToString();
+        }
+    }
+
+    public static unsafe Logger<PxrFTInfo> build(string path)
+    {
+        return new Logger<PxrFTInfo>(path, new PicoDataExtractor());
+    }
+}