%begin
%recordformat  peak(%integer l,r,th)

%record (peak) %array order(0:128*512)

%integer       r,th,max,i,x0,y0,
               x127,y511,x1,numpeaks=0,
               x2,y1,y2,ff,numpoints,num,oldnum

%constantinteger masklength=3,
                 maxspaces=4,
                 mergedist=10,
                 minlen=8

%real          st,ct

%integerarray  list(0:100,1:2)

%bytearray     a(0:527,0:359)

%string(255)   ouths

!*****************************************************!
!        MOVEDOWN                                     !
!  Move elements between a and b in array ORDER down  !
!  one element.                                       !
!*****************************************************!
%routine movedown(%integer a,b)
%integer i
%for i=b+1,-1,a+1 %cycle
  order(i)_l=order(i-1)_l
  order(i)_r=order(i-1)_r
  order(i)_th=order(i-1)_th
%repeat
%end

!*****************************************************!
!        INSERT                                       !
!   Insert peak (num,r,th) into sorted array ORDER    !
!   between bot and top.                              !
!*****************************************************!
%routine insert(%integer num,r,th,bot,top)
%integer middle
%while bot#top %cycle       {Iteratively find position in array}
  middle=(bot+top)>>1
  %if num>=order(middle)_l %then top=middle %else bot=middle+1
%repeat
%if num>order(bot)_l %start {Make space in arry for element to be ins.} 
  movedown(bot,numpeaks-1)
  order(bot)_l=num
  order(bot)_r=r
  order(bot)_th=th
%finishelsestart
  movedown(bot+1,numpeaks-1) {""}
  order(bot+1)_l=num
  order(bot+1)_r=r
  order(bot+1)_th=th
%finish
numpeaks=numpeaks+1
%end

!*****************************************************!
!         INSERT PEAK                                 !
!   Insert peak into array order.                     !
!*****************************************************!
%routine insert peak(%integer l,r,th)
%integer j,i=0

%if numpeaks=0 %start  {set up array if empty}
  order(0)_l=l
  order(0)_r=r
  order(0)_th=th
  numpeaks=1
%finishelse insert(l,r,th,0,numpeaks-1)
%end                    {otherwise insert it}
!*****************************************************!
!         MOD360                                      !
!  Count using modulus 360                            !
!*****************************************************!
%integerfunction mod360(%integer a)
%label l1,l2
!%if a<0 %then a=a+360
    *MOVE.L  D0,D0                
    *BGE     L1                   
    *ADDI.L  #16_0168,a             
L1:
!%if a>359 %then a=a-360
    *CMPI.L  #16_0167,a             
    *BLE     L2                   
    *ADDI.L  #16_FFFFFE98,a         
L2:
!%result=a
    *MOVE.L  a,D0                 
%end


%routine cycle(%integer bot,top,%bytearrayname a(0:527,0:359))
%integer r,t,l,p1,m1
%for t=bot,1,top %cycle
 %for r=1,1,526 %cycle
   l=a(r,t);p1=mod360(t+1);m1=mod360(t-1) {Wraparound array}
   %if l>2 %start
     %if (l>=a(r+1,t) %and l>=a(r-1,t) %and l>=a(r,p1) %c
     %and l>=a(r,m1) %and l>=a(r+1,p1) %c
     %and l>=a(r-1,m1) %and l>=a(r-1,p1) %c
     %and l>=a(r+1,m1)) %then insert peak(l,r,t)
   %finish
 %repeat
%repeat
%for t=bot,1,top %cycle
   l=a(0,t); p1=mod360(t+1);m1=mod360(t-1)
   %if l>2 %start
     %if (l>=a(r+1,t) %and l>=a(r,p1) %c
     %and l>=a(r,m1) %and l>=a(r+1,p1) %c
     %and l>=a(r+1,m1)) %then insert peak(l,0,t)
   %finish
   l=a(527,t);p1=mod360(t+1);m1=mod360(t-1)
   %if l>2 %start
     %if (l>=a(r-1,t) %and l>=a(r,p1) %and l>=a(r,m1) %c
     %and l>=a(r-1,m1) %and l>=a(r-1,p1)) %then insert peak(l,527,t)
   %finish
%repeat
%end

!*****************************************************!
!         FINDPEAKS                                   !
!  Locate all peaks in accumulator array.             !
!*****************************************************!
%routine findpeaks
%integer r,t,l,p1,m1,i,j,k
  cycle(0,179,a)
  cycle(271,359,a)
%end

%endofprogram
