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

%begin

%recordformat vertice(%real x,y,z)
%record(vertice)%array vert(1:8)

%real          cx,cy,cz

%integer       l,total,x,y,
               i,j,ch,inc,
               numlines,
               bx,by,fx,fy

%integer       frames=1,
               steps=21,
               exposure,
               outtofile=0,
               obx=0,
               cox=250,
               coy=250,
               dohough=0,
               homeset=0,
               dothin=0,
               threshhold=50,           
               px=86,
               py=1

%byte          a,b,c

%string (255)  file,reply

%integerarray  lines(0:500,1:4)
%integerarray  pulses(0:5)

%bytearray     pic(0:65535)
%bytearray     image,copy1,copy2,lpic(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,
               lens=8,
               elbow=4,
               shoulder=2,
               turntable=1,
               cubeside=50,
               strobe = 128

@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
   %repeat
   acia data = ch
%end


!*****************************************************!
!         GRAB FRAME                                  !
!*****************************************************!
%routine 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                               !
!*****************************************************!
%routine 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                                   !
!*****************************************************!
%routine transform
%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

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

   command = noalt ! narrow ! eightbit ! onearray
%for exposure=0,2,32 %cycle
   printstring("Exposure=");write(exposure,3);newline
   grab frame(command, exposure, addr(pic(0)), rows,cols, actbytes)
   transform
   display frame(px,py,image(0))
%repeat
%end

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

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

   set terminal mode(8)

   colour map(i)=i<<10!i<<5!i %for i=0,1,31
   clear
   enable(255)

    clear frame(image(0))
    camera
%endofprogram
