#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <check.h>


#include "libharkio3.h"

harkio_Positions* goodPositions(void)
{
    harkio_Positions *ps;
    int i;

    ps = harkio_Positions_new("tsp", 1, 0, NULL);
    for (i=0; i<5; i++) {
        float coord[3] = {0.1*i, 0.2*i, 0.3*i};
        harkio_Position* po = harkio_Position_new(i, Cartesian, coord, "/home");
        harkio_Positions_append(ps, po);
        //harkio_Position_delete(&po);
    }
    return ps;
}

harkio_Positions* goodPositions2(void)
{
    harkio_Positions *ps;
    int i;

    ps = harkio_Positions_new("tsp", 1, 0, NULL);
    for (i=0; i<5; i++) {
        float coord[3] = {0.2*i, 0.3*i, 0.4*i};
        harkio_Positions_append(ps, harkio_Position_new(i, Cartesian, coord, "/home"));
    }
    return ps;
}

START_TEST(test_new)
{
    harkio_Positions *p = goodPositions();
    harkio_Neighbors *nbrs = harkio_Neighbors_new(p);
    int i;

    ck_assert_int_eq(nbrs->size, p->size);

    for (i=0; i<nbrs->size; i++) {
        ck_assert_int_eq(nbrs->ids[i], p->pos[i]->id);
        ck_assert_int_eq(nbrs->numNeighbors[i], 0);
    }
    ck_assert_int_eq(harkio_Neighbors_getNeighborAlgorithm(nbrs), Undefined);

    harkio_Neighbors_delete(&nbrs);
    harkio_Positions_delete(&p);
}
END_TEST

START_TEST(test_copy)
{
    harkio_Positions *p = goodPositions();
    harkio_Neighbors *nbrs = harkio_Neighbors_new(p);
    nbrs->alg = NearestNeighbor;
    harkio_Neighbors *nbrs1 = harkio_Neighbors_copy(nbrs);

    ck_assert_int_eq(nbrs1->size, nbrs->size);
    ck_assert_int_eq(nbrs1->size, nbrs->size);

    ck_assert_int_eq(harkio_Neighbors_getNeighborAlgorithm(nbrs1), harkio_Neighbors_getNeighborAlgorithm(nbrs));

    harkio_Neighbors_delete(&nbrs1);
    harkio_Neighbors_delete(&nbrs);
    harkio_Positions_delete(&p);
}
END_TEST

START_TEST(test_delete)
{
    harkio_Neighbors *nbrs = harkio_Neighbors_new(goodPositions());
    harkio_Positions_delete(&(nbrs->poses));

    harkio_Neighbors_delete(&nbrs);
    ck_assert(nbrs == NULL);

    harkio_Neighbors_delete(&nbrs);
    ck_assert(nbrs == NULL);

    harkio_Neighbors_delete(NULL);
}
END_TEST

START_TEST(test_calcNeighbors)
{
    harkio_Positions *ps = harkio_Positions_new("tsp", 1, 0, NULL);
    int ret, i;
    float coords[][3] = {{0, 0, 0}, {0, 0, 1}, {0, 0, 3}, {0, 0, 3.5}, {0, 0, 10.95}, {0, 0, 11}};
    for (i=0; i<6; i++) {
        harkio_Positions_append(ps, harkio_Position_new(i, Cartesian, coords[i], "/home"));
    }
    harkio_Neighbors *nbrs = harkio_Neighbors_new(ps);

    ret = harkio_Neighbors_calcNeighbors(nbrs, NearestNeighbor, 3, -3.0);
    ck_assert_int_eq(ret, EXIT_FAILURE);

    ret = harkio_Neighbors_calcNeighbors(nbrs, NearestNeighbor, -3, 1.5);
    ck_assert_int_eq(ret, EXIT_FAILURE);

    ret = harkio_Neighbors_calcNeighbors(nbrs, NearestNeighbor, -3, -3.0);
    ck_assert_int_eq(ret, EXIT_FAILURE);

    ret = harkio_Neighbors_calcNeighbors(nbrs, NearestNeighbor, 3, 1.5);
    ck_assert_int_eq(ret, EXIT_SUCCESS);
    ck_assert_int_eq(nbrs->numNeighbors[0], 2);
    ck_assert_int_eq(nbrs->numNeighbors[1], 2);
    ck_assert_int_eq(nbrs->numNeighbors[2], 2);
    ck_assert_int_eq(nbrs->numNeighbors[3], 2);
    ck_assert_int_eq(nbrs->numNeighbors[4], 2);
    ck_assert_int_eq(nbrs->numNeighbors[5], 2);

    ck_assert_int_eq(nbrs->neighbors[0][0], 0);
    ck_assert_int_eq(nbrs->neighbors[0][1], 1);
    ck_assert_int_eq(nbrs->neighbors[1][0], 1);
    ck_assert_int_eq(nbrs->neighbors[1][1], 0);
    ck_assert_int_eq(nbrs->neighbors[2][0], 2);
    ck_assert_int_eq(nbrs->neighbors[2][1], 3);
    ck_assert_int_eq(nbrs->neighbors[3][0], 3);
    ck_assert_int_eq(nbrs->neighbors[3][1], 2);
    ck_assert_int_eq(nbrs->neighbors[4][0], 4);
    ck_assert_int_eq(nbrs->neighbors[4][1], 5);
    ck_assert_int_eq(nbrs->neighbors[5][0], 5);
    ck_assert_int_eq(nbrs->neighbors[5][1], 4);

    ck_assert_int_eq(harkio_Neighbors_getNeighborAlgorithm(nbrs), NearestNeighbor);

    ret = harkio_Neighbors_calcNeighbors(nbrs, 100);
    ck_assert_int_eq(ret, EXIT_FAILURE);

    harkio_Positions_delete(&(nbrs->poses));
    harkio_Neighbors_delete(&nbrs);
}
END_TEST

