using Unity.VisualScripting;
using UnityEngine;

public class RocketPhysics : MonoBehaviour
{
    [Header("References")]
    public MainEngine mainEngine;
    public GimbalSystem gimbalSystem;
    public RCSController rcsController;
    public Rigidbody rb;
    public Altimeter altimeter;

    [Header("Data")]
    public DoublePrecisionVector3 position;
    public DoublePrecisionVector3 velocity;
    public DoublePrecisionVector3 rotation;

    [Header("Physics Mode Settings")]
    public float physicsTransitionAltitude = 1000f; // meters - switch to Unity physics below this
    public bool useUnityPhysics = true; // current physics mode
    public bool isFrozen = false; // when true, freezes rocket physics (thrust arrow still works)
    
    [Header("Visualization")]
    public bool showThrustArrow = true;
    public float thrustArrowScale = 0.0001f; // scales thrust magnitude for arrow length
    public float thrustArrowMaxLength = 100f; // prevents arrow from being too long
    
    private Vector3 lastPhysicsPosition;

    void Start()
    {
        position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
        velocity = new DoublePrecisionVector3(rb.linearVelocity.x, rb.linearVelocity.y, rb.linearVelocity.z);
        rotation = new DoublePrecisionVector3(rb.rotation.eulerAngles.x, rb.rotation.eulerAngles.y, rb.rotation.eulerAngles.z);
        
        rb.useGravity = false;
        lastPhysicsPosition = rb.position;
    }

    void FixedUpdate()
    {
        // Determine which physics mode to use
        bool shouldUseUnityPhysics = altimeter.altitude < physicsTransitionAltitude;
        
        // Handle mode transition (only if not frozen)
        if (!isFrozen && shouldUseUnityPhysics != useUnityPhysics)
        {
            TransitionPhysicsMode(shouldUseUnityPhysics);
        }
        
        // Always calculate physics, but only apply if not frozen
        if (useUnityPhysics)
        {
            UpdateWithUnityPhysics();
        }
        else
        {
            UpdateWithDoublePrecision();
        }
        
        // Draw thrust visualization (works even when frozen)
        if (showThrustArrow)
        {
            DrawThrustArrow();
        }
    }

    void TransitionPhysicsMode(bool toUnityPhysics)
    {
        if (toUnityPhysics)
        {
            // Transitioning to Unity physics (descending)
            rb.isKinematic = false;
            rb.position = new Vector3((float)position.x, (float)position.y, (float)position.z);
            rb.linearVelocity = new Vector3((float)velocity.x, (float)velocity.y, (float)velocity.z);
            lastPhysicsPosition = rb.position;
        }
        else
        {
            // Transitioning to double precision (ascending)
            rb.isKinematic = true;
            position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
            velocity = new DoublePrecisionVector3(rb.linearVelocity.x, rb.linearVelocity.y, rb.linearVelocity.z);
        }
        
        useUnityPhysics = toUnityPhysics;
        Debug.Log($"Physics mode switched to: {(useUnityPhysics ? "Unity Physics" : "Double Precision")}");
    }

