#include "LKH.h"

/*
 * The CreateNNCandidateSet function creates for each node
 * a candidate set consisting of the K least costly neighbor edges.
 *
 * Time complexity is O(K*N*N).
 *
 * The function is called from the CreateCandidateSet and 
 * AddExtraCandidates functions. It is intended for use on non-geometric
 * and toroidal instances.
 */

static int compareX(const void *Na, const void *Nb);

void CreateNNCandidateSet(int K)
{
    Node **XNearList, **To, *Na, *Nb;
    int *Cost, d, a, b, k, Count, Forward;

    if (TraceLevel >= 2)
        printff("Creating NN candidate set ... \n");
    XNearList = (Node **) malloc(Dimension * sizeof(Node *));
    To = (Node **) malloc((K + 1) * sizeof(Node *));
    Cost = (int *) malloc((K + 1) * sizeof(int));
    for (Na = FirstNode, k = 0; k < Dimension; Na = Na->Suc, k++)
        XNearList[k] = Na;
    qsort(XNearList, Dimension, sizeof(Node *), compareX);
    for (a = 0; a < Dimension; a++) {
        Na = XNearList[a];
        Count = 0;
        for (Forward = 0; Forward <= 1; Forward++) {
            b = Forward ? a + 1 : a - 1;
            while (b >= 0 && b < Dimension) {
                Nb = XNearList[b];
                d = Distance(Na, Nb);
                k = Count < K ? Count++ : K;
                while (k > 0 && d < Cost[k - 1]) {
                    To[k] = To[k - 1];
                    Cost[k] = Cost[k - 1];
                    k--;
                }
                To[k] = Nb;
                Cost[k] = d;
                b = Forward ? b + 1 : b - 1;
            }
        }
        for (k = 0; k < Count; k++)
            AddCandidate(Na, To[k], D(Na, To[k]), 2);
    }
    free(Cost);
    free(To);
    free(XNearList);
    if (TraceLevel >= 2)
        printff("done\n");
}

static int compareX(const void *Na, const void *Nb)
{
    double x1 = (*(Node **) Na)->X;
    double y1 = (*(Node **) Na)->Y;
    double x2 = (*(Node **) Nb)->X;
    double y2 = (*(Node **) Nb)->Y;
    return x1 < x2 ? -1 : x1 > x2 ? 1 : y1 < y2 ? -1 : y1 > y2 ? 1 : 0;
}