START_TEST(test_getNeighbors)
{
    harkio_Positions *ps = harkio_Positions_new("tsp", 1, 0, NULL);
    int ret, i;
    int numNeighbors;
    int *neighbors = NULL;
    float coords[][3] = {{0, 0, 0}, {0, 0, 1}, {0, 0, 3}, {0, 0, 3.5}, {0, 0, 10.95}, {0, 0, 11}};
    // return;
    // fprintf(stderr, "==================================");
    for (i=0; i<6; i++) {
        harkio_Positions_append(ps, harkio_Position_new(i, Cartesian, coords[i], "/home"));
    }
    harkio_Neighbors *nbrs = harkio_Neighbors_new(ps);
    ret = harkio_Neighbors_getNeighbors(nbrs, i/*nbrs->ids[i]*/, &numNeighbors, &neighbors);
    ck_assert_int_eq(ret, EXIT_FAILURE);


    ret = harkio_Neighbors_calcNeighbors(nbrs, NearestNeighbor, 3, 1.5);
    for (i=0; i<6; i++) {
        harkio_Neighbors_print(nbrs);
        ret = harkio_Neighbors_getNeighbors(nbrs, nbrs->ids[i], &numNeighbors, &neighbors);
        fprintf(stderr, "-----=%d\n", ret);
        ck_assert_int_eq(ret, EXIT_SUCCESS);
        fprintf(stderr, "%d\n", numNeighbors);
        ck_assert_int_eq(numNeighbors, 2);
        fprintf(stderr, "%p\n", neighbors);
        fprintf(stderr, "%d\n", neighbors[0]);
        fprintf(stderr, "%d\n", neighbors[1]);
        switch (i) {
        case 0:
            ck_assert_int_eq(neighbors[0], 0);
            ck_assert_int_eq(neighbors[1], 1);
            break;
        case 1:
            ck_assert_int_eq(neighbors[0], 1);
            ck_assert_int_eq(neighbors[1], 0);
            break;
        case 2:
            ck_assert_int_eq(neighbors[0], 2);
            ck_assert_int_eq(neighbors[1], 3);
            break;
        case 3:
            ck_assert_int_eq(neighbors[0], 3);
            ck_assert_int_eq(neighbors[1], 2);
            break;
        case 4:
            ck_assert_int_eq(neighbors[0], 4);
            ck_assert_int_eq(neighbors[1], 5);
            break;
        case 5:
            ck_assert_int_eq(neighbors[0], 5);
            ck_assert_int_eq(neighbors[1], 4);
            break;
        }
        fprintf(stderr, "%d\n", ret);
        //free(neighbors);
        fprintf(stderr, "%d\n", ret);
    }
    harkio_Positions_delete(&nbrs->poses);
    harkio_Neighbors_delete(&nbrs);
}
END_TEST

START_TEST(test_append)
{
    harkio_Neighbors *nbrs = harkio_Neighbors_new(goodPositions());
    int size = nbrs->size;
    int n[3] = {100, 200};
    int n2[3] = {101, 201};
    int ret;

    ret = harkio_Neighbors_append(NULL, 100, 2, n);
    ck_assert_int_eq(ret, EXIT_FAILURE);

    ret = harkio_Neighbors_append(nbrs, 100, 2, n);
    ck_assert_int_eq(ret, EXIT_SUCCESS);
    ck_assert_int_eq(nbrs->size, size + 1);
    ck_assert_int_eq(nbrs->numNeighbors[nbrs->size - 1], 2);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][0], 100);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][1], 200);
    ck_assert_int_eq(nbrs->ids[nbrs->size - 1] ,100);

    ret = harkio_Neighbors_append(nbrs, 100, 2, n2);
    ck_assert_int_eq(ret, EXIT_SUCCESS);
    ck_assert_int_eq(nbrs->size, size + 1);
    ck_assert_int_eq(nbrs->numNeighbors[nbrs->size - 1], 4);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][0], 100);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][1], 200);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][2], 101);
    ck_assert_int_eq(nbrs->neighbors[nbrs->size - 1][3], 201);
    ck_assert_int_eq(nbrs->ids[nbrs->size - 1] ,100);

    harkio_Positions_delete(&nbrs->poses);
    harkio_Neighbors_delete(&nbrs);
}
END_TEST

