library(stats)
eurodistmat <- as.matrix(eurodist)

#A function for drawing routes
drawRoute <- function(sq, distances){
  #find the 2d coordinates
  loc <- -cmdscale(distances, add = TRUE)$points #PCA
  x <- loc[,1]; y <- loc[,2]
  s <- seq_len(nrow(distances))
  tspinit <- loc[c(sq, sq[1]),] #create a cycle
  #draw the cities and the inital solution
  plot(x, y, type = "n", asp = 1, xlab = "", ylab = "",
       main = "TSP route", axes = FALSE)
  arrows(tspinit[s,1], tspinit[s,2], tspinit[s+1,1], tspinit[s+1,2],
         angle = 10, col = "red")
  # Loop through the sequence and draw arrows with distances
  for (i in s) {
    j <- i + 1
    if(j > length(s)){
      j <- 1
    }
    xi <- tspinit[i,1]; yi <- tspinit[i,2]
    xj <- tspinit[j,1]; yj <- tspinit[j,2]
    arrows(xi, yi, xj, yj, angle = 10, col = "red")
    
    # Calculate and draw the distance on each arrow
    dist <- distances[sq[i], sq[j %% (nrow(distances) + 1)]]
    text((xi + xj) / 2, (yi + yj) / 2, labels = round(dist, 1), cex = 0.8, col = "blue")
  }
  text(x, y, labels(eurodist), cex = 0.8)
}


#Draw the starting solution
drawRoute(1:21, eurodistmat)

#Basic local search for TSP

# Define the objective function
objective_function <- function(solution) {
  # Evaluate the solution and return its objective value
  indexes <- embed(c(solution, solution[1]), 2)
  sum(eurodistmat[cbind(indexes[,2], indexes[,1])])
}

# Define the function to generate neighbors
generate_neighbors2 <- function(current_solution) {
  # Generate and return a list of neighboring solutions
  neighbors <- list()
  all_swaps <- combn(1:length(current_solution), 2)
  for(i in 1:ncol(all_swaps)){
    a <- all_swaps[1, i]
    b <- all_swaps[2, i]
    #cat(paste0(a, " - ", b, "\n"))
    i_neighbor <- current_solution
    i_neighbor[a] <- current_solution[b]
    i_neighbor[b] <- current_solution[a]
    neighbors[[i]] <- i_neighbor
  }
  neighbors
}
#Test
generate_neighbors2(1:21)

# Define the function for classical local search algorithm
local_search <- function(initial_solution, max_iterations = 1000) {
  current_solution <- initial_solution
  current_objective_value <- objective_function(current_solution)
  
  for (iteration in 1:max_iterations) {
    # Generate neighbors of the current solution
    neighbors <- generate_neighbors2(current_solution)
    
    # Evaluate the neighbors and find the best one
    best_neighbor <- NULL
    best_neighbor_objective_value <- Inf
    
    for (neighbor in neighbors) {
      neighbor_objective_value <- objective_function(neighbor)
      
      if (neighbor_objective_value < best_neighbor_objective_value) {
        best_neighbor <- neighbor
        best_neighbor_objective_value <- neighbor_objective_value
      }
    }
    
    # If the best neighbor is better than the current solution, 
    #update the current solution
    if (best_neighbor_objective_value < current_objective_value) {
      current_solution <- best_neighbor
      current_objective_value <- best_neighbor_objective_value
    } else {
      # If there is no improvement, stop the search
      break
    }
  }
  
  return(list(solution = current_solution, objective_value = current_objective_value))
}

local_search(1:21)

#Implementing variable neighborhood search VNS

#We need at least 2 neighborhoods for VNS
generate_neighbors3 <- function(current_solution, force3 = F) {
  # Generate and return a list of neighboring solutions
  neighbors <- list()
  all_swaps <- combn(1:length(current_solution), 3)
  for(v in 1:ncol(all_swaps)){
    a <- all_swaps[1, v]
    b <- all_swaps[2, v]
    c <- all_swaps[3, v]
    if(force3){
      i <- (v-1)*2
    }else{
      i <- (v-1)*5  
    }
    #acb
    if(!force3){
      i_neighbor <- current_solution
      i_neighbor[a] <- current_solution[a]
      i_neighbor[b] <- current_solution[c]
      i_neighbor[c] <- current_solution[b]
      neighbors[[i+1]] <- i_neighbor
    }
    #bac
    if(!force3){
      i_neighbor <- current_solution
      i_neighbor[a] <- current_solution[b]
      i_neighbor[b] <- current_solution[a]
      i_neighbor[c] <- current_solution[c]
      neighbors[[i+2]] <- i_neighbor
    }
    #bca
    i_neighbor <- current_solution
    i_neighbor[a] <- current_solution[b]
    i_neighbor[b] <- current_solution[c]
    i_neighbor[c] <- current_solution[a]
    neighbors[[i+3]] <- i_neighbor
    #cab
    i_neighbor <- current_solution
    i_neighbor[a] <- current_solution[c]
    i_neighbor[b] <- current_solution[a]
    i_neighbor[c] <- current_solution[b]
    neighbors[[i+4]] <- i_neighbor
    #cba
    if(!force3){
      i_neighbor <- current_solution
      i_neighbor[a] <- current_solution[c]
      i_neighbor[b] <- current_solution[b]
      i_neighbor[c] <- current_solution[a]
      neighbors[[i+5]] <- i_neighbor
    }
  }
  neighbors
}

