Penalty_ Capacitated vehicle routing problem with multiple time windows

Question

Brief introduction:

LKH-3 is an implementation of the Lin-Kernighan traveling salesman heuristic. The goal is to build an LKH-3 version able to solve the following problem type:

CVRPMTW: Capacitated vehicle routing problem with multiple time windows

Check: comment that says "how to add waiting time"

What I need to do:

Output waiting time (Earliest) and in which node we are facing waiting time

---

Sample for clear understanding:

|--------|____________________|--------|

Waiting | Time window node N | Penalty

GainType Penalty_CVRPMTW()
{
    static Node *StartRoute = 0;
    Node *N, *NextN, *CurrentRoute;
    GainType CostSum, DemandSum, P = 0;
    int Forward = SUCC(Depot)->Id != Depot->Id + DimensionSaved;
    int j;
    int RouteCounter = 0;

    if (!StartRoute)
        StartRoute = Depot;
    if (StartRoute->Id > DimensionSaved)
        StartRoute -= DimensionSaved;
    N = StartRoute;
    do {
        CurrentRoute = N;
        CostSum = DemandSum = 0;
        do {
            if (N->Id <= Dim && N != Depot) {
                if ((DemandSum += N->Demand) > Capacity)
                        P += DemandSum - Capacity;

                if (MultipleTimeWindowPenaltyMode == PENALTY_ON_EARLIEST || MultipleTimeWindowPenaltyMode == NO) {
                    // Loop over the earliest values of the node's time windows and find the appropriate time window
                    if (CostSum > N->Latest)
                        P += CostSum - N->Latest;
                    else {
                        for (j = 0; j < N->NumberTimeWindows; j++){
                            if (CostSum < N->EarliestList[j]){
                                if (MultipleTimeWindowPenaltyMode == NO) {
                                    *CostSum = N->EarliestList[j]*; //how can i add the waiting time in this line?
                                }
                                else if (MultipleTimeWindowPenaltyMode == PENALTY_ON_EARLIEST) {
                                    // CostSum = N->EarliestList[j]; /* Enable this to also add the waiting time to the cost */
                                    P += N->EarliestList[j] - CostSum;
                                }
                                break;
                            } else if (CostSum >= N->EarliestList[j] && CostSum <= N->LatestList[j]){
                                break;
                            }
                        }
                    }  
                }