navigation.nls

to load_LUT
  if have_lut = 0
  [
    set LUT []

    foreach file_list [
      let current_file word file_dir ?
      set LUT lput csv:from-file current_file LUT
    ]
    set have_lut 1
  ]
end

to setup-patches

  set costmap (bitmap:to-grayscale (bitmap:import "costmap2.png"))
  set im_w bitmap:width costmap
  set im_h bitmap:height costmap

  ; set the world size to the exact image size, with positive indexing only
  resize-world ( 0 ) ( im_w ) ( 0 ) ( im_h )
  ; resize the patches for visualization
  set-patch-size 4

  ; copy the pixel values to the patch colors
  bitmap:copy-to-pcolors (costmap) false

  ; set the patches initial states
  ask patches [
    set traversal_cost ((item 1 pcolor) / 25.5 * 0.9) + 1
    set is_goal -1 ;means not a goal
    ]

   let goal_number 0
   set goalList table:make
   foreach LUT
   [

     let X (item 0 item 0 ?)
     let Y (item 1 item 0 ?)

     ask patch X Y [
       set pcolor [255 0 0]
       set is_goal goal_number
       set plabel goal_number
     ]
     table:put goalList goal_number (array:from-list (list X Y -1))

     set goal_number goal_number + 1
   ]
end

to set-random-world-pos
    setxy (random im_w) (random im_h )
end

to-report check-if-done
  let goal-not-visited false
  let max-ticks 0
  foreach table:keys goalList
  [
    let goal table:get goalList ?
    let nticks (array:item goal 2)
    if nticks < 0
    [
      set goal-not-visited true
    ]
    if nticks > max-ticks
    [
      set max-ticks nticks
    ]
  ]

  if not goal-not-visited
  [
    show max-ticks
  ]
  report not goal-not-visited
end

to-report cost-from-to [start_x start_y goal_idx]
  let line_nr (start_x * im_h) + (im_h - start_y)  + 1
  report item 2 (item line_nr item goal_idx LUT)
end

to-report get-next-waypoint[start_x start_y goal_idx]
  let line_nr (start_x * im_h) + (im_h - start_y) + 1

  let numSteps item 3 (item line_nr item goal_idx LUT)
  let X item 3 (item line_nr item goal_idx LUT)
  let Y item 4 (item line_nr item goal_idx LUT)
  report (list X Y);
end

to set-closest-goal
  let min-index -1
  let min-cost 10000000
  let X [pxcor] of patch-here
  let Y [pycor] of patch-here
  foreach table:keys goals
  [
    let XY table:get goals ?
    let key ?
    let cost cost-from-to X Y key
    if cost < min-cost
    [
      set min-index key
      set min-cost cost
    ]
  ]
  if not (min-index = -1)
  [
    set goal_nr min-index
    table:remove goals goal_nr
    add-intention "scan-here" "true"
    add-intention "do-movement" "at-goal-current-goal"
    set waypoint get-next-waypoint X Y goal_nr
  ]
end

to do-movement
  let X [pxcor] of patch-here 
  let Y [pycor] of patch-here
  let cost [traversal_cost] of patch-here
  let goal-nr-here [is_goal] of patch-here

  set label (list who ":" goal_nr "+" table:keys goals)

  ifelse X = item 0 waypoint and Y = item 1 waypoint
  [
    set waypoint get-next-waypoint X Y goal_nr
  ]
  [
    facexy item 0 waypoint item 1 waypoint
    forward (speed) / (cost)
  ]
end

to-report at-goal-current-goal
  let goal-nr-here [is_goal] of patch-here
  report (goal_nr = goal-nr-here)
end

to scan-here
  ask patch-here [
    set pcolor [0 255 0]
    let goal table:get goalList is_goal
    let nticks ticks
    array:set goal 2 nticks
    set plabel (list is_goal nticks)
  ]
end