using System;
using System.Numerics;
using Raylib_cs;

namespace PhysicsEngine
{
    public class Box : Body
    {
        public Vector2 Size;

        public Box(Vector2 position, Vector2 size, float mass)
        {
            Position = position;
            Size = size;
            Mass = mass;
            MomentOfInertia = (1.0f / 12.0f) * mass * (size.X * size.X + size.Y * size.Y);
        }

        public override void Draw(float pixelsPerMeter = 100.0f)
        {
            DrawHelper.DrawRotatedBox(Position, Size, Rotation, BaseColor, pixelsPerMeter);
        }

        private Vector2 RotatePoint(Vector2 point, float radians)
        {
            float cos = MathF.Cos(radians);
            float sin = MathF.Sin(radians);
            return new Vector2(
                point.X * cos - point.Y * sin,
                point.X * sin + point.Y * cos
            );
        }

        public override void ApplyAerodynamicDrag(float dt)
        {
            if (DragCoefficient <= 0.0f) return;

            const float airDensity = 1.225f;
            float speed = Velocity.Length();
            if (speed < 0.01f) return;

            Vector2 windDir = -Velocity / speed;

            float cos = MathF.Cos(Rotation);
            float sin = MathF.Sin(Rotation);

            Vector2 worldAxisY = new Vector2(-sin, cos); // Major axis vector
            Vector2 worldAxisX = new Vector2(cos, sin);  // Minor axis vector

            Vector2 windPerp = new Vector2(-windDir.Y, windDir.X);
            float projX = MathF.Abs(Vector2.Dot(worldAxisX, windPerp));
            float projY = MathF.Abs(Vector2.Dot(worldAxisY, windPerp));
            float effectiveWidth = Size.X * projX + Size.Y * projY;

            float dragMagnitude = 0.5f * airDensity * speed * speed * DragCoefficient * effectiveWidth;
            Vector2 dragForce = windDir * dragMagnitude;
            ApplyForce(dragForce);

            float cross = worldAxisY.X * windDir.Y - worldAxisY.Y * windDir.X;
            float restoringTorque = cross * 0.5f * airDensity * speed * speed * DragCoefficient * Size.X * Size.Y;
            ApplyTorque(restoringTorque);

            float angularSpeed = MathF.Abs(AngularVelocity);
            if (angularSpeed > 0.0001f)
            {
                float angularDragTorque = -MathF.Sign(AngularVelocity) * 0.5f * airDensity * angularSpeed * angularSpeed * DragCoefficient * (Size.X + Size.Y);
                ApplyTorque(angularDragTorque);
            }
        }
    }
}