using System;
using System.Numerics;
using Raylib_cs;

namespace PhysicsEngine
{
    public class Body
    {
        public Vector2 Position;
        public Vector2 Velocity;
        public Vector2 ForceAccumulator;
        public Color BaseColor = Color.Blue;
        public float Rotation;
        public float AngularVelocity;
        public float TorqueAccumulator;
        public float Mass;
        public float InverseMass => Mass > 0.0f ? 1.0f / Mass : 0.0f;
        public float MomentOfInertia;
        public float InverseInertia => MomentOfInertia > 0.0f ? 1.0f / MomentOfInertia : 0.0f;
        public float Restitution = 0.5f;
        public float Friction = 0.2f;
        public float DragCoefficient = 0.0f; // Drag per m^2 (ignored if 0.0)
        public bool IsStatic => Mass == 0.0f;

        public void ApplyForce(Vector2 force)
        {
            if (IsStatic) return;
            ForceAccumulator += force;
        }

        public void ApplyTorque(float torque)
        {
            if (IsStatic) return;
            TorqueAccumulator += torque;
        }

        public void ApplyImpulse(Vector2 impulse)
        {
            if (IsStatic) return;
            Velocity += impulse * InverseMass;
        }

        public void ApplyImpulseAtOffset(Vector2 impulse, Vector2 localOffset)
        {
            if (IsStatic) return;

            Velocity += impulse * InverseMass;

            float cos = MathF.Cos(Rotation);
            float sin = MathF.Sin(Rotation);
            Vector2 worldOffset = new Vector2(
                localOffset.X * cos - localOffset.Y * sin,
                localOffset.X * sin + localOffset.Y * cos
            );

            float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
            AngularVelocity += torque * InverseInertia;
        }

        public void ApplyImpulseAtWorldPosition(Vector2 impulse, Vector2 worldPosition)
        {
            if (IsStatic) return;

            Velocity += impulse * InverseMass;

            Vector2 worldOffset = worldPosition - Position;

            float torque = worldOffset.X * impulse.Y - worldOffset.Y * impulse.X;
            AngularVelocity += torque * InverseInertia;
        }

        public void ApplyForceAtWorldPosition(Vector2 force, Vector2 worldPosition)
        {
            if (IsStatic) return;

            ForceAccumulator += force;
            Vector2 worldOffset = worldPosition - Position;
            float torque = worldOffset.X * force.Y - worldOffset.Y * force.X;
            TorqueAccumulator += torque;
        }

        public Vector2 GetWorldPointFromLocal(Vector2 localPoint)
        {
            float cos = MathF.Cos(Rotation);
            float sin = MathF.Sin(Rotation);
            Vector2 rotated = new Vector2(
                localPoint.X * cos - localPoint.Y * sin,
                localPoint.X * sin + localPoint.Y * cos
            );
            return Position + rotated;
        }

        public Vector2 GetLocalPointFromWorld(Vector2 worldPoint)
        {
            Vector2 delta = worldPoint - Position;
            float cos = MathF.Cos(-Rotation);
            float sin = MathF.Sin(-Rotation);
            return new Vector2(
                delta.X * cos - delta.Y * sin,
                delta.X * sin + delta.Y * cos
            );
        }

        public void ClearForces()
        {
            ForceAccumulator = Vector2.Zero;
            TorqueAccumulator = 0.0f;
        }

        public virtual void Draw(float pixelsPerMeter = 100.0f) {}
        public virtual void ApplyAerodynamicDrag(float dt) {}
    }
}