!******************************************************************************!
!                                                                              !
!                                  ROBOT VISION                                !
!                                                                              !
!                               IAN R GEBBIE (CS4)                             !
!                                                                              !
!******************************************************************************!
%option "-nons-nowarn"
%include "inc:util.imp"
%include "camera:imgraph.inc"
%include "camera:region.inc"
!%include "inc:level1.imp"

%begin

%integer       x,y,
               i,j,ch,inc,

%integer       exposure,
               initial,
               final,
               grey,
               gnog,
               step,
               add,
         ref,size,i,
               strobe=128,
               outtofile=0,
               obx=0,
               cox=250,
               coy=250,
               px=86,
               py=1
;

%byte          a,b,c

%string (255)  file,reply


%bytearray     pic(0:65535)
%bytearray     image(0:128*512)


%BYTEARRAY snap ( 0:128, 0:513 )
%BYTEARRAY fsnap ( 0:128, 0:513 )

%INTEGERARRAY transfer ( 0:128, 0:255 )

%constinteger  base = 16_400FF0,
               tag = 16_C0,
               noalt=32,
               narrow=16,
               eightbit=8,
               twoarray=4,
               soak=2,
               nosend=1,
               alt=0,
               wide=0,
               sevenbit=0,
               onearray=0,
               refresh=0,
               send=0

@base+16_00 %byte acia cont;  @base+16_02 %byte acia data
@base+16_08 %byte pia areg;   @base+16_0A %byte pia acont
@base+16_0C %byte pia breg;   @base+16_0E %byte pia bcont

!******************************************************************************!
!                                  WAIT                                        !
!******************************************************************************!
%routine wait(%integer msec)
   msec = cputime + msec
   %while cputime <= msec %cycle
   %repeat
%end

!******************************************************************************!
!                                  OPEN ACIA                                   !
!******************************************************************************!
%routine open acia
   %constinteger acset = 16_03, {master reset}
                 acopt = 16_14  {div 1, 8 bits 1 stop bit}
   acia cont = acset
   acia cont = acopt
%end

!******************************************************************************!
!                                  OUTCH                                       !
!******************************************************************************!
%routine outch(%integer ch)
   %integer c, time
   time=cputime+1000
   %cycle
       c = acia cont
       %exit %if c&2 # 0
!       %signal 15,1 %if time=cputime
       %signal 15,1 %if time >= cputime  {BSC !}
   %repeat
   acia data = ch
%end


!******************************************************************************!
!                                  GRAB FRAME                                  !
!******************************************************************************!
%externalroutine grab frame(%integer command, exposure, address,
                                                   %integername rows,cols,bytes)
      %integer start, end
      %label waitch
      open acia
      wait(2)
      command = (command & (\3)) ! tag ! refresh ! nosend
      outch(command)

      wait(5)
      
      command = (command & (\3)) ! soak ! nosend
      outch(command)

      wait(exposure)
      rows=64; cols=16
      command = (command & (\3)) ! send ! noalt ! refresh
   {*** The above line altered so that the exposure can be set by the user ***}
   {*** Old version was:                                                   ***}
   !  command = (command & (\3)) ! send ! noalt ! soak
   {***                                          JST, 5/1/87               ***}

      %if command & twoarray # 0 %then rows=rows*2
      %if command & narrow = 0   %then cols=cols*2
      %if command & noalt # 0 %then    rows=rows*2 %and cols=cols*2
      bytes=rows*cols

      outch(command)

    start = address; end = address+bytes
 
    *movea.l start,a0
    *movea.l end,a1
 
 waitch:
    *btst    #0,aciacont
    *beq     waitch
    *move.b  aciadata,0(a0)
    *adda.l  #1,a0
    *cmpa.l   a0,a1
    *bne     waitch
 
     command = (command & (\3)) ! nosend ! refresh
     outch(command)
 %end

!---------------------------------- clear box ----------------------------------

! clear box => Clear the inside of the frame plotting box
   %ROUTINE clear box ( %INTEGER pic )

