CLS
OPTION BASE 1

'Read in Map.
'The format is 0 is impassible while 1 is passable.
READ MapX%, MapY%
READ StrX%, StrY%, EndX%, EndY%
DIM Map%(MapX%, MapY%)

FOR i% = 1 TO MapY%
  FOR z% = 1 TO MapX%
    READ Map%(z%, i%)
  NEXT z%
NEXT i%

'Here Parent means the following:
'-1 : No parent. (Start location)
' 0 : No parent yet! (UNVISITED)
' 1 : Parent to the left
' 2 : Parent to the right
' 3 : Parent up
' 4 : Parent down
DIM Parent%(MapX%, MapY%)

'Allocate the queue, big enough to avoid wrap-around problem.
DIM X%(MapX% * MapY%), Y%(MapX% * MapY%)
QP% = 0
QL% = 0 'Queue Length

'Add the initial position to the queue
'The the parent to be null. (0 means not visited, -1 means null)
QL% = QL% + 1
X%(QL%) = StrX%: Y%(QL%) = StrY%
Parent%(StrX%, StrY%) = -1

DO WHILE QP% < QL%
  'Temporary reference current position
  QP% = QP% + 1
  CurX% = X%(QP%): CurY% = Y%(QP%)
 
  'Are we at the end?
  IF CurX% = EndX% AND CurY% = EndY% THEN EXIT DO

  'Try going right
  IF Map%(CurX% + 1, CurY%) AND Parent%(CurX% + 1, CurY%) = 0 THEN
    QL% = QL% + 1
    X%(QL%) = CurX% + 1: Y%(QL%) = CurY%
    Parent%(CurX% + 1, CurY%) = 1
  END IF

  'Try going left
  IF Map%(CurX% - 1, CurY%) AND Parent%(CurX% - 1, CurY%) = 0 THEN
    QL% = QL% + 1
    X%(QL%) = CurX% - 1: Y%(QL%) = CurY%
    Parent%(CurX% - 1, CurY%) = 2
  END IF

  'Try going down
  IF Map%(CurX%, CurY% + 1) AND Parent%(CurX%, CurY% + 1) = 0 THEN
    QL% = QL% + 1
    X%(QL%) = CurX%: Y%(QL%) = CurY% + 1
    Parent%(CurX%, CurY% + 1) = 3
  END IF

  'Try going up
  IF Map%(CurX%, CurY% - 1) AND Parent%(CurX%, CurY% - 1) = 0 THEN
    QL% = QL% + 1
    X%(QL%) = CurX%: Y%(QL%) = CurY% - 1
    Parent%(CurX%, CurY% - 1) = 4
  END IF
LOOP

'Display Map
FOR i% = 1 TO MapY%
  FOR z% = 1 TO MapX%
    IF Map%(z%, i%) = 1 THEN
      PRINT " ";
    ELSE
      PRINT "#";
    END IF
  NEXT z%
  PRINT
NEXT i%
LOCATE StrY%, StrX%: PRINT "S"
LOCATE EndY%, EndX%: PRINT "E"


IF QP% = QL% AND (X%(QL%) <> EndX% OR Y%(QL%) <> EndY%) THEN LOCATE MapY% + 1: PRINT "No Possible Path": END

'Backtrace, here we start at the end and then make it back to the beginning
'Since we know which move caused the arrival to each cell we can just
'take the reverse direction.
CurX% = EndX%: CurY% = EndY%
'We can make use of the space of X%, Y% now
i% = 0

DO
  i% = i% + 1
  X%(i%) = CurX%: Y%(i%) = CurY%
 
  SELECT CASE Parent%(CurX%, CurY%)
    CASE 1
      CurX% = CurX% - 1
    CASE 2
      CurX% = CurX% + 1
    CASE 3
      CurY% = CurY% - 1
    CASE 4
      CurY% = CurY% + 1
    CASE -1
      EXIT DO
    CASE ELSE
      EXIT DO
  END SELECT
LOOP
QP% = i%

COLOR 14
FOR i% = 2 TO QP% - 1
  LOCATE Y%(i%), X%(i%): PRINT "x";
NEXT i%

'Map X, Map Y
DATA 10,10
'Map StartX, StartY, EndX, EndY
DATA 2,2,9,9
DATA 0,0,0,0,0,0,0,0,0,0
DATA 0,1,1,1,1,1,1,0,1,0
DATA 0,1,0,0,0,1,1,0,1,0
DATA 0,1,1,1,1,1,1,1,1,0
DATA 0,0,1,0,1,0,1,0,1,0
DATA 0,0,1,0,1,1,1,0,1,0
DATA 0,1,1,0,0,1,0,1,1,0
DATA 0,1,0,1,0,0,1,0,0,0
DATA 0,1,1,1,1,1,1,1,1,0
DATA 0,0,0,0,0,0,0,0,0,0