#include "Physics.hpp"
#include <algorithm>
#include <box2d/box2d.h>
#include <box3d/box3d.h>
#include <cmath>
#include <stdexcept>
#include <unordered_map>

namespace faset::runtime::detail {
namespace {
b3Quat quaternion(Vec3 e) {
    auto x = b3MakeQuatFromAxisAngle({1, 0, 0}, e[0]);
    auto y = b3MakeQuatFromAxisAngle({0, 1, 0}, e[1]);
    auto z = b3MakeQuatFromAxisAngle({0, 0, 1}, e[2]);
    return b3MulQuat(z, b3MulQuat(y, x));
}
Vec3 euler(b3Quat q) {
    const float x = q.v.x, y = q.v.y, z = q.v.z, w = q.s;
    return {std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
            std::asin(std::clamp(2 * (w * y - z * x), -1.0f, 1.0f)),
            std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))};
}
} // namespace
struct Physics::Impl {
    struct Body {
        b2BodyId two{};
        b3BodyId three{};
        std::uint64_t shape{};
        bool dynamic{};
        bool contactQueryValid{};
    };
    int dimension;
    int substeps;
    Vec3 up{0, 1, 0};
    b2WorldId world2{};
    b3WorldId world3{};
    std::unordered_map<std::uint32_t, Body> bodies;
    std::unordered_map<std::uint64_t, std::uint32_t> shapes;
    Impl(int dim, Vec3 gravity, int count) : dimension(dim), substeps(count) {
        const float length = std::sqrt(gravity[0] * gravity[0] + gravity[1] * gravity[1] +
                                       (dim == 3 ? gravity[2] * gravity[2] : 0));
        if (length > 0.00001f)
            up = {-gravity[0] / length, -gravity[1] / length, dim == 3 ? -gravity[2] / length : 0};
        if (dim == 2) {
            auto def = b2DefaultWorldDef();
            def.gravity = {gravity[0], gravity[1]};
            world2 = b2CreateWorld(&def);
        } else {
            auto def = b3DefaultWorldDef();
            def.gravity = {gravity[0], gravity[1], gravity[2]};
            world3 = b3CreateWorld(&def);
        }
    }
    ~Impl() {
        if (dimension == 2)
            b2DestroyWorld(world2);
        else
            b3DestroyWorld(world3);
    }
};
Physics::Physics(int dimension, Vec3 gravity, int substeps)
    : impl_(std::make_unique<Impl>(dimension, gravity, substeps)) {}
