#include "Segment.h"
#include "LKH.h"
#include "Sequence.h"

/*
 * The PatchCycles function attempts to improve the tour by patching the
 * M >= 2 cycles that would appear if the move defined by t[1..2k] and
 * incl[1..2k] was made. If the composite move results in a shorter
 * tour, then the move is made, and the function returns the gain.
 *
 * On entry, Gain is the gain that could be obtained by making the non-
 * feasible move defined by t[1..2k] and incl[1..2k].
 *
 * The function tries to patch the cycles by interleaving the alternating
 * path represented by t with one or more alternating cycles.
 *
 * The function is called from BestKOptMove.
 */

static GainType PatchCyclesRec(int k, int m, int M, GainType G0);
static int ShortestCycle(int M, int k);
static int Cycle(Node * N, int k);

static int CurrentCycle, Patchwork = 0, RecLevel = 0;
#define MaxPatchwork Dimension

/*
 * The PatchCycles function tries to find a gainful move by patching the
 * cycles that would occur if the move represented by t[1..2k] and incl[1..2k]
 * was made using one one or more alternating cycles.
 * The alternating cycles are put in continuation of t, starting at 2k+1.
 */

GainType PatchCycles(int k, GainType Gain)
{
    Node *s1, *s2, *sStart, *sStop, *SUCFirstNode = SUC(FirstNode);
    GainType NewGain;
    int M, i;

    FindPermutation(k);
    M = Cycles(k);
    PenaltyGain = 0;
    if (M == 1 && (CurrentPenalty != 0 || TSPTW_Makespan || Gain > 0)) {
        MakeKOptMove(k);
        if (Improvement(&Gain, FirstNode, SUCFirstNode))
            return Gain;
    }
    if (M == 1 || M > PatchingC || k + M > NonsequentialMoveType)
        return 0;
    if (RecLevel == 0)
        Patchwork = 0;
    CurrentCycle = ShortestCycle(M, k);
    for (i = 0; i < k; i++) {
        if (cycle[p[2 * i]] != CurrentCycle)
            continue;
        sStart = t[p[2 * i]];
        sStop = t[p[2 * i + 1]];
        for (s1 = sStart; s1 != sStop; s1 = s2) {
            s2 = SUC(s1);
            if (FixedOrCommon(s1, s2))
                continue;
            if (++Patchwork > MaxPatchwork)
                return 0;
            t[2 * k + 1] = s1;
            t[2 * k + 2] = s2;
            MarkDeleted(s1, s2);
            /* Find a set of gainful alternating cycles */
            NewGain = PatchCyclesRec(k, 2, M, Gain + C(s1, s2));
            UnmarkDeleted(s1, s2);
            if (PenaltyGain > 0 || NewGain > 0)
                return NewGain;
        }
    }
    return 0;
}