! Local Variables :
   %INTEGER ycorner

! Begin clear box
   %IF ( pic = 0 ) %THEN ycorner = 1 %ELSE ycorner = 141
   colour ( black ) ;
   fill ( 87, ycorner, 600, ycorner+128 )

   %END ; ! of routine clear box -----------------------------------------------

!------------------------------- plot box border -------------------------------

! plot box border => Plot the border for the frame plotting box
   %ROUTINE plot box border ( %INTEGER pic )

! Local Variables
   %INTEGER ycorner

! Begin plot box border
   %IF ( pic = 0 ) %THEN %START
      colour ( blue ) ;
      ycorner = 0
   %FINISH %ELSE %START
      colour ( red ) ;
      ycorner = 140
   %FINISH

   fill ( 86, ycorner, 601, ycorner+130 ) ;
   clear box ( pic )

   %END ; ! of routine plot box border -----------------------------------------

!--------------------------------- plot frame ----------------------------------

! plot frame => Plot image frame with left hand bottom corner at given coords.
   %EXTERNALROUTINE plot frame ( %INTEGER pic,
                                 %BYTEARRAYNAME snap ( 0:128, 0:513 ) )

! Local Variables :
   %INTEGER row, column, offset, row1, rowoffset

! Begin plot frame

   %IF ( pic = 0 ) %THEN rowoffset = 129 %ELSE rowoffset = 269
   clear box ( pic )
   colour ( white ) ;
   %FOR row = 0, 1, 128 %CYCLE
      row1 = rowoffset - row
      %FOR column = 0, 1, 513 %CYCLE
         %IF ( snap ( row, column ) = 1 ) %THEN plot(column+87, row1) ;
      %REPEAT
   %REPEAT

   %END ; ! of routine plot frame ----------------------------------------------

!---------------------------------- clear snap----------------------------------

! clear snap => Set whole of snap array to zero's
   %ROUTINE clear snap ( %BYTEARRAYNAME snap ( 0:128, 0:513 ) )

! Local Variables :
   %INTEGER x, y

! Begin clear snap

   %FOR x = 0, 1, 128 %CYCLE
      %FOR y = 0, 1, 513 %CYCLE
         snap ( x, y ) = 0
      %REPEAT
   %REPEAT

   %END ; ! of routine clear snap ----------------------------------------------

%EXTERNALROUTINESPEC fast transform ( %BYTENAME packed, %BYTENAME snap,
                                      %INTEGERNAME transfer )

!--------------------------------- raw to snap ---------------------------------

! raw to snap => Convert raw data frame to a snap frame
   %EXTERNALROUTINE raw to snap ( %BYTEARRAYNAME pic ( 0:65535 ),
                                  %BYTEARRAYNAME snap ( 0:128, 0:513 ) )

! Local Variables :
   %INTEGER start, finish, duration

! Begin raw to snap

   start = cputime

! perform packed raw data -> snap frame transformation using lookup table
   fast transform ( pic ( 0 ), snap ( 0, 0 ), transfer ( 0, 0 ) )

   finish = cputime
   duration = finish - start
   PRINTSTRING ( "Duration = " ) ; WRITE ( duration, 6 ) ; NEWLINE

   %END ; ! of routine raw to snap ---------------------------------------------

!-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----!
!-----=======<<<<<<<<<<  ROBOT CONTROL  >>>>>>>>>>=======-----!
!-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----!

!*****************************************************!
!         PULSE                                       !
!  Sends a single pulse to the robot.                 !
!*****************************************************!
%externalroutine pulse(%integer msec)
   pia bcont = 16_3C
   wait(msec)
   pia bcont = 16_34
   wait(msec)
%end

