remstart
   ==============================================================
   =  Title  : Maze Challenge
   =  Author : latch
   =  Date   : 2/19/2007
   =  Update : 02/20/2007
   =  Version:
   ==============================================================
   Comments: Create AI to get through maze and move cones to other side
   ==============================================================
remend
 
rem =============================================================
rem = SET UP DISPLAY
rem =============================================================
   autocam off
   set display mode 800,600,16
   sync on
   sync rate 40
 
   gosub _init
 
   rem MY ADDITION TO PROGRAM ********
   gosub _init2
   rem *******************************
 
rem =============================================================
rem = MAIN
rem =============================================================
   _main:
   do
 
      rem MY ADDITION TO PROGRAM ********
      gosub Point_Karol
      rem *******************************
 
      rem can't change these ************
      gosub _move_robot
      `gosub _move_friends
      gosub _timer
      text 0,0,str$(screen fps() )
      rem *******************************
 
      sync
   loop
 
   end
 
rem =============================================================
rem = SUBROUTINES - PROCEDURES
rem =============================================================
   _init:
      gosub _plot_maze
      gosub _karol_the_robot
 
      rem camera vars
      camx#=matx/2
      camy#=300
      camz#=-100
 
      position camera camx#,camy#,camz#
      point camera camx#,0,matz/2
 
      rem timer variables
      timestart=0
 
      repeat
         center text screen width()/2,0,"Press CONTROL key to begin"
         rem wait here for control key
         sync
      until controlkey()=1
 
   return
 
`----------------------------------------------------------------
   _plot_maze:
      rem maze variables
      mazex=11
      mazez=10
      dim grid(mazex,mazez)
      rem read coordinates into an array
      for z=1 to mazez
         for x=1 to mazex
            read grid(x,z)
         next x
      next z
      restore
 
      matx=mazex*25
      matz=mazez*25
      across=0
      up=0
      obj=0
      size=mazez*mazex
 
      rem make matrix as a floor
      make matrix 1,matx,matz,mazex,mazez
 
      rem draw walls x direction
      x=0
      for z= 1 to mazez
         repeat
            inc x
            `read block
            if grid(x,z)=1
               obflag=1
               inc wallx,25
            else
               if obflag=1
                  inc obj
                  make object box obj,wallx,25,25
                  position object obj,(x*25)-(wallx/2)-25,12.5,((z-1)*25)+12.5
                  wallx=0
                  obflag=0
               endif
            endif
         until x = mazex
         if obflag=1
            inc obj
            make object box obj,wallx,25,25
            position object obj,(x*25)-(wallx/2),12.5,((z-1)*25)+12.5
            wallx=0
            obflag=0
            x=0
         endif
      next z
 
      rem draw walls z direction
      z=0
      for x= 1 to 11
         repeat
            inc z
            `read block
            if grid(x,z)=1
               obflag=1
               inc wallz,25
            else
               if obflag=1
                  inc obj
                  make object box obj,25,25,wallz
                  position object obj,((x-1)*25)+12.5,12.5,(((z-1)*25)-(wallz/2))
                  wallz=0
                  obflag=0
               endif
            endif
         until z =mazez
         if obflag=1
            inc obj
            make object box obj,25,25,wallz
            position object obj,((x-1)*25)+12.5,12.5,(z*25)-(wallz/2)
            wallz=0
            obflag=0
            z=0
         else
            wallz=0
            obflag=0
            z=0
         endif
      next x
 
      rem cleanup individual box items and set collision for remaining objects (walls)
      for n=1 to obj
         set object collision to boxes n
         if object size z(n)=25 and object size x(n)=25
            delete object n
         endif
      next n
 
 
   return
`----------------------------------------------------------------
   _karol_the_robot:
      robot=avail_obj(1)
      dim friend(3)
 
      rem create robot
      make object cone robot,10
      set object collision to boxes robot
      position object robot,0,5,-20
      color object robot,rgb(0,255,0)
      xrotate object robot,90
      fix object pivot robot
      turn object right robot,90
 
      rem create robot friends
      for n=1 to 3
         friend(n)=avail_obj(1)
         make object cone friend(n),16
         position object friend(n),n*70,8,130+matz
         color object friend(n),rgb(n*50,n*50,n*50)
         set object collision to boxes friend(n)
      next n
 
      rem create target
      make matrix 2,75,25,3,1
      position matrix 2,210,0,-50
 
   return
`----------------------------------------------------------------
   _collision_with_walls:
      rem check for wall collision
      bang=object collision(robot,0)
      if bang = friend(1) or bang = friend(2) or bang = friend(3) or bang=0
         rem do nothing
      else
         turn object left robot,180
      endif
   return