static GainType PatchCyclesRec(int k, int m, int M, GainType G0)
{
    Node *s1, *s2, *s3, *s4, *s5, *s6, *S3 = 0, *S4 = 0, *SUCs1;
    Candidate *Ns2, *Ns4;
    GainType G1, G2, G3, G4, Gain, CloseUpGain,
        BestCloseUpGain = PatchingAExtended ? MINUS_INFINITY : 0;
    int X4, X6;
    int i, NewCycle, *cycleSaved = 0, *pSaved = 0;
    int Breadth2 = 0, Breadth4;

    s1 = t[2 * k + 1];
    s2 = t[i = 2 * (k + m) - 2];
    incl[incl[i] = i + 1] = i;
    SUCs1 = SUC(s1);

    /* Choose (s2,s3) as a candidate edge emanating from s2 */
    for (Ns2 = s2->CandidateSet; (s3 = Ns2->To); Ns2++) {
        if (s3 == s2->Pred || s3 == s2->Suc || Added(s2, s3) ||
            (NewCycle = Cycle(s3, k)) == CurrentCycle)
            continue;
        if (++Breadth2 > MaxBreadth)
            break;
        MarkAdded(s2, s3);
        t[2 * (k + m) - 1] = s3;
        G1 = G0 - Ns2->Cost;
        /* Choose s4 as one of s3's two neighbors on the tour */
        for (X4 = 1; X4 <= 2; X4++) {
            s4 = X4 == 1 ? s3->Pred : s3->Suc;
            if (FixedOrCommon(s3, s4) || Deleted(s3, s4))
                continue;
            MarkDeleted(s3, s4);
            t[2 * (k + m)] = s4;
            G2 = G1 + C(s3, s4);
            if (M > 2) {
                if (!cycleSaved) {
                    cycleSaved = (int *) malloc(2 * k * sizeof(int));
                    memcpy(cycleSaved, cycle + 1, 2 * k * sizeof(int));
                }
                for (i = 1; i <= 2 * k; i++)
                    if (cycle[i] == NewCycle)
                        cycle[i] = CurrentCycle;
                /* Extend the current alternating path */
                Gain = PatchCyclesRec(k, m + 1, M - 1, G2);
                if (PenaltyGain > 0 || Gain > 0) {
                    UnmarkAdded(s2, s3);
                    UnmarkDeleted(s3, s4);
                    goto End_PatchCyclesRec;
                }
                memcpy(cycle + 1, cycleSaved, 2 * k * sizeof(int));
                if (PatchingA >= 2 && Patchwork < MaxPatchwork &&
                    k + M < NonsequentialMoveType &&
                    !Forbidden(s4, s1) &&
                    (!PatchingARestricted || IsCandidate(s4, s1))) {
                    GainType Bound = BestCloseUpGain >= 0 ||
                        IsCandidate(s4, s1) ? BestCloseUpGain : 0;
                    if ((!c || G2 - c(s4, s1) > Bound) &&
                        (CloseUpGain = G2 - C(s4, s1)) > Bound) {
                        S3 = s3;
                        S4 = s4;
                        BestCloseUpGain = CloseUpGain;
                    }
                }
            } else if (!Forbidden(s4, s1) &&
                       (CurrentPenalty != 0 ||
                        (!c || G2 - c(s4, s1) > 0))) {
                Gain = G2 - C(s4, s1);
                if (CurrentPenalty != 0 || TSPTW_Makespan || Gain > 0) {
                    if (!pSaved) {
                        pSaved = (int *) malloc(2 * k * sizeof(int));
                        memcpy(pSaved, p + 1, 2 * k * sizeof(int));
                    }
                    incl[incl[2 * k + 1] = 2 * (k + m)] = 2 * k + 1;
                    MakeKOptMove(k + m);
                    if (Improvement(&Gain, s1, SUCs1)) {
                        UnmarkAdded(s2, s3);
                        UnmarkDeleted(s3, s4);
                        goto End_PatchCyclesRec;
                    }
                    memcpy(p + 1, pSaved, 2 * k * sizeof(int));
                    for (i = 1; i <= 2 * k; i++)
                        q[p[i]] = i;
                }
            }
            UnmarkDeleted(s3, s4);
        }
        UnmarkAdded(s2, s3);
    }
    if (M == 2 && !PatchingCRestricted) {
        /* Try to patch the two cycles by a sequential 3-opt move */
        incl[incl[2 * (k + m)] = 2 * (k + m) + 1] = 2 * (k + m);
        incl[incl[2 * k + 1] = 2 * (k + m) + 2] = 2 * k + 1;
        Breadth2 = 0;
        /* Choose (s2,s3) as a candidate edge emanating from s2 */
        for (Ns2 = s2->CandidateSet; (s3 = Ns2->To); Ns2++) {
            if (s3 == s2->Pred || s3 == s2->Suc || Added(s2, s3))
                continue;
            if (++Breadth2 > MaxBreadth)
                break;
            t[2 * (k + m) - 1] = s3;
            G1 = G0 - Ns2->Cost;
            NewCycle = Cycle(s3, k);
            /* Choose s4 as one of s3's two neighbors on the tour */
            for (X4 = 1; X4 <= 2; X4++) {
                s4 = X4 == 1 ? s3->Pred : s3->Suc;
                if (FixedOrCommon(s3, s4) || Deleted(s3, s4))
                    continue;
                t[2 * (k + m)] = s4;
                G2 = G1 + C(s3, s4);
                Breadth4 = 0;
                /* Choose (s4,s5) as a candidate edge emanating from s4 */
                for (Ns4 = s4->CandidateSet; (s5 = Ns4->To); Ns4++) {
                    if (s5 == s4->Pred || s5 == s4->Suc || s5 == s1 ||
                        Added(s4, s5) ||
                        (NewCycle == CurrentCycle &&
                         Cycle(s5, k) == CurrentCycle))
                        continue;
                    if (++Breadth4 > MaxBreadth)
                        break;
                    G3 = G2 - Ns4->Cost;
                    /* Choose s6 as one of s5's two neighbors on the tour */
                    for (X6 = 1; X6 <= 2; X6++) {
                        s6 = X6 == 1 ? s5->Pred : s5->Suc;
                        if (s6 == s1 || Forbidden(s6, s1)
                            || FixedOrCommon(s5, s6)
                            || Deleted(s5, s6)
                            || Added(s6, s1))
                            continue;
                        G4 = G3 + C(s5, s6);
                        if (!pSaved) {
                            pSaved = (int *) malloc(2 * k * sizeof(int));
                            memcpy(pSaved, p + 1, 2 * k * sizeof(int));
                        }
                        Gain = G4 - C(s6, s1);
                        if (CurrentPenalty != 0 ||
                            TSPTW_Makespan || Gain > 0) {
                            t[2 * (k + m) + 1] = s5;
                            t[2 * (k + m) + 2] = s6;
                            if (FeasibleKOptMove(k + m + 1)) {
                                MakeKOptMove(k + m + 1);
                                if (Improvement(&Gain, s1, SUCs1))
                                    goto End_PatchCyclesRec;
                            }
                        }
                        memcpy(p + 1, pSaved, 2 * k * sizeof(int));
                        for (i = 1; i <= 2 * k; i++)
                            q[p[i]] = i;
                    }
                }
            }
        }
    }
    Gain = 0;
    if (S4) {
        int OldCycle = CurrentCycle;
        if (!pSaved) {
            pSaved = (int *) malloc(2 * k * sizeof(int));
            memcpy(pSaved, p + 1, 2 * k * sizeof(int));
        }
        t[2 * (k + m) - 1] = S3;
        t[2 * (k + m)] = S4;
        incl[incl[2 * k + 1] = 2 * (k + m)] = 2 * k + 1;
        /* Find a new alternating cycle */
        PatchingA--;
        RecLevel++;
        MarkAdded(s2, S3);
        MarkDeleted(S3, S4);
        MarkAdded(S4, s1);
        Gain = PatchCycles(k + m, BestCloseUpGain);
        UnmarkAdded(s2, S3);
        UnmarkDeleted(S3, S4);
        UnmarkAdded(S4, s1);
        RecLevel--;
        PatchingA++;
        if (PenaltyGain <= 0 && Gain <= 0) {
            memcpy(cycle + 1, cycleSaved, 2 * k * sizeof(int));
            memcpy(p + 1, pSaved, 2 * k * sizeof(int));
            for (i = 1; i <= 2 * k; i++)
                q[p[i]] = i;
            CurrentCycle = OldCycle;
        }
    }
  End_PatchCyclesRec:
    free(cycleSaved);
    free(pSaved);
    return Gain;
}

