using Unity.VisualScripting;
using UnityEngine;

public class RocketPhysics : MonoBehaviour
{
    [Header("References")]
    public MainEngine mainEngine;
    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
    
    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
        if (shouldUseUnityPhysics != useUnityPhysics)
        {
            TransitionPhysicsMode(shouldUseUnityPhysics);
        }
        
        if (useUnityPhysics)
        {
            UpdateWithUnityPhysics();
        }
        else
        {
            UpdateWithDoublePrecision();
        }
    }

    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);
        
        Vector3 thrustDirection = rb.rotation * Vector3.up;
        Vector3 thrustForce = thrustDirection * mainEngine.thrust;
        
        rb.AddForce(gravityForce, ForceMode.Force);
        rb.AddForce(thrustForce, ForceMode.Force);
        
        // 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 / rb.mass;

        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;

        velocity = velocity + totalAcceleration * Time.fixedDeltaTime;
        position = position + velocity * Time.fixedDeltaTime;

        // 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);
    }
}

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);
    }
}