import Crafter.Math;
import std;

using namespace Crafter;

int main() {
    // Intersection Test

    // Define Box A (size and matrix for transformation)
    Vector<float, 3, 0> sizeA(1.0f, 1.0f, 1.0f);  // Box A size
    MatrixRowMajor<float, 4, 3, 1> boxA;
    boxA.m[0][0] = 1.0f; boxA.m[1][0] = 0.0f; boxA.m[2][0] = 0.0f;
    boxA.m[1][0] = 0.0f; boxA.m[1][1] = 1.0f; boxA.m[2][1] = 0.0f;
    boxA.m[2][0] = 0.0f; boxA.m[1][2] = 0.0f; boxA.m[2][2] = 1.0f;

    // Define Box B (size and matrix for transformation)
    Vector<float, 3, 0> sizeB(1.0f, 1.0f, 1.0f);  // Box B size
    MatrixRowMajor<float, 4, 3, 1> boxB;
    boxB.m[0][0] = 1.0f; boxB.m[1][0] = 0.0f; boxB.m[2][0] = 0.0f;
    boxB.m[1][0] = 0.0f; boxB.m[1][1] = 1.0f; boxB.m[2][1] = 0.0f;
    boxB.m[2][0] = 0.0f; boxB.m[1][2] = 0.0f; boxB.m[2][2] = 1.0f;

    // Apply a small translation to box B (so that they overlap)
    boxB.m[0][3] = 0.5f;
    boxB.m[1][3] = 0.5f;
    boxB.m[2][3] = 0.0f;

    // Check if the boxes intersect (they should, as they're overlapping)
    bool result = IntersectionTestOrientedBoxOrientedBox(sizeA, boxA, sizeB, boxB);
    std::cout << result << std::endl;


    // Miss Test

    // Define Box A (size and matrix for transformation)
    Vector<float, 3, 0> sizeC(1.0f, 1.0f, 1.0f);  // Box C size
    MatrixRowMajor<float, 4, 3, 1> boxC;
    boxC.m[0][0] = 1.0f; boxC.m[1][0] = 0.0f; boxC.m[2][0] = 0.0f;
    boxC.m[1][0] = 0.0f; boxC.m[1][1] = 1.0f; boxC.m[2][1] = 0.0f;
    boxC.m[2][0] = 0.0f; boxC.m[1][2] = 0.0f; boxC.m[2][2] = 1.0f;

    // Define Box D (size and matrix for transformation)
    Vector<float, 3, 0> sizeD(1.0f, 1.0f, 1.0f);  // Box D size
    MatrixRowMajor<float, 4, 3, 1> boxD;
    boxD.m[0][0] = 1.0f; boxD.m[1][0] = 0.0f; boxD.m[2][0] = 0.0f;
    boxD.m[1][0] = 0.0f; boxD.m[1][1] = 1.0f; boxD.m[2][1] = 0.0f;
    boxD.m[2][0] = 0.0f; boxD.m[1][2] = 0.0f; boxD.m[2][2] = 1.0f;

    // Apply a large translation to box D (so that they do not intersect)
    boxD.m[0][3] = 3.0f;
    boxD.m[1][3] = 3.0f;
    boxD.m[2][3] = 0.0f;

    // Check if the boxes do not intersect (they shouldn't, as they are far apart)
    bool missResult = IntersectionTestOrientedBoxOrientedBox(sizeC, boxC, sizeD, boxD);
    std::cout << missResult << std::endl;

    return 0;
}