library("TSP")

##' global mutation operator EA
##'
##' @param inst - problem instance we want to mutate
##' @param rate - Mutation probability
##' @return (x,y) coordinates
uniform_mutation <- function(inst, rate) {
  cities_to_mutate <- which(runif(nrow(inst)) < rate)
  inst[cities_to_mutate,] <- matrix(runif(2*length(cities_to_mutate)), ncol=2)
  inst
}

##' local mutation operator EA
##'
##' @param inst - problem instance we want to mutate
##' @param rate - Mutation probability
##' @param sd - standard deviation of normal noise
##' @return (x,y) coordinates
normal_mutation <- function(inst, mutOp = 0.1, sigma=0.0025) {
  cities_to_mutate <- which(runif(nrow(inst)) < mutOp)
  ## pmin(pmax(...)) used to ensure we stay in bounds:
  if (length(cities_to_mutate) > 0) {
    delta <- matrix(rnorm(2*length(cities_to_mutate), sd=sigma), ncol=2)
    inst[cities_to_mutate,] <- pmin(pmax(inst[cities_to_mutate,] + delta, 0), 1)
  }
  inst
}

##' mating pool generation
##'
##' @param poolSize - number of instances to be included to the mating pool
##' @param population - source population for mating pool generation
##' @param fitness - fitness of members of the population
##' @return (x,y) coordinates
create_mating_pool <- function(poolSize, population, fitness) {
  matingPool <- list()
  ## save population size
  n <- length(population)
  for (i in 1:poolSize) {
    ## select two random instances from population
    idx <- sample(1:n, 2)
    ## put member with better fitness value into the mating pool
    matingPool[[i]] <- if (fitness[idx[1]] >= fitness[idx[2]]) { 
      population[[idx[1]]]
    } else {
      population[[idx[2]]]
    }
  }
  return(matingPool)
}


##' round instance 
##' points are placed in the center of the grids 
##' @param n : number of cells desired (grid resolution)
##' @return rounded instance
round_grid <- function(instance,n=100){
   gr  <- seq(0,1,1/n)
   rnd_grid_pt  <- apply(instance,2,function(x){ sapply(x, function(y) {gr[which.min((y-gr)[(y-gr)>=0])] })})  
   
   #avoid outliers outside boundary
   helper <- function(x){
       if(all(x!=1)){ y <- x + 1/(2*n)}
       if(all(x==1)){ y <- x - 1/(2*n)}
       if((x[2]==1) & (x[1] != 1)){ y<- c(x[1] + 1/(2*n),x[2]-1/(2*n))}
       if((x[1]==1) & (x[2] != 1)){ y<- c(x[1] - 1/(2*n),x[2]+1/(2*n))}       
       return(y)
   }
   
   rnd_instance <- t(apply(rnd_grid_pt,1,helper))
   return(rnd_instance)
}



##' TSP generating EA
##'
##' @param popSize Number of tsp instances maintained in each population
##' @param instSize Number of nodes for each tsp instance
##' @param generations Number of generations
##' @param time_limit Time limit in seconds.
##' @param uniform_mutation_rate Mutation probability in uniform mutation (in [0,1])
##' @param normal_mutation_rate Mutation probability in normal mutation (in [0,1])
##' @param normal_mutation_sd Standard deviation of normal noise in normal mutation 
##' @param cells_round Grid resolution for rounding
##' @param fitness_function Fitness function used to judge the
##'   fitness of a TSP instance.
##' @param rnd Round the coordinates before
##'   normal mutation.
##' 
##' @return list TSP instance 
tsp_generation_ea <- function(popSize = 30,
                              instSize = 50,
                              generations = 100,
                              time_limit = 60 * 60, # 1h
                              uniform_mutation_rate,
                              normal_mutation_rate,
                              normal_mutation_sd,
                              cells_round=100,
                              fitness_function,
                              rnd=TRUE) {
  ## size of mating pool is half of the population size
  poolSize <- round(popSize / 2)
  
  ## define overall best problem instance according to fitness value
  overallBest <- NULL
  overallBestFitness <- Inf
  
  ## build initial population randomly by selecting 2*instSize randoms
  ## (i.e. instSize coordinates) from a R[0,1] distribution
  coords <- matrix(runif(popSize*2*instSize), ncol = 2)
  population <- list()
  for (i in 1:popSize) {
    population_scale <- rescale_instance(coords[((i-1)*instSize + 1):(i*instSize), ])
    population[[i]]  <- round_grid(population_scale,cells_round)
    if(rnd==TRUE){
        population[[i]] <- normal_mutation(population[[i]], normal_mutation_rate, normal_mutation_sd)
    }        
  }

  start_time <- proc.time()[[3]]
  ## do the evolutian baby!
  generational_fitness <- numeric(generations)
  for (g in 1:generations) {
    
    ## apply 2-opt to all instances in current population
    fitness <- sapply(population, fitness_function)
    current_time <- proc.time()[[3]]
    if (current_time - start_time > time_limit) {
      warning("Time limit reached.")
      break
    }

    currentBest <- population[[which.min(fitness)]]
    currentBestFitness <- min(fitness)
    generational_fitness[g] <- currentBestFitness
    if (currentBestFitness < overallBestFitness) {
      overallBestFitness <- currentBestFitness
      overallBest <- currentBest
    }
    matingPool <- create_mating_pool(poolSize, population, -fitness)
    message(sprintf("%4i %5.3f %5.3f %3i",
                    g, overallBestFitness, currentBestFitness,
                    which.max(fitness)))

    ## inspired by the LION 2010 paper we use 1-elitism, i.e. the
    ## "best" instance of current population survives
    nextPopulation <- vector(length(population), mode="list")
    nextPopulation[[1]] <- currentBest    
    ## two tournament selection
    for (k in 2:popSize) {
      ## choose two instances randomly from mating pool
      idx <- sample(1:poolSize, 2)
      parent1 <- matingPool[[idx[1]]]
      parent2 <- matingPool[[idx[2]]]
      
      ## create empty child
      offspring <- matrix(ncol = 2, nrow = instSize)
      
      ## uniform crossover
      for (j in 1:instSize) {
        if (runif(1) < 0.5) {
          offspring[j, ] <- parent1[j, ]
        } else {
          offspring[j, ] <- parent2[j, ]
        }        
      }
      
      ## mutation
      offspring <- uniform_mutation(offspring, uniform_mutation_rate)
      
      if(rnd==TRUE) {
        offspring <- rescale_instance(offspring)
        offspring <- round_grid(offspring, cells_round)
        offspring <- normal_mutation(offspring, normal_mutation_rate, normal_mutation_sd)
      }else {
        offspring <- normal_mutation(offspring, normal_mutation_rate, normal_mutation_sd)
        offspring <- rescale_instance(offspring)
        offspring <- round_grid(offspring, cells_round)
      }
      nextPopulation[[k]] <- offspring
    }
    ## replace population
    population <- nextPopulation
  }
  return(list(par=currentBest,
              value=currentBestFitness,
              fitness=generational_fitness,
              last_population=population))
}
