using System;
using UnityEditor;
using UnityEngine;

namespace Tracking
{
    public class FrontWheelTracker : CalibratableTracker
    {
        protected override string KeyPrefix => "fw";
        public float trackerDistanceToWheelCenterPoint;
        public KineticLegTracker legTracker;

        public Vector3 DebugPosOnBikePlane { private set; get; }

        public float SteerRotation
        {
            get
            {
                //legTracker position is added to compensate movements of the whole installation
                Vector3 legTrackerPos = Vector3.zero;
                if (legTracker.isActiveAndEnabled)
                {
                    legTrackerPos = legTracker.RelativePosition;
                }

                var posOnBikePlane = Vector3.ProjectOnPlane(RelativePosition + legTrackerPos, bicycleTransform.up);
                DebugPosOnBikePlane = posOnBikePlane;
                if (Mathf.Abs(posOnBikePlane.x / trackerDistanceToWheelCenterPoint) >= 1f)
                {
                    Debug.LogError("Strange behaviour!");
                    //EditorApplication.isPaused = true;
                }

                var theta = Mathf.Acos(Mathf.Clamp(posOnBikePlane.x / trackerDistanceToWheelCenterPoint, -1f, 1f));
                return 90f - theta * Mathf.Rad2Deg;
            }
        }
    }
}