!******************************************************************************!
!                                                                              !
!                                  ROBOT VISION                                !
!                                                                              !
!                               IAN R GEBBIE (CS4)                             !
!                                                                              !
!******************************************************************************!
%option "-nons-nowarn"

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

!******************************************************************************!
!                                  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 %alias "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

!--------------------------------- 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
   %EXTERNALROUTINE raw to snap %alias "RAW_TO_SNAP" %c
                                ( %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 ---------------------------------------------

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

!******************************************************************************!
!                                  CAMERA                                      !
!******************************************************************************!
%externalroutine camera(%integer init exp,final exp)
   %integer command,actbytes,rows,cols,i,grey,exposure,step
   %bytearray pic(0:65535)
   %bytearray snap,fsnap(0:128,0:513)

   %if init exp=final exp %then grey=0 %else grey=1
   exposure = init exp
   step=(final exp-init exp)//15

%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 %alias "DUMP_BITMAP" %c
                   (%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
   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 = 5 + ( ( column - 1 ) * 2 )
         offset = ( row * 514 ) + x
         transfer_tr ( row, column ) = offset
         transfer_tr ( 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_tr ( row, column ) = offset
         transfer_tr ( row, column+1 ) = offset + 513
      %REPEAT
   %REPEAT

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

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

! initialise => Perform program initialisations.
   %EXTERNALROUTINE initialise

! Begin initialise

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

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

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