#pragma once

#include <cmath>

#include <QVector2D>

// Orbit movement helper (REQ-SHP-ORBIT). Behaviors that keep a ship circling a
// target (attack, repair, salvage, rally) supply an orbit center and radius via
// the movement intent; MovementIntentSystem resolves the orbit direction and
// destination using these helpers.
namespace OrbitMath
{
    // Lead angle (radians) by which the radial direction is rotated to produce
    // tangential motion. The orbit direction (sign of the rotation) is chosen
    // per ship by resolveOrbitSign from the ship's current velocity, so ships
    // approaching a target from different sides circle it in different senses
    // instead of all bunching on one side.
    constexpr float kOrbitLeadAngle_rad = 0.6f;

    // Returns the orbit sense (+1 counter-clockwise, -1 clockwise) that matches
    // the ship's movement around `center`, so steering reinforces the motion the
    // ship already has. The sense is taken from the ship's velocity *relative to
    // the center* (`centerVelocity`): for a moving target this both removes the
    // target's own motion from the decision and dissolves the degenerate case
    // where two ships orbiting each other translate in a straight line — there
    // their shared velocity cancels, leaving ~zero relative velocity. When the
    // relative velocity is nearly radial or near zero (a head-on approach, a
    // freshly spawned ship, or that mutual-translation case) the sense is
    // ill-defined; this is an unstable point the ship leaves within a tick or
    // two, so a deterministic fallback of +1 is returned.
    inline float resolveOrbitSign(const QVector2D& shipPos, const QVector2D& center,
                                  const QVector2D& velocity,
                                  const QVector2D& centerVelocity = QVector2D())
    {
        const QVector2D radial = shipPos - center;
        const QVector2D relativeVelocity = velocity - centerVelocity;
        const float radialLength   = radial.length();
        const float velocityLength = relativeVelocity.length();
        if (radialLength < 1.0e-4f || velocityLength < 1.0e-4f)
        {
            return 1.0f;
        }

        // z-component of radial x relativeVelocity, normalised to sin(angle).
        const float cross = radial.x() * relativeVelocity.y()
                          - radial.y() * relativeVelocity.x();
        const float sinAngle = cross / (radialLength * velocityLength);

        constexpr float kRadialEpsilon = 1.0e-3f;
        if (std::abs(sinAngle) < kRadialEpsilon)
        {
            return 1.0f;
        }
        return (sinAngle > 0.0f) ? 1.0f : -1.0f;
    }

    // Returns a destination on the orbit circle of `radius` around `center`. The
    // result always lies exactly `radius` from `center`, so steering toward it
    // both corrects the standoff distance and advances the ship tangentially.
    // `sign` selects the orbit sense (+1 counter-clockwise, -1 clockwise). A
    // radius of zero or less falls back to the center (legacy "approach the
    // target" behavior), e.g. when the ship has no tool range to orbit at.
    inline QVector2D computeOrbitDestination(const QVector2D& shipPos,
                                             const QVector2D& center, float radius,
                                             float sign = 1.0f)
    {
        if (radius <= 0.0f) { return center; }

        QVector2D radial = shipPos - center;
        float length = radial.length();
        if (length < 1.0e-4f)
        {
            // Ship sits on the center; pick an arbitrary radial direction.
            radial = QVector2D(1.0f, 0.0f);
            length = 1.0f;
        }
        const QVector2D radialDirection = radial / length;

        const float leadAngle = sign * kOrbitLeadAngle_rad;
        const float cosLead = std::cos(leadAngle);
        const float sinLead = std::sin(leadAngle);
        const QVector2D leadDirection(
            radialDirection.x() * cosLead - radialDirection.y() * sinLead,
            radialDirection.x() * sinLead + radialDirection.y() * cosLead);

        return center + radius * leadDirection;
    }
}