/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 * All rights reserved.
 *
 * Licensed under the Oculus SDK License Agreement (the "License");
 * you may not use the Oculus SDK except in compliance with the License,
 * which is provided at the time of installation or download, or which
 * otherwise accompanies this software in either electronic or hard copy form.
 *
 * You may obtain a copy of the License at
 *
 * https://developer.oculus.com/licenses/oculussdk/
 *
 * Unless required by applicable law or agreed to in writing, the Oculus SDK
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;
using Oculus.Interaction.Input;
using UnityEngine;
using UnityEngine.Assertions;
using System.Collections.Generic;

namespace Oculus.Interaction.PoseDetection
{
    /// <summary>
    /// Tracks velocities (position deltas over the last two frames) for a list of joints and compares them to a velocity target along the provided axes.
    /// If the velocity target (units per second) is met for the minimum time threshold, the state becomes Active.
    /// </summary>
    public class JointVelocityActiveState : MonoBehaviour,
        IActiveState, ITimeConsumer
    {
        public enum RelativeTo
        {
            Hand = 0,
            World = 1,
            Head = 2,
        }

        public enum WorldAxis
        {
            PositiveX = 0,
            NegativeX = 1,
            PositiveY = 2,
            NegativeY = 3,
            PositiveZ = 4,
            NegativeZ = 5,
        }

        public enum HeadAxis
        {
            HeadForward = 0,
            HeadBackward = 1,
            HeadUp = 2,
            HeadDown = 3,
            HeadLeft = 4,
            HeadRight = 5,
        }

        public enum HandAxis
        {
            PalmForward = 0,
            PalmBackward = 1,
            WristUp = 2,
            WristDown = 3,
            WristForward = 4,
            WristBackward = 5,
        }

        [Serializable]
        public struct JointVelocityFeatureState
        {
            /// <summary>
            /// The world target vector for a
            /// <see cref="JointVelocityFeatureConfig"/>
            /// </summary>
            public readonly Vector3 TargetVector;

            /// <summary>
            /// The normalized joint velocity along the target
            /// vector relative to <see cref="_minVelocity"/>
            /// </summary>
            public readonly float Amount;

            public JointVelocityFeatureState(Vector3 targetVector, float velocity)
            {
                TargetVector = targetVector;
                Amount = velocity;
            }
        }

        [Serializable]
        public class JointVelocityFeatureConfigList
        {
            [SerializeField]
            private List<JointVelocityFeatureConfig> _values;

            public List<JointVelocityFeatureConfig> Values => _values;
        }

        [Serializable]
        public class JointVelocityFeatureConfig : FeatureConfigBase<HandJointId>
        {
            [Tooltip("The detection axis will be in this coordinate space.")]
            [SerializeField]
            private RelativeTo _relativeTo = RelativeTo.Hand;

            [Tooltip("The world axis used for detection.")]
            [SerializeField]
            private WorldAxis _worldAxis = WorldAxis.PositiveZ;

            [Tooltip("The axis of the hand root pose used for detection.")]
            [SerializeField]
            private HandAxis _handAxis = HandAxis.WristForward;

            [Tooltip("The axis of the head pose used for detection.")]
            [SerializeField]
            private HeadAxis _headAxis = HeadAxis.HeadForward;

            public RelativeTo RelativeTo
            {
                get => _relativeTo;
                set => _relativeTo = value;
            }

            public WorldAxis WorldAxis
            {
                get => _worldAxis;
                set => _worldAxis = value;
            }

            public HandAxis HandAxis
            {
                get => _handAxis;
                set => _handAxis = value;
            }

            public HeadAxis HeadAxis
            {
                get => _headAxis;
                set => _headAxis = value;
            }
        }

        [Tooltip("Provided joints will be sourced from this IHand.")]
        [SerializeField, Interface(typeof(IHand))]
        private UnityEngine.Object _hand;
        public IHand Hand { get; private set; }

        [Tooltip("JointDeltaProvider caches joint deltas to avoid " +
            "unnecessary recomputing of deltas.")]
        [SerializeField, Interface(typeof(IJointDeltaProvider))]
        private UnityEngine.Object _jointDeltaProvider;
        public IJointDeltaProvider JointDeltaProvider { get; private set; }

        [Tooltip("Reference to the Hmd providing the HeadAxis pose.")]
        [SerializeField, Optional, Interface(typeof(IHmd))]
        private UnityEngine.Object _hmd;
        public IHmd Hmd { get; private set; }

        [SerializeField]
        private JointVelocityFeatureConfigList _featureConfigs;

        [SerializeField]
        private JointVelocityFeatureConfigList _featureConfigurations;

        [Tooltip("The velocity used for the detection " +
            "threshold, in units per second.")]
        [SerializeField, Min(0)]
        private float _minVelocity = 0.5f;

        [Tooltip("The min velocity value will be modified by this width " +
            "to create differing enter/exit thresholds. Used to prevent " +
            "chattering at the threshold edge.")]
        [SerializeField, Min(0)]
        private float _thresholdWidth = 0.02f;

        [Tooltip("A new state must be maintaned for at least this " +
            "many seconds before the Active property changes.")]
        [SerializeField, Min(0)]
        private float _minTimeInState = 0.05f;

        public bool Active
        {
            get
            {
                if (!isActiveAndEnabled)
                {
                    return false;
                }

                UpdateActiveState();
                return _activeState;
            }
        }

        private Func<float> _timeProvider = () => Time.time;
        public void SetTimeProvider(Func<float> timeProvider)
        {
            _timeProvider = timeProvider;
        }

        public IReadOnlyList<JointVelocityFeatureConfig> FeatureConfigs =>
#if ISDK_OPENXR_HAND
            _featureConfigurations.Values;
#else
            _featureConfigs.Values;
#endif

        public IReadOnlyDictionary<JointVelocityFeatureConfig, JointVelocityFeatureState> FeatureStates =>
            _featureStates;

        private Dictionary<JointVelocityFeatureConfig, JointVelocityFeatureState> _featureStates =
            new Dictionary<JointVelocityFeatureConfig, JointVelocityFeatureState>();

        private JointDeltaConfig _jointDeltaConfig;

        private int _lastStateUpdateFrame;
        private float _lastStateChangeTime;
        private float _lastUpdateTime;
        private bool _internalState;
        private bool _activeState;

        protected bool _started = false;

        protected virtual void Awake()
        {
            Hand = _hand as IHand;
            JointDeltaProvider = _jointDeltaProvider as IJointDeltaProvider;

            if (_hmd != null)
            {
                Hmd = _hmd as IHmd;
            }
        }

        protected virtual void Start()
        {
            this.BeginStart(ref _started);

            this.AssertField(Hand, nameof(Hand));
            this.AssertField(JointDeltaProvider, nameof(JointDeltaProvider));
            this.AssertField(_jointDeltaProvider, nameof(_jointDeltaProvider));
            this.AssertField(_timeProvider, nameof(_timeProvider));

            IList<HandJointId> allTrackedJoints = new List<HandJointId>();
            foreach (var config in FeatureConfigs)
            {
                allTrackedJoints.Add(config.Feature);
                _featureStates.Add(config, new JointVelocityFeatureState());

                Assert.IsTrue(config.RelativeTo != RelativeTo.Head || Hmd != null);

                this.AssertIsTrue(config.RelativeTo != RelativeTo.Head || Hmd != null,
                    $"One of the {AssertUtils.Nicify(nameof(FeatureConfigs))} is not relative to the head or the {nameof(Hmd)}");
            }
            _jointDeltaConfig = new JointDeltaConfig(GetInstanceID(), allTrackedJoints);


            _lastUpdateTime = _timeProvider();
            this.EndStart(ref _started);
        }

        private bool CheckAllJointVelocities()
        {
            bool result = true;

            float deltaTime = _timeProvider() - _lastUpdateTime;
            float threshold = _internalState ?
                  _minVelocity + _thresholdWidth * 0.5f :
                  _minVelocity - _thresholdWidth * 0.5f;

            threshold *= deltaTime;

            foreach (var config in FeatureConfigs)
            {
                if (Hand.GetJointPose(HandJointId.HandWristRoot, out Pose wristPose) &&
                    Hand.GetJointPose(config.Feature, out _) &&
                    JointDeltaProvider.GetPositionDelta(
                        config.Feature, out Vector3 worldDeltaDirection))
                {
                    Vector3 worldTargetDirection = GetWorldTargetVector(wristPose, config);
                    float velocityAlongTargetAxis =
                        Vector3.Dot(worldDeltaDirection, worldTargetDirection);

                    _featureStates[config] = new JointVelocityFeatureState(
                                             worldTargetDirection,
                                             threshold > 0 ?
                                             Mathf.Clamp01(velocityAlongTargetAxis / threshold) :
                                             1);

                    bool velocityExceedsThreshold = velocityAlongTargetAxis > threshold;
                    result &= velocityExceedsThreshold;
                }
                else
                {
                    result = false;
                }
            }

            return result;
        }

        protected virtual void Update()
        {
            UpdateActiveState();
        }

        protected virtual void OnEnable()
        {
            if (_started)
            {
                JointDeltaProvider.RegisterConfig(_jointDeltaConfig);
            }
        }

        protected virtual void OnDisable()
        {
            if (_started)
            {
                JointDeltaProvider.UnRegisterConfig(_jointDeltaConfig);
            }
        }

        private void UpdateActiveState()
        {
            if (Time.frameCount <= _lastStateUpdateFrame)
            {
                return;
            }
            _lastStateUpdateFrame = Time.frameCount;

            bool newState = CheckAllJointVelocities();

            if (newState != _internalState)
            {
                _internalState = newState;
                _lastStateChangeTime = _timeProvider();
            }

            if (_timeProvider() - _lastStateChangeTime >= _minTimeInState)
            {
                _activeState = _internalState;
            }
            _lastUpdateTime = _timeProvider();
        }

        private Vector3 GetWorldTargetVector(Pose wristPose, JointVelocityFeatureConfig config)
        {
            switch (config.RelativeTo)
            {
                default:
                case RelativeTo.Hand:
                    return GetHandAxisVector(config.HandAxis, wristPose);
                case RelativeTo.World:
                    return GetWorldAxisVector(config.WorldAxis);
                case RelativeTo.Head:
                    return GetHeadAxisVector(config.HeadAxis);
            }
        }

        private Vector3 GetWorldAxisVector(WorldAxis axis)
        {
            switch (axis)
            {
                default:
                case WorldAxis.PositiveX:
                    return Vector3.right;
                case WorldAxis.NegativeX:
                    return Vector3.left;
                case WorldAxis.PositiveY:
                    return Vector3.up;
                case WorldAxis.NegativeY:
                    return Vector3.down;
                case WorldAxis.PositiveZ:
                    return Vector3.forward;
                case WorldAxis.NegativeZ:
                    return Vector3.back;
            }
        }

        private Vector3 GetHandAxisVector(HandAxis axis, Pose wristPose)
        {
            switch (axis)
            {
                case HandAxis.PalmForward:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftPalmar : Constants.RightPalmar);
                case HandAxis.PalmBackward:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftDorsal : Constants.RightDorsal);
                case HandAxis.WristUp:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftThumbSide : Constants.RightThumbSide);
                case HandAxis.WristDown:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftPinkySide : Constants.RightPinkySide);
                case HandAxis.WristForward:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftDistal : Constants.RightDistal);
                case HandAxis.WristBackward:
                    return wristPose.rotation * (Hand.Handedness == Handedness.Left ?
                        Constants.LeftProximal : Constants.RightProximal);
                default:
                    return Vector3.zero;
            }
        }

        private Vector3 GetHeadAxisVector(HeadAxis axis)
        {
            Hmd.TryGetRootPose(out Pose headPose);

            Vector3 result;
            switch (axis)
            {
                case HeadAxis.HeadForward:
                    result = headPose.forward;
                    break;
                case HeadAxis.HeadBackward:
                    result = -headPose.forward;
                    break;
                case HeadAxis.HeadUp:
                    result = headPose.up;
                    break;
                case HeadAxis.HeadDown:
                    result = -headPose.up;
                    break;
                case HeadAxis.HeadRight:
                    result = headPose.right;
                    break;
                case HeadAxis.HeadLeft:
                    result = -headPose.right;
                    break;
                default:
                    result = Vector3.zero;
                    break;
            }
            return result;
        }

        #region Inject

        public void InjectAllJointVelocityActiveState(JointVelocityFeatureConfigList featureConfigs,
                                                      IHand hand, IJointDeltaProvider jointDeltaProvider)
        {
            InjectFeatureConfigList(featureConfigs);
            InjectHand(hand);
            InjectJointDeltaProvider(jointDeltaProvider);
        }

        public void InjectFeatureConfigList(JointVelocityFeatureConfigList featureConfigs)
        {
            _featureConfigs = featureConfigs;
        }

        public void InjectHand(IHand hand)
        {
            _hand = hand as UnityEngine.Object;
            Hand = hand;
        }

        public void InjectJointDeltaProvider(IJointDeltaProvider jointDeltaProvider)
        {
            JointDeltaProvider = jointDeltaProvider;
            _jointDeltaProvider = jointDeltaProvider as UnityEngine.Object;
        }

        [Obsolete("Use SetTimeProvider()")]
        public void InjectOptionalTimeProvider(Func<float> timeProvider)
        {
            _timeProvider = timeProvider;
        }

        public void InjectOptionalHmd(IHmd hmd)
        {
            _hmd = hmd as UnityEngine.Object;
            Hmd = hmd;
        }

        #endregion

    }
}