/* To compile for the Fx parallel implementation define the following,
   or do it in the Makefile: */
/* #define __Fx__ 1 */
/* #define NUMPROCS 6  -- or however many you have, of course */

/* These parameters define the size of the image being processed: */
#define pNUMIMGS 4
#define pIMGROWS 486
#define pIMGCOLS 528
#define pMIDROW 243.0
#define pMIDCOL 264.0

/* These parameters define the size of the workspace */
#define pWORKROWS 100
#define pWORKCOLS 100
#define pWORKDEPTHS 100

      program stereo
c   
c This is a simple implementation of Webb-Zitnick projective stereo.
C  The code here is the result of the effort of many people including
C     Jon A. Webb, C. Lawrence Zitnick, Peter Dinda, LeeAnn Tzeng, and
C     others; and of course Kanade-Okutomi multi-baseline stereo was
C     developed by Takeo Kanade, Okutomi, and Tomoharu Nakahara.
C
C We feel that the innovations represented
C here are significant enough to deserve protection and we are in fact
C seeking this protection.  Please do not distribute the ideas
C represented here; you should not even have access to this code
C unless you have registered yourself with the authors through
C a cooperative work agreement. The distribution of this code
C does not represent a disclosure of the algorithm; everyone with
C access to the code should keep the ideas in it confidential.
C
C                                Jon A. Webb
C                                Carnegie Mellon University
c                                webb+@cmu.edu
C                                1 (412) 268-3579

C The several repetitions of the PARAMETER declarations below are an
C attempt to combine the convenience of a macro preprocessor with some
C of the stylistic conventions of Fortran.  Perhaps I have attained the
C worst of both worlds.

C The parameters below describe the input images -- number of them and
C their size.
      integer NUMIMGS, IMGROWS, IMGCOLS
      PARAMETER(NUMIMGS=pNUMIMGS, IMGROWS=pIMGROWS, IMGCOLS=pIMGCOLS)

C     Middle row and column of image
      real MIDROW, MIDCOL
      PARAMETER(MIDROW=pMIDROW, MIDCOL=pMIDCOL)

C These parameters describe the array dimensions (number of elements)
C of the workspace. There is no necessary relationship between these
C and the image size.
      integer WORKROWS, WORKCOLS, WORKDEPTHS
      PARAMETER(WORKROWS=pWORKROWS, WORKCOLS=pWORKCOLS, 
     $          WORKDEPTHS=pWORKDEPTHS)

C     The numbers of channels used for I/O
      integer imagein, imageout
      PARAMETER(imagein=10, imageout=11)
      
/* #define VERBOSE 1 */

      real img(NUMIMGS, IMGCOLS, IMGROWS),
     $     tempimg(WORKCOLS, WORKROWS)
      real workspace(WORKDEPTHS, WORKCOLS, WORKROWS)
      real sumsq(WORKCOLS, WORKROWS)
      real sum(WORKCOLS, WORKROWS)

#if defined(__Fx__)
      align workspace(i,j,k) with t(i)
      template t(WORKDEPTHS)
      distribute t(block(NUMPROCS))
#endif

C Note that the following variables are all part of a COMMON block
C and must be redeclared in exactly the same way wherever they are
C referenced.

c     Projective transforms for every camera
      real proj_xform(NUMIMGS,3,4)
c     Actual number of bands being processed
c     (Must be less than NUMIMGS)
      integer NIMGS

      common proj_xform, NIMGS

#if defined(iWarp)
      external timer_start
      external timer_stop
      integer timer_stop
#endif
      real time, total_time, dtime, tarray(2)
      real cont
      integer i, j, k, d
      real minx, miny, minz, maxx, maxy, maxz
      real dummy
      real depth
      character*100 filename
      integer blank
      logical created
      integer index

#if defined(VERBOSE)
      integer viewset
#endif


      call getarg(1, filename)
      blank = index(filename, ' ')
      print *, "Opening ", filename(1:blank-1), " for input"
      open(unit=imagein, file=filename(1:blank-1), form="unformatted",
     $     status="old")
      call getarg(2, filename)
      blank = index(filename, ' ')
      print *, "Opening ", filename(1:blank-1), " for output"
      inquire(file=filename(1:blank-1), exist=created)
      if (created) then
	  open(unit=imageout, file=filename(1:blank-1), 
     $         form="unformatted",
     $         status="old")
      else
  	  open(unit=imageout, file=filename(1:blank-1), 
     $         form="unformatted",
     $         status="new")
      endif


