if(!exists("ncust")){ncust = 10}
source(paste0(wd.path, "/probs.R"))
source(paste0(wd.path, "/utilities.R"))
source(paste0(wd.path, "/feasibility.R"))
source(paste0(wd.path, "/parameters.R"))
source(paste0(wd.path, "/moves.R"))
source(paste0(wd.path, "/intra_moves.R"))
source(paste0(wd.path, "/inter_moves.R"))
source(paste0(wd.path, "/shorthand_funcs.R"))

generate_initial_route <- function(functype = "tonn", show.steps = F,
                                   second.type = "edd", vehicles = NULL){
  vehicles = ifelse(is.null(vehicles), ceiling(sum(df$demand)/truck.capacity), vehicles)
  cap = ifelse(is.null(vehicles), truck.capacity, ceiling(sum(df$demand)/vehicles))
  print(functype)
  feasible = FALSE
  k = 1
  tr = 1
  while(TRUE){
    trs = vector("list", vehicles)
    drs = vector("list", vehicles)
    rand.custs = sample(2:(ncust+1),ncust)
    for(v in 1:vehicles){
      custs = rand.custs[1:min(cap, length(rand.custs))]
      rand.custs = rand.custs[!(rand.custs%in%custs)]
      if(functype == "tonn"){
        trs[[v]] = time_oriented_nn(max.trial = 100, custs = custs)
        drs[[v]] = matrix(0,1,3)
      }else if(functype == "edd"){
        tr = df %>% arrange(lt,et) %>% filter(!(n %in% c(0,ncust+1))) %>%
          select(n) %>% unlist() %>% unname()
        trs[[v]] = c(1, tr[1:ncust]+1, 1)
        drs[[v]] = matrix(0,1,3)
      }else if(functype == "drone"){
        r = initial_route_with_drone()
        trs[[v]] = r[[1]]
        drs[[v]] = r[[2]]
      } 
    }
    feasible = is_solution_feasible(trs, drs)
    if(feasible){
      schedule <<- candidate.schedule
      break()
    }else{
      k = k + 1
    }
  }
  print(feasible)
  last.solution.time <<- round(as.numeric(difftime(Sys.time(),
                                                   starting.time, units = "secs")))
  starting.trs <<- trs
  starting.drs <<- drs
  truck.routes <<- trs
  drone.routes <<- drs
  plot_dronetruck_kvehicle(trs, drs)
}

generate_edd_routes <- function(k){
  remaining = df %>% arrange(lt,et) %>% filter(!(n %in% c(0,ncust+1))) %>%
                     select(n) %>% unlist() %>% unname() + 1
  trs = vector("list", k)
  drs = vector("list", k)
  while (TRUE) {
    for(v in 1:k){
      trs[[v]] = c(trs[[v]], remaining[1])
      remaining = remaining[-1]
      if(length(remaining)==0){
        break()
      }
    }
    if(length(remaining)==0){
      break()
    }
  }
  for(v in 1:k){
    trs[[v]] = c(1,trs[[v]],1)
    drs[[v]] = matrix(c(0,0,0),1,3)
  }
  feasible = is_solution_feasible(trs, drs)
  if(feasible){
    truck.routes <<- trs
    drone.routes <<- drs
  }else{
    generate_edd_routes(k+1)
  }
  #print(truck.routes)
}

generate_random_routes <- function(k){
  remaining = sample(2:(ncust+1),ncust)
  trs = vector("list", k)
  drs = vector("list", k)
  while (TRUE) {
    for(v in 1:k){
      trs[[v]] = c(trs[[v]], remaining[1])
      remaining = remaining[-1]
      if(length(remaining)==0){
        break()
      }
      trs[[v]] = c(trs[[v]], remaining[1])
      remaining = remaining[-1]
      if(length(remaining)==0){
        break()
      }
    }
    if(length(remaining)==0){
      break()
    }
  }
  for(v in 1:k){
    trs[[v]] = c(1,trs[[v]],1)
    drs[[v]] = matrix(c(0,0,0),1,3)
  }
  feasible = is_solution_feasible(trs, drs)
  if(feasible){
    truck.routes <<- trs
    drone.routes <<- drs
  }else{
    generate_random_routes(k+1)
  }
}

