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;

    // Start is called once before the first execution of Update after the MonoBehaviour is created
    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);
    }

    // Update is called once per frame
    void Update()
    {
        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.deltaTime;
        position = position + velocity * Time.deltaTime;

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