using UnityEngine;
using System.Collections;

[AddComponentMenu("Kiz Custom/Rigidbody Character Controller")]
[RequireComponent(typeof(Rigidbody))]
[RequireComponent(typeof(Collider))]

public class PhysicsController : MonoBehaviour {
    Rigidbody RigidbodyComponent = null;
    CapsuleCollider CapsuleComponent = null;
    Transform CachedTransform = null;

    public float SlopeLimit = 45.0f;
    public Vector3 GravityDirection = new Vector3(0.0f, -1.0f, 0.0f);

    [HideInInspector]
    public bool IsGrounded = false;
    protected Vector3 movementAccumulator = Vector3.zero;
    protected Vector3 jumpAccumulator = Vector3.zero;
    protected Vector3 correctionAccumulator = Vector3.zero;
    public const float Gravity = 9.8f;
    protected float ReferenceCos;
    protected LayerMask ignoreLayer;

    public Vector3 Velocity {
        get { return RigidbodyComponent.velocity; }
        set { RigidbodyComponent.velocity = value; }
    }

    public Vector3 DesiredVelocity {
        get { return movementAccumulator; }
        set { movementAccumulator = value; }
    }

    public Vector3 JumpVelocity {
        get { return jumpAccumulator; }
        set { jumpAccumulator = value; }
    }

    void Start() {
        RigidbodyComponent = GetComponent<Rigidbody>();
        CapsuleComponent = GetComponent<CapsuleCollider>();
        CachedTransform = GetComponent<Transform>();

        if (RigidbodyComponent == null || CapsuleComponent == null) {
            if (RigidbodyComponent == null)
                Debug.LogError("[" + gameObject.name + "] CustomController: Rigidbody component required!");
            if (CapsuleComponent == null)
                Debug.LogError("[" + gameObject.name + "] CustomController: CapsuleCollider component required!");
        }

        RigidbodyComponent.freezeRotation = true;
        RigidbodyComponent.useGravity = false;
        RigidbodyComponent.isKinematic = false;
        CapsuleComponent.isTrigger = false;
        ReferenceCos = Mathf.Cos(SlopeLimit);

        ignoreLayer = 1 << LayerMask.NameToLayer("Player");
        ignoreLayer = ~ignoreLayer;
    }

    public void Move(Vector3 moveVector) {
        movementAccumulator += moveVector;
    }

    public void Jump(Vector3 jumpVector) {
        jumpAccumulator += jumpVector;
    }

    void FixedUpdate() {
        UpdateVelocity(movementAccumulator, jumpAccumulator);
        movementAccumulator = Vector3.zero;
        jumpAccumulator = Vector3.zero;
        IsGrounded = false;
    }

    void LateUpdate() {
        if (CachedTransform.up != -1.0f * GravityDirection)
            CachedTransform.up = -1.0f * GravityDirection;
    }

    void UpdateVelocity(Vector3 desiredVelocity, Vector3 jumpVelocity) {
        float Distance = CapsuleComponent.radius * 2.0f;
        RaycastHit hitInfo;

        if (Physics.Raycast(CachedTransform.position, desiredVelocity.normalized, out hitInfo, Distance, ignoreLayer))
            desiredVelocity = hitInfo.normal * hitInfo.distance;

        Vector3 VelocityDelta = desiredVelocity - RigidbodyComponent.velocity;
        Vector3 AntiGravity = GravityDirection * DotProduct(VelocityDelta, GravityDirection) * -1.0f;

        RigidbodyComponent.AddForce(VelocityDelta + AntiGravity + jumpVelocity, ForceMode.VelocityChange);
        RigidbodyComponent.AddForce(GravityDirection * Gravity * RigidbodyComponent.mass * Time.deltaTime, ForceMode.VelocityChange);
    }

    protected static Vector3 Vec3Mul(Vector3 v1, Vector3 v2) {
        return new Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
    }

    protected static float DotProduct(Vector3 v1, Vector3 v2) {
        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
    }

    void TrackGrounded(Collision collision) {
        foreach (ContactPoint contact in collision.contacts) {
            Vector3 LocalHitNormal = contact.normal;
            if (Vector3.Dot(CachedTransform.up, LocalHitNormal) <= ReferenceCos) {
                // TODO: MAYBE SLIDE?
            } else // if (contact.otherCollider.tag == "ground")
                IsGrounded = true;
        }
    }

    void OnCollisionExit(Collision collision) {
        IsGrounded = false;
    }

    void OnCollisionStay(Collision collision) {
        TrackGrounded(collision);
    }

    void OnCollisionEnter(Collision collision) {
        TrackGrounded(collision);
    }
}