using Framework.Debugging;
using Framework.Entities;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UnityEngine;
using Framework.Extensions;

namespace Framework.ActorComponents
{
    /// <summary>
    /// This base component setups up the mechanics for moving an object around in the world
    /// while using the physics engine. It should be extended to create complex functionality 
    /// such as defining how legs walk or wings fly. Anything implementing this should have a 
    /// lower script execution order than any other components that are commanding it.
    /// </summary>
    [RequireComponent(typeof(Rigidbody))]
    [AddComponentMenu("Scripts/Framework/Actor Components/Base/Physics Mover")]
    public class PhysicsMover : CoreBehaviour
    {
        public bool onGround = false; // Is this behaviour on the ground
        //public bool byWall = false;
        public float groundScanDistance = 0.4f; // The distance at which to accept a collision point contact as ground measured down in the relative y direction to this behaviour
        public int groundMemoryCount = 5; // The number of ground positions to store
        public bool oppositeGroundForce = true;
        
        public Vector3 groundVelocity; // The velocity of the ground itself, used to keep the object moving relative to the thing it stands on
        private Vector3? lastGroundTransformPosition; // The last position that the ground behaviour transform was known to be at
        private float lastGroundTime; // The last time that the ground was touched
        private GameObject lastGroundGameObject; // The last known ground behaviour that was touched
        private List<Vector3> groundPositions = new List<Vector3>(); // A list of stored ground positions that can be used to trace where an object has walked so that it can return to that point when it falls off the ground
        

        /// <summary>
        /// Gets the ground relative velocity of this behaviour, i.e. the velocity at which this
        /// entity is moving relative to the object it is standing on. Will be Vector3.zero if not
        /// on the ground.
        /// </summary>
        /// <returns></returns>
        public Vector3 GetGroundRelativeVelocity()
        {
            return this.GetComponent<Rigidbody>().velocity - this.groundVelocity;
        }

        /// <summary>
        /// Called when this behaviour is colliding with anothe game object.
        /// </summary>
        /// <param name="collision">The collision.</param>
        protected void OnCollisionStay(Collision collision)
        {
            // If we have a contact point
            if (collision.contacts.Length == 0)
            {
                return;
            }

            // Calculate relative ground scan and ground contact point y values
            // Not used: float relativeYVelocity = Vector3.Dot(this.rigidbody.velocity + this.groundVelocity, this.transform.up);
            float relativeYGroundScan = Vector3.Dot(this.transform.position - (this.transform.up * this.groundScanDistance), this.transform.up);

            float relativeYContactPoint = Vector3.Dot(collision.contacts[0].point, this.transform.up);

            // If the contact is closer than the defined groundScanDistance set the object to grounded
            if (relativeYGroundScan >= relativeYContactPoint)
            {
                this.onGround = true;

                // Update the ground velocity
                CalculateGroundVelocity(collision);

                // Update the ground position
                UpdateGroundPositions();
            }
            else 
            {
                //this.onGround = false; // HACK, should this be here? Causes issues with multiple colliders
            }
        }

        private void CalculateGroundVelocity(Collision collision)
        {
            // Get the velocity of the ground
            if (this.lastGroundTransformPosition != null && this.lastGroundGameObject == collision.gameObject)
            {
                this.groundVelocity = ((Vector3)(collision.transform.position - this.lastGroundTransformPosition)) / (Time.time - this.lastGroundTime);
                //this.groundVelocity = collision.rigidbody.velocity;
            }
            else
            {
                this.groundVelocity = Vector3.zero;
            }
            this.lastGroundTransformPosition = collision.transform.position;
            this.lastGroundGameObject = collision.gameObject;
            this.lastGroundTime = Time.time;
        }

        private void UpdateGroundPositions()
        {
            if (this.groundPositions.Count > 0 && Vector3.Distance(this.groundPositions[this.groundPositions.Count - 1], this.transform.position) <= 0.5f)
            {
                return;
            }
            // Add the ground position to our historical reference
            this.groundPositions.Add(this.transform.position);

            // Remove ground positions if we have too many
            if (this.groundPositions.Count > this.groundMemoryCount)
            {
                this.groundPositions.RemoveAt(0);
            }
        }

        /// <summary>
        /// Called when this behaviour leaves a collision with another object's collider.
        /// Resets the on ground parameters.
        /// </summary>
        /// <param name="collisionInfo">The collision info.</param>
        protected void OnCollisionExit(Collision collision)
        {
            this.onGround = false;
            //this.byWall = false;
            this.lastGroundTransformPosition = null;
            this.groundVelocity = Vector3.zero;
        }

