HoHCI-BikeSteering/Assets/Scripts/Sensors/Polar/PolarReceiver.cs

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

namespace Sensors.Polar
{
    [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 AsyncLogFileWriter ecgPlotFile;
        private AsyncLogFileWriter 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
        }
    }
}