C
C A straight Fortran implementation of Okutomi/Kanade multiple
C baseline stereo using Jon Webb's column summing scheme.   
C
C Peter A. Dinda
C
C
C Notes:
C   1. The input arrays ref, m1, m2 are of type real in this code because
C      this better fits the LIW-ness of iWarp.  Integer arrays may be more
C      appropriate on other hardware.
C
C   2. Despite the parameters WINX and WINY, there are places in this code
C      where a 13x13 window is assumed.
C
C
      program stereo

      integer ROWS, COLS, WINX, WINY, MAXDISP, ITERS
      parameter (ROWS=256)
      parameter (COLS=240)
      parameter (WINX=13)
      parameter (WINY=13)
      parameter (MAXDISP=16)
      parameter (ITERS=10)

      integer ref(ROWS+WINY,COLS)
      integer m1(ROWS+WINY,COLS)
      integer m2(ROWS+WINY,COLS)
      real diffimg(ROWS+WINY,COLS)

      real curbesterr(ROWS,COLS)
      integer curbestdisp(ROWS,COLS)

      integer count, curdisp


      print *,"#Program Begins..."


      do count=1,ITERS
c         print *, "#Iteration ", count
         call getdata(ref,m1,m2,curbesterr,curbestdisp)
         do curdisp=0,MAXDISP-1
c            print *,"#  Disparity ", curdisp 
            call gendiffimg(ref, m1, m2, diffimg, curdisp)
            call updatedispimg(diffimg, curbesterr, 
     $                         curbestdisp, curdisp)
         enddo

         print *, "#Testing Results...."
         call testdata(curbestdisp)

c         call dumpimg(curbestdisp,ROWS,COLS)

      enddo

      print *, "#Program Completed ", ITERS, " iterations of"
      print *, "#",MAXDISP, " disparities each."

      end


C
C This routine forms the difference image  
C
C
      subroutine gendiffimg(ref, m1, m2, diffimg, curdisp) 

      integer ROWS, COLS, WINX, WINY, MAXDISP, ITERS
      parameter (ROWS=256)
      parameter (COLS=240)
      parameter (WINX=13)
      parameter (WINY=13)
      parameter (MAXDISP=16)
      parameter (ITERS=10)

      integer ref(ROWS+WINY,COLS), m1(ROWS+WINY,COLS) 
      integer m2(ROWS+WINY,COLS)
      real diffimg(ROWS+WINY,COLS)
      integer curdisp

      integer i, j
      
      do i=1,ROWS+WINY
         do j=1,COLS-2*curdisp
            diffimg(i,j)=(ref(i,j)-m1(i,j+curdisp))**2 +
     $                   (ref(i,j)-m2(i,j+2*curdisp))**2
         enddo
      enddo

c      print *,"ref(124,109)=",ref(124,109)

      end


C
C This routine sums the difference image over a moving window and updates
C the current best error and best disparity images.
C
      subroutine updatedispimg(diffimg, curbesterr, 
     $                         curbestdisp, curdisp)

      integer ROWS, COLS, WINX, WINY, MAXDISP, ITERS
      parameter (ROWS=256)
      parameter (COLS=240)
      parameter (WINX=13)
      parameter (WINY=13)
      parameter (MAXDISP=16)
      parameter (ITERS=10)

      real diffimg(ROWS+WINY,COLS)
      real curbesterr(ROWS,COLS)
      integer curbestdisp(ROWS,COLS)
      integer curdisp

      real sum

      real csum(COLS)

      integer i, j

      integer local

      real localf
      
c      do i=118,131
c         do j=1, COLS
c            local=diffimg(i,j)
c            print *,"diffimg(",i,",",j,")=",local
c        enddo
c      enddo


      print *, "diffimg(124,109)=",diffimg(124,109)

      do j=1,COLS
         csum(j) = 0.0
         do i=1,WINX
            csum(j) = csum(j) + diffimg(i,j)
         enddo
      enddo

      do i=1,ROWS
         sum = 0.0
         do j=1,7
            sum = sum + csum(j)
         enddo
         
         if (sum .lt. curbesterr(i,1)) then
            curbesterr(i,1) = sum
            curbestdisp(i,1) = curdisp
         endif
         
         do j=2,7
            sum = sum + csum(j+6)
            if (sum .lt. curbesterr(i,j)) then
               curbesterr(i,j) = sum
               curbestdisp(i,j) = curdisp
            endif
         enddo
         
         do j=8,COLS-6
            sum = sum - csum(j-7)
            sum = sum + csum(j+6)
            if (sum .lt. curbesterr(i,j)) then
               curbesterr(i,j) = sum
               curbestdisp(i,j) = curdisp
            endif
         enddo
         
         do j=COLS-5,COLS
            sum = sum - csum(j-7)
            if (sum .lt. curbesterr(i,j)) then
               curbesterr(i,j) = sum
               curbestdisp(i,j) = curdisp
            endif
         enddo
         
         do j=1,COLS
            csum(j) = csum(j) - diffimg(i,j) + diffimg(i+WINY,j)
         enddo
         
      Enddo

      do i=118,131
         do j=1, COLS
            localf=curbesterr(i,j)
            print *,"curbesterr(",i,",",j,")=",localf
        enddo
      enddo


      print *, "curdisp=",curdisp

      print *, "curbesterr(124,109)=",curbesterr(124,109)
      print *, "curbestdisp(124,109)=",curbestdisp(124,109)

      end
      