# Shaking function - randomly selects one neighbor from a chosen neighborhood
# This part can be speed up greatly
shaking <- function(solution, k) {
  if (k == 1) {
    genereated_neigs <- generate_neighbors2(solution)
  } else if (k == 2) {
    genereated_neigs <- generate_neighbors3(solution)
  }
  n <- length(genereated_neigs)
  genereated_neigs[[sample(1:n, 1)]] #return a random neighbor in k-th neighborhood
}

# VNS algorithm
vns <- function(initial_solution, max_iterations, k_max) {
  current_solution <- initial_solution
  current_objective_value <- objective_function(current_solution)
  #MAX iterations for VNS(note: local search has its own max iterations value)
  for (iteration in 1:max_iterations) { 
    k <- 1
    while(k <= k_max) { #For each neighborhood
      # Shaking phase
      shaken_x <- shaking(current_solution, k) #choose a random neighbor
      
      # Local search phase
      ls_result <- local_search(shaken_x)
      improved_x <- ls_result$solution
      improved_objective_value <- ls_result$objective_value
      
      # Update the current solution if a better solution is found
      if (improved_objective_value < current_objective_value) {
        current_solution <- improved_x
        current_objective_value <- improved_objective_value
        cat("Better solution found at iteration ", iteration, 
            " with neighboorhood ", k, ".\n", sep = "")
        cat("New objective value: ", current_objective_value, "\n", sep = "")
        k <- 1
      }else{ #If a better solution is not found move to next neighborhood
        k <- k + 1
      }
    }
  }
  
  return(list(solution = current_solution, objective_value = current_objective_value))
}

# Run the VNS algorithm
result <- vns(1:21, 1000, 2)

result


#Lets add more neighborhoods by generalizing the neighborhood function

#We need a helper function to return all permutations
generate_permutations <- function(v) {
  if(length(v) == 1) {
    return(list(v))
  }
  
  result <- list()
  
  for (i in seq_along(v)) {
    sub_v <- v[-i]
    sub_permutations <- generate_permutations(sub_v)
    
    for (p in sub_permutations) {
      result <- append(result, list(c(v[i], p)))
    }
  }
  
  return(result)
}

#Generalized neighborhood function
generate_neighborsN <- function(current_solution, N = 4, forceN = T) {
  # Generate and return a list of neighboring solutions
  neighbors <- list()
  all_swaps <- combn(1:length(current_solution), N)
  for(v in 1:ncol(all_swaps)){
    s <- all_swaps[, v]
    perms <- generate_permutations(s)
    if(forceN)
    { #derangaments
      i <- (v-1)*(floor(factorial(N)/exp(1) + 0.5))
    }
    else
    {
      i <- (v-1)*(factorial(N)-1)
    }
    offset <- 1
    for(p in perms){
      if(forceN)  {
        if(sum(p == s) == 0){
          i_neighbor <- current_solution
          i_neighbor[s] <- current_solution[p]
          neighbors[[i+offset]] <- i_neighbor
          offset <- offset + 1
        }
      }else{
        i_neighbor <- current_solution
        i_neighbor[s] <- current_solution[p]
        neighbors[[i+offset]] <- i_neighbor
        offset <- offset + 1
      }
    }
  }
  neighbors
}

#Redefined shaking function
shaking <- function(solution, k) {
  if (k == 1) {
    genereated_neigs <- generate_neighbors2(solution)
  } else if (k == 2) {
    genereated_neigs <- generate_neighbors3(solution)
  } else {
    genereated_neigs <- generate_neighborsN(solution, N = k)
  }
  n <- length(genereated_neigs)
  genereated_neigs[[sample(1:n, 1)]] #return a random neighbor in k-th neighborhood
}

#Testing purposes
generate_neighborsN(c(1,2,3,4,5,6,7), 4, T)

#Run with 3rd neighborhood
result <- vns(1:21, 1000, 3)

#First possible improvement - use first descent instead of best descent
#Shaking does not need to generate all the neighbors
#To be implemented as an exercise