generate_one_cust_routes <- function(ncust){
  trs = vector("list", ncust)
  drs = vector("list", ncust)
  for(v in 1:ncust){
    trs[[v]] = c(1, v+1, 1)
    drs[[v]] = matrix(0, 1, 3)
  }
  truck.routes <<- trs
  drone.routes <<- drs
}

find_solution_cost <- function(trs, drs, type = "all"){
  n = length(trs)
  new.cost = numeric(n)
  for(v in 1:n){
    new.cost[v] = find_route_cost(trs[[v]], drs[[v]]) + vehicle.cost
  }
  if(type == "all"){
    return(new.cost)
  }else{
    return(sum(new.cost))
  }
}

find_route_cost <- function(tr, dr = matrix(0,1,3)) {
  tr.len = length(tr)
  tr.cost.incurred = 0
  dr.cost.incurred = 0
  if(sum(dr) == 0){
    # there is no drone assignment
    for(i in 1:(tr.len-1)){
      tr.cost.incurred = tr.cost.incurred + (dist.mat[tr[i],tr[i+1]] *
                                               truck.cost)
    }
  }else{
    # there is drone assignment
    # cost calculated in two parts
    for(i in 1:(tr.len-1)){
      tr.cost.incurred = tr.cost.incurred + (dist.mat[tr[i],tr[i+1]] *
                                               truck.cost)
    }
    for(j in 1:nrow(dr)){
      dr.cost.incurred = dr.cost.incurred + ((dist.mat[dr[j,1], dr[j,2]] + 
                                              dist.mat[dr[j,2], dr[j,3]]) *
                                               drone.cost)
    }
  }
  
  return(round(tr.cost.incurred+dr.cost.incurred,4)+(vehicle.number-g)*500)
}


find_timeloc_mat <- function(){
  # for shaw heuristic 
  # calculate the relative closeness of nodes according to
  # their time window overlap and distances between them
  tw.df = matrix(0, nodes, nodes)
  for(i in 1:nodes){
    for(j in 1:nodes){
      if(i != j){
        tw.df[i,j] = max(min(df$lt[i], df$lt[j]) - max(df$et[i], df$et[j]), 0) / 
          dist.mat[i,j]
      }else{
        tw.df[i,j] = 0
      }
    }
  }
  tw.df[1,nodes] = 0
  tw.df[nodes,1] = 0
  # relative metric to use as if a probability 
  tw.df = sweep(tw.df, 1, STATS = rowSums(tw.df), "/") %>% round(3)
  return(tw.df[-nodes,-nodes])
}


time_oriented_nn <- function(w = NULL, call.no = 1, custs = 2:11,
                             max.trial = 20, plot.route = F){
  ### time oriented nearest neighbor 
  
  # if no initial weight is given, randomly decide on one
  if(is.null(w)){
    w = runif(3)
  }
  # initializations
  unassigned.custs = custs
  route = 1
  last.start = df$et[1]
  
  # find coefs for normalizing 3 differently ranged values to range between 0 and 1
  m1 = max(dist.mat[c(1,custs),c(1,custs)])/truck.velocity
  m2 = max(diff(df$et[c(1,custs)]))
  m3 = max(df$lt[c(1,custs)])
  w = w / c(m1, m2, m3)
  
  while(length(unassigned.custs)!=0){
    min.val = 10e2
    i = last(route)
    si = last.start
    # find the closest unassigned customer to current customer, add it into the route
    for(j in unassigned.custs){
      tij = dist.mat[i,j] / truck.velocity
      s.start = ifelse(tij + si + df$st[i] < df$et[j], df$et[j], tij + si + df$st[i])
      cij = (w[1] * tij) +
            (w[2] * (s.start - si - df$st[i])) +
            (w[3] * max(0, df$lt[j] - tij - si - df$st[i]))
      if(cij<min.val){
        min.val = cij
        ind = j
        last.start = s.start
      }
    }
    route = c(route, ind)
    unassigned.custs = unassigned.custs[unassigned.custs!=ind]
  }
  route = c(route,1)
  
  # check if the route is feasible - it can be infeasible due to 
  # poorly chosen weights and approximate range standardization
  
  # if feasible, return the route
  # if not, recall the function with different weights
  if(is_route_feasible(route)){
    print(call.no)
    if(plot.route){
      plot_dronetruck(route)
    }
    return(route)
  }else if(call.no==max.trial){
    print("max trials reached")
    return(NULL)
  }else{
    return(time_oriented_nn(call.no = call.no + 1, max.trial = max.trial))
  }
}


