VRCycling/Assets/Scripts/SicknessReduction/Haptic/VibrationController.cs

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

namespace SicknessReduction.Haptic
{
    public enum VibrationControllerMode
    {
        Continuous,
        AlternatingFixed,
        AlternatingCadenceBased
    }

    [RequireComponent(typeof(DynamicReductionSource))]
    public class VibrationController : EspController
    {
        private const string TOPIC_CYCLE = "Vibration/Control/Cycle";
        private const string TOPIC_CADENCE = "Vibration/Control/Cadence";
        private const int THRES_CADENCE_CHANGE = 4;

        public int minCycle = 100;
        public int maxCycle = 178; //vibro motor specs: 11000 ± 3,000rpm = 133.33 - 183.33 Hz 
        public float reductionValueThreshold = 0.05f;
        public VibrationControllerMode mode;
        public int fixedCycleRpm;

        private int currentCycle;
        private bool stopped = false;

        private bool initialCyclePublished;
        private int previousCadence = -1;
        private DynamicReductionSource reductionSource;

        protected override void Start()
        {
            base.Start();
            reductionSource = GetComponent<DynamicReductionSource>();
        }

        protected override async void Update()
        {
            base.Update();

            // > 0 while cornering
            var reductionValue = reductionSource.CurrentValue;
            var cycleValue = reductionValue < reductionValueThreshold
                ? 0
                : (int) Mathf.Lerp(minCycle, maxCycle, reductionValue);

            if (!DoUpdate || PreviousUpdateActive) return;
            if (reductionValue > 0)
                switch (mode)
                {
                    case VibrationControllerMode.Continuous:
                        await VibrateContinuous(cycleValue);
                        break;
                    case VibrationControllerMode.AlternatingFixed:
                        await VibrateAlternatingFixed(cycleValue);
                        break;
                    case VibrationControllerMode.AlternatingCadenceBased:
                        await VibrateAlternatingCadenceBased();
                        break;
                    default:
                        throw new ArgumentOutOfRangeException();
                }
            else
                await StopVibrating();

            PreviousUpdateActive = false;
        }

        private async Task StopVibrating()
        {
            if (stopped) return;
            await Broker.Publish(TOPIC_CYCLE, "0");
            currentCycle = 0;
            stopped = true;
        }

        private async Task VibrateAlternatingCadenceBased()
        {
            var cadence = BikeSensorData.Instance.PowermeterData?.InstantaneousCadence;
            if (!cadence.HasValue) return;
            var c = cadence.Value;

            stopped = false;
            PreviousUpdateActive = true; //flag to avoid concurrent updates
            //as soon as we have a cadence, we want the motors to vibrate
            if (!initialCyclePublished)
            {
                await Broker.Publish(TOPIC_CYCLE, $"{maxCycle}");
                initialCyclePublished = true;
            }

            //if the cadence changes to 0, we have to switch off vibration
            if (c == 0)
            {
                await Broker.Publish(TOPIC_CYCLE, "0");
                previousCadence = c;
            }
            //as soon as we have cadence again, we want to switch on vibration again, and then immediately set cadence again
            else if (previousCadence == 0)
            {
                await Broker.Publish(TOPIC_CYCLE, $"{maxCycle}");
                await PublishCadence(c);
            }
            //if we have never set cadence, or the change of cadence is high enough, we tell the ESP to change cadence
            else if (previousCadence < 0 || c - previousCadence > THRES_CADENCE_CHANGE)
            {
                await PublishCadence(c);
            }
        }

        private async Task VibrateAlternatingFixed(int cycleValue)
        {
            if (Math.Abs(cycleValue - currentCycle) > 0)
            {
                await Broker.Publish(TOPIC_CYCLE, $"{cycleValue}");
                await PublishCadence(fixedCycleRpm);
            }
        }

        private async Task VibrateContinuous(int cycleValue)
        {
            if (Math.Abs(cycleValue - currentCycle) > 2)
            {
                stopped = false;
                //Debug.Log($"Sending Cycle {cycleValue}");
                await Broker.Publish(TOPIC_CYCLE, $"{cycleValue}");
                currentCycle = cycleValue;
            }
        }

        private async Task PublishCadence(int cadence)
        {
            //Debug.Log($"Sending Cadence {cadence}");
            await Broker.Publish(TOPIC_CADENCE, $"{cadence}");
            previousCadence = cadence;
        }
    }
}