#include "LKH.h"

/* The CVRP_InitialTour function computes an initial tour using the 
 * Clarke and Wright savings algorithm.
 */

#define ITERATIONS 10

#define Degree V
#define Size LastV
#define Load Loc

static Node *Find(Node * v);
static void Union(Node * x, Node * y);
static void MakeSets();
static void Distribute(int Constrained, double R);
static int compareValue(const void *s1, const void *s2);
static void CreateS();

typedef struct Saving {
    int Value, i, j;
} Saving;

static Saving *S;
static int Sets, SSize;

GainType CVRP_InitialTour()
{
    Node *N, *Last, *Next, *Tour;
    int s, Dim = Dimension - Salesmen + 1, it;
    GainType Cost, BestCost = PLUS_INFINITY, BestPenalty = PLUS_INFINITY;
    double EntryTime = GetTime();

    if (TraceLevel >= 1)
        printff("CVRP = ");
    assert(!Asymmetric);
    if (!S)
        CreateS();
    for (it = 0; it < ITERATIONS; it++) {
        for (s = 0; s < Salesmen; s++) {
            Tour = s == 0 ? Depot : &NodeSet[Dim + s];
            if (Tour == FirstNode)
                FirstNode = Tour->Suc;
            Follow(Tour, Depot);
            Tour->Dad = Tour;
            Tour->Prev = Tour->Next = 0;
            Tour->Degree = 0;
            Tour->Size = 1;
        }
        MakeSets();
        if (it > 0)
            Distribute(1, 0.01);
        if (Sets > Salesmen)
            Distribute(1, 0);
        if (Sets > Salesmen) {
            if (BestPenalty == 0)
                continue;
            Distribute(0, 0);
        }
        Sets += Salesmen;
        N = FirstNode;
        do {
            if (N->Degree <= 1 && (s = N->Special) > 0) {
                Tour = s == 1 ? Depot : &NodeSet[Dim + s - 1];
                Union(N, Tour);
                s = s == Salesmen ? 1 : s + 1;
                if (N->Degree <= 1) {
                    Tour = s == 1 ? Depot : &NodeSet[Dim + s - 1];
                    Union(N, Tour);
                }
            }
        } while ((N = N->Suc) != FirstNode);
        while (Sets > 0) {
            if (N->Degree <= 1) {
                for (s = 1; s <= Salesmen; s++) {
                    Tour = s == 1 ? Depot : &NodeSet[Dim + s - 1];
                    if (Tour->Degree <= 1 &&
                        (Sets == 1 || Find(N) != Find(Tour))) {
                        Union(N, Tour);
                        if (N->Degree <= 1)
                            N = N->Pred;
                        break;
                    }
                }
            }
            N = N->Suc;
        }
        Last = N = FirstNode = Depot;
        do {
            Next = N->Next != Last ? N->Next : N->Prev;
            Follow(N, Last);
            Last = N;
        } while ((N = Next) != FirstNode);
        N = FirstNode = Depot;
        Cost = 0;
        do {
            Node *Na = N->DepotId ? Depot : N;
            Node *Nb = N->Suc->DepotId ? Depot : N->Suc;
            Cost += OldDistance(Na, Nb);
        } while ((N = N->Suc) != FirstNode);
        Cost += ServiceTime * (Dim - 1);
        CurrentPenalty = PLUS_INFINITY;
        CurrentPenalty = Penalty();
        if (CurrentPenalty < BestPenalty ||
            (CurrentPenalty == BestPenalty && Cost < BestCost)) {
            N = FirstNode;
            while ((N = N->OldSuc = N->Suc) != FirstNode);
            BestCost = Cost;
            BestPenalty = CurrentPenalty;
        }
    }
    N = FirstNode;
    do
        (N->Suc = N->OldSuc)->Pred = N;
    while ((N = N->Suc) != FirstNode);
    Cost = BestCost;
    CurrentPenalty = BestPenalty;
    if (TraceLevel >= 1) {
        if (Salesmen > 1 || ProblemType == SOP)
            printff(GainFormat "_" GainFormat, CurrentPenalty, Cost);
        else
            printff(GainFormat, Cost);
        if (Optimum != MINUS_INFINITY && Optimum != 0)
            printff(", Gap = %0.2f%%", 100.0 * (Cost - Optimum) / Optimum);
        printff(", Time = %0.2f sec.\n", fabs(GetTime() - EntryTime));
    }
    if (Run == Runs) {
        free(S);
        S = 0;
    }
    return Cost;
}

