
#include "phys.hh"

#include <cstdint>
#include <cmath>
#include <algorithm>

void Engine::apply(float t)
{
        auto v = b->v.decomp(n);
        if (v.x < s)
                v.x = std::min(v.x + a * t, s);
        else
                v.x = std::max(v.x - a * t, s);
        b->v = v.recomp(n);
}

void Phys::test_lf(Body& b, float t) const
{
        long x = std::floor(b.box.l + b.v.x * t);
        long u = std::floor(b.box.u);
        long d = std::ceil(b.box.d);
        for (long y = u; y != d; ++y)
                if (at(x, y) != 0) {
                        b.p.x = x + 1 + b.r.x;
                        b.v.x = 0;
                        b.set(Body::HIT_LF);
                        return;
                }
        b.p.x += b.v.x * t;
}

void Phys::test_rt(Body& b, float t) const
{
        long x = std::floor(b.box.r + b.v.x * t);
        long u = std::floor(b.box.u);
        long d = std::ceil(b.box.d);
        for (long y = u; y != d; ++y)
                if (at(x, y) != 0) {
                        b.p.x = x - b.r.x;
                        b.v.x = 0;
                        b.set(Body::HIT_RT);
                        return;
                }
        b.p.x += b.v.x * t;
}

void Phys::test_up(Body& b, float t) const
{
        long y = std::floor(b.box.u + b.v.y * t);
        long l = std::floor(b.box.l);
        long r = std::ceil(b.box.r);
        for (long x = l; x != r; ++x)
                if (at(x, y) != 0) {
                        b.p.y = y + 1 + b.r.y;
                        b.v.y = 0;
                        b.set(Body::HIT_UP);
                        return;
                }
        b.p.y += b.v.y * t;
}

void Phys::test_dn(Body& b, float t) const
{
        long y = std::floor(b.box.d + b.v.y * t);
        long l = std::floor(b.box.l);
        long r = std::ceil(b.box.r);
        for (long x = l; x != r; ++x)
                if (at(x, y) != 0) {
                        b.p.y = y - b.r.y;
                        b.v.y = 0;
                        b.set(Body::HIT_DN);
                        return;
                }
        b.p.y += b.v.y * t;
}

void Phys::kiss_lf(Body& b, float t) const
{
        long x = b.box.l;
        if (b.box.l == x) {
                --x;
                long u = std::floor(b.box.u);
                long d = std::ceil(b.box.d);
                for (long y = u; y != d; ++y)
                        if (at(x, y) != 0) {
                                b.set(Body::HIT_LF);
                                return;
                        }
        }
}

void Phys::kiss_rt(Body& b, float t) const
{
        long x = b.box.r;
        if (b.box.r == x) {
                long u = std::floor(b.box.u);
                long d = std::ceil(b.box.d);
                for (long y = u; y != d; ++y)
                        if (at(x, y) != 0) {
                                b.set(Body::HIT_RT);
                                return;
                        }
        }
}

void Phys::kiss_up(Body& b, float t) const
{
        long y = b.box.u;
        if (b.box.u == y) {
                --y;
                long l = std::floor(b.box.l);
                long r = std::ceil(b.box.r);
                for (long x = l; x != r; ++x)
                        if (at(x, y) != 0) {
                                b.set(Body::HIT_UP);
                                return;
                        }
        }
}

void Phys::kiss_dn(Body& b, float t) const
{
        long y = b.box.d;
        if (b.box.d == y) {
                long l = std::floor(b.box.l);
                long r = std::ceil(b.box.r);
                for (long x = l; x != r; ++x)
                        if (at(x, y) != 0) {
                                b.set(Body::HIT_DN);
                                return;
                        }
        }
}

void Phys::solid_body(Body& b, float t) const
{
        b.p /= 8;
        b.v /= 8;
        b.r /= 8;

        b.v.x = std::clamp<float>(b.v.x, -1, 1);
        b.v.y = std::clamp<float>(b.v.y, -1, 1);

        b.flags &= ~0xf;
        b.box = itty::Box::from_center(b.p, b.r);

        if (b.v.x < 0) test_lf(b, t);
        else if (b.v.x > 0) test_rt(b, t);
        else { kiss_lf(b, t); kiss_rt(b, t); }

        b.box.l = b.p.x - b.r.x;
        b.box.r = b.p.x + b.r.x;

        if (b.v.y < 0) test_up(b, t);
        else if (b.v.y > 0) test_dn(b, t);
        else { kiss_up(b, t); kiss_dn(b, t); }

        b.box.u = b.p.y - b.r.y;
        b.box.d = b.p.y + b.r.y;

        b.p *= 8;
        b.v *= 8;
        b.r *= 8;
        b.box *= 8;
}

void Phys::ghost_body(Body& b, float t) const
{
        b.p += b.v * t;
        b.box = itty::Box::from_center(b.p, b.r);
}

void Phys::move_body(Body& b, float t) const
{
        if (b.is(Body::SOLID))
                solid_body(b, t);
        else
                ghost_body(b, t);
}

void Phys::step(float t)
{
        for (auto& e : _engines)
                e.apply(t);
        _engines.clear();

        for (auto& b : _bodies)
                move_body(*b, t);
        _bodies.clear();
}

void Phys::add_body(Body *b)
{
        _bodies.push_back(b);
}

void Phys::add_engine(Body *b, itty::Vec n, float s, float a)
{
        _engines.emplace_back(b, n, s, a);
}

int Phys::at(const itty::Vec& p)
{
        return at(std::floor(p.x / 8), std::floor(p.y / 8));
}
