aStarDM=function(roads,car,packages){
  nextMove=0
  toGo=0
  offset=0
  if (car$load==0) {
    toGo=which(packages[,5]==0)[1]
  } else {
    toGo=car$load
    offset=2
  }
  goal = c(packages[toGo,1+offset], packages[toGo,2+offset])

  #Pseudocode for the code under
  #open <- []
  #start <- start

  #while start is not goal {
  #  add successors of start to open
  #  start <- select from open (choose the node with lowest cost plus heuristic value)
  #  remove start from open
  #}

  #return start

  #List for the frontier
  open <- list()
  #List to keep track of moves
  moves <- list()
  #Start is the current position of the car and only has a heuristic value
  start = c(car$x, car$y, 0, h(c(car$x, car$y), goal), 5)

  #Loop runs until start == goal, we always expand the neighbor with lowest cost
  while((start[1]!=goal[1]) || (start[2]!=goal[2])){
    neighborDown = neighborLeft = neighborRight = neighborUp = 0

    #The if statements below finds the neighbors  start and adds them to the frontier
    #Down - 2
    if(start[2] != 1){
      neighborDown = c(start[1], start[2]-1, g(start, 2, roads) + start[3], h(c(start[1], start[2]-1), goal), 2)
      open[[length(open)+1]] <- neighborDown
    }
    #Left - 4
    if(start[1] != 1){
      neighborLeft = c(start[1]-1, start[2], g(start, 4, roads) + start[3], h(c(start[1]-1, start[2]), goal), 4)
      open[[length(open)+1]] <- neighborLeft
    }
    #Right - 6
    if(start[1] != 10){
      neighborRight = c(start[1]+1, start[2], g(start, 6, roads) + start[3], h(c(start[1]+1, start[2]),goal), 6)
      open[[length(open)+1]] <- neighborRight
    }
    #Up - 8
    if(start[2] != 10){
      neighborUp = c(start[1], start[2]+1, g(start, 8, roads) + start[3], h(c(start[1], start[2]+1), goal), 8)
      open[[length(open)+1]] <- neighborUp
    }

    #Find node with lowest f = cost(g) + heursitic(h)
    lowestF = 2 ** 20
    for(node in open){
      f = node[3] + node[4]
      if(lowestCost >= f){
        start = node
        lowestF = f
      }
    }

    #Remove start node from the frontier
    for(i in 1:length(open)){
      if(all(start==open[[i]])){
        open[[i]] <- NULL
        break
      }
    }

    #Add the move to end of moves list
    moves[[length(moves)+1]] <- start[5]
  }

  #the if statements here are for error handling, but should always set nextMove to moves[[1]]
  if(length(moves)==0){
    print(paste("nextMove: 5"))
    nextMove = 5
  } else{
    print(paste("nextMove:",moves[[1]]))
    nextMove = moves[[1]]
  }

  car$nextMove=nextMove
  car$mem=list()
  return (car)
}