VRCycling/Assets/Scripts/Sensors/Polar/PolarReceiver.cs

using System;
using System.Linq;
using System.Threading.Tasks;
using Plotting;
using UnityEngine;
using UnityEngine.Serialization;

[System.Serializable]
public struct PolarSensorConfig
{
    public int port;
    public String ipAddress;
    public bool plotAcc;
    public bool plotEcg;
    public int accSampleRate; //TODO: let user choose between 25, 50, 100, 200

    public PolarSensorConfig(int port = 9099, string ipAddress = "0.0.0.0", bool plotAcc = false, bool plotEcg = false,
        int accSampleRate = 25)
    {
        this.port = port;
        this.ipAddress = ipAddress;
        this.plotAcc = plotAcc;
        this.plotEcg = plotEcg;
        this.accSampleRate = accSampleRate;
    }
}

public struct PolarSensorData
{
    public Vector3 Acc;
    public float EcgValue;
}

public struct TimeSync
{
    public DateTime StartTime;
    public long DifStartAccStart;
    public long DifStartEcgStart;
}

public class PolarReceiver
{
    private UdpConnection connection;
    private PolarSensorData sensorData;

    public PolarSensorConfig SensorConfig { get; private set; }
    public PolarSensorData SensorData => sensorData;

    private PlotFileWriter ecgPlotFile;
    private PlotFileWriter accPlotFile;

    private static long startTimeAcc = -1;
    private static long startTimeEcg = -1;

    private static TimeSync timeSync;
    private const int ECG_SAMPLE_RATE = 130;


    [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.BeforeSceneLoad)]
    private static void InitTime()
    {
        timeSync = new TimeSync {StartTime = DateTime.Now, DifStartAccStart = -1, DifStartEcgStart = -1};
    }

    public PolarReceiver(PolarSensorConfig config)
    {
        SensorConfig = config;
    }

    public async Task StartListening()
    {
        if (SensorConfig.plotAcc)
        {
            accPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/acc.tsv");
            await accPlotFile.WriteDataLine("timestamp", new[] {"x", "y", "z"});
        }

        if (SensorConfig.plotEcg)
        {
            ecgPlotFile = DebugPlot.Instance.StartPlotting("Assets/Plotting/plots/ecg.tsv");
            await ecgPlotFile.WriteDataLine("timestamp", new[] {"voltage"});
        }

        if (SensorConfig.plotAcc || SensorConfig.plotEcg)
        {
            DebugPlot.Instance.ShowPlots();
        }

        connection = new UdpConnection(SensorConfig.ipAddress, SensorConfig.port, OnAccData, OnEcgData);
        connection.Listen();
    }

    public void Dispose()
    {
        connection.StopListening();
        DebugPlot.DestroyInstance();
    }

    private async void OnAccData(AccData data)
    {
        if (timeSync.DifStartAccStart < 0)
        {
            timeSync.DifStartAccStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds;
        }

        Debug.Log($"#ACC DATA items: {data.Values.Count}");
        await Task.WhenAll(UpdateSensorDataForAcc(data), PlotAcc(data));
    }

    private async Task PlotAcc(AccData data)
    {
        if (startTimeAcc < 0)
        {
            startTimeAcc = data.Timestamp;
        }

        if (SensorConfig.plotAcc)
        {
            var internalTimestamp =
                (data.Timestamp - startTimeAcc) / 1000000;
            var timestamp = internalTimestamp + timeSync.DifStartAccStart;
            foreach (var item in data.Values)
            {
                await accPlotFile.WriteDataLine(timestamp, new[] {item.x, item.y, item.z});
                timestamp += 1000 / SensorConfig.accSampleRate;
            }
        }
    }

    private async Task UpdateSensorDataForAcc(AccData data)
    {
        foreach (var item in data.Values)
        {
            sensorData.Acc = item;
            await Task.Delay(1000 / SensorConfig.accSampleRate);
        }
    }

    private async void OnEcgData(EcgData data)
    {
        if (timeSync.DifStartEcgStart < 0)
        {
            timeSync.DifStartEcgStart = (long) (DateTime.Now - timeSync.StartTime).TotalMilliseconds;
        }

        if (startTimeEcg < 0)
        {
            startTimeEcg = data.Timestamp;
        }

        if (SensorConfig.plotEcg)
        {
            var internalTimestamp =
                (data.Timestamp - startTimeEcg) / 1000000;
            var timestamp = internalTimestamp + timeSync.DifStartEcgStart;
            foreach (var item in data.Values)
            {
                await ecgPlotFile.WriteDataLine(timestamp, new[] {item});
                timestamp += 1000 / ECG_SAMPLE_RATE;
            }
        }

        sensorData.EcgValue = data.Values[0]; //TODO
    }
}