#include <iostream>
#include "vde/collision/gjk.h"
#include "vde/collision/ray_intersect.h"

using namespace vde::collision;
using namespace vde::core;

int main() {
    auto sphere = [](const Point3D& c, double r) -> SupportFunc {
        return [=](const Vector3D& d) -> Point3D {
            double n = d.norm();
            if (n < 1e-12) return Point3D(c.x() + r, c.y(), c.z());
            Vector3D nd = d / n;
            return Point3D(c.x() + nd.x() * r,
                           c.y() + nd.y() * r,
                           c.z() + nd.z() * r);
        };
    };

    auto a = sphere({0,0,0}, 1.0);
    auto b = sphere({3,0,0}, 1.0);
    std::cout << "Separated spheres intersect: " << (gjk_intersect(a, b) ? "yes" : "no") << "\n";

    auto c = sphere({0,0,0}, 1.0);
    auto d = sphere({0.5,0,0}, 1.0);
    std::cout << "Overlapping spheres intersect: " << (gjk_intersect(c, d) ? "yes" : "no") << "\n";

    Triangle3D tri({0,0,0},{1,0,0},{0,1,0});
    auto hit = ray_triangle_intersect(Ray3Dd({0.25,0.25,-1}, Vector3D(0,0,1)), tri);
    std::cout << "Ray hits triangle: " << (hit.has_value() ? "yes" : "no") << "\n";
    if (hit) std::cout << "  at (" << hit->point.transpose() << "), t=" << hit->t << "\n";

    return 0;
}