DECLARE SUB thickline (cx%(), cy%(), x%, y%, ex%, ey%, count%)
DECLARE FUNCTION chk% (x%, y%, ex%, ey%, map%())
DECLARE SUB drawline (cx%(), cy%(), x%, y%, ex%, ey%, count%)
DECLARE SUB delpath (cx%(), cy%(), start%, finish%, count%)
DECLARE SUB addpath (cx%(), cy%(), count%, tx%(), ty%(), ln%, start%)
DECLARE SUB rotate (dx%, dy%)
DECLARE SUB rotatec (dx%, dy%)
SCREEN 12
'define the scale and sizes
sc% = 10
maxi% = 100
maxj% = 75
RANDOMIZE TIMER
DIM map%(200, 100)
DIM cx%(10000)
DIM cy%(10000)
DIM tx%(5000)
DIM ty%(5000)
DIM tempx%(5000)
DIM tempy%(5000)
'set start location
x% = 10
y% = 10
map%(x%, y%) = 12

'set goal location
ex% = 62
ey% = 27
map%(ex%, ey%) = 13


FOR j% = 5 TO 20
  map%(26, j% + 9) = 3
NEXT j%
FOR j% = 5 TO 20
  map%(30, j% + 9) = 3
NEXT j%
FOR j% = 5 TO 25
  map%(32, j%) = 3
NEXT j%
  map%(31, 25) = 3
  map%(0, 0) = 3
FOR j% = 5 TO 25
  map%(33, j% + 9) = 3
NEXT j%

FOR j% = 5 TO 25
  map%(35, j%) = 3
NEXT j%


'put in an object in the way
FOR j% = 5 TO 25
  map%(46 + 1, j% * 2) = 3
NEXT j%
FOR j% = 5 TO 25
  map%(47 + 1, j% * 2 + 1) = 3
NEXT j%
FOR j% = 5 TO 25
  map%(48 + 1, j% * 2) = 3
NEXT j%

'put in an object in the way
'FOR j% = 5 TO 25
'  map%(33, j% + 10) = 3
'NEXT j%
'FOR j% = 5 TO 25
'  map%(34, j%) = 3
'NEXT j%
'
'FOR j% = 1 TO 10
'  map%(35 + j%, 20 + j%) = 3
'NEXT j%
'
'FOR j% = 1 TO 10
'  map%(45 - j%, 20 + j%) = 3
'NEXT j%



'draw the map
FOR i% = 0 TO imax%
 FOR j% = 0 TO jmax%
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
 NEXT j%
NEXT i%

'line
'walk around objects
'remove excess travel
'trivial case-end is same as start

count% = 0      'location in cx%...point alont the path

CALL thickline(cx%(), cy%(), x%, y%, ex%, ey%, count%)
count% = count% + 1
FOR i% = 0 TO count% - 1
  IF map%(cx%(i%), cy%(i%)) = 0 THEN map%(cx%(i%), cy%(i%)) = 1
NEXT i%

FOR i% = 0 TO maxi%
 FOR j% = 0 TO maxj%
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
 NEXT j%
NEXT i%

