#include "DynamicBodySystem.h"

#include <algorithm>
#include <cmath>

#include <QVector2D>

#include "DynamicBodyComponent.h"
#include "EntityAdmin.h"
#include "FacingComponent.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 DynamicBodySystem::tick(EntityAdmin& admin)
{
	TRACE();
    admin.forEach<PositionComponent, FacingComponent, DynamicBodyComponent>(
        [](entt::entity /*e*/, PositionComponent& pos, FacingComponent& facing,
           DynamicBodyComponent& body)
        {
            // Integrate angular velocity, clamp to max rotation speed, then advance facing.
            body.angularVelocity_rpt += body.angularAcceleration_rptt;
            body.angularVelocity_rpt  = std::max(-body.maxRotationSpeed_rpt,
                                                  std::min(body.angularVelocity_rpt,
                                                           body.maxRotationSpeed_rpt));
            facing.radians = wrapAngle(facing.radians + body.angularVelocity_rpt);

            // Integrate linear velocity and cap to max speed.
            body.velocity_tpt += body.linearAcceleration_tptt;
            const float speed = body.velocity_tpt.length();
            if (speed > body.maxSpeed_tpt)
            {
                body.velocity_tpt = body.velocity_tpt.normalized() * body.maxSpeed_tpt;
            }

            // Advance position.
            pos.value += body.velocity_tpt;

            // Reset per-tick fields so stale values don't linger if the intent
            // system is skipped for this entity in a future tick.
            body.linearAcceleration_tptt  = QVector2D(0.0f, 0.0f);
            body.angularAcceleration_rptt = 0.0f;
        });
}

