#include "LKH.h"
#include "GeoConversion.h"

/*
 * The SolveKMeansSubproblems function attempts to improve a given tour
 * by means of a partitioning scheme based on K-means clustering.
 *
 * The overall region containing the nodes is subdivided into K clusters,
 * where K = ceil(Dimension/SubproblemSize). Each cluster together with
 * the given tour induces a subproblem consisting of all nodes in the
 * cluster and with edges fixed between nodes that are connected by tour
 * segments whose interior points do not belong to the cluster.
 *  
 * If an improvement is found, the new tour is written to TourFile.
 * The original tour is given by the SubproblemSuc references of the nodes.
 */

static void KMeansClustering(int K);

void SolveKMeansSubproblems()
{
    Node *N;
    GainType GlobalBestCost, OldGlobalBestCost;
    double EntryTime = GetTime();
    int CurrentSubproblem, Subproblems;

    AllocateStructures();
    ReadPenalties();

    /* Compute upper bound for the original problem */
    GlobalBestCost = 0;
    N = FirstNode;
    do {
        if (!Fixed(N, N->SubproblemSuc))
            GlobalBestCost += Distance(N, N->SubproblemSuc);
        N->Subproblem = 0;
    }
    while ((N = N->SubproblemSuc) != FirstNode);
    if (TraceLevel >= 1) {
        if (TraceLevel >= 2)
            printff("\n");
        printff("*** K-means partitioning *** [Cost = " GainFormat "]\n",
                GlobalBestCost);
    }

    Subproblems = (int) ceil((double) Dimension / SubproblemSize);
    KMeansClustering(Subproblems);
    for (CurrentSubproblem = 1;
         CurrentSubproblem <= Subproblems; CurrentSubproblem++) {
        OldGlobalBestCost = GlobalBestCost;
        SolveSubproblem(CurrentSubproblem, Subproblems, &GlobalBestCost);
        if (SubproblemsCompressed && GlobalBestCost == OldGlobalBestCost)
            SolveCompressedSubproblem(CurrentSubproblem, Subproblems,
                                      &GlobalBestCost);
    }
    printff("\nCost = " GainFormat, GlobalBestCost);
    if (Optimum != MINUS_INFINITY && Optimum != 0)
        printff(", Gap = %0.4f%%",
                100.0 * (GlobalBestCost - Optimum) / Optimum);
    printff(", Time = %0.2f sec. %s\n", fabs(GetTime() - EntryTime),
            GlobalBestCost < Optimum ? "<" : GlobalBestCost ==
            Optimum ? "=" : "");
    if (SubproblemBorders && Subproblems > 1)
        SolveSubproblemBorderProblems(Subproblems, &GlobalBestCost);
}

#define M Beta

/*
 * The KMeansClustering function performs K-means clustering. Each node
 * is given a unique cluster number in its Subproblem field.
 *
 * The algorithm is accellerated using ideas from
 *
 *     Dan Judd, Philip K. McKinley, Anil K. Jain:
 *     Large-Scale Parallel Data Clustering.
 *     IEEE Transactions on Pattern Analysis and
 *     Machine Intelligence 20(8):871-876 (1998)
 */