'find path around objects
'***********************************
tc% = 0
dtx% = 0
dty% = 0
half% = 0
block% = 0
i% = 0
DO
  i% = i% + 1

  IF map%(cx%(i%), cy%(i%)) > 2 THEN
    block% = 1
    xs% = cx%(i% - 1)
    ys% = cy%(i% - 1)
    x% = xs%
    y% = ys%
    dx% = cx%(i%) - cx%(i% - 1)
    dy% = cy%(i%) - cy%(i% - 1)
    dxflag% = 0
      WHILE map%(x% + dx%, y% + dy%) > 2
        CALL rotate(dx%, dy%)
      WEND
    dxs% = dx%
    dys% = dy%
    DO
      tx%(tc%) = x% + dx%
      ty%(tc%) = y% + dy%
      tc% = tc% + 1
      LOCATE 20, 1
      x% = x% + dx%
      y% = y% + dy%
      IF map%(x%, y%) = 1 OR map%(x%, y%) = 2 THEN
        IF (dtx% * dtx% + dty% * dty%) <= (x% - xs%) * (x% - xs%) + (y% - ys%) * (y% - ys%) THEN
          half% = tc% - 1
          dtx% = ABS(x% - xs%)
          dty% = ABS(y% - ys%)
          IF map%(x%, y%) = 2 THEN map%(x%, y%) = 1
        END IF
      END IF
      CALL rotatec(dx%, dy%)
      CALL rotatec(dx%, dy%)
      WHILE map%(x% + dx%, y% + dy%) > 2
        CALL rotate(dx%, dy%)
      WEND
    LOOP UNTIL x% = xs% AND y% = ys% AND dxs% = dx% AND dys% = dy%
    last% = 1
  ELSE
    IF map%(cx%(i%), cy%(i%)) <= 2 THEN last% = 0  'set last marker to available
  END IF
 
  IF half% >= tc% - 1 AND block% = 1 THEN
    count% = i%
  ELSEIF (tc% - 1) / 2 >= half% AND block% = 1 THEN
    j% = i%
    erx% = ABS(tx%(half%) - cx%(j%))
    ery% = ABS(ty%(half%) - cy%(j%))
    WHILE erx% >= ABS(tx%(half%) - cx%(j%)) AND ery% >= ABS(ty%(half%) - cy%(j%))
      erx% = ABS(tx%(half%) - cx%(j%))
      ery% = ABS(ty%(half%) - cy%(j%))
      j% = j% + 1
    WEND
    FOR p% = j% TO count% - 1
      tempx%(p%) = cx%(p%)
      tempy%(p%) = cy%(p%)
    NEXT p%
    FOR p% = 0 TO half%
      cx%(p% + i%) = tx%(p%)
      cy%(p% + i%) = ty%(p%)
    NEXT p%
    PRINT j%, i%
    FOR p% = j% TO count% - 1
      cx%(p% + half% - (j% - i%) + 1) = tempx%(p%)
      cy%(p% + half% - (j% - i%) + 1) = tempy%(p%)
    NEXT p%
    count% = count% + half% - (j% - i%) + 1
    block% = 0
    i% = i% + half% - (j% - i%) + 1
    tc% = 0
    dtx% = 0
    dty% = 0
    half% = 0
  ELSEIF (tc% - 1) / 2 < half% AND block% = 1 THEN
    FOR k% = 0 TO tc% - 2
      tempx%(k%) = tx%(tc% - k% - 2)
      tempy%(k%) = ty%(tc% - k% - 2)
    NEXT k%
    FOR k% = 0 TO tc% - 2
      tx%(k%) = tempx%(k%)
      ty%(k%) = tempy%(k%)
    NEXT k%
    half% = tc% - half% - 2
    j% = i%
    erx% = ABS(tx%(half%) - cx%(j%))
    ery% = ABS(ty%(half%) - cy%(j%))
    WHILE erx% >= ABS(tx%(half%) - cx%(j%)) AND ery% >= ABS(ty%(half%) - cy%(j%))
      erx% = ABS(tx%(half%) - cx%(j%))
      ery% = ABS(ty%(half%) - cy%(j%))
      j% = j% + 1
    WEND
    FOR p% = j% TO count% - 1
      tempx%(p%) = cx%(p%)
      tempy%(p%) = cy%(p%)
    NEXT p%
    FOR p% = 0 TO half%
      cx%(p% + i%) = tx%(p%)
      cy%(p% + i%) = ty%(p%)
    NEXT p%
    PRINT j%, i%
    FOR p% = j% TO count% - 1
      cx%(p% + half% - (j% - i%) + 1) = tempx%(p%)
      cy%(p% + half% - (j% - i%) + 1) = tempy%(p%)
    NEXT p%
    count% = count% + half% - (j% - i%) + 1
    block% = 0
    i% = i% + half% - (j% - i%) + 1
    tc% = 0
    dtx% = 0
    dty% = 0
    half% = 0
  END IF
LOOP UNTIL i% >= count% - 2


FOR i% = 0 TO maxi%
 FOR j% = 0 TO maxj%
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), map%(i%, j%), BF
   LINE (i% * sc%, j% * sc%)-(i% * sc% + sc%, j% * sc% + sc%), 7, B
 NEXT j%
NEXT i%


start% = 0
DO
'see if any of the path can be removed
i% = count% - 1
lin% = 0
xs% = cx%(start%)
ys% = cy%(start%)
leave% = 0
'draw line from each node to furthest point away possible without blockage
DO
  block% = chk%(xs%, ys%, cx%(i%), cy%(i%), map%())
  IF block% = 0 THEN
    lin% = 1
    CALL drawline(tempx%(), tempy%(), xs%, ys%, cx%(i%), cy%(i%), ln%)
    CALL delpath(cx%(), cy%(), start%, i%, count%)
    c% = count% - 1
    CALL addpath(cx%(), cy%(), c%, tempx%(), tempy%(), ln%, start%)
