HoHCI-BikeSteering/Assets/Scripts/Tracking/MatchPlayerPositionAndCalibrate.cs

using System;
using Display;
using UnityEngine;
using Valve.VR;

namespace Tracking
{
    public enum CalibrationMode
    {
        None,
        MatchOnly,
        AutoImmediate,
        AutoCountdown
    }

    public class MatchPlayerPositionAndCalibrate : MonoBehaviour
    {
        public Transform player;
        public CalibrationMode calibrationMode;
             public CountdownDisplay countdownHud;

        [Header("Front Wheel")] public Transform frontWheelDesiredPosition;
        public FrontWheelTracker frontWheelTracker;

        [Header("Trainer Leg")] public Transform legDesiredPosition;
        public KineticLegTracker legTracker;

        private Transform fwtTransform;
        private Transform legTransform;
        private Transform playerTransform;

        private SteamVR_TrackedObject fwTrackedObject;
        private SteamVR_TrackedObject legTrackedObject;

        private int matching;
        private bool autoSet;

        private void Start()
        {
            fwtTransform = frontWheelTracker.transform;
            legTransform = legTracker.transform;
            playerTransform = player.transform;

            fwTrackedObject = legTracker.GetComponent<SteamVR_TrackedObject>();
            legTrackedObject = frontWheelTracker.GetComponent<SteamVR_TrackedObject>();
        }

        public void Match()
        {
            matching = 1;
        }

        public void setDesiredToReal()
        {
            //TODO
        }

        private void MatchFrontWheel()
        {
            var dif = frontWheelDesiredPosition.position - fwtTransform.position;
            playerTransform.position += dif;
        }

        private void MatchLeg()
        {
            //The only way to match it now is by rotating
            var fwtPosition = fwtTransform.position;
            var fwtToDesiredLeg = legDesiredPosition.position - fwtPosition;
            var fwtToRealLeg = legTransform.position - fwtPosition;

            var dl2d = new Vector2(fwtToDesiredLeg.x, fwtToDesiredLeg.z);
            var dr2d = new Vector2(fwtToRealLeg.x, fwtToRealLeg.z);

            var angle = Vector2.Angle(dl2d, dr2d);
            playerTransform.RotateAround(fwtPosition, playerTransform.up, -angle);
        }

        private void Update()
        {
            AutoCalibration();
            DoMatching();
        }

        private void AutoCalibration()
        {
            if (calibrationMode == CalibrationMode.None || autoSet || !fwTrackedObject.isValid ||
                !legTrackedObject.isValid) return;
            
            //TODO: show tracker status
            
            if (calibrationMode == CalibrationMode.AutoCountdown)
            {
                var display = Instantiate(countdownHud);
                display.OnCountdownDone = () =>
                {
                    Match();
                    return "Calibrated!";
                };
                autoSet = true;
            }
            else
            {
                Match();
                autoSet = true;
            }

            
        }

        private void DoMatching()
        {
            if (matching == 1)
            {
                MatchFrontWheel();
                matching++;
            }
            else if (matching == 2)
            {
                MatchLeg();
                if (calibrationMode == CalibrationMode.MatchOnly || calibrationMode == CalibrationMode.None)
                {
                    matching = 0;
                }
                else
                {
                    matching++;
                }
            }
            else if (matching == 3)
            {
                legTracker.Calibrate();
                frontWheelTracker.Calibrate();
                matching = 0;
            }
        }
    }
}