// SitpointVehiclePrototype.cs
using Simuspaces.Simulation;

public class SitpointVehiclePrototype : SimuScriptBase
{
    public float DriveForce = 100f;
    public float TurnTorque = 25f;
    public float UphillLift = 8f;
    public float MaxSpeed = 12f;

    private Rigidbody rb;

    public override void Start()
    {
        RegisterSitpointInput(OnSitpointInput);
    }

    private void OnSitpointInput(SVSitpointInputInfo input)
    {
        if (rb == null)
        {
            rb = gameObject.GetComponent<Rigidbody>();
            Log($"rb lazy={(rb != null ? "OK" : "NULL")}");

            if (rb != null)
            {
                rb.constraints = RigidbodyConstraints.FreezeRotationX | RigidbodyConstraints.FreezeRotationZ;
                rb.centerOfMass = new Vector3(0f, -0.5f, 0f);
                rb.angularDamping = 5f;
                rb.linearDamping = 0.5f;
                rb.maxAngularVelocity = 4f;
            }
        }

        if (rb == null)
            return;

        if (!IsFinite(rb.position) || !IsFinite(rb.rotation))
            return;

        float move = Mathf.Clamp(input.MoveY, -1f, 1f);
        float turn = Mathf.Clamp(input.MoveX, -1f, 1f);

        if (!IsFinite(move) || !IsFinite(turn))
            return;

        float yaw = rb.rotation.eulerAngles.y;
        if (!IsFinite(yaw))
            return;

        rb.MoveRotation(Quaternion.Euler(0f, yaw, 0f));

        Vector3 forward = Vector3.ProjectOnPlane(gameObject.transform.forward, Vector3.up);
        if (forward.sqrMagnitude < 0.001f)
            forward = Vector3.forward;

        forward.Normalize();

        float sprintMult = input.Sprint ? 1.8f : 1f;
        Vector3 horizontalVelocity = Vector3.ProjectOnPlane(rb.linearVelocity, Vector3.up);

        if (Mathf.Abs(move) > 0.01f)
        {
            if (horizontalVelocity.magnitude < MaxSpeed || Vector3.Dot(horizontalVelocity, forward * move) < 0f)
                rb.AddForce(forward * move * DriveForce * sprintMult, ForceMode.Acceleration);

            rb.AddForce(Vector3.up * UphillLift, ForceMode.Acceleration);
        }

        if (Mathf.Abs(turn) > 0.01f)
            rb.AddTorque(Vector3.up * turn * TurnTorque, ForceMode.Acceleration);
    }

    private bool IsFinite(Vector3 value)
    {
        return IsFinite(value.x) && IsFinite(value.y) && IsFinite(value.z);
    }

    private bool IsFinite(Quaternion value)
    {
        return IsFinite(value.x) && IsFinite(value.y) && IsFinite(value.z) && IsFinite(value.w);
    }

    private bool IsFinite(float value)
    {
        return !float.IsNaN(value) && !float.IsInfinity(value);
    }
}