select_longest_waitings <- function(route, d, type = "drone"){
  # xx <<- route
  # xd <<- d
  # xt <<- type

  if(type == "drone"){
    temp.df = data.frame(n = 1:nrow(route), duration = 0)
    tryCatch({
      for(j in 1:nrow(route)){
        temp.df[j,2] = schedule$dr.arrival[route[j,3]] - schedule$dr.leaving[route[j,1]]
      }
    },
    error = function(cond){
      temp.df = temp.df[sample(1:nrow(route),nrow(route)),]
    })
  }else{
    nl = length(route)
    temp.df = data.frame(n = 1:nl, duration = 0)
    tryCatch({
      for(j in 2:(nl-1)){
        temp.df[j,2] = schedule$tr.arrival[route[j+1]] - schedule$tr.leaving[route[j]]
      }
      temp.df[1,2] = temp.df[nl,2] = -1
    },
    error = function(cond){
      temp.df = temp.df[sample(1:nl,nl),]
    })
  }
  custs.inds = temp.df %>% arrange(desc(duration)) %>% select(n) %>% unlist() %>% head(d)
  return(custs.inds)
}

remove_connected_nodes <- function(tr, dr, custs, custs.inds){
  check = TRUE
  non.changed = 1:nrow(dr)
  non.changed = non.changed[!(non.changed%in%custs.inds)]
  while(check == TRUE && length(non.changed)){
    check = FALSE
    custs = c(custs, as.numeric(dr[custs.inds,]))
    custs = unique(custs)
    if(any(custs %in% dr[non.changed,])){
      check = TRUE
      custs.inds = which(dr[,1] %in% custs, arr.ind = T)
      custs.inds = c(custs.inds, which(dr[,3] %in% custs, arr.ind = T))
      custs.inds = unique(custs.inds)
      custs =  c(custs, as.numeric(dr[custs.inds,2]))
      non.changed = 1:nrow(dr)
      non.changed = non.changed[!(non.changed%in%custs.inds)]
    }
  }
  custs = unique(custs)
  custs = custs[custs!=1]
  tr = tr[!(tr%in%custs)]
  d = length(custs)
  return(list(tr, dr, custs, custs.inds))
}


nearest_insertion_truck_route <- function(tr, custs){
  # ntr <<- tr
  # ntc <<- custs
  if(any(is.na(custs)) || length(custs)==0){
    errors <<- errors + 1
    return(tr)
  }
  for(cust in custs){
    min.increase = 1e5
    for(i in 1:(length(tr)-1)){
      dik = dist.mat[tr[i], cust]
      dkj = dist.mat[tr[i+1], cust]
      dij = dist.mat[tr[i], tr[i+1]]
      
      increasen = dik + dkj - dij
      if((increasen < min.increase) ){
        min.increase = increasen
        index = i
      }
    }
    
    tr = append(tr, cust, index)
  }
  
  return(tr)
}

regret2 <- function(tr, custs){
  # rtr <<- tr
  # rtc <<- custs
  tr.len = length(tr)
  n = length(custs)
  if(any(is.na(custs)) || n==0){
    errors <<- errors + 1
    return(tr)
  }
  for(k in 1:n){
    cost2 = numeric(n)
    inds = numeric(n)
    for(cust in custs){
      a = which(custs == cust)
      increaser = rep(1e5, tr.len-1)
      for(i in 1:(tr.len-1)){
        dik = dist.mat[tr[i], cust]
        djk = dist.mat[tr[i+1], cust]
        dij = dist.mat[tr[i], tr[i+1]]
        
        increaser[i] = dik + djk - dij
      }
      cost2[a] = diff(sort(increaser)[1:2])
      inds[a] = which.min(increaser)
    }
    cust.ind = which.max(cost2)
    cust.change = custs[which.max(cost2)]
    custs = custs[custs!=cust.change]
    
    ##### remove after debugging
    # if(tr.len == 2){
    #   print("bug")
    #   bugs <<- bugs + 1
    #   tr = append(tr, cust.change, 1)
    # }else{
    #   tr = append(tr, cust.change, inds[cust.ind])
    # }
    tr = append(tr, cust.change, inds[cust.ind])
    #plot_dronetruck(tr, dr)
  }
  
  return(tr)
}

furthest_truck_nodes <- function(tr, d){
  x = numeric(length(tr)-2)
  for(i in 2:(length(tr)-1)){
    x[i-1] = dist.mat[tr[i-1], tr[i]] + 
             dist.mat[tr[i], tr[i+1]]
  }
  x = data.frame(n = 2:(length(tr)-1), x) %>%
      arrange(desc(x)) %>% select(n) %>% head(d)
  return(x$n)
}

tr.center.x = 10
tr.center.y = 10

outer_nodes_selection <- function(tr, d){
  tr.center.x = mean(df$x[tr[-1]])
  tr.center.y = mean(df$y[tr[-1]])
  n = tr[-c(1,length(tr))]
  df2 = data.frame(n = 2:(length(tr)-1),
                   x = df$x[n] - tr.center.x,
                   y = df$y[n] - tr.center.y)
  custs = df2 %>% mutate(value = abs(x) + abs(y)) %>%
                  arrange(desc(value)) %>% select(n) %>%
                  unlist() %>% as.numeric() %>% head(d)
  return(custs)
}

cluster_removal <- function(tr, d){
  n = length(tr)
  if(n-2<=d){
    d = n-3
  }
  if(n-d==2) i = 2
  i = sample(2:(n-d),1)
  custs.inds = i:(i+d-1)
  return(custs.inds)
}