`----------------------------------------------------------------
   _move_robot:
      move object robot,2
      gosub _collision_with_walls
   return
`----------------------------------------------------------------
   _timer:
 
      if timestart=0
         thetimer=timer()
         timestart=1
      endif
      if timestart=1
         current_time=timer()-thetimer
         text 0,20,str$(current_time)
      endif
 
      rem find out if friends have been moved to the end matrix
      f1_x#=object position x(friend(1))
      f1_z#=object position z(friend(1))
      f2_x#=object position x(friend(2))
      f2_z#=object position z(friend(2))
      f3_x#=object position x(friend(3))
      f3_z#=object position z(friend(3))
 
      if f1_x# > 209 and f1_x# < 210+25 and f1_z# >-51 and f1_z# < -50+25
         goal_1=1
      else
         goal_1=0
      endif
      if f2_x# > 209+25 and f2_x# < 210+50 and f2_z# >-51 and f2_z# < -50+25
         goal_2=1
      else
         goal_2=0
      endif
      if f3_x# > 209+50 and f3_x# < 210+75 and f3_z# >-51 and f3_z# < -50+25
         goal_3=1
      else
         goal_3=0
      endif
 
      finalgoal=goal_1+goal_2+goal_3
 
      if finalgoal=3
         set text size 100
         set text opaque
         repeat
            center text screen width()/2,screen height()/2,"DONE! Time: "+str$(current_time)
            sync
         until scancode()>0
         end
      endif
 
   return