Physics::~Physics() = default;
void Physics::add(std::uint32_t id, const Transform& t, const BodySettings& settings) {
    if (contains(id))
        throw std::logic_error("physics body already exists");
    Impl::Body body{};
    body.dynamic = settings.type == "dynamic";
    if (impl_->dimension == 2) {
        auto def = b2DefaultBodyDef();
        def.type = settings.type == "static"      ? b2_staticBody
                   : settings.type == "kinematic" ? b2_kinematicBody
                                                  : b2_dynamicBody;
        def.position = {t.position[0], t.position[1]};
        def.rotation = b2MakeRot(t.rotation[2]);
        def.linearVelocity = {settings.linearVelocity[0], settings.linearVelocity[1]};
        def.gravityScale = settings.gravityScale;
        body.two = b2CreateBody(impl_->world2, &def);
        auto shape = b2DefaultShapeDef();
        shape.density = settings.density;
        shape.material.friction = settings.friction;
        shape.material.restitution = settings.restitution;
        shape.enableContactEvents = true;
        shape.filter.categoryBits = settings.categoryBits;
        shape.filter.maskBits = settings.maskBits;
        const auto box = b2MakeBox(settings.halfExtents[0] * std::abs(t.scale[0]),
                                   settings.halfExtents[1] * std::abs(t.scale[1]));
        body.shape = b2StoreShapeId(b2CreatePolygonShape(body.two, &shape, &box));
    } else {
        auto def = b3DefaultBodyDef();
        def.type = settings.type == "static"      ? b3_staticBody
                   : settings.type == "kinematic" ? b3_kinematicBody
                                                  : b3_dynamicBody;
        def.position = {t.position[0], t.position[1], t.position[2]};
        def.rotation = quaternion(t.rotation);
        def.linearVelocity = {settings.linearVelocity[0], settings.linearVelocity[1],
                              settings.linearVelocity[2]};
        def.gravityScale = settings.gravityScale;
        body.three = b3CreateBody(impl_->world3, &def);
        auto shape = b3DefaultShapeDef();
        shape.density = settings.density;
        shape.baseMaterial.friction = settings.friction;
        shape.baseMaterial.restitution = settings.restitution;
        shape.enableContactEvents = true;
        shape.filter.categoryBits = settings.categoryBits;
        shape.filter.maskBits = settings.maskBits;
        auto box = b3MakeBoxHull(settings.halfExtents[0] * std::abs(t.scale[0]),
                                 settings.halfExtents[1] * std::abs(t.scale[1]),
                                 settings.halfExtents[2] * std::abs(t.scale[2]));
        body.shape = b3StoreShapeId(b3CreateHullShape(body.three, &shape, &box.base));
    }
    impl_->shapes.emplace(body.shape, id);
    impl_->bodies.emplace(id, body);
}
void Physics::remove(std::uint32_t id) {
    const auto it = impl_->bodies.find(id);
    if (it == impl_->bodies.end())
        return;
    impl_->shapes.erase(it->second.shape);
    if (impl_->dimension == 2)
        b2DestroyBody(it->second.two);
    else
        b3DestroyBody(it->second.three);
    impl_->bodies.erase(it);
}
bool Physics::contains(std::uint32_t id) const {
    return impl_->bodies.contains(id);
}
bool Physics::dynamic(std::uint32_t id) const {
    return impl_->bodies.at(id).dynamic;
}
Transform Physics::transform(std::uint32_t id, Transform t) const {
    const auto& body = impl_->bodies.at(id);
    if (impl_->dimension == 2) {
        auto p = b2Body_GetPosition(body.two);
        t.position[0] = p.x;
        t.position[1] = p.y;
        t.rotation[2] = b2Rot_GetAngle(b2Body_GetRotation(body.two));
    } else {
        auto p = b3Body_GetPosition(body.three);
        t.position = {float(p.x), float(p.y), float(p.z)};
        t.rotation = euler(b3Body_GetRotation(body.three));
    }
    return t;
}
Vec3 Physics::velocity(std::uint32_t id) const {
    const auto& body = impl_->bodies.at(id);
    if (impl_->dimension == 2) {
        auto v = b2Body_GetLinearVelocity(body.two);
        return {v.x, v.y, 0};
    }
    auto v = b3Body_GetLinearVelocity(body.three);
    return {v.x, v.y, v.z};
}
bool Physics::grounded(std::uint32_t id) const {
    const auto& body = impl_->bodies.at(id);
    if (!body.contactQueryValid)
        return false;
    auto supports = [&](Vec3 normal, float sign) {
        return sign * (normal[0] * impl_->up[0] + normal[1] * impl_->up[1] +
                       normal[2] * impl_->up[2]) >
               0.6f;
    };
    if (impl_->dimension == 2) {
        const int capacity = b2Body_GetContactCapacity(body.two);
        if (capacity <= 0)
            return false;
        std::vector<b2ContactData> contacts(static_cast<std::size_t>(capacity));
        const int count = b2Body_GetContactData(body.two, contacts.data(), capacity);
        for (int i = 0; i < count; ++i) {
            const auto& c = contacts[i];
            const float sign = b2StoreShapeId(c.shapeIdA) == body.shape ? -1.0f : 1.0f;
            if (!supports({c.manifold.normal.x, c.manifold.normal.y, 0}, sign))
                continue;
            for (int p = 0; p < c.manifold.pointCount; ++p)
                if (c.manifold.points[p].separation <= 0.02f)
                    return true;
        }
    } else {
        const int capacity = b3Body_GetContactCapacity(body.three);
        if (capacity <= 0)
            return false;
        std::vector<b3ContactData> contacts(static_cast<std::size_t>(capacity));
        const int count = b3Body_GetContactData(body.three, contacts.data(), capacity);
        for (int i = 0; i < count; ++i) {
            const auto& c = contacts[i];
            const float sign = b3StoreShapeId(c.shapeIdA) == body.shape ? -1.0f : 1.0f;
            // Box3D's manifold pointer is consumed now and never retained.
            for (int m = 0; m < c.manifoldCount; ++m) {
                const auto& manifold = c.manifolds[m];
                if (!supports({manifold.normal.x, manifold.normal.y, manifold.normal.z}, sign))
                    continue;
                for (int p = 0; p < manifold.pointCount; ++p)
                    if (manifold.points[p].separation <= 0.02f)
                        return true;
            }
        }
    }
    return false;
}
void Physics::teleport(std::uint32_t id, const Transform& t) {
    auto& body = impl_->bodies.at(id);
    body.contactQueryValid = false;
    if (impl_->dimension == 2) {
        b2Body_SetTransform(body.two, {t.position[0], t.position[1]}, b2MakeRot(t.rotation[2]));
        b2Body_SetAwake(body.two, true);
    } else {
        b3Body_SetTransform(body.three, {t.position[0], t.position[1], t.position[2]},
                            quaternion(t.rotation));
        b3Body_SetAwake(body.three, true);
    }
}
void Physics::setVelocity(std::uint32_t id, Vec3 v) {
    const auto& body = impl_->bodies.at(id);
    if (impl_->dimension == 2)
        b2Body_SetLinearVelocity(body.two, {v[0], v[1]});
    else
        b3Body_SetLinearVelocity(body.three, {v[0], v[1], v[2]});
}
void Physics::impulse(std::uint32_t id, Vec3 v) {
    const auto& body = impl_->bodies.at(id);
    if (impl_->dimension == 2)
        b2Body_ApplyLinearImpulseToCenter(body.two, {v[0], v[1]}, true);
    else
        b3Body_ApplyLinearImpulseToCenter(body.three, {v[0], v[1], v[2]}, true);
}
std::vector<Contact> Physics::step(float delta) {
    std::vector<Contact> contacts;
    auto append = [&](std::uint64_t a, std::uint64_t b, bool began) {
        auto first = impl_->shapes.find(a), second = impl_->shapes.find(b);
        if (first != impl_->shapes.end() && second != impl_->shapes.end())
            contacts.push_back({first->second, second->second, began});
    };
    if (impl_->dimension == 2) {
        b2World_Step(impl_->world2, delta, impl_->substeps);
        auto events = b2World_GetContactEvents(impl_->world2);
        for (int i = 0; i < events.beginCount; ++i)
            append(b2StoreShapeId(events.beginEvents[i].shapeIdA),
                   b2StoreShapeId(events.beginEvents[i].shapeIdB), true);
        for (int i = 0; i < events.endCount; ++i)
            append(b2StoreShapeId(events.endEvents[i].shapeIdA),
                   b2StoreShapeId(events.endEvents[i].shapeIdB), false);
    } else {
        b3World_Step(impl_->world3, delta, impl_->substeps);
        auto events = b3World_GetContactEvents(impl_->world3);
        for (int i = 0; i < events.beginCount; ++i)
            append(b3StoreShapeId(events.beginEvents[i].shapeIdA),
                   b3StoreShapeId(events.beginEvents[i].shapeIdB), true);
        for (int i = 0; i < events.endCount; ++i)
            append(b3StoreShapeId(events.endEvents[i].shapeIdA),
                   b3StoreShapeId(events.endEvents[i].shapeIdB), false);
    }
    for (auto& [id, body] : impl_->bodies) {
        (void)id;
        body.contactQueryValid = true;
    }
    return contacts;
}
} // namespace faset::runtime::detail