!*****************************************************!
!         MOVE AXES                                   !
!  Moves all axes of robot according to contents of   !
!  array PULSES. Each location in PULSES stores the   !
!  number of pulses to be sent to the corresponding   !
!  axis.                                              !
!*****************************************************!
%externalroutine move axes(%integer mdp, %integerarray pulses(0:5))
   %integer mep, i, pulsewdth
   pia breg = mdp
   pia breg = mdp ! strobe
   wait(1)
   pia breg = mdp & (\strobe)
   %if pulses(0) >0 %then pulsewdth = 8 %else pulsewdth = 5
   %cycle
      mep = 0
      %for i=0, 1, 5 %cycle
         %if pulses(i)>0 %then mep = mep ! (1<<i) %and pulses(i)=pulses(i)-1
      %repeat
      pia breg = mep
      pulse(pulsewdth)
      %if pulses(0)<=0 %then pulsewdth=5
   %repeatuntil mep=0
%end

!*****************************************************!
!         INITIALISE MOTORS                           !
!*****************************************************!
%externalroutine initialise motors   {Switch all motors off}
   pia breg = 0
   pia breg = strobe
   wait(1)
   pia breg = 0
%end

!*****************************************************!
!         OPEN INTERFACE                              !
!  Initialise interface to robot.                     !
!*****************************************************!
%externalroutine open interface
   pia acont = 0
   pia bcont = 4
   pia breg = 0
   pia bcont = 16_30
   pia breg  = 16_BF
   pia bcont = 16_34
%end

!*****************************************************!
!         FILLIMAGE                                   !
!  Routine fills out blank values in image array.     !
!  If blank pixel has two or more neighbours set to 1 !
!  then set pixel to a 1 as well                      !
!*****************************************************!
%externalroutine fillimage(%bytearrayname image(0:128*512))
%integer i,j,col,row,k,np
i=0;j=0
%for col=1,1,511 %cycle
  %for row=2+i,2,124+i %cycle
    k=row<<9+col
    np=image(k-512)+image(k+512)
    %if j=0 %then np=np+image(k-1) %else np=np+image(k+1)
    %if np=2 %or np=3 %then image(k)=1 %else image(k)=0
  %repeat                          
    %if j=0 %then j=1 %elsestart
      %if i=1 %then i=0 %else i=1
      j=0
    %finish
%repeat
%end

!--------------------------------- filter snap ---------------------------------

! filter snap => Apply a filter to the image image.
   %EXTERNALROUTINE filter snap ( %BYTEARRAYNAME snap ( 0:128, 0:513 ),
                                  %BYTEARRAYNAME fsnap ( 0:128, 0:513 ) )

! Local Variables :
   %INTEGER row, column, sum, rowp1, rowm1, colp1, colm1
   %INTEGER start, finish, duration

! Begin

   start = cputime

   %FOR column = 0, 1, 513 %CYCLE
      fsnap ( 0, column ) = snap ( 0, column )
      fsnap ( 128, column ) = snap ( 128, column )
   %REPEAT

   %FOR row = 1, 1, 127 %CYCLE
      fsnap ( row, 0 ) = snap ( row, 0 )
      %FOR column = 1, 1, 512 %CYCLE
         rowp1 = row + 1
         rowm1 = row - 1
         colp1 = column + 1
         colm1 = column - 1
         sum = snap( rowm1,colm1 ) + snap( rowm1,column ) + snap( rowm1,colp1 )
         sum = sum + snap( row,colm1 ) + snap( row,column ) + snap( row,colp1 )
         sum = sum + snap(rowp1,colm1) + snap(rowp1,column) + snap(rowp1,colp1)
         %IF ( sum > 2 ) %THEN fsnap ( row, column ) = 1 %C
         %ELSE fsnap ( row, column ) = 0
      %REPEAT
      fsnap ( row, 513 ) = snap ( row, 513 )
   %REPEAT
   finish = cputime
   duration = finish - start
   PRINTSTRING ( "Filter duration = " ) ; WRITE ( duration, 6 ) ; NEWLINE ;

   %END ; ! of routine filter snap ---------------------------------------------

!******************************************************************************!
!                                  CAMERA                                      !
!******************************************************************************!
%externalroutine camera
   %integer command,actbytes,rows,cols,i

