VRCycling/Assets/Scripts/Sensors/Bluetooth/BleReceiver.cs

using System;
using System.Threading.Tasks;
using Sensors.Polar;
using UniRx;
using UnityEngine;

namespace Sensors.Bluetooth
{
    [Serializable]
    public struct BleSensorConfig
    {
        public int port;
        public string ipAddress;
        [Tooltip("25, 50, 100 or 200")] public int accSampleRate; //TODO: let user choose between 25, 50, 100, 200

        public BleSensorConfig(int port = 9099, string ipAddress = "0.0.0.0", int accSampleRate = 25)
        {
            this.port = port;
            this.ipAddress = ipAddress;
            this.accSampleRate = accSampleRate;
        }
    }

    public struct BleSensorData
    {
        public Vector3 Acc;
        public float EcgValue;
        public int Hr;
        public PowermeterData PowermeterData;
    }

    public class BleReceiver
    {
        private const float TOLERANCE = 0.001f;

        private readonly Subject<AccData> rawAccDataSubject = new Subject<AccData>();

        private readonly Subject<EcgData> rawEcgDataSubject = new Subject<EcgData>();
        private readonly Subject<HRData> rawHRDataSubject = new Subject<HRData>();
        private UdpConnection connection;
        private BleSensorData sensorData;

        public BleReceiver(BleSensorConfig config)
        {
            SensorConfig = config;
        }

        public IObservable<EcgData> RawEcgData => rawEcgDataSubject.AsObservable();
        public IObservable<AccData> RawAccData => rawAccDataSubject.AsObservable();

        public IObservable<HRData> RawHRData => rawHRDataSubject.AsObservable();

        public BleSensorConfig SensorConfig { get; }
        public BleSensorData SensorData => sensorData;

        private RawPowermeterData? currentRawPowermeterData = null;
        private RawPowermeterData? previousRawPowermeterData = null;

        public void StartListening()
        {
            connection = new UdpConnection(SensorConfig.ipAddress, SensorConfig.port, OnAccData, OnEcgData, OnHRData,
                OnPowermeterData);
            connection.Listen();
            Debug.Log("PolarReceiver: Listening!");
        }

        public void Dispose()
        {
            rawAccDataSubject.Dispose();
            rawEcgDataSubject.Dispose();
            rawHRDataSubject.Dispose();
            connection?.StopListening();
        }

        private async void OnAccData(AccData data)
        {
            rawAccDataSubject.OnNext(data);
            await UpdateSensorDataForAcc(data);
        }

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

        private void OnEcgData(EcgData data)
        {
            rawEcgDataSubject.OnNext(data);
            sensorData.EcgValue = data.Values[0]; //TODO
        }

        private void OnHRData(HRData data)
        {
            rawHRDataSubject.OnNext(data);
            sensorData.Hr = data.HeartRate;
        }

        private void OnPowermeterData(RawPowermeterData data)
        {
            previousRawPowermeterData = currentRawPowermeterData;
            currentRawPowermeterData = data;

            var previousCadence = sensorData.PowermeterData.cadence;
            var previousTorque = sensorData.PowermeterData.torque;

            if (previousRawPowermeterData != null)
            {
                var p = previousRawPowermeterData.Value;
                var crankEventDif = data.lastCrankEventTime - p.lastCrankEventTime;
                var accumulatedTorqueDif = data.accumulatedTorque - p.accumulatedTorque;
                var crankRevDif = data.crankRevolutions - p.crankRevolutions;

                var cadence = previousCadence;
                var torque = previousTorque;

                if (crankEventDif > TOLERANCE)
                {
                    cadence = Mathf.RoundToInt((crankRevDif / crankEventDif) * 60f);
                }
                else if (data.instantaniousPower == 0 && p.instantaniousPower == 0)
                {
                    cadence = 0;
                }

                if (accumulatedTorqueDif > 0)
                {
                    torque = accumulatedTorqueDif;
                }
                else if (data.instantaniousPower == 0 && p.instantaniousPower == 0)
                {
                    cadence = 0;
                }

                sensorData.PowermeterData = new PowermeterData(data.instantaniousPower, cadence, torque);
            }
        }
    }
}