    void UpdateWithUnityPhysics()
    {
        // Use Unity's physics engine - allows ground interaction
        double gravityAcceleration = altimeter.gh;
        Vector3 gravityForce = new Vector3(0, (float)(-gravityAcceleration * rb.mass), 0);
        
        // Thrust force (converted from kN to N)
        Vector3 thrustDirection = rb.rotation * Vector3.up;
        Vector3 thrustForce = thrustDirection * mainEngine.thrust * 1000f;
        
        if (!isFrozen)
        {
            // Apply forces only when not frozen
            rb.AddForce(gravityForce, ForceMode.Force);
            rb.AddForce(thrustForce, ForceMode.Force);
            
            Vector3 totalTorque = Vector3.zero;
            if (gimbalSystem != null)
            {
                totalTorque += gimbalSystem.appliedTorque;
            }
            if (rcsController != null)
            {
                totalTorque += rcsController.appliedTorque;
            }
            if (totalTorque.sqrMagnitude > 0f)
            {
                rb.AddTorque(totalTorque, ForceMode.Force);
            }
        }
        else
        {
            // When frozen, manually calculate what velocity would be
            Vector3 totalForce = gravityForce + thrustForce;
            Vector3 acceleration = totalForce / rb.mass;
            Vector3 newVelocity = rb.linearVelocity + acceleration * Time.fixedDeltaTime;
            velocity = new DoublePrecisionVector3(newVelocity.x, newVelocity.y, newVelocity.z);
            // Position stays the same when frozen
            position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
            rotation = new DoublePrecisionVector3(rb.rotation.eulerAngles.x, rb.rotation.eulerAngles.y, rb.rotation.eulerAngles.z);
            return;
        }
        
        // Update double precision tracking from Unity physics
        position = new DoublePrecisionVector3(rb.position.x, rb.position.y, rb.position.z);
        velocity = new DoublePrecisionVector3(rb.linearVelocity.x, rb.linearVelocity.y, rb.linearVelocity.z);
        rotation = new DoublePrecisionVector3(rb.rotation.eulerAngles.x, rb.rotation.eulerAngles.y, rb.rotation.eulerAngles.z);
    }

    void UpdateWithDoublePrecision()
    {
        // Manual double-precision integration - no ground interaction
        double gravityAcceleration = altimeter.gh;
        double thrustAcceleration = (mainEngine.thrust * 1000.0) / rb.mass; // Convert kN to N

        Vector3 thrustDirection = rb.rotation * Vector3.up;
        DoublePrecisionVector3 gravityAccelerationVector = new DoublePrecisionVector3(0, -gravityAcceleration, 0);
        DoublePrecisionVector3 thrustAccelerationVector = new DoublePrecisionVector3(
            thrustDirection.x * thrustAcceleration,
            thrustDirection.y * thrustAcceleration,
            thrustDirection.z * thrustAcceleration
        );
        DoublePrecisionVector3 totalAcceleration = gravityAccelerationVector + thrustAccelerationVector;

        // Always calculate new velocity and position
        velocity = velocity + totalAcceleration * Time.fixedDeltaTime;
        position = position + velocity * Time.fixedDeltaTime;

        // Only apply to rigidbody if not frozen
        if (!isFrozen)
        {
            Vector3 totalTorque = Vector3.zero;
            if (gimbalSystem != null)
            {
                totalTorque += gimbalSystem.appliedTorque;
            }
            if (rcsController != null)
            {
                totalTorque += rcsController.appliedTorque;
            }
            if (totalTorque.sqrMagnitude > 0f)
            {
                ApplyManualTorque(totalTorque);
            }

            // Update Unity transform (kinematic mode)
            Vector3 newPosition = new Vector3((float)position.x, (float)position.y, (float)position.z);
            rb.position = newPosition;
            // Do not set linearVelocity for kinematic bodies
            // rb.linearVelocity = new Vector3((float)velocity.x, (float)velocity.y, (float)velocity.z);
            rb.rotation = Quaternion.Euler((float)rotation.x, (float)rotation.y, (float)rotation.z);
        }
    }

