using System;
using System.Collections.Generic;
using System.Linq;
using UnityEditor;
using UnityEngine;

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

        public float SteerRotation => framesToConsider > 1
            ? CalculateSteerRotationMultiFrame()
            : CalculateSteerRotationSingleFrame();

        public Vector3 DebugPosOnBikePlane { private set; get; }

        private Queue<float> previousValues;

        private void OnEnable()
        {
            if (framesToConsider > 1)
            {
                previousValues = new Queue<float>(framesToConsider);
            }
        }

        private float CalculateSteerRotationSingleFrame()
        {
            if (useTrackerRotationInsteadOfPosition) return RelativeRotation.y;
            var posOnBikePlane = GetPosOnBikePlane();
            return TrackerPositionToAngle(posOnBikePlane);
        }

        private float CalculateSteerRotationMultiFrame()
        {
            var angleThisFrame = CalculateSteerRotationSingleFrame();
            if (previousValues.Count > framesToConsider)
            {
                previousValues.Dequeue();
            }

            previousValues.Enqueue(angleThisFrame);

            //easiest way -> avg; TODO maybe in future, remove outliers
            return previousValues.Average();
        }

        private Vector3 GetPosOnBikePlane()
        {
            //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;
            }*/
            return posOnBikePlane;
        }

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