C
C Generate some input data
C
C This routine is pretty dull.
C
      subroutine getdata(ref,m1,m2,curbesterr,curbestdisp)

      integer ROWS, COLS, WINX, WINY, MAXDISP, ITERS
      parameter (ROWS=256)
      parameter (COLS=240)
      parameter (WINX=13)
      parameter (WINY=13)
      parameter (MAXDISP=16)
      parameter (ITERS=10)

      integer tardis
      parameter (tardis=12)
      integer isize
      parameter (isize=37)
      integer jsize
      parameter (jsize=37)

      integer i, j

      integer ref(ROWS+WINY,COLS), m1(ROWS+WINY,COLS)
      integer m2(ROWS+WINY,COLS)
      real curbesterr(ROWS,COLS)
      integer curbestdisp(ROWS,COLS)

      integer ipos,jpos
      
      do i=1,6
         do j=1,COLS
            ref(i,j)=0
            m1(i,j)=0
            m2(i,j)=0
         enddo
      enddo


      do i=7,ROWS+6
         do j=1,COLS
            ref(i,j) = (i + j)
            m1(i,j) = (i + j)
            m2(i,j) = (i + j)
         enddo
      enddo


      do i=ROWS+7,ROWS+13
         do j=1,COLS
            ref(i,j)=0
            m1(i,j)=0
            m2(i,j)=0
         enddo
      enddo


      ipos=(ROWS-isize)/2 + 6
      jpos=((COLS-jsize)/2) - tardis
      
      do i=ipos,ipos+isize-1
         do j=jpos,jpos+jsize-1
            ref(i,j) = ((i-ipos+1) + (j-jpos+1))
            m1(i,j+tardis) = ((i-ipos+1) + (j-jpos+1))
            m2(i,j+2*tardis) = ((i-ipos+1) + (j-jpos+1))
         enddo
      enddo
      
      do i=1,ROWS
         do j=1,COLS
            curbesterr(i,j) = 9999999.0
            curbestdisp(i,j) = 0
         enddo
      enddo
      
      
      end


c
c Routine to test output (matched with input routine above)
c
c
      subroutine testdata(disp)

      integer ROWS, COLS, WINX, WINY, MAXDISP, ITERS
      parameter (ROWS=256)
      parameter (COLS=240)
      parameter (WINX=13)
      parameter (WINY=13)
      parameter (MAXDISP=16)
      parameter (ITERS=10)

      integer tardis
      parameter (tardis=12)
      integer isize
      parameter (isize=37)
      integer jsize
      parameter (jsize=37)

      integer disp(ROWS,COLS)

      integer ipos, jpos, i, j

      ipos=(ROWS-isize)/2
      jpos=((COLS-jsize)/2) - tardis
      
      do i=ipos+10,ipos+isize-11
         do j=jpos+10,jpos+jsize-11
            if (disp(i,j).ne.tardis) then
               write (*,100) i,j,tardis,disp(i,j)
            end if
         end do
      end do
      
      do i=1,ipos-10
         do j=1,jpos+jsize+2*tardis+9
            if (disp(i,j).ne.0) then
               write (*,100) i,j,0,disp(i,j)
            end if
         end do
      end do
      
      do i=1,ipos+isize+9
         do j=jpos+jsize+2*tardis+9,COLS
            if (disp(i,j).ne.0) then
               write (*,100) i,j,0,disp(i,j)
            end if
         end do
      end do
      
      do i=ipos+isize+9,ROWS
         do j=jpos-10,COLS
            if (disp(i,j).ne.0) then
               write (*,100) i,j,0,disp(i,j)
            end if
         end do
      end do
      
      do i=ipos-10,ROWS
         do j=1,jpos-10
            if (disp(i,j).ne.0) then
               write (*,100) i,j,0,disp(i,j)
            end if
         end do
      end do

 100  format("Error at position (",I3,",",I3,") - expected disp ",I,
     $       " but got ",I)

      
      end

      subroutine dumpimg(img,ROWS,COLS)

      integer rows, cols
      integer img(rows,cols)

      integer i,j
      
      print *,"#row col val"

      do i=1, ROWS
         do j=1,COLS
            write (*,200) i,j,img(i,j)
         enddo
      enddo

 200  format(I3," ",I3," ", I)

      end