#include "MovementIntentSystem.h"

#include <algorithm>
#include <cmath>

#include <QVector2D>

#include "DynamicBodyComponent.h"
#include "EntityAdmin.h"
#include "FacingComponent.h"
#include "MovementIntentComponent.h"
#include "OrbitMath.h"
#include "PositionComponent.h"
#include "tracing.h"

static float wrapAngle(float a)
{
    constexpr float kPi = 3.14159265f;
    a = std::fmod(a, 2.0f * kPi);
    if (a >  kPi) { a -= 2.0f * kPi; }
    if (a < -kPi) { a += 2.0f * kPi; }
    return a;
}

void MovementIntentSystem::tick(EntityAdmin& admin)
{
	TRACE();
    admin.forEach<PositionComponent, FacingComponent, DynamicBodyComponent,
                  MovementIntentComponent>(
        [](entt::entity /*e*/, const PositionComponent& pos, const FacingComponent& facing,
           DynamicBodyComponent& body, const MovementIntentComponent& intent)
        {
            if (!intent.active)
            {
                // No movement intent: brake using available thrust.
                const float linearBraking = std::min(body.velocity_tpt.length(),
                                                     body.maneuveringAcceleration_tptt);
                body.linearAcceleration_tptt = (body.velocity_tpt.length() > 0.0001f)
                    ? -body.velocity_tpt.normalized() * linearBraking
                    : QVector2D(0.0f, 0.0f);

                const float angBraking = std::min(std::abs(body.angularVelocity_rpt),
                                                  body.maxAngularAcceleration_rptt);
                body.angularAcceleration_rptt =
                    (body.angularVelocity_rpt >= 0.0f) ? -angBraking : angBraking;
                return;
            }

            // Resolve the steering destination. For orbit intents, pick the orbit
            // sense from the ship's current velocity (so ships circling the same
            // target spread to both sides) and aim at a point on the orbit circle.
            QVector2D destination = intent.target;
            if (intent.orbitRadius_tiles > 0.0f)
            {
                const float sign = OrbitMath::resolveOrbitSign(
                    pos.value, intent.target, body.velocity_tpt,
                    intent.orbitCenterVelocity_tpt);
                destination = OrbitMath::computeOrbitDestination(
                    pos.value, intent.target, intent.orbitRadius_tiles, sign);
            }

            const QVector2D delta = destination - pos.value;
            const float dist = delta.length();

            if (dist < 0.001f)
            {
                // Already at target: no new thrust. The ship drifts; it will
                // re-approach next tick once it has moved away.
                body.linearAcceleration_tptt  = QVector2D(0.0f, 0.0f);
                body.angularAcceleration_rptt = 0.0f;
                return;
            }

            // --- Angular acceleration ---

            const float desiredAngle = std::atan2(delta.y(), delta.x());
            const float angleDiff    = wrapAngle(desiredAngle - facing.radians);

            const float rotDelta = std::max(-body.maxAngularAcceleration_rptt,
                                            std::min(angleDiff,
                                                     body.maxAngularAcceleration_rptt));

            float newAngVel = body.angularVelocity_rpt + rotDelta;

            // Overshoot prevention: if the accumulated angular velocity already
            // exceeds the remaining angle, snap it to exactly that angle so the
            // ship doesn't rotate past its heading.
            const bool sameSign = (newAngVel >= 0.0f) == (angleDiff >= 0.0f);
            if (sameSign && std::abs(newAngVel) > std::abs(angleDiff))
            {
                newAngVel = angleDiff;
            }

            body.angularAcceleration_rptt = newAngVel - body.angularVelocity_rpt;
            // DynamicBodySystem applies the clamp to maxRotationSpeed_rpt after
            // integrating, so we do not clamp here.

            // --- Linear acceleration ---
            // Use the projected facing (after this tick's angular integration) so
            // that the main thruster aligns with where the ship will actually be
            // pointing when DynamicBodySystem applies the forces.

            const float projectedRadians = wrapAngle(facing.radians + newAngVel);
            const QVector2D facingVec(std::cos(projectedRadians),
                                      std::sin(projectedRadians));

            const float manAccel     = body.maneuveringAcceleration_tptt;
            const float stoppingDist = (body.maxSpeed_tpt * body.maxSpeed_tpt)
                                       / (2.0f * manAccel);
            // Cap to dist so the ship never overshoots the target in a single tick.
            const float baseDesiredSpeed = (dist <= stoppingDist)
                ? std::sqrt(2.0f * manAccel * dist)
                : body.maxSpeed_tpt;
            const float desiredSpeed = std::min(dist, baseDesiredSpeed);

            const QVector2D desiredVel = delta.normalized() * desiredSpeed;
            const QVector2D velError   = desiredVel - body.velocity_tpt;

            const float mainAligned  = std::max(0.0f,
                QVector2D::dotProduct(velError, facingVec));
            const float mainApplied  = std::min(mainAligned,
                                                body.mainAcceleration_tptt);
            const QVector2D mainDelta = facingVec * mainApplied;

            const QVector2D remaining = velError - mainDelta;
            const float remainLen     = remaining.length();
            const QVector2D maneuverDelta = (remainLen > manAccel)
                ? remaining.normalized() * manAccel
                : remaining;

            body.linearAcceleration_tptt = mainDelta + maneuverDelta;
        });
}