void CreateS()
{
    int Dim = Dimension - Salesmen + 1, i, j;
    Node *Ni, *Nj;
    SSize = 0;
    S = (Saving *) malloc((Dim - 2) * (Dim - 1) / 2 * sizeof(Saving));
    /* Compute savings */
    for (i = 1; i < Dim; i++) {
        Ni = &NodeSet[i];
        if (Ni == Depot)
            continue;
        for (j = i + 1; j <= Dim; j++) {
            Nj = &NodeSet[j];
            if (Nj == Depot || Forbidden(Ni, Nj))
                continue;
            S[SSize].Value = FixedOrCommon(Ni, Nj) ? INT_MAX :
                OldDistance(Ni, Depot) +
                OldDistance(Depot, Nj) - OldDistance(Ni, Nj);
            S[SSize].i = i;
            S[SSize].j = j;
            SSize++;
        }
    }
    /* Rank the savings in descending order */
    qsort(S, SSize, sizeof(Saving), compareValue);
}

static Node *Find(Node * v)
{
    if (v != v->Dad)
        v->Dad = Find(v->Dad);
    return v->Dad;
}

static void Union(Node * x, Node * y)
{
    Node *u = Find(x), *v = Find(y);
    int d = x == y || x->DepotId || y->DepotId ? 0 :
            OldDistance(x, y) -
            OldDistance(x, Depot) - OldDistance(y, Depot);
    if (u->Size < v->Size) {
        u->Dad = v;
        v->Size += u->Size;
        v->Load += u->Load;
        v->Cost += u->Cost + d;
    } else {
        v->Dad = u;
        u->Size += v->Size;
        u->Load += v->Load;
        u->Cost += v->Cost + d;
    }
    if (x->Degree++ == 0)
        x->Prev = y;
    else
        x->Next = y;
    if (y->Degree++ == 0)
        y->Prev = x;
    else
        y->Next = x;
    Sets--;
}

static void MakeSets()
{
    Node *N = FirstNode;
    Sets = 0;
    do {
        N->Dad = N;
        N->Prev = N->Next = 0;
        N->Degree = 0;
        N->Size = 1;
        N->Load = N->Demand;
        N->Cost = 2 * OldDistance(N, Depot);
        Sets++;
    } while ((N = N->Suc) != FirstNode);
}

static void Distribute(int Constrained, double R)
{
    Node *Ni, *Nj, *u, *v;
    int i;

    for (i = 0; i < SSize && Sets > Salesmen; i++) {
        if (R > 0 && Random() % 1000 <= 1000 * R)
            continue;
        Ni = &NodeSet[S[i].i];
        Nj = &NodeSet[S[i].j];
        if (Ni->Degree < 2 && Nj->Degree < 2) {
            u = Find(Ni);
            v = Find(Nj);
            if (u == v)
                continue;
            if (!Constrained ||
                (u->Load + v->Load <= Capacity &&
                 (DistanceLimit == DBL_MAX ||
                  u->Cost + v->Cost + OldDistance(Ni, Nj) -
                  OldDistance(Ni, Depot) - OldDistance(Nj, Depot) +
                  (u->Size + v->Size) * ServiceTime <= DistanceLimit)))
                Union(Ni, Nj);
        }
    }
}

static int compareValue(const void *s1, const void *s2)
{
    int v1 = ((Saving *) s1)->Value;
    int v2 = ((Saving *) s2)->Value;
    return v1 > v2 ? -1 : v1 == v2 ? 0 : 1;
}