`----------------------------------------------------------------
   _move_friends:
      if current_time >= 6*1000
         for n = 1 to 3
            position object friend(n),210+((n*25)-12.5),object position y(friend(n)),-37.5
         next n
      endif
   return
 
   _init2:
 
      ` Dimension Pathfind Arrays
      Dim OpenList(198,3)
      Dim ClosedList(198,1)
      Dim Waypoint#(100,1)
      Dim Hval(198)
      Dim Gval(198)
      Dim Fval(198)
 
      ` Dimension Grid Array
      Dim Grid2(11,18)
 
      ` Set Grid To All Walkable
      for x=1 to 11
         for y=1 to 18
            Grid2(x,y)=1
         next y
      next x
 
      ` Set Robot Start Position
      DestX#=12.5
      DestZ#=-12.5
      CurrX#=1
      CurrZ#=2
 
      ` Set High Hval for sort
      Fval(0)=10000
 
      ` Create Collision Cube
      Impact_Obj=100
      make object cube Impact_Obj,20
      set object collision to boxes Impact_Obj
      hide object Impact_Obj
 
      rem Find Where Friends Are Located
      x1=object position x(friend(1))
      z1=object position z(friend(1))
      x2=object position x(friend(2))
      z2=object position z(friend(2))
      x3=object position x(friend(3))
      z3=object position z(friend(3))
 
      ` Set destination to friend 1
      DestX=x1/25+1
      DestZ=(z1+50)/25
 
      ` Set State Counter
      State=1
 
      ` Get Start Path
      gosub Find_New_Path
 
   return
 
   Point_Karol:
 
      point object robot,Waypoint#(CurrWay,0),object position y(robot),Waypoint#(CurrWay,1)
      set cursor 0,60 : print CurrWay
      set cursor 0,75 : print Waypoint#(CurrWay,0),"   ",Waypoint#(CurrWay,1)
      for x=CurrWay to WaypointMax
         if x=1 then ink rgb(0,0,0),0 else ink rgb(255,255,255),0
         set cursor 700,x*15-15
         print ClosedList(x,0),"   ",ClosedList(x,1)
      next x
      set cursor 500,0
      for x=2 to 11
         for z=1 to 18
            print Grid2(x,z)
         next z
      next x
      if sqrt((Waypoint#(CurrWay,0)-object position x(robot))^2+(Waypoint#(CurrWay,1)-object position z(robot))^2)<=1
        gosub Sensory_Input
        gosub Inc_Waypoint
      endif
 
   return
 
   Inc_Waypoint:
 
   Waypoints=Waypoints-1
   if Waypoints=1
      gosub Inc_State
      gosub Find_New_Path
   else
      for x=1 to Counter2-1
         if Grid2(ClosedList(x,0),ClosedList(x,1))=0 then ReCalc=1
      next x
      CurrWay=WaypointMax-Waypoints
   endif
 
   return
 
   Sensory_Input:
 
      ` Have Robot Check for Walls Around it
      for x=-1 to 1
         for y=-1 to 1
            position object Impact_Obj,Waypoint#(CurrWay,0)+(x*25),object position y(Impact_Obj),Waypoint#(CurrWay,1)+(y*25)
            TileX=CurrX+x
            TileY=CurrZ+y
            if TileX>0 and TileY>0 and TileX<12 and TileY<19
               bang=object collision(Impact_Obj,0)
               if bang<>friend(1) and bang<>robot and bang<>friend(2) and bang<>friend(3) and bang<>0
                  Grid2(TileX,TileY)=0
               endif
            endif
         next x
      next x
 
   return
 
   Inc_State:
 
   inc State
 
   if State=2
      glue object to limb friend(1),robot,0
      DestX=9
      DestZ=1
   endif
 
   if State=3
      unglue object friend(1)
      DestX=x2/25
      DestZ=(z2+50)/25
   endif
 
   if State=4
      glue object to limb friend(2),robot,0
      DestX=10
      DestZ=1
   endif
 
   if State=5
      unglue object friend(2)
      DestX=x3/25
      DestZ=(z3+50)/25
   endif
 
   if State=6
      glue object to limb friend(3),robot,0
      DestX=11
      DestZ=1
   endif
 
   if State=7
      unglue object friend(3)
   endif
 
   return
 
Find_New_Path:
 
` Initialize values
StartX=((object position x(Robot)+12.5)/25)
StartY=((object position z(Robot)+62.5)/25)
 
Counter1=1
Counter2=1
 
CurrX=StartX
CurrZ=StartZ
 
OpenList(1,0)=CurrX
OpenList(1,1)=CurrZ
 
Finish=0
Fval(1)=1
 
repeat
 
   ` Find Lowest F-Cost Square
   for x=1 to Counter1-1
      Skip=0
      for y=1 to Counter2-1
         if OpenList(x,0)=ClosedList(y,0) and OpenList(x,1)=ClosedList(y,1) then Skip=1
      next y
      if Skip=0
         if Fval(x)<Fval(BestTile)
            BestTile=x
         endif
      endif
   next x
 
   ` Add it to the Closed List
   ClosedList(Counter2,0)=OpenList(BestTile,0)
   ClosedList(Counter2,1)=OpenList(BestTile,1)
   inc Counter2
 
   ` Set Current Tile
   CurrX=ClosedList(Counter2-1,0)
   CurrZ=ClosedList(Counter2-1,1)
 
   ` Set Current Tile as Parent
   ParentX=CurrX
   ParentZ=CurrZ
 
   ` Add Adjacent Squares to Open List
   for x=-1 to 1
      for y=-1 to 1
         Run=1
         Skip=0
         TileX=CurrX+x
         TileY=CurrZ+y
         if TileX>0 and TileX<12 and TileY>0 and TileY<19
         if Grid2(TileX,TileY)=1
            if TileX<>CurrX or TileY<>CurrZ
               for z=0 to Counter2-1
                  if ClosedList(z,0)=TileX and ClosedList(z,1)=TileY then Run=0
               next z
               if Run=1
                  for z=1 to Counter1
                     if OpenList(z,0)=TileX and OpenList(z,1)=TileY
                        Skip=1
                        Zval=z
                     endif
                  next z
                  if Skip=0
                     OpenList(Counter1,0)=TileX : OpenList(Counter1,1)=TileY
                     OpenList(Counter1,2)=ParentX : OpenList(Counter1,3)=ParentY
                     ValX=abs(TileX-DestX)
                     ValZ=abs(TileY-DestZ)
                     Hval(Counter1)=(ValX+ValZ)*10
                     if y=0 or x=0 then Gval(Counter1)=Gval(BestTile)+G2 else Gval(Counter1)=Gval(BestTile)+G1
                     Fval(Counter1)=Hval(Counter1)+Gval(Counter1)
                     inc Counter1
                  else
                     if y=0 or x=0 then Gval=Gval(BestTile)+G2 else Gval=Gval(BestTile)+G1
                     if Gval<Gval(Zval)
                        ValX=abs(TileX-DestX)
                        ValZ=abs(TileY-DestZ)
                        Hval=(ValX+ValZ)*10
                        OpenList(Zval,2)=ParentX : OpenList(Zval,3)=ParentZ
                        Gval(Zval)=Gval : Hval(Zval)=Hval
                        Fval(Zval)=Gval+Hval
                     endif
                  endif
               endif
            endif
         endif
         endif
      next y
   next x
 
   rem Reset BestTile
   BestTile=0
 
   ` Check for Dest on OpenList
   for x=0 to Counter1-1
      if OpenList(x,0)=DestX and OpenList(x,1)=DestZ
         Finish=1
         Value=x
      endif
   next x
 
until Finish=1
 
   Waypoint#(Counter2-1,0)=OpenList(Value,0)*25-12.5
   Waypoint#(Counter2-1,1)=OpenList(Value,1)*25-62.5
 
for x=1 to Counter2-2
   Waypoint#(x,0)=ClosedList(x,0)*25-12.5
   Waypoint#(x,1)=ClosedList(x,1)*25-62.5
next x
 
Waypoints=Counter2-1
WaypointMax=Waypoints
CurrWay=2
 
return
 
rem =============================================================
rem = FUNCTIONS
rem =============================================================
   function avail_obj(num)
      while object exist(num)=1
         inc num
      endwhile
   endfunction num
 
rem =============================================================
rem = DATA STATEMENTS
rem =============================================================
 
   data 1,1,0,1,1,1,1,1,1,1,1
   data 1,0,0,0,0,0,1,0,0,0,1
   data 1,0,1,1,1,0,1,1,0,0,1
   data 1,0,0,0,1,0,0,0,0,0,1
   data 1,1,1,0,1,0,1,0,1,1,1
   data 1,0,1,0,0,0,1,0,0,0,1
   data 1,0,1,1,1,0,1,0,1,0,1
   data 1,0,0,0,0,0,1,0,1,0,1
   data 1,0,0,1,0,0,0,0,1,0,1
   data 1,1,0,1,1,1,1,1,1,0,1