/*
 * The Cycle function returns the number of the cycle containing
 * a given node, N.
 *
 * Time complexity: O(log k).
 */

static int Cycle(Node * N, int k)
{
    /* Binary search */
    int Low = 1, High = k;
    while (Low < High) {
        int Mid = (Low + High) / 2;
        if (BETWEEN(t[p[2 * Low]], N, t[p[2 * Mid + 1]]))
            High = Mid;
        else
            Low = Mid + 1;
    }
    return cycle[p[2 * Low]];
}

/*
 * The ShortestCycle function returns the number of the cycle with
 * the smallest number of nodes. Note however that if the two-level
 * list is used, the number of nodes of each cycle is only approximate
 * (for efficiency reasons).
 *
 * Time complexity: O(k + M), where M = Cycles(k).
 *
 * The function may only be called after a call of the Cycles function.
 */

static int ShortestCycle(int M, int k)
{
    int i, Cycle, MinCycle = 0;
    int *Size, MinSize = INT_MAX;

    Size = (int *) calloc(1 + M, sizeof(int));
    p[0] = p[2 * k];
    for (i = 0; i < 2 * k; i += 2)
        Size[cycle[p[i]]] += SegmentSize(t[p[i]], t[p[i + 1]]);
    for (Cycle = 1; Cycle <= M; Cycle++) {
        if (Size[Cycle] < MinSize) {
            MinSize = Size[Cycle];
            MinCycle = Cycle;
        }
    }
    free(Size);
    return MinCycle;
}
