using System.Collections.Generic;
using UnityEngine;
using UnityEditor.Animations;
using LinearAlgebra;

namespace CreatureControl {

    public class CreatureAnimator : MonoBehaviour {
        public Creature creature;
        public RuntimeAnimatorController animatorController;
        private Animator animator;

        public enum RootMotionMode {
            Normal,
            Inverse
        };
        public RootMotionMode rootMotionMode = RootMotionMode.Normal;

        // This part may move to editor time only
        [System.Serializable]
        public class RootMotion {
            public string clipName;
            public Vector3 linearVelocity;
            public Vector3 angularVelocity;

            public readonly static RootMotion zero = new() {
                linearVelocity = Vector3.zero,
                angularVelocity = Vector3.zero
            };
        }

        public List<RootMotion> rootVelocities = new();
        public RootMotion GetRootMotion(string name) {
            return this.rootVelocities.Find(rm => rm.clipName == name);
        }
        // end of future editor-time 

        public Dictionary<Vector3, string> animationDirections = new();
        public float rootMotionScaleForward = 1;
        public float rootMotionScaleRight = 1;
        public float rootMotionScaleRotate = 1;

        public void CalculateRootMotionScale() {
            this.rootMotionScaleForward = 0;
            this.rootMotionScaleRight = 0;
            this.rootMotionScaleRotate = 0;
            // We may want to check for consistency:
            // rm scale for an axis should be the same for all animations
            foreach (RootMotion rootVelocity in this.rootVelocities) {
                if (IsDirectionCloseTo(Vector3.forward, rootVelocity.linearVelocity)) {
                    if (rootVelocity.linearVelocity.z > 0)
                        this.rootMotionScaleForward = rootVelocity.linearVelocity.z;
                }
                else if (IsDirectionCloseTo(Vector3.right, rootVelocity.linearVelocity)) {
                    if (rootVelocity.linearVelocity.x > 0)
                        this.rootMotionScaleRight = rootVelocity.linearVelocity.x;
                }
                else if (IsDirectionCloseTo(Vector3.up, rootVelocity.angularVelocity)) {
                    if (rootVelocity.angularVelocity.y > 0)
                        this.rootMotionScaleRotate = rootVelocity.angularVelocity.y;
                }
            }
        }

        private bool IsDirectionCloseTo(Vector3 direction, Vector3 vector, float threshold = 0.9f) {
            if (vector.sqrMagnitude == 0)
                return false;
            // Normalize the target vector to ensure comparisons are based on direction
            Vector3 normalizedTarget = vector.normalized;
            return Vector3.Dot(normalizedTarget, direction) > threshold;
        }

        protected virtual void Start() {
            this.creature = GetComponent<Creature>();
            this.animator = creature.animator; // GetComponent<Animator>();

            this.animator.applyRootMotion = this.rootMotionMode == RootMotionMode.Normal;
        }

        protected virtual void Update() {
            if (rootMotionMode == RootMotionMode.Inverse) {
                InverseRootMotionUpdate();
            }
            else {
                this.creature.targetRig.transform.GetPositionAndRotation(out Vector3 targetRigPosition, out Quaternion targetRigOrientation);
                this.creature.transform.SetPositionAndRotation(targetRigPosition, targetRigOrientation);
                this.creature.targetRig.transform.SetPositionAndRotation(targetRigPosition, targetRigOrientation);
            }
        }

        private Vector3 lastPosition;
        private Quaternion lastOrientation;
        protected void InverseRootMotionUpdate() {
            if (lastPosition.sqrMagnitude > 0) {
                Vector3 translation = this.transform.position - lastPosition;
                Vector3 worldVelocity = translation / Time.deltaTime;
                Vector3 localVelocity = transform.InverseTransformDirection(worldVelocity);

                float fwdAnimationSpeed = localVelocity.z / rootMotionScaleForward;
                this.animator.SetFloat("Forward", fwdAnimationSpeed);
                float rightAnimationSpeed = localVelocity.x / rootMotionScaleRight;
                this.animator.SetFloat("Right", rightAnimationSpeed);

                Quaternion rotation = this.transform.rotation * Quaternion.Inverse(lastOrientation);
                float rotationAngleY = Angles.Normalize(rotation.eulerAngles.y);
                float rotationSpeed = rotationAngleY / Time.deltaTime;
                float rotAnimationSpeed = rotationSpeed / rootMotionScaleRotate;
                this.animator.SetFloat("Rotation", rotAnimationSpeed);
            }
            lastPosition = this.transform.position;
            lastOrientation = this.transform.rotation;
        }

    }

}