exposure = initial
%for i=0,1,grey*15 %cycle { Grey either 0 or 1 }
   command = noalt ! narrow ! eightbit ! onearray
   printstring("Exposure=");write(exposure,3);newline

   grab frame(command, exposure, addr(pic(0)), rows,cols, actbytes)

   raw to snap ( pic, snap )

!   filter snap ( snap, fsnap )

   plot frame ( 0, snap )

!   plot frame ( 1, fsnap )

   exposure = exposure + step
%repeat
%end

!******************************************************************************!
!                                  DUMP BITMAP                                 !
!                           Dumps image to a file.                             !
!******************************************************************************!
%externalroutine dump bitmap(%bytearrayname image(0:128*512),%string(255) file)
%integer ref,size
  open output(1,file)
  select output(1)
  printsymbol('*')
  closeoutput
  selectoutput(0)
  accessfile(file,1,ref,size)
  writeregion(ref,0,128*512,image(0)) 
  deaccessfile(ref)
  printstring("Done");newline
%end

!-------------------------------- init transfer --------------------------------

! init transfer => Initialse the raw data -> image data transfer array.
   %ROUTINE init transfer

! Local Variables :
   %INTEGER row, column, x, index, temp, offset

! Begin init transfer

! edge pixels first
   %FOR row = 0, 2, 126 %CYCLE
      offset = ( row * 514 ) + 513
      transfer ( row, 0 ) = offset + 1
      transfer ( row, 255 ) = offset
   %REPEAT

! even row pixels
   %FOR row = 0, 2, 126 %CYCLE
      %FOR column = 1, 2, 253 %CYCLE
         x = 5 + ( ( column - 1 ) * 2 )
         offset = ( row * 514 ) + x
         transfer ( row, column ) = offset
         transfer ( row, column+1 ) = offset + 513
      %REPEAT
   %REPEAT

! odd row pixels
   %FOR row = 1, 2, 127 %CYCLE
      %FOR column = 0, 2, 254 %CYCLE
         x = 3 + ( column * 2 )
         offset = ( row * 514 ) + x
         transfer ( row, column ) = offset
         transfer ( row, column+1 ) = offset + 513
      %REPEAT
   %REPEAT

   %END ; ! of routine init transfer -------------------------------------------

!---------------------------------- initialise ---------------------------------

! initialise => Perform program initialisations.
   %ROUTINE initialise

! Begin initialise

! set terminal into continuous scroll mode
   set terminal mode(8)

! clear graphics screen
   clear

! plot image box borders
   plot box border ( 0 )
   plot box border ( 1 )

! set all elements of the image array to zero
   clear snap ( snap )
   clear snap ( fsnap )

! initialise raw data -> image data mapping array
   init transfer

   %END ; ! of routine initialise ----------------------------------------------

!------------------------------------------------------------------------------!
!                      M  A  I  N    P  R  O  G  R  A  M                       !
!------------------------------------------------------------------------------!

! perform program initialisations
   initialise

! loop indefinately
   %CYCLE

      final = 0
      grey=0

      prompt("Grey/Nogrey ?")
      readsymbol(gnog)
      readline(file)
      %if (gnog & 16_5f) ='G' %then grey = 1
      %if grey=0 %then prompt ("Exposure ?") %else prompt("Initial exposure ?")
      read(initial)
      readline(file)
      %if grey=1 %then %start
         prompt("Final exposure ?")
         read(final)
         readline(file)
      %finish
      prompt(":")
      step = int ((final - initial)/15) { 15 different greys. }
      %if grey=0 %then add=32 %else add=0
      colour map(i+add)=i<<10!i<<5!i %for i=0,1,31
      enable(255)

      prompt ( "Continuous ? " ) ;
      readline ( file ) ;
      %IF ( file = "y" ) %THEN %START
         %CYCLE
            camera
         %REPEAT
      %FINISH

      camera

!      prompt("Fill image?") ; readline(file)
!      %if file = "y" %then fillimage(image) %and display frame(px,py+128,image(0))

!      prompt("output file ?") ; readline(file)
!      %if file # "" %then dump bitmap(image,file)

   %REPEAT

%endofprogram