HoHCI-BikeSim/Assets/Scripts/SicknessReduction/Haptic/FanController.cs

using System;
using System.Threading.Tasks;
using Controller.Bicycle;
using UnityEngine;

namespace SicknessReduction.Haptic
{
    
    public class FanController : EspController
    {
        private const int MAX_FAN_CYCLE = 256; //for RES=8
        private const float MAX_CYCLE_AT_SPEED = 6.94f; //25km/h
        private const int THRES_CYCLE_CHANGE = 6;
        private const string TOPIC = "Fan/CHANNEL_NUMBER/Control";
        private const string CHANNEL_NUMBER_PLACEHOLDER = "CHANNEL_NUMBER";

        public RbBicycleController bicycleController;
        [Header("ESP Channels")] public int channelTopLeft = 0;
        public int channelBottomLeft = 1;
        public int channelTopRight = 2;
        public int channelBottomRight = 3;

        private Vector2 deviationFromCenter;
        private Transform cameraTransform;
        private bool cameraTransformAvailable;

        private Vector4 previousFanCycles;

        private string channelTL;
        private string channelBL;
        private string channelTR;
        private string channelBR;

        protected override void Start()
        {
            base.Start();
            if (Camera.main != null)
            {
                cameraTransform = Camera.main.transform;
                cameraTransformAvailable = true;
            }

            channelTL = TOPIC.Replace(CHANNEL_NUMBER_PLACEHOLDER, channelTopLeft.ToString());
            channelBL = TOPIC.Replace(CHANNEL_NUMBER_PLACEHOLDER, channelBottomLeft.ToString());
            channelTR = TOPIC.Replace(CHANNEL_NUMBER_PLACEHOLDER, channelTopRight.ToString());
            channelBR = TOPIC.Replace(CHANNEL_NUMBER_PLACEHOLDER, channelBottomRight.ToString());
        }

        protected override async void Update()
        {
            base.Update();
            if (!DoUpdate || PreviousUpdateActive) return;
            PreviousUpdateActive = true;
            
            var cycle = FanCycleForSpeed();
            CalculateDeviation();
            await SetFanSpeeds(cycle);
            
            PreviousUpdateActive = false;
        }

        private async Task SetFanSpeeds(int maxCycle)
        {
            var minCycle = (int) (maxCycle * (1f - Mathf.Abs(deviationFromCenter.x)));
            var leftCycle = deviationFromCenter.x >= 0 ? minCycle : maxCycle;
            var rightCycle = deviationFromCenter.x <0 ? minCycle: maxCycle;
            
            minCycle = (int) (maxCycle * (1f - Mathf.Abs(deviationFromCenter.y)));
            var bottomCycle = deviationFromCenter.y >= 0 ? minCycle : maxCycle;
            var topCycle = deviationFromCenter.y < 0 ? minCycle : maxCycle;

            var cycleTopLeft = Math.Min(topCycle, leftCycle);
            var cycleBottomLeft = Math.Min(bottomCycle, leftCycle);
            var cycleTopRight = Math.Min(topCycle, rightCycle);
            var cycleBottomRight = Math.Min(bottomCycle, rightCycle);
            
            await SendFanCycles(cycleTopLeft, cycleBottomLeft, cycleTopRight, cycleBottomRight);
        }

        private async Task SendFanCycles(int tl, int bl, int tr, int br)
        {
            var currentCycles = new Vector4(tl, bl, tr, br);
            if (currentCycles.magnitude <= 0.2f && previousFanCycles.magnitude >0.2f || (currentCycles - previousFanCycles).magnitude > THRES_CYCLE_CHANGE)
            {
                Debug.Log($"Sending FanCycles (tl = {tl}, bl = {bl}, tr = {tr}, br = {br}");
                previousFanCycles = currentCycles;
                await Broker.Publish(channelTL, tl.ToString());
                await Broker.Publish(channelBL, bl.ToString());
                await Broker.Publish(channelTR, tr.ToString());
                await Broker.Publish(channelBR, br.ToString());
            }
        }

        private void CalculateDeviation()
        {
            if (cameraTransformAvailable)
            {
                var hmdForward = cameraTransform.forward;
                var bikeForward = bicycleController.Forward;
                //Debug.DrawLine(cameraTransform.position, cameraTransform.position + hmdForward * 5, Color.blue);
                var hmdOnForwardPlane = Vector3.ProjectOnPlane(hmdForward, bicycleController.Up);
                //Debug.DrawLine(cameraTransform.position, cameraTransform.position + hmdOnForwardPlane * 4, Color.magenta);
                var hmdOnUpPlane = Vector3.ProjectOnPlane(hmdForward, bicycleController.Right);
                //Debug.DrawLine(cameraTransform.position, cameraTransform.position + hmdOnUpPlane * 8, Color.green);
               // Debug.DrawLine(cameraTransform.position, cameraTransform.position + bicycleController.Forward * 10, Color.white);
                
                var devLeftRight = Mathf.Clamp(Vector3.Angle(bikeForward, hmdOnForwardPlane), -90f, 90f);
                var devUpDown = Mathf.Clamp(Vector3.Angle(bikeForward, hmdOnUpPlane), -90f, 90f);
                deviationFromCenter = new Vector2(devLeftRight, devUpDown)/90f;
            }
            else
            {
                deviationFromCenter = Vector2.zero;
            }
        }

        private int FanCycleForSpeed()
        {
            return (int) Mathf.Lerp(0f, MAX_FAN_CYCLE, bicycleController.CurrentSpeed/MAX_CYCLE_AT_SPEED);
        }
    }
}