using UnityEngine;

public class GimbalSystem : MonoBehaviour
{
    [Header("References")]
    public MainEngine mainEngine;
    public RocketPhysics rocketPhysics;
    public Rigidbody rb;

    [Header("Gimbal Settings")]
    [Range(0f, 20f)]
    public float maxGimbalAngle = 10f; // degrees - maximum deflection angle
    public float gimbalResponseRate = 30f; // degrees per second - how fast gimbal can move

    [Header("Input")]
    public float pitchInput = 0f; // -1 to 1 (negative = pitch down, positive = pitch up)
    public float yawInput = 0f; // -1 to 1 (negative = yaw left, positive = yaw right)
    public float rollInput = 0f; // -1 to 1 (optional roll control)

    [Header("Current Gimbal State")]
    public float currentPitchAngle = 0f; // degrees
    public float currentYawAngle = 0f; // degrees
    public Vector3 appliedTorque = Vector3.zero; // N⋅m

    [Header("Gimbal Physics")]
    public float gimbalLeverArm = 5f; // meters - distance from CoM to thrust vector application point
    
    [Header("Debug")]
    public bool showDebugInfo = false;

    void FixedUpdate()
    {
        UpdateGimbalAngles();
        CalculateGimbalTorque();
    }

    /// <summary>
    /// Updates the current gimbal angles based on input, respecting limits and response rates
    /// </summary>
    private void UpdateGimbalAngles()
    {
        // Calculate target angles from input
        float targetPitchAngle = pitchInput * maxGimbalAngle;
        float targetYawAngle = yawInput * maxGimbalAngle;

        // Smoothly move toward target angles at the response rate
        float maxDelta = gimbalResponseRate * Time.fixedDeltaTime;
        currentPitchAngle = Mathf.MoveTowards(currentPitchAngle, targetPitchAngle, maxDelta);
        currentYawAngle = Mathf.MoveTowards(currentYawAngle, targetYawAngle, maxDelta);

        // Enforce gimbal limits (safety clamp)
        currentPitchAngle = Mathf.Clamp(currentPitchAngle, -maxGimbalAngle, maxGimbalAngle);
        currentYawAngle = Mathf.Clamp(currentYawAngle, -maxGimbalAngle, maxGimbalAngle);
    }

    /// <summary>
    /// Calculates torque based on gimbal angles and engine thrust (does not apply it)
    /// RocketPhysics will read and apply this torque
    /// </summary>
    private void CalculateGimbalTorque()
    {
        // Only calculate torque if engine is producing thrust
        if (mainEngine == null || !mainEngine.engineIgnited || mainEngine.thrust <= 0f)
        {
            appliedTorque = Vector3.zero;
            return;
        }

        // Convert gimbal angles to radians for calculation
        float pitchRad = currentPitchAngle * Mathf.Deg2Rad;
        float yawRad = currentYawAngle * Mathf.Deg2Rad;

        // Calculate perpendicular force components from gimbal deflection
        // For small angles: F_perp ≈ F_thrust × sin(angle) ≈ F_thrust × angle_rad
        // Using full sin for accuracy at larger gimbal angles
        float pitchForce = mainEngine.thrust * 1000f * Mathf.Sin(pitchRad); // Convert kN to N
        float yawForce = mainEngine.thrust * 1000f * Mathf.Sin(yawRad);

        // Calculate torque: τ = F × r (cross product, but simplified for perpendicular forces)
        // Pitch gimbal creates torque around the local right axis (X)
        // Yaw gimbal creates torque around the local forward axis (Z)
        // Torque directions: positive pitch input → nose up, positive yaw input → nose right
        Vector3 localTorque = new Vector3(
            pitchForce * gimbalLeverArm,   // Pitch torque around X axis
            0f,                             // No direct Y torque from gimbal
            -yawForce * gimbalLeverArm     // Yaw torque around Z axis (negative for correct direction)
        );

        // Convert local torque to world space
        appliedTorque = rb.rotation * localTorque;

        if (showDebugInfo)
        {
            Debug.Log($"[GimbalSystem] Pitch: {currentPitchAngle:F2}°, Yaw: {currentYawAngle:F2}°, Torque: {appliedTorque.magnitude:F2} N⋅m");
        }
    }

    /// <summary>
    /// Sets the pitch input (-1 to 1)
    /// </summary>
    public void SetPitchInput(float value)
    {
        pitchInput = Mathf.Clamp(value, -1f, 1f);
    }

    /// <summary>
    /// Sets the yaw input (-1 to 1)
    /// </summary>
    public void SetYawInput(float value)
    {
        yawInput = Mathf.Clamp(value, -1f, 1f);
    }

    /// <summary>
    /// Sets the roll input (-1 to 1) - for future use with RCS or other control surfaces
    /// </summary>
    public void SetRollInput(float value)
    {
        rollInput = Mathf.Clamp(value, -1f, 1f);
    }

    /// <summary>
    /// Resets gimbal to neutral position
    /// </summary>
    public void ResetGimbal()
    {
        pitchInput = 0f;
        yawInput = 0f;
        rollInput = 0f;
    }
}