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

int main() {
    using namespace vde::collision;
    // Sphere support
    auto sphere = [](const Point3D& c, double r) -> SupportFunc {
        return [=](const Vector3D& d) { return c + d.normalized() * r; };
    };

    auto a = sphere({0,0,0}, 1.0);
    auto b = sphere({2,0,0}, 1.0);
    std::cout << "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";

    // Ray-triangle
    Triangle3D tri({0,0,0},{1,0,0},{0,1,0});
    auto hit = ray_triangle_intersect({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;
}