c     $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
c     READ IN THE RECTIFICATION PARAMETERS 
c     $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
c     Read in the number of bands
      read(imagein) dummy
      NIMGS = dummy
c     Read in the depth ranges
      read(imagein) minx
      read(imagein) miny
      read(imagein) minz
      read(imagein) maxx
      read(imagein) maxy
      read(imagein) maxz

#if defined(VERBOSE)
      print *,"Bounding box coordinate = (",minx, miny, minz,") - (",
     $        maxx, maxy, maxz,")"
#endif
C Initialize workspace array
#if defined(__Fx__)
      pdo d = 1, WORKDEPTHS
        pout workspace(d,:,:)
#else
      do d = 1, WORKDEPTHS
#endif
        do i = 1, WORKROWS
          do j = 1, WORKCOLS
            workspace(d,j,i) = 0
          end do
        end do
#if defined(__Fx__)
      end pdo
#else
      end do
#endif
c     $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
c     MAIN LOOP: READ IN IMAGES AND PROJECT
c     $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
#if defined(VERBOSE)
      viewset = 1
#endif
      total_time = 0
 101  read(imagein) cont
      if (cont.eq.1.0) then
c       Read in the projective transformations
         do i = 1,NIMGS
            do j = 1,3
               read(imagein) (proj_xform(i,j,k), k=1,4)
            end do
         end do
#if defined(VERBOSE)
         do i = 1,NIMGS
            print *,"Projective transformation", i
	    do j = 1,3
              print *, proj_xform(i,j,1), proj_xform(i,j,2), 
     $                 proj_xform(i,j,3), proj_xform(i,j,4)
	    end do
         enddo
#endif
#if defined(VERBOSE)
         print *, "Reading view set",viewset
#endif
         read(imagein) (((img(i,k,j), k=1,IMGCOLS),
     $   j=1, IMGROWS), i=1,NIMGS)
#if defined(VERBOSE)
         print *, "Done reading viewset",viewset
         viewset = viewset + 1
#endif
C
#if defined(iWarp)
         call timer_start()
#else
         time = dtime(tarray)
#endif
#if defined(__Fx__)
         pdo d=0,WORKDEPTHS-1
            pout workspace(d,:,:)
#else
         do d=1,WORKDEPTHS
#endif
            depth = minz + (d-1) * (maxz-minz)/WORKDEPTHS
#if defined(VERBOSE)
	    print *, "Processing depth ", depth
#endif
  	    do j = 1, WORKROWS
	      do k = 1, WORKCOLS
                sum(k,j) = 0 
               sumsq(k,j) = 0
	      end do
	    end do
            do i=1,NIMGS
               call warp(img, tempimg, i,
     $              minx, miny, maxx, maxy,
     $              depth)
     	       do j = 1, WORKROWS
	         do k = 1, WORKCOLS
                   sum(k,j) = sum(k,j) + tempimg(k,j)
                   sumsq(k,j) = sumsq(k,j) + tempimg(k,j)**2
	         end do
               end do
            end do
            call variance(sum, sumsq)
            do i = 1, WORKROWS
               do j = 1, WORKCOLS
	         workspace(d,j,i) = workspace(d,j,i) +
     $                         1 / sumsq(j,i)
               end do
            end do
#if defined(__Fx__)
         end pdo
#else
         end do
#endif
#if defined(iWarp)
         total_time = total_time + timer_stop() * 8 / 20000000.0
#else
         total_time = total_time + dtime(tarray)
#endif
         goto 101
      else
         if (cont .eq. 2.0) then
#if defined(VERBOSE)
           print *, "Writing out workspace image"
#endif
           print *, "Processors = ", NUMPROCS,"; Elapsed time =",
     $              total_time
	   do d = 1, WORKDEPTHS
	     do j = 1, WORKROWS
	       do k = 1, WORKCOLS
	         sumsq(k,j) = workspace(d,k,j)
	       end do
	     end do
             write(imageout) ((sumsq(k,j), k = 1, WORKCOLS),
     $                        j = 1, WORKROWS)
	   end do
#if defined(VERBOSE)
           print *, "Done writing out workspace image"
#endif
#if defined(__Fx__)
 	   pdo d = 1, WORKDEPTHS
             pout workspace(d,:,:)
#else
 	   do d = 1, WORKDEPTHS
#endif
             do i = 1, WORKROWS
               do j = 1, WORKCOLS
                 workspace(d,j,i) = 0
               end do
             end do
#if defined(__Fx__)
           end pdo
#else
           end do
#endif
           total_time = 0
#if defined(VERBOSE)
	   viewset = 1