        /// <summary>
        /// Moves toward the given position with the given speed and acceleration force.
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="speed">The speed.</param>
        protected void MoveToward(Vector3 position, float speed, float force)
        {
            MoveToward(position, speed, force, Vector3.one);
        }

        /// <summary>
        /// Moves toward the given position with the given speed and acceleration force. The axisMask
        /// parameter is used to mask out certain axes, for example a mask of 1,0,1 will reduce all
        /// relative y movement to zero causing the object to only move toward the point in its own
        /// x/z plane. 
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="axisMask">The axis mask.</param>
        /// <param name="speed">The speed.</param>
        protected void MoveToward(Vector3 position, float speed, float force, Vector3 axisMask)
        {
            Draw.Axis(position, Color.red);
            Vector3 moveVector = (position - this.transform.position).normalized;
            ChangeVelocity(moveVector * speed, force, axisMask);
        }

        /// <summary>
        /// Leaps toward the given position with the given force.
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="force">The force.</param>
        protected void LeapToward(Vector3 position, float force)
        {
            this.GetComponent<Rigidbody>().AddForce((position - this.transform.position).normalized * force);
        }

        /// <summary>
        /// Stops this behaviour with the given force.
        /// </summary>
        /// <param name="force">The force.</param>
        protected void Stop(float force)
        {
            if (GetGroundRelativeVelocity().magnitude > 0)
            {
                ChangeVelocity(Vector3.zero, force);
            }
        }

        /// <summary>
        /// Stops this behaviour with the given force. The axisMask to mask out certain axes, for 
        /// example a mask of 1,0,1 will reduce all relative y movement to zero causing the object 
        /// to only stop in its own x/z plane. 
        /// </summary>
        /// <param name="force">The force.</param>
        protected void Stop(float force, Vector3 axisMask)
        {
            if (GetGroundRelativeVelocity().magnitude > 0)
            {
                ChangeVelocity(Vector3.zero, force, axisMask);
            }
        }

        /// <summary>
        /// Changes the velocity of this behaviour towards the target velocity by the given force.
        /// </summary>
        /// <param name="targetVelocity">The target velocity.</param>
        /// <param name="force">The force.</param>
        protected void ChangeVelocity(Vector3 targetVelocity, float force)
        {
            ChangeVelocity(targetVelocity, force, Vector3.one);
        }

        /// <summary>
        /// Changes the velocity of this behaviour towards the target velocity by the given force.
        /// </summary>
        /// <param name="targetVelocity">The target velocity.</param>
        /// <param name="force">The force.</param>
        /// <param name="axisMask">The axis mask.</param>
        protected void ChangeVelocity(Vector3 targetVelocity, float force, Vector3 axisMask)
        {
            Vector3 velocityDifference = ((targetVelocity + this.groundVelocity) - this.GetComponent<Rigidbody>().velocity);
            Vector3 projectedVelocityDifference = velocityDifference.ProjectRelative(this.transform, axisMask);
            this.GetComponent<Rigidbody>().AddForce(projectedVelocityDifference * force);

            // Exert force on the ground opposite to the movement force above
            if (this.oppositeGroundForce && this.onGround && this.lastGroundGameObject.GetComponent<Rigidbody>() != null)
            {
                this.lastGroundGameObject.GetComponent<Rigidbody>().AddForce(-projectedVelocityDifference * force);
            }
        }

        /// <summary>
        /// Turns the toward the given position with given turn speed. The axisMask to mask out certain axes, for 
        /// example a mask of 1,0,1 will reduce all relative y rotation to zero causing the object 
        /// to only turn in its own x/z plane (or around its y axis).
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="turnSpeed">The turn speed.</param>
        /// <param name="axisMask">The axis mask.</param>
        protected void TurnToward(Vector3 position, float turnSpeed, Vector3 axisMask)
        {
            Vector3 targetVector = position - this.transform.position;
            Vector3 projectedTargetVector = targetVector.ProjectRelative(this.transform, axisMask);
            Draw.Line(this.transform.position, this.transform.position + projectedTargetVector, Color.red);
            if (Vector3.Distance(projectedTargetVector, Vector3.zero) > 0.5f)
            {
                Vector3 targetAngles = Quaternion.LookRotation(projectedTargetVector, this.transform.up).eulerAngles;
                this.transform.rotation = Quaternion.Slerp(this.transform.rotation, Quaternion.Euler(targetAngles) * Quaternion.Euler(0, 0, 0), turnSpeed * Time.deltaTime);
            }
        }

        protected new void Log(string message)
        {
            Debug.Log(message);
        }
    }
}