choose_destruction_degree <- function(type = "rand-equal", fix.d = 1){
  if(type == "random-high"){
    destruction.degree = sample(d.range, 1, prob = d.range)
  }else if(type == "random-equal"){
    destruction.degree = sample(d.range, 1)
  }else if(type == "random-low"){
    destruction.degree = sample(d.range, 1, prob = rev(d.range))
  }else if(type == "fixed"){
    destruction.degree = fix.d
  }else if(type == "prob"){
    destruction.degree = sample(d.range, 1, prob = destruction.prob)
  }
  
  return(destruction.degree)
}



initial_route_with_drone <- function(init.type = "tonn"){
  if(init.type == "tonn"){
    tr = time_oriented_nn(max.trial = 1000)
    if(is.null(tr)){
      init.type = "edd"
    }
  }
  if(init.type == "edd"){
    tr = df %>% arrange(lt,et) %>% filter(!(n %in% c(0,ncust+1))) %>%
      select(n) %>% unlist() %>% unname()
    tr = c(1, tr[1:ncust]+1, 1)
  }
  
  dr = matrix(0,1,3)
  i = 1
  k = 1
  while(TRUE){
    temp.tr = tr
    temp.dr = dr
    sortie = tr[k:(k+2)]
    tr = tr[-(k+1)]
    
    if(i == 1){
      dr[i,] = sortie
    }else{
      dr = matrix(rbind(dr, sortie), i, 3, F)
    }
    if(is_route_feasible(tr,dr,verbose = F) & runif(1) < 0.6){
      plot_dronetruck(tr,dr)
      k = which(tr==dr[i,3])
      if(length(k)==2){break()}
      i = i + 1
    }else{
      k = k + 1
      tr = temp.tr
      dr = temp.dr
      if(k+2>length(tr)){
        break()
      }
    }
    
  }
  return(list(tr, dr))
}

is_drone_available <- function(sortie, availability){
  if(any(availability[sortie[1]:sortie[3]] == 0 )){
    return(FALSE)
  }else{
    return(TRUE)
  }
}

can_drone_endure <- function(sortie){
  if(dist.mat[sortie[1], sortie[2]]+dist.mat[sortie[2],sortie[3]] < 
     drone.endurance * drone.velocity * 60){
    return(TRUE)
  }else{
    return(FALSE)
  }
}

add_drone_route <- function(trs, drs){
  for(v in 1:vehicle.number){
    tr = trs[[v]]
    dr = drs[[v]]
    nn = numeric(length(tr)-2)
    for(i in 2:(length(tr)-1)){
      nn[i] = dist.mat[i-1,i] + dist.mat[i,i+1]
    }
    x = which.min(nn)+1
    dr = matrix(c(x-1,x,x+1), nrow = 1, ncol = 3)
    tr = tr[-x]
    trs[[v]] = tr
    drs[[v]] = dr
  }
  return(list(trs,drs))
}



