#include <pslib.h>
#include "tap.h"
#include "pstap.h"

#define NUM 10000                       // Number of points

int main(int argc, char *argv[])
{
    psLibInit(NULL);
    plan_tests(6);

    {
        psMemId id = psMemGetId();

        psVector *x = psVectorAlloc(NUM, PS_TYPE_F64);
        psVector *y = psVectorAlloc(NUM, PS_TYPE_F64);

        psRandom *rng = psRandomAlloc(PS_RANDOM_TAUS, 0);
        for (int i = 0; i < NUM; i++) {
            x->data.F64[i] = 2.0 * psRandomUniform(rng) - 1.0;
            y->data.F64[i] = 2.0 * psRandomUniform(rng) - 1.0;
        }
        psFree(rng);

        psTree *tree = psTreePlant(2, 2, x, y);

        ok(tree, "Tree planted");
        skip_start(!tree, 4, "tree died");
        {
            //            psTreePrint(stderr, tree);

            psVector *coords = psVectorAlloc(2, PS_TYPE_F64);
            psVectorInit(coords, 0);

            long closeIndex = psTreeNearest(tree, coords);
            psFree(coords);
            ok(closeIndex >= 0 && closeIndex < tree->numNodes, "found point: %ld", closeIndex);

            long bestIndex = -1;
            double bestDist = INFINITY;
            for (int i = 0; i < NUM; i++) {
                double dist = PS_SQR(x->data.F64[i]) + PS_SQR(y->data.F64[i]);
                if (dist < bestDist) {
                    bestIndex = i;
                    bestDist = dist;
                }
            }
            ok(bestIndex == closeIndex, "correct point: %ld vs %ld", closeIndex, bestIndex);

            psVector *closest = psTreeCoords(NULL, tree, closeIndex);
            ok(closest, "got coords: %lf,%lf", closest->data.F64[0], closest->data.F64[1]);
            ok(closest->data.F64[0] == x->data.F64[bestIndex] &&
               closest->data.F64[1] == y->data.F64[bestIndex],
               "correst coords: %lf,%lf(%lf) vs %lf,%lf(%lf)",
               closest->data.F64[0], closest->data.F64[1],
               sqrt(PS_SQR(closest->data.F64[0]) + PS_SQR(closest->data.F64[1])),
               x->data.F64[bestIndex], y->data.F64[bestIndex],
               sqrt(PS_SQR(x->data.F64[bestIndex]) + PS_SQR(y->data.F64[bestIndex])));
            psFree(closest);
        }
        skip_end();

        psFree(tree);
        psFree(x);
        psFree(y);

        ok(!psMemCheckLeaks(id, NULL, NULL, false), "no memory leaks");
    }

    psLibFinalize();
}

