!******************************************************************************!
!                                                                              !
!                                  ROBOT VISION                                !
!                                                                              !
!                               IAN R GEBBIE (CS4)                             !
!                                                                              !
! incorporating later hacks by Brian Craigie, Dave Baines, and Julian Turnbull !
!                                                                              !
!******************************************************************************!
%option "-nons-nowarn"

! The following declaration is to allow space to be allocated from the heap
! to hold the lookup table used in unscrambling the geometry of the image.
%record %format trfm (%integer %array tr(0:128,0:255))
%own %record (trfm) %name transfer

%include "inc:util.imp"
%include "camera:region.inc"
%include "inc:level1.imp"

%constinteger  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
               
!Zapped interface addresses
!%constinteger base = 16_400FF0

!BEPI interface addresses (Thanks Rainer)
%constinteger base = 16_400600

@base+16_00 %byte acia cont;  @base+16_02 %byte acia data

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

!******************************************************************************!
!                                  OPEN ACIA                                   !
!******************************************************************************!
%bytefn inv8(%byte i)
   %constintegerarray inv(0:15)= %c
   16_00,16_08,16_04,16_0C,16_02,16_0A,16_06,16_0E,
   16_01,16_09,16_05,16_0D,16_03,16_0B,16_07,16_0F
   %result = inv(i&15)<<4+inv(i>>4&15)
%end

%routine puta(%integer ad, %byte val)
   !Inverts the bit order of a byte
   byte(ad) = inv8(val)
%end

%bytefn geta(%integer ad)
   !Inverts bit order
   %result = inv8(byte(ad))
%end
   

%routine open acia
   %constinteger acset = 16_03, {master reset}
                 acopt = 16_14  {div 1, 8 bits 1 stop bit}
   puta(acia cont, acset)
   puta(acia cont, acopt)
%end

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


!******************************************************************************!
!                                  GRAB FRAME                                  !
!******************************************************************************!
%routine grab frame (%integer command, exposure, address,
                     %integername rows,cols,bytes)

      %integer start, end, i
      %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    #7,aciacont   ;!Note inverted
    *beq     waitch
    *move.b  aciadata,0(a0)
    *adda.l  #1,a0
    *cmpa.l   a0,a1
    *bne     waitch
 
     %for i = address, 1, address+bytes-1 %cycle
        byte(i) = inv8(byte(i))
     %repeat
     command = (command & (\3)) ! nosend ! refresh
     outch(command)
 %end

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

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

!---------------------------------- 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 -----------------------------------------------

! 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,
                                      %RECORD(TRFM)%NAME transfer )

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

! raw to snap => Convert raw data frame to a snap frame
   %ROUTINE 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)

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

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

!------------------------get_frame---------------------------------------------
!
%externalroutine get frame %alias "GET_FRAME" (%integer exposure,
                                  %bytearrayname pic(0:128,0:513))

%bytearray rawpic(0:65535)
%integer command,actbytes,rows,cols

   command=noalt ! narrow ! eightbit ! onearray

   grab frame(command,exposure,addr(rawpic(0)),rows,cols,actbytes)
   raw to snap(rawpic,pic)

%end {get_frame}

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

! filter snap => Apply a filter to the image image.
   %EXTERNALROUTINE filter snap %alias "FILTER_SNAP" %c
                                ( %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 ---------------------------------------------

!*****************************************************!
!         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 fillim %alias "FILL_IMAGE" (%bytearrayname image(0:128,0:513))
%integer row,col
%mitearray neighbours(1:127,1:512)

   print string("Filling of image started - please be patient".snl)

   %for row=1,1,127 %cycle
      %for col=1,1,512 %cycle
         %if image(row,col)=0 %then neighbours(row,col)=0 %else %c
         neighbours(row,col)=(image(row-1,col-1)&1)+(image(row-1,col+1)&1) %c
                            +(image(row+1,col-1)&1)+(image(row+1,col+1)&1)
      %repeat
   %repeat

   %for row=1,1,127 %cycle
      %for col=1,1,512 %cycle
         %if neighbours(row,col)>1 %then image(row,col)=1
      %repeat
   %repeat

%end


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

! init transfer => Initialse the raw data -> image data transfer array.
   %EXTERNALROUTINE init transfer %ALIAS "INIT_CAMERA"

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

! Begin init transfer
! Get space for lookup table from heap
   transfer==new(transfer)

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

! even row pixels
   %FOR row = 0, 2, 126 %CYCLE
      %FOR column = 1, 2, 253 %CYCLE
         x = column * 2
         offset = ( row * 514 ) + x
         transfer_tr ( row, column ) = offset
         transfer_tr ( row, column+1 ) = offset + 519
      %REPEAT
   %REPEAT

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

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


!******************************************************************************!
!                          INIT DISPLAY                                        !
!******************************************************************************!
%external %routine init display %alias "INIT_DISPLAY"

!------------------------------- 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

   hline(86,601,ycorner)
   hline(86,601,ycorner+130)
   vline(86,ycorner,ycorner+130)
   vline(601,ycorner,ycorner+130)

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

! clear graphics screen
   clear

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

%end {init display}

%endoffile