using System.Collections;
using System.Collections.Generic;
using UnityEngine;


public class ColliderAddSlopeAdjustment : MonoBehaviour
{
    [Header("Steering Direction Config")]
    [Tooltip("A Transform that has an Axis that always shows into steering direction. It is assumed, that this transform is always on the floor!")]
    public Transform steerTransform;
    public Axis steerTransformForward = Axis.Z;
    public bool backwardIsForward = false;
    public int collisionLayer = 11;

    private const float RAYCAST_DIST = 0.2f;
    private float rayLength = 10f; //TODO: calculate -> front wheel


    // Start is called before the first frame update
    void Start()
    {
        
    }

    void FixedUpdate()
    {
        bool hit = AdjustSlope(); //Check above ground -> ascending
        if(!hit) AdjustSlope(underground: true); //Check underground -> descending
    }

    private bool AdjustSlope(bool underground = false)
    {
        var hitDistLower = CastRay(underground: underground);
        var hitDistUpper = CastRay(dist: RAYCAST_DIST, underground:underground);

        if (hitDistLower != null && hitDistUpper != null)
        {
            var slope = CalculateHitSlope(hitDistLower.Value, hitDistUpper.Value);
            //Debug.Log($"Hit - Slope = {(underground ? -slope : slope )}");
            return true;
        }

        return false;
    }

    //TODO: does only work for going up! We also need rays underground
    private float CalculateHitSlope(float hitDistLower, float hitDistUpper)
    {
        return Mathf.Atan(RAYCAST_DIST / Mathf.Abs(hitDistUpper - hitDistLower)) * Mathf.Rad2Deg;
    }

    private float? CastRay(float dist = 0f, bool underground = false)
    {
        var layerMask = 1 << collisionLayer;
        RaycastHit hit;
        var forward = CalculateForward();
        var position = steerTransform.position + Vector3.up * (dist - (underground ? 1 : 0)); // TODO: probably needs to be transformed to local axes
        // Does the ray intersect any objects excluding the player layer
        if (Physics.Raycast(position,forward , out hit, rayLength, layerMask))
        {
            Debug.DrawRay(position, forward * hit.distance, Color.yellow);
            if (hit.collider.isTrigger)
            {
                return null;
            }
            return hit.distance;
            //Debug.Log("Did Hit: ");
        }
        else
        {
            Debug.DrawRay(position, forward * rayLength, Color.white);
            //Debug.Log("Did not Hit");
        }

        return null;
    }

    private Vector3 CalculateForward()
    {
        Vector3 forward;
        switch (steerTransformForward)
        {
            case Axis.X:
                forward = steerTransform.right;
                break;
            case Axis.Y:
                forward = steerTransform.up;
                break;
            default:
                forward = steerTransform.forward;
                break;
        }

        return backwardIsForward ? -forward : forward;
    }
}