'    FOR j% = 1 TO c%
'  LINE (cx%(j%) * sc%, cy%(j%) * sc%)-(cx%(j%) * sc% + sc%, cy%(j%) * sc% + sc%), INT(RND * 15), BF
'    NEXT j%
    count% = c% + 1
    start% = start% + ln%

  END IF
  i% = i% - 1
LOOP UNTIL i% < start% + 2 OR lin% = 1
LOOP UNTIL start% >= count% - 2 OR lin% = 0


'try one last line from end of path
i% = start%
lin% = 0
xs% = cx%(count% - 1)
ys% = cy%(count% - 1)
leave% = 0
DO
  block% = chk%(xs%, ys%, cx%(i%), cy%(i%), map%())
  IF block% = 0 THEN
    lin% = 1
    CALL drawline(tempx%(), tempy%(), cx%(i%), cy%(i%), xs%, ys%, ln%)
    CALL delpath(cx%(), cy%(), i%, count% - 1, count%)
    c% = count% - 1
    CALL addpath(cx%(), cy%(), c%, tempx%(), tempy%(), ln%, i%)
'    FOR j% = 1 TO c%
'  LINE (cx%(j%) * sc%, cy%(j%) * sc%)-(cx%(j%) * sc% + sc%, cy%(j%) * sc% + sc%), INT(RND * 15), BF
'    NEXT j%
    count% = c% + 1
    start% = start% + ln%

  END IF
  i% = i% + 1
LOOP UNTIL i% >= count% - 2 OR lin% = 1



count% = count% - 1



'walk along path
PRINT count%
FOR i% = 0 TO count%
  LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), 4, BF
  now = TIMER
  DO
  LOOP UNTIL TIMER > now + .1
  LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), map%(cx%(i%), cy%(i%)), BF
  LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), 7, B
NEXT i%
i% = count%
LINE (cx%(i%) * sc%, cy%(i%) * sc%)-(cx%(i%) * sc% + sc%, cy%(i%) * sc% + sc%), 4, BF

SUB addpath (cx%(), cy%(), count%, tx%(), ty%(), ln%, start%)
DIM tempx%(1000)
DIM tempy%(1000)
FOR i% = 0 TO ln% + count%
  IF i% < start% THEN
    tempx%(i%) = cx%(i%)
    tempy%(i%) = cy%(i%)
  END IF
  IF i% >= start% AND i% <= start% + ln% THEN
    tempx%(i%) = tx%(i% - start%)
    tempy%(i%) = ty%(i% - start%)
  END IF
  IF i% > start% + ln% THEN
    tempx%(i%) = cx%(i% - ln% - 1)
    tempy%(i%) = cy%(i% - ln% - 1)
  END IF

NEXT i%
FOR i% = 0 TO ln% + count%
    cx%(i%) = tempx%(i%)
    cy%(i%) = tempy%(i%)
NEXT i%
count% = count% + ln%

END SUB

FUNCTION chk% (x%, y%, ex%, ey%, map%())
chk% = 0
'line drawing algorithm
dx% = ex% - x%
dy% = ey% - y%
xdir% = SGN(dx%)
ydir% = SGN(dy%)

IF ABS(dx%) >= ABS(dy%) THEN     'quadrant 0 or 3
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dyx2% = dy% * 2
  dyx2mdx2% = dyx2% - dx% * 2
  er% = dyx2% - dx%
  x0% = x%
  y0% = y%

  FOR i% = dx% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      y0% = y0% + ydir%
      er% = er% + dyx2mdx2%

      'IF map%(x0%, y0%) = 0 THEN map%(x0%, y0%) = 2

    ELSE
      er% = er% + dyx2%
    END IF
    x0% = x0% + xdir%
    IF map%(x0%, y0%) > 2 THEN chk% = 1
  NEXT i%
ELSE
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dxx2% = dx% * 2
  dxx2mdy2% = dxx2% - dy% * 2
  er% = dxx2% - dy%
  x0% = x%
  y0% = y%

  FOR i% = dy% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      x0% = x0% + xdir%
      er% = er% + dxx2mdy2%
    ELSE
      er% = er% + dxx2%
    END IF
    y0% = y0% + ydir%
    IF map%(x0%, y0%) > 2 THEN chk% = 1
  NEXT i%