static void KMeansClustering(int K)
{
    Node *Center, **CenterRef, **Perm, *N, Old;
    int *Count, i, j, d, OldSubproblem;
    double *SumXc, *SumYc, *SumZc, Xc, Yc, Zc;
    int *Movement, *MMax, Max;
    int Moving = 0;
    CostFunction OldDistance = Distance;

    Center = (Node *) calloc((K + 1), sizeof(Node));
    CenterRef = (Node **) calloc((K + 1), sizeof(Node *));
    SumXc = (double *) calloc(K + 1, sizeof(double));
    SumYc = (double *) calloc(K + 1, sizeof(double));
    SumZc = (double *) calloc(K + 1, sizeof(double));
    Count = (int *) calloc(K + 1, sizeof(int));
    Movement = (int *) calloc(K + 1, sizeof(int));
    MMax = (int *) calloc(K + 1, sizeof(int));
    Perm = (Node **) malloc(Dimension * sizeof(Node *));

    /* Pick random initial centers */
    for (i = 0; i < Dimension; i++)
        Perm[i] = &NodeSet[i + 1];
    for (j = 1; j < Dimension; j++) {
        i = Random() % j;
        N = Perm[j];
        Perm[j] = Perm[i];
        Perm[i] = N;
    }
    for (i = 1; i <= K; i++) {
        Center[i].X = Perm[i - 1]->X;
        Center[i].Y = Perm[i - 1]->Y;
        Center[i].Z = Perm[i - 1]->Z;
    }

    /* Assign each node to center 0 (a ghost cluster) */
    N = FirstNode;
    do {
        N->BestPi = N->Pi;
        N->Pi = 0;
        if (WeightType == GEO || WeightType == GEO_MEEUS)
            GEO2XYZ(N->X, N->Y, &N->Xc, &N->Yc, &N->Zc);
        else if (WeightType == GEOM || WeightType == GEOM_MEEUS)
            GEOM2XYZ(N->X, N->Y, &N->Xc, &N->Yc, &N->Zc);
        else {
            N->Xc = N->X;
            N->Yc = N->Y;
            N->Zc = N->Z;
        }
        N->Cost = INT_MAX / 2;
        N->M = INT_MIN;
        N->Subproblem = N->LastV = 0;
        SumXc[0] += N->Xc;
        SumYc[0] += N->Yc;
        SumZc[0] += N->Zc;
        Count[0]++;
    } while ((N = N->Suc) != FirstNode);
    Xc = Center[0].Xc = SumXc[0] / Count[0];
    Yc = Center[0].Yc = SumYc[0] / Count[0];
    Zc = Center[0].Zc = SumZc[0] / Count[0];
    if (WeightType == GEO || WeightType == GEO_MEEUS)
        XYZ2GEO(Xc, Yc, Zc, &Center[0].X, &Center[0].Y);
    if (WeightType == GEOM || WeightType == GEOM_MEEUS)
        XYZ2GEOM(Xc, Yc, Zc, &Center[0].X, &Center[0].Y);
    else {
        Center[0].X = Xc;
        Center[0].Y = Yc;
        Center[0].Z = Zc;
    }
    if (Distance == Distance_TOR_2D)
        Distance = Distance_EUC_2D;
    else if (Distance == Distance_TOR_3D)
        Distance = Distance_EUC_3D;
    for (;;) {
        /* Assign each node to its closest center */
        Moving = 0;
        do {
            N->M -= Movement[N->Subproblem] + MMax[N->Subproblem];
            if (N->M < 0) {
                OldSubproblem = N->Subproblem;
                if (OldSubproblem == 0)
                    N->Cost = N->NextCost = INT_MAX;
                else {
                    N->Cost =
                        Distance(N, &Center[N->Subproblem]) * Precision;
                    N->NextCost =
                        Distance(N, &Center[N->LastV]) * Precision;
                    if (N->Cost > N->NextCost) {
                        i = N->LastV;
                        N->LastV = N->Subproblem;
                        N->Subproblem = i;
                        d = N->NextCost;
                        N->NextCost = N->Cost;
                        N->Cost = d;
                    }
                }
                for (i = 1; i <= K; i++) {
                    if (i == N->Subproblem || i == N->LastV)
                        continue;
                    d = INT_MIN;
                    if ((!c || c(N, &Center[i]) <= N->Cost) &&
                        (d =
                         Distance(N, &Center[i]) * Precision) <= N->Cost) {
                        N->NextCost = N->Cost;
                        N->Cost = d;
                        N->LastV = N->Subproblem;
                        if (d < N->NextCost)
                            N->Subproblem = i;
                    } else if (d < N->NextCost &&
                               (!c || c(N, &Center[i]) < N->NextCost) &&
                               (d != INT_MIN || (d =
                                                 Distance(N,
                                                          &Center[i]) *
                                                 Precision) <
                                N->NextCost)) {
                        N->NextCost = d;
                        N->LastV = i;
                    }
                }
                N->M = N->NextCost - N->Cost;
                if (N->Subproblem != OldSubproblem) {
                    Moving++;
                    SumXc[OldSubproblem] -= N->Xc;
                    SumYc[OldSubproblem] -= N->Yc;
                    SumZc[OldSubproblem] -= N->Zc;
                    Count[OldSubproblem]--;
                    SumXc[N->Subproblem] += N->Xc;
                    SumYc[N->Subproblem] += N->Yc;
                    SumZc[N->Subproblem] += N->Zc;
                    Count[N->Subproblem]++;
                }
            }
        } while ((N = N->Suc) != FirstNode);
        if (!Moving)
            break;
        if (TraceLevel >= 2)
            printff("Moving %d %s\n", Moving,
                    Moving > 1 ? "points" : "point");
        /* Move centers */
        Max = INT_MIN;
        for (i = 1; i <= K; i++) {
            if (Count[i] > 0) {
                Old.X = Center[i].X;
                Old.Y = Center[i].Y;
                Old.Z = Center[i].Z;
                Xc = Center[i].Xc = SumXc[i] / Count[i];
                Yc = Center[i].Yc = SumYc[i] / Count[i];
                Zc = Center[i].Zc = SumZc[i] / Count[i];
                if (WeightType == GEO || WeightType == GEO_MEEUS)
                    XYZ2GEO(Xc, Yc, Zc, &Center[i].X, &Center[i].Y);
                else if (WeightType == GEOM || WeightType == GEOM_MEEUS)
                    XYZ2GEOM(Xc, Yc, Zc, &Center[i].X, &Center[i].Y);
                else {
                    Center[i].X = Xc;
                    Center[i].Y = Yc;
                    Center[i].Z = Zc;
                }
                Movement[i] = Distance(&Old, &Center[i]) * Precision;
                if (Movement[i] > Max)
                    Max = Movement[i];
            } else
                Movement[i] = 0;
        }
        for (i = 1; i <= K; i++) {
            if (Movement[i] != Max)
                MMax[i] = Max;
            else {
                MMax[i] = INT_MIN;
                for (j = 1; j <= K; j++)
                    if (j != i && Movement[j] > MMax[i])
                        MMax[i] = Movement[j];
            }
        }
    }
    Distance = OldDistance;
    N = FirstNode;
    do
        N->Pi = N->BestPi;
    while ((N = N->Suc) != FirstNode);
    for (i = 1; i <= K; i++)
        CenterRef[i] = &Center[i];
    AdjustClusters(K, CenterRef);
    free(Center);
    free(CenterRef);
    free(SumXc);
    free(SumYc);
    free(SumZc);
    free(Count);
    free(Perm);
    free(Movement);
    free(MMax);
}
