using UnityEngine;
using System.Collections;
using CharacterEngine.Controller;

namespace CharacterEngine.Primitives
{
    public sealed class CEPrimitives
    {
        #region Controller Globals 

        public const int sweepResolution = 3;

        private static float gravity = -8.0f;
        public static float Gravity
        {
            get { return gravity; }
            set { gravity = value; }
        }

        #endregion

        #region Controller Variables

        private CEController controller;
        public readonly CECollider collider;

        private Vector3 velocity;
        public Vector3 Velocity
        {
            get { return velocity; }
        }

        public Transform transform
        {
            get { return controller.transform; }
        }

        #endregion

        #region Initializers & Constructors

        public CEPrimitives (CEController controller)
        {
            this.controller = controller;
            collider = new CECollider(controller.GetComponent<Collider>());
        }

        #endregion

        #region Steps

        /*Summary:
            Here in steps, there are the methods called in the UPDATES, that means that every step
            will be executed every frame.
        */

        public void Step ()
        {
            PhysicsStep();
        }

        void PhysicsStep()
        {
            GetVelocity();

            GravityStep();
            CollisionStep();

            ApplyVelocity();
            Move();
        }

        void GetVelocity()
        {

        }

        void GravityStep()
        {
            velocity.y += gravity * controller.weight * Time.deltaTime * 1.2f;
            velocity.x = -gravity * 0.5f;
        }

        void CollisionStep()
        {
            for (int i = 0; i < 5; i++)
                if (!CollideAndSlide()) break;
        }

        bool CollideAndSlide()
        {
            Vector3 oldPos = transform.position;
            Vector3 predictedPos = oldPos + velocity * Time.fixedDeltaTime;
            Vector3 correctedPos = predictedPos;

            RaycastHit hit = collider.SweepTest(velocity, velocity.magnitude * Time.fixedDeltaTime, controller.skinWidth);

            if (hit.collider == null) { return false; }

            Vector3 obstacleHitPoint = hit.point;
            Vector3 obstacleNormal = hit.normal;
            Vector3 obstacleTangent = Vector3.Cross(obstacleNormal, Vector3.forward);
            if (obstacleTangent.magnitude == 0)
                obstacleTangent = Vector3.Cross(obstacleNormal, Vector3.up);

            /*
            Plane obstaclePlane = new Plane(obstacleNormal, obstacleHitPoint);
            Ray velRay = new Ray(oldPos, velocity);
            float distanceFromPlane = 0.0f;

            obstaclePlane.Raycast(velRay, out distanceFromPlane);
            Vector3 vectorFromCenter = obstacleHitPoint - Coll.bounds.center ;
            */

            correctedPos = oldPos + velocity.normalized * hit.distance;

            /*
            GameObject a = Instantiate(this.gameObject, correctedPos, transform.rotation) as GameObject;
            a.GetComponent<EntityController>().enabled = false;
           */

            Vector3 epsilon = correctedPos - predictedPos;

            /*
            Vector3 normalForce = Vector3.Dot(obstacleNormal, oldVel) * obstacleNormal;

            newVel = oldVel - normalForce;
            //newVel = (oldVel.magnitude / newVel.magnitude) * newVel;
            */

            Vector3 projectedForce = Vector3.Project(velocity, obstacleTangent);

            velocity = projectedForce;
            velocity.z = 0.0f;

            return true;
        }

        void ApplyVelocity()
        {
            Debug.DrawRay(transform.position, velocity);
        }

        void Move()
        {
            transform.position += velocity * Time.deltaTime;
        }

        #endregion
    }
}