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

%begin

%bytearray epic(0:128*512)

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


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


!******************************************************************************!
!                                  DISPLAY FRAME                               !
!******************************************************************************!
%externalroutine display frame(%integer x,y,%bytename line)
%label ok

  *move.l d0,d2
  *lsl.l #7,d1
  *andi.l #16_FFFFFFF0,d0
  *lsr.l #3,d0
  *add.l d0,d1
  *lea   frame,a1
  *lea   0(a1,d1.l),a1
  *move.l a1,a3
  *adda.l #128,a3
  *lea   colourreg,a2
  *andi.l #15,d2
  *eor.l  #15,d2
  *clr.l  d1
  *bset   d2,d1
  *moveq  #127,d3
z:*move.l  #511,d0
a:*move.b (a0)+,(a2)
  *move.w d1,(a1)
  *ror.w #1,d1
  *btst  #15,d1
  *beq   ok
  *lea    2(a1),a1
ok:
  *dbra   d0,a
  *move.l a3,a1
  *adda.l #128,a3
  *bset   d2,d1
  *dbra   d3,z
%end


!******************************************************************************!
!                                  TRANSFORM                                   !
!******************************************************************************!
%externalroutine transform(%bytearrayname pic(0:65535),
                                         %bytearrayname image(0:128*512))
%integer r,c,i,j,k,l,pix,x,y

%for r=0,1,127 %cycle
  %for c=0,1,31 %cycle
    k = (127-r)<<5+c
    %for j=7,-1,0 %cycle
      l=c<<3 + j
      %if pic(k)&(1<<j)#0 %then pix=1 %else pix=0
      y=r; x=l<<1+1
      %if (r&1=0 %and l&1=0) %or (r&1#0 %and l&1#0) %then %c
      y=y-1 %and x=x-3
      %if (x&1)=0 %then y=y+2
      i = y<<9+x
      image(i)=image(i)+pix %if 0<=x<=511 %and 0<=y<=127
    %repeat
  %repeat
%repeat
%end

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

!******************************************************************************!
!                                  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)
   transform(pic,image)
   display frame(px,py,image(0))
   exposure = exposure + step
%repeat
%end

!******************************************************************************!
!                                  CLEAR FRAME                                 !
!******************************************************************************!

%externalroutine clear frame(%bytename base)
%label LP
    *MOVEQ   #-4,D0
LP: *ADDQ.L  #4,D0
    *MOVE.L  #0,0(A0,D0.L)
    *CMPI.L  #16_10000,D0
    *BNE     LP
%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

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

   set terminal mode(8)

   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
   clear
   enable(255)

    clear frame(image(0))
    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)
 %endofprogram