#endif
           goto 101
         end if
      end if
      close(imagein)
      close(imageout)
      stop
      end

      subroutine variance(sum,sumsq)
      integer NUMIMGS, IMGROWS, IMGCOLS
      integer WORKROWS, WORKCOLS
      PARAMETER(NUMIMGS=pNUMIMGS, IMGROWS=pIMGROWS, IMGCOLS=pIMGCOLS,
     $          WORKROWS=pWORKROWS, WORKCOLS=pWORKCOLS)

      real sum(WORKCOLS, WORKROWS)
      real sumsq(WORKCOLS, WORKROWS)

c     Projective transforms for every camera
      real proj_xform(NUMIMGS,3,4)
c     Actual number of bands being processed
c     (Must be less than NUMIMGS)
      integer NIMGS

      common proj_xform, NIMGS

      integer i,j

      do i = 1, WORKROWS
	do j = 1, WORKCOLS
          sumsq(j,i) = (NIMGS * sumsq(j,i) - sum(j,i)**2)
     $                  / (NIMGS * (NIMGS - 1))
        end do
      end do
      end

C     Perform a projective warp from inimg to outimg
C     Inimg is the imgnumth image, which determines which
C     projective transformation we use.
C     depth is the depth that outimg is placed in the
C     common coordinate system.

      subroutine warp(inimg, outimg, imgnum, minx, miny,
     $                maxx, maxy, z)
      integer NUMIMGS, IMGROWS, IMGCOLS
      integer WORKROWS, WORKCOLS
      real MIDROW, MIDCOL
      PARAMETER(NUMIMGS=pNUMIMGS, IMGROWS=pIMGROWS, IMGCOLS=pIMGCOLS,
     $          WORKROWS=pWORKROWS, WORKCOLS=pWORKCOLS)
      PARAMETER(MIDROW=pMIDROW, MIDCOL=pMIDCOL)

      real inimg(NUMIMGS, IMGCOLS, IMGROWS),
     $     outimg(WORKCOLS, WORKROWS)
      integer imgnum
      real minx, miny, maxx, maxy, z

      real a, b, c, d, mid1val, mid2val
      real pu, pv, du, dv
      real u, v, w
      integer iu, iv
      integer i, j
      real trans(3,4)
      integer irand, randint

c     Projective transforms for every camera
      real proj_xform(NUMIMGS,3,4)
c     Actual number of bands being processed
c     (Must be less than NUMIMGS)
      integer NIMGS

      common proj_xform, NIMGS


C     First determine the projective transform from the output image
C     to the input image.
      do i = 1,3
        do j = 1,4
          trans(i,j) = proj_xform(imgnum,i,j)
        end do
      end do
C      Now do the transformation

C     Uninitialized data in the array gets set to a random value.
C     This ensures that any voxel which is not seen by any camera
C     has a high variance.
      randint = IRAND(0)
      randint = randint - (randint/256)*256

      do i = 1, WORKROWS
         u =   trans(1,1) * (minx)
     $       + trans(1,2) * (miny + (i-1) * (maxy-miny)/WORKROWS)
     $       + trans(1,3) * (z)
     $       + trans(1,4)
         v =   trans(2,1) * (minx)
     $       + trans(2,2) * (miny + (i-1) * (maxy-miny)/WORKROWS)
     $       + trans(2,3) * (z)
     $       + trans(2,4)
         w =   trans(3,1) * (minx)
     $       + trans(3,2) * (miny + (i-1) * (maxy-miny)/WORKROWS)
     $       + trans(3,3) * (z)
     $       + trans(3,4)
         do j = 1, WORKCOLS
            pu = u / w + MIDCOL + 1
            pv = v / w + MIDROW + 1
            iu = pu
            iv = pv
            if (iu.ge.1.and.iu.lt. IMGCOLS .and.
     $          iv.ge.1.and.iv.lt. IMGROWS ) then
               du = pu - iu
               dv = pv - iv
               a = inimg(imgnum, iu, iv)
               b = inimg(imgnum, iu, iv + 1)
               c = inimg(imgnum, iu+1, iv)
               d = inimg(imgnum, iu+1, iv+1)
               mid1val = ((a) + (b-a)*dv)
               mid2val = ((c) + (d-c)*dv)
               outimg(j,i) = (mid1val + (mid2val-mid1val)*du)
	    else
               outimg(j,i) = randint
            end if
            u = u + trans(1,1) * (maxx-minx)/WORKCOLS
            v = v + trans(2,1) * (maxx-minx)/WORKCOLS
            w = w + trans(3,1) * (maxx-minx)/WORKCOLS
         end do
      end do
      end

