using UnityEngine;

public class RCSController : MonoBehaviour
{
    [Header("References")]
    public Rigidbody rb;
    public FuelTank fuelTank;

    [Header("RCS Settings")]
    public bool rcsEnabled = true;
    public bool consumeFuel = true;
    public float maxTorque = 2000f; // N*m
    public float torqueResponseRate = 6000f; // N*m per second
    public float leverArm = 2f; // meters
    public float Isp = 250f; // seconds
    public float inputDeadzone = 0.05f;

    [Header("Input")]
    public Vector3 controlInput; // x = yaw, y = pitch, z = roll (-1 to 1)

    [Header("Output")]
    public Vector3 appliedTorque = Vector3.zero; // world space
    public float fuelFlowRate = 0f; // kg/s

    private const float g0 = 9.80665f;
    private Vector3 currentLocalTorque = Vector3.zero;

    void FixedUpdate()
    {
        if (!rcsEnabled || rb == null)
        {
            appliedTorque = Vector3.zero;
            currentLocalTorque = Vector3.zero;
            fuelFlowRate = 0f;
            return;
        }

        Vector3 clampedInput = new Vector3(
            Mathf.Abs(controlInput.x) < inputDeadzone ? 0f : Mathf.Clamp(controlInput.x, -1f, 1f),
            Mathf.Abs(controlInput.y) < inputDeadzone ? 0f : Mathf.Clamp(controlInput.y, -1f, 1f),
            Mathf.Abs(controlInput.z) < inputDeadzone ? 0f : Mathf.Clamp(controlInput.z, -1f, 1f)
        );

        float pitchInput = clampedInput.y;
        float yawInput = clampedInput.x;
        float rollInput = clampedInput.z;

        Vector3 targetLocalTorque = new Vector3(pitchInput, yawInput, rollInput) * maxTorque;
        float maxDelta = torqueResponseRate * Time.fixedDeltaTime;
        currentLocalTorque = Vector3.MoveTowards(currentLocalTorque, targetLocalTorque, maxDelta);

        Vector3 fueledLocalTorque = ApplyFuelLimit(currentLocalTorque);
        appliedTorque = rb.rotation * fueledLocalTorque;
    }

    private Vector3 ApplyFuelLimit(Vector3 desiredLocalTorque)
    {
        fuelFlowRate = 0f;
        if (!consumeFuel || fuelTank == null)
        {
            return desiredLocalTorque;
        }

        if (leverArm <= 0f || Isp <= 0f)
        {
            return desiredLocalTorque;
        }

        float torqueMagnitude = desiredLocalTorque.magnitude;
        if (torqueMagnitude <= 0f)
        {
            return Vector3.zero;
        }

        float requiredForce = torqueMagnitude / leverArm;
        fuelFlowRate = requiredForce / (Isp * g0);
        float fuelRequested = fuelFlowRate * Time.fixedDeltaTime;
        float fuelProvided = fuelTank.RequestRcsFuel(fuelRequested);

        if (fuelProvided < fuelRequested && fuelRequested > 0f)
        {
            float scale = Mathf.Clamp01(fuelProvided / fuelRequested);
            return desiredLocalTorque * scale;
        }

        return desiredLocalTorque;
    }

    public void SetControlInput(Vector3 input)
    {
        controlInput = input;
    }
}