#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <stdint.h>
#include <errno.h>
#include <assert.h>
#include <stdbool.h>

#include "record.h"
#include "euclidean_distance.h"
#include "coord_query.h"

struct record* closest_point;
double closest_point_distance;

int lon_comp_func(const void* a, const void* b) {
    return (
        ((struct record*)a)->lon > ((struct record*)b)->lon
    );
}

int lat_comp_func(const void* a, const void* b) {
    return (
        ((struct record*)a)->lat > ((struct record*)b)->lat
    );
}

struct node {
    struct record* record;
    double point[2];
    int axis;
    struct node* left;
    struct node* right;
};

struct node* kdtree(struct record* points, int depth, int n) {
    int axis = depth % 2;

    if (axis == 0) {
        qsort(points, n, sizeof(struct record), lon_comp_func);
    } else {
        qsort(points, n, sizeof(struct record), lat_comp_func);
    }
    struct record* median = &points[n / 2];
    struct node* new_node = malloc(sizeof(struct node));
    new_node->record = median;
    new_node->point[0] = median->lon;
    new_node->point[1] = median->lat;
    new_node->axis = axis;
    if (n/2 != 0) {
        new_node->left = kdtree(points, depth+1, n/2);
    } else {
        new_node->left = NULL;
    }
    if (n/2 + 1 <= n - 1) {
        new_node->right = kdtree(&points[n/2 + 1], depth+1, n/2 - 1 + (n % 2));
    } else {
        new_node->right = NULL;
    }

    return new_node;
}

struct node* mk_kdtree(struct record* rs, int n) {
    struct node* data = kdtree(rs, 0, n);

    return data;
}

void free_kdtree(struct node* data) {
    if (data == NULL) {
        return;
    }

    struct node* left = data->left;
    struct node* right = data->right;
    free(data);

    free_kdtree(left);
    free_kdtree(right);
}

void lookup(double query[2], struct node* tree) {
    if (tree == NULL) {
        return;
    }

    double compare_coord[2] = {tree->point[0], tree->point[1]};
    double dist = calc_euclidean(query, compare_coord);

    double closest_coords[2] = {closest_point->lon, closest_point->lat};
    closest_point_distance = calc_euclidean(query, closest_coords);
    if (dist < closest_point_distance) {
        closest_point = tree->record;
    }

    double diff = tree->point[tree->axis] - query[tree->axis];

    if (diff >= 0 || closest_point_distance > fabs(diff)) {
        lookup(query, tree->left);
    }

    if (diff <= 0 || closest_point_distance > fabs(diff)) {
        lookup(query, tree->right);
    }

    return;
}

const struct record* lookup_kdtree(struct node *data, double lon, double lat) {
    closest_point = data->record;
    double query[2] = {lon, lat};
    double compare_coord[2] = {data->point[0], data->point[1]};
    closest_point_distance = calc_euclidean(query, compare_coord);

    lookup(query, data);

    return closest_point;
}

int main(int argc, char** argv) {
    return coord_query_loop(argc, argv,
                                                    (mk_index_fn)mk_kdtree,
                                                    (free_index_fn)free_kdtree,
                                                    (lookup_fn)lookup_kdtree);
}