find_drone_availability <- function(tr, dr){
  drone.availability = rep(1, length(tr))
  for(i in 1:(length(tr)-1)){
    if(tr[i] %in% dr[,1]){
      # if the node is drone launch node, drone flight planning
      j = which(dr[,1] == tr[i])
      ii = which(tr %in% dr[j,1], arr.ind = T)
      kk = which(tr %in% dr[j,3], arr.ind = T)
      
      # if drone launched or recovered from depot,
      # make the necessary adjustments
      if(length(ii)==2){ ii = 1 }
      if(length(kk)==2){ kk = kk[2] }
      
      # make drone unavailable between i and k
      if(length(ii)!=1 || length(kk)!=1){
        return(drone.availability)
      }else if(kk < ii){
        return(drone.availability)
      }else if(any(drone.availability[ii:(kk-1)]==0)){
        return(drone.availability)
      }else{
        drone.availability[ii:(kk-1)] = 0
      }
    }
  }
  
  return(drone.availability)
}

find_nearest_truck_pair <- function(tr, dr, node){
  #print("pair")
  min.dist = 10e3
  pair1 = 1
  for(i in 1:(length(tr)-1)){
    if(tr[i] %in% dr[,1] || (tr[i+1] %in% dr[,3])){next()}
    d = dist.mat[tr[i], node]+dist.mat[tr[i+1], node] 
    if(d < min.dist){
      pair1 = i
      min.dist = d
    }
  }
  #print("pair-end")
  return(c(tr[pair1], node, tr[pair1+1]))
}

find_closest_truck_nodes <- function(tr, dr, node){
  #print("closest")
  # tr <<- tr
  # dr <<- dr
  # node <<- node
  launch.nodes = tr[!(tr %in% dr[,1])]
  recover.nodes = tr[!(tr %in% dr[,3])]
  min.dist = 10e3
  launch = 1
  recovery = 2
  if(is.na(node)){
    algo.errors <<- algo.errors + 1
    return(c(launch, 1, recovery))
  }
  if(length(launch.nodes)!=0 && length(recover.nodes)!=0){
    for(l in launch.nodes){
      for(r in recover.nodes){
        d = dist.mat[l, node] + dist.mat[node, r]
        check = TRUE
        if(l!=1){
          check = l!=r
        }
        if((d < min.dist) && check){
          l.ind = which(tr == l)[1]
          r.ind = which(tr == r)
          if(length(r.ind)!=1) r.ind = r.ind[2]
          if((l.ind<r.ind)){
            launch = l
            recovery = r
            min.dist = d
          }
        }
      }
    }
  }

  #print("closest-end")
  return(c(launch, node, recovery))
}

plan_random_drone_flight <- function(tr, dr, node){
  rtr <<- tr
  rdr <<- dr
  rnode <<- node
  launch.nodes = tr[!(tr %in% dr[,1])]
  len = length(launch.nodes)
  launch = ifelse(len==0, 1,
                  ifelse(len==1, launch.nodes, sample(launch.nodes, 1)))
  
  recovery.nodes = tr[!(tr %in% dr[,3]) & tr != launch]
  if(launch == 1){
    recovery.nodes = c(recovery.nodes,1)
  }
  len = length(recovery.nodes)
  recovery = ifelse(len==0, 1,
                    ifelse(len==1, recovery.nodes, sample(recovery.nodes, 1)))
  l = which(tr == launch)[1]
  r = which(tr == recovery)
  if(length(r)!=1) r = r[2]
  if(l > r){
    temp = launch
    launch = recovery
    recovery = temp
  }
  
  sortie = c(launch, node, recovery)
  return(sortie)
}

remove_connected_drone_custs <- function(dr, custs){
  # print(move.operator)
  dr.inds = numeric()
  for(i in 1:nrow(dr)){
    if(sum(custs %in% dr[i,])!=0)
      dr.inds = c(dr.inds, i)
  }
  dr.inds = unique(dr.inds)
  custs = c(custs, dr[dr.inds, 2])
  
  ndr = nrow(dr)
  if(ndr == length(dr.inds)){
    dr = matrix(0, 1, 3)
  }else{
    dr = matrix(dr[-dr.inds,], ndr-length(dr.inds), 3, T)
  }
  return(list(dr, custs))
}

reconnect_drone_custs <- function(tr, dr, drcusts){
  repm = sample(trdr.repair, 1, prob = repair.probs[trdr.repair])
  for(cust in drcusts){
    if(repm == 1){
      new.sortie = plan_random_drone_flight(tr, dr, cust)
    }else if(repm == 4){
      new.sortie = find_closest_truck_nodes(tr, dr, cust)
    }else{
      new.sortie = find_nearest_truck_pair(tr, dr, cust)
    }

    if(sum(dr)==0){
      dr = matrix(new.sortie, 1, 3)
    }else{
      a = c(t(dr), new.sortie)
      dr = matrix(a, length(a)/3, 3, T)
    }
  }
  return(dr)
}

arrange_drone_custs <- function(tr, dr, custs, d, move.type = "intra"){
  res = remove_connected_drone_custs(dr, custs)
  dr = res[[1]]
  custs = res[[2]]
  n = length(custs)
  x = n - d
  total.changes <<- total.changes + x
  inds = sample(1:n, sample(1:x,1,prob = 1:x))
  drcusts = custs[inds]
  trcusts = custs[-inds]
  return(list(dr, trcusts, drcusts))
}