START_TEST(test_compare)
{
    harkio_Neighbors *nbrs1 = harkio_Neighbors_new(goodPositions());
    harkio_Neighbors *nbrs2 = harkio_Neighbors_new(goodPositions());
    harkio_Neighbors *nbrs3 = harkio_Neighbors_new(goodPositions2());

    ck_assert_int_eq(harkio_Neighbors_isEqual(nbrs1, nbrs2), 1);
    ck_assert_int_eq(harkio_Neighbors_isEqual(nbrs1, nbrs3), 0);

    harkio_Positions_delete(&nbrs1->poses);
    harkio_Positions_delete(&nbrs2->poses);
    harkio_Positions_delete(&nbrs3->poses);
    harkio_Neighbors_delete(&nbrs1);
    harkio_Neighbors_delete(&nbrs2);
    harkio_Neighbors_delete(&nbrs3);
}
END_TEST

START_TEST(test_smallSetGet)
{
    harkio_Neighbors *nbrs = harkio_Neighbors_new(goodPositions());

    ck_assert_int_eq(harkio_Neighbors_getSize(nbrs), 5);
    ck_assert_int_eq(harkio_Neighbors_getSize(NULL), EXIT_FAILURE);

    harkio_Positions_delete(&nbrs->poses);
    harkio_Neighbors_delete(&nbrs);
}
END_TEST

START_TEST(test_euclidDistance)
{
    harkio_Position *pos_a, *pos_b;
    int i;
    float a_coords[][3] = {{3, 4, 0}, {0, 3, 4}, {4, 0, 3}};
    float b_coords[][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    float fdist, fdist_c;
    for (i = 0 ; i < 3 ; i++) {
        pos_a = harkio_Position_new(0, Cartesian, a_coords[i], NULL);
        pos_b = harkio_Position_new(0, Cartesian, b_coords[i], NULL);
        fdist = euclidDistance(pos_a, pos_b, 3);
        fdist_c = sqrt(pow(a_coords[i][0]-b_coords[i][0], 2.0)
                       + pow(a_coords[i][1]-b_coords[i][1], 2.0)
                       + pow(a_coords[i][2]-b_coords[i][2], 2.0));
        fdist_c = 5.0;
        ck_assert(abs(fdist - fdist_c) < 0.0001);

        harkio_Position_delete(&pos_a);
        harkio_Position_delete(&pos_b);
    }
}
END_TEST

Suite* str_suite(void)
{
    Suite *suite = suite_create("libharkio3_neighbors");


    TCase *tcase1 = tcase_create("new/delete");
    tcase_add_test(tcase1, test_new);
    tcase_add_test(tcase1, test_delete);
    tcase_add_test(tcase1, test_copy);
    suite_add_tcase(suite, tcase1);

    TCase *tcase2 = tcase_create("manipulation");
    tcase_add_test(tcase2, test_calcNeighbors);
    tcase_add_test(tcase2, test_append);
    ///tcase_add_test(tcase2, test_sub);
    ///tcase_add_test(tcase2, test_sum);
    ///tcase_add_test(tcase2, test_clear);
    suite_add_tcase(suite, tcase2);

    TCase *tcase3 = tcase_create("setter / getter");
    tcase_add_test(tcase3, test_getNeighbors);
    tcase_add_test(tcase3, test_smallSetGet);
    /// tcase_add_test(tcase3, test_setgetData);
    suite_add_tcase(suite, tcase3);

    TCase *tcase4 = tcase_create("utilities");
    // tcase_add_test(tcase4, test_size);
    tcase_add_test(tcase4, test_compare);
    tcase_add_test(tcase4, test_euclidDistance);
    suite_add_tcase(suite, tcase4);

    return suite;
}

int main(int argc, char *argv[])
{
    int number_failed;
    Suite *suite = str_suite();
    SRunner *runner = srunner_create(suite);
    srunner_run_all(runner, CK_NORMAL);
    number_failed = srunner_ntests_failed(runner);
    srunner_free(runner);
    return number_failed;
}