    /// <summary>
    /// Manually applies torque for double-precision physics mode
    /// </summary>
    private void ApplyManualTorque(Vector3 torque)
    {
        // Get moment of inertia tensor
        Vector3 inertiaTensor = rb.inertiaTensor;
        if (inertiaTensor == Vector3.zero)
        {
            // Fallback if inertia tensor not set - use a simplified calculation
            float I = rb.mass * 25f; // Simplified inertia estimate
            inertiaTensor = new Vector3(I, I, I);
        }

        // Calculate angular acceleration: α = τ / I (in local space)
        Vector3 localTorque = Quaternion.Inverse(rb.rotation) * torque;
        Vector3 angularAcceleration = new Vector3(
            inertiaTensor.x > 0 ? localTorque.x / inertiaTensor.x : 0,
            inertiaTensor.y > 0 ? localTorque.y / inertiaTensor.y : 0,
            inertiaTensor.z > 0 ? localTorque.z / inertiaTensor.z : 0
        );

        // Integrate angular velocity (simplified Euler integration)
        Vector3 angularDisplacement = angularAcceleration * Time.fixedDeltaTime * Time.fixedDeltaTime * Mathf.Rad2Deg;
        
        // Update rotation
        Quaternion deltaRotation = Quaternion.Euler(angularDisplacement);
        rb.rotation = rb.rotation * deltaRotation;

        // Update rotation tracking
        rotation.x = rb.rotation.eulerAngles.x;
        rotation.y = rb.rotation.eulerAngles.y;
        rotation.z = rb.rotation.eulerAngles.z;
    }

    /// <summary>
    /// Draws a debug arrow showing the direction and magnitude of thrust
    /// </summary>
    private void DrawThrustArrow()
    {
        // Base thrust direction
        Vector3 thrustDirection = Vector3.up;
        
        // Apply gimbal deflection if gimbal system is available
        if (gimbalSystem != null)
        {
            // Apply pitch rotation around local X axis (right)
            Quaternion pitchRotation = Quaternion.AngleAxis(gimbalSystem.currentPitchAngle, Vector3.right);
            thrustDirection = pitchRotation * thrustDirection;
            
            // Apply yaw rotation around local Z axis (forward)
            Quaternion yawRotation = Quaternion.AngleAxis(-gimbalSystem.currentYawAngle, Vector3.forward);
            thrustDirection = yawRotation * thrustDirection;
        }
        
        // Transform to world space
        thrustDirection = rb.rotation * thrustDirection;
        
        float thrustMagnitude = mainEngine.thrust * 1000f; // Convert kN to N
        
        // Scale the arrow length proportionally but cap it
        float arrowLength = Mathf.Min(thrustMagnitude * thrustArrowScale, thrustArrowMaxLength);
        
        if (arrowLength > 0.1f) // Only draw if there's meaningful thrust
        {
            Vector3 arrowEnd = rb.position + thrustDirection * arrowLength;
            
            // Draw main arrow line
            Debug.DrawLine(rb.position, arrowEnd, Color.green, 0f);
            
            // Draw arrowhead (two lines forming a V)
            Vector3 arrowHeadSize = thrustDirection * (arrowLength * 0.15f);
            Vector3 perpendicular1 = Vector3.Cross(thrustDirection, Vector3.up).normalized * (arrowLength * 0.1f);
            if (perpendicular1.magnitude < 0.01f) // Handle case where thrust is vertical
            {
                perpendicular1 = Vector3.Cross(thrustDirection, Vector3.right).normalized * (arrowLength * 0.1f);
            }
            Vector3 perpendicular2 = Vector3.Cross(thrustDirection, perpendicular1).normalized * (arrowLength * 0.1f);
            
            Debug.DrawLine(arrowEnd, arrowEnd - arrowHeadSize + perpendicular1, Color.green, 0f);
            Debug.DrawLine(arrowEnd, arrowEnd - arrowHeadSize - perpendicular1, Color.green, 0f);
        }
    }
}

public class DoublePrecisionVector3
{
    public double x;
    public double y;
    public double z;

    public DoublePrecisionVector3(double x, double y, double z)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public static DoublePrecisionVector3 operator +(DoublePrecisionVector3 a, DoublePrecisionVector3 b)
    {
        return new DoublePrecisionVector3(a.x + b.x, a.y + b.y, a.z + b.z);
    }

    public static DoublePrecisionVector3 operator *(DoublePrecisionVector3 a, double d)
    {
        return new DoublePrecisionVector3(a.x * d, a.y * d, a.z * d);
    }
}