END IF
END FUNCTION

SUB delpath (cx%(), cy%(), start%, finish%, count%)
DIM tempx%(1000)
DIM tempy%(1000)
new% = 0
FOR j% = 0 TO count%
  IF j% < start% OR j% > finish% THEN
    cx%(new%) = cx%(j%)
    cy%(new%) = cy%(j%)
    new% = new% + 1
  END IF
NEXT j%
count% = new%
END SUB

SUB drawline (cx%(), cy%(), x%, y%, ex%, ey%, count%)
'line drawing algorithm
count% = 0

dx% = ex% - x%
dy% = ey% - y%
xdir% = SGN(dx%)
ydir% = SGN(dy%)

IF ABS(dx%) >= ABS(dy%) THEN     'quadrant 0 or 3
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dyx2% = dy% * 2
  dyx2mdx2% = dyx2% - dx% * 2
  er% = dyx2% - dx%
  x0% = x%
  y0% = y%

  cx%(count%) = x0%
  cy%(count%) = y0%
  count% = count% + 1

  FOR i% = dx% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      y0% = y0% + ydir%
      er% = er% + dyx2mdx2%


    ELSE
      er% = er% + dyx2%
    END IF
    x0% = x0% + xdir%
    cx%(count%) = x0%
    cy%(count%) = y0%
    count% = count% + 1
  NEXT i%
ELSE
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dxx2% = dx% * 2
  dxx2mdy2% = dxx2% - dy% * 2
  er% = dxx2% - dy%
  x0% = x%
  y0% = y%

  cx%(count%) = x0%
  cy%(count%) = y0%
  count% = count% + 1

  FOR i% = dy% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      x0% = x0% + xdir%
      er% = er% + dxx2mdy2%
    ELSE
      er% = er% + dxx2%
    END IF
    y0% = y0% + ydir%
    cx%(count%) = x0%
    cy%(count%) = y0%
    count% = count% + 1
  NEXT i%

END IF
count% = count% - 1
END SUB

SUB rotate (dx%, dy%)
dxo% = dx%
dx% = (.7 * dx% + .7 * dy%)
dy% = (-.7 * dxo% + .7 * dy%)
END SUB

SUB rotatec (dx%, dy%)
dxo% = dx%
dx% = (.7 * dx% + -.7 * dy%)
dy% = (.7 * dxo% + .7 * dy%)
END SUB

SUB thickline (cx%(), cy%(), x%, y%, ex%, ey%, count%)
'line drawing algorithm
count% = 0

dx% = ex% - x%
dy% = ey% - y%
xdir% = SGN(dx%)
ydir% = SGN(dy%)

IF ABS(dx%) >= ABS(dy%) THEN     'quadrant 0 or 3
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dyx2% = dy% * 2
  dyx2mdx2% = dyx2% - dx% * 2
  er% = dyx2% - dx%
  x0% = x%
  y0% = y%

  cx%(count%) = x0%
  cy%(count%) = y0%
  count% = count% + 1

  FOR i% = dx% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      y0% = y0% + ydir%
      er% = er% + dyx2mdx2%
      cx%(count%) = x0%
      cy%(count%) = y0%
      count% = count% + 1
    ELSE
      er% = er% + dyx2%
    END IF
    x0% = x0% + xdir%
    cx%(count%) = x0%
    cy%(count%) = y0%
    count% = count% + 1
  NEXT i%
ELSE
  dx% = ABS(dx%)
  dy% = ABS(dy%)
  dxx2% = dx% * 2
  dxx2mdy2% = dxx2% - dy% * 2
  er% = dxx2% - dy%
  x0% = x%
  y0% = y%

  cx%(count%) = x0%
  cy%(count%) = y0%
  count% = count% + 1

  FOR i% = dy% - 1 TO 0 STEP -1
    IF er% >= 0 THEN
      x0% = x0% + xdir%
      er% = er% + dxx2mdy2%
      cx%(count%) = x0%
      cy%(count%) = y0%
      count% = count% + 1
    ELSE
      er% = er% + dxx2%
    END IF
    y0% = y0% + ydir%
    cx%(count%) = x0%
    cy%(count%) = y0%
    count% = count% + 1
  NEXT i%

END IF
count% = count% - 1
END SUB