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

public class CustomWheelColliderSlopeAdjustment : MonoBehaviour
{
    public CustomWheelCollider frontWheel;
    public CustomWheelCollider rearWheel;
    public Transform rotatePoint;
    public float timeDifferenceTolerance = 0.0001f;

    public Vector3 dif; //TODO: remove

    private Transform t;

    private bool isHitting = false;
    private float angle = 0f;

    private void OnGUI()
    {
        GUI.TextField(new Rect(Screen.width - 200, 10, 190, 80), $"Hitting? {isHitting}\nAngle: {angle:n2}\nrw: {rearWheel.CurrentHit?.Hit.point}\nfw: {frontWheel.CurrentHit?.Hit.point}");
    }

    private void Start()
    {
        t = transform;
    }

    private void FixedUpdate()
    {
        isHitting = false;

        if (frontWheel.CurrentHitNumber > 1)
        {
            angle = -.5f;
            Rotate();
            return;
        }
        if (rearWheel.CurrentHitNumber > 1)
        {
            angle = .5f;
            Rotate();
            return;
        }
        
        //TODO: this is probably called almost never
        if (frontWheel.CurrentHit == null || rearWheel.CurrentHit == null) return;
            
        var fwHit = frontWheel.CurrentHit.Value;
        var rwHit = rearWheel.CurrentHit.Value;

        if (fwHit.Timestamp - rwHit.Timestamp <= timeDifferenceTolerance)
        {
            UpdateTransformRotation(fwHit, rwHit);
        }
    }

    private void UpdateTransformRotation(HitInfo fwHit, HitInfo rwHit)
    {
        isHitting = true;
        dif = t.TransformPoint(fwHit.Hit.point) - t.TransformPoint(rwHit.Hit.point);
        angle = -Mathf.Atan(dif.y / dif.z) * Mathf.Rad2Deg; //TODO: is it that easy?
        
        Debug.DrawLine(rwHit.Hit.point, rwHit.Hit.point + t.right, Color.red);
        
        Rotate();
        //t.RotateAround(rotatePoint.position, t.right, angle) ;
        //t.localRotation = Quaternion.Euler(t.localRotation.eulerAngles + Vector3.right * angle);
    }

    private void Rotate()
    {
        t.RotateAround(rotatePoint.position, t.right, angle) ;
        angle = 0f;
    }
}