c****************************************************************

      Program kill_poly

c****************************************************************
c**     
c**   FILE NAME: kill_poly.f
c**     
c**   DATE WRITTEN: 9/29/01
c**     
c**   PROGRAMMER: Scott Hensley
c**     
c**   FUNCTIONAL DESCRIPTION: This routine will set all the points
c**   is a user specified region to a Null Value.
c**     
c**   ROUTINES CALLED:
c**     
c**   NOTES: 
c**     
c**   UPDATE LOG:
c**
c**   Date Changed        Reason Changed                  CR # and Version #
c**   ------------       ----------------                 -----------------
c**     
c*****************************************************************

      implicit none

c     PARAMETER STATEMENTS:

      integer MAXSAMPS,MAXLINES
      parameter(MAXSAMPS=6000,MAXLINES=6000)

      byte PLUS, MINUS, CHARGE, CUT, VISIT, LAWN, TREE, TWIG, NEUTRON, LCORR 
      parameter(PLUS=1,MINUS=2,CHARGE=3,CUT=4,VISIT=8,LAWN=16,TREE=16,TWIG=32)
      parameter(NEUTRON=64,LCORR=-128) 

c     INPUT VARIABLES:

C     OUTPUT VARIABLES:

c     LOCAL VARIABLES:

      character*120 a_infile,a_string,a_polyfile
      character*8 a_junk
      integer i,j,i_samples,i_ss,i_es,i_sl,i_el,i_sseed,i_lseed,iargc,i_killed_points
      integer i_sll,i_ell,i_numpts,k,i_cp(2),i_cpp(2),i_dis,m,i_found,i_dir,i_ip(2),i_ipp(2),i_mp(2)
      integer i_read,i_cnt
      real*4 r_null,r_data(MAXSAMPS,MAXLINES),r_data_sl(MAXSAMPS),r_distance,r_f,r_omf
      real*4 r_vertex(2,1000)
      byte b_trees(MAXSAMPS,MAXLINES)    !Data Null Mask

      real*8 r_segdir(2,1000),r_seg0(2,1000),r_int,r_ip(2),r_perp(2),r_difvec(2),r_dis,r_int2,r_mp(2)

c     COMMON BLOCKS:

c     DATA STATEMENTS:

      real r_cassini_null,r_junk
      byte b_cass_null(4),b_junk(4)

c     EQUIVALENCE STATEMENTS:

      equivalence(b_cass_null,r_cassini_null)
      equivalence(r_junk,b_junk)

C     FUNCTION STATEMENTS:

c     SAVE STATEMENTS:

      save r_data,b_trees

c     PROCESSING STEPS:

      write(6,*) ' '
      write(6,*) '   << KILL POLYGON >>'
      write(6,*) ' '

      if(iargc() .lt. 4)then
         write(6,'(a)') 'Usage: kill_poly infile samples null polyfile [s_seed l_seed]'
         write(6,*) ' '
         stop
      endif

      a_junk = 'fbff7fff'
      read(a_junk,'(4z2.2)') b_cass_null

      call getarg(1,a_infile) 

      call getarg(2,a_string)
      read(a_string,*) i_samples
      call getarg(3,a_string)
      read(a_string,*) r_null
      call getarg(4,a_polyfile)
      if(iargc() .gt. 4)then
         call getarg(5,a_string)
         read(a_string,*) i_sseed
         call getarg(6,a_string)
         read(a_string,*) i_lseed
      endif

c     Read in the polygon file

      open(12,file=a_polyfile,status='old')
      open(10,file=a_infile,access='direct',form='unformatted',recl=4*i_samples)

      i_ss = 10000000
      i_es = 0
      i_sl = 10000000
      i_el = 0

      do i=1,1000
         read(12,*,err=988) r_vertex(1,i),r_vertex(2,i)
         i_ss = min(i_ss,nint(r_vertex(1,i)))
         i_es = max(i_es,nint(r_vertex(1,i)))
         i_sl = min(i_sl,nint(r_vertex(2,i)))
         i_el = max(i_el,nint(r_vertex(2,i)))
      enddo
 988  i_numpts = i - 1

      i_ss = max(i_ss - 1,1)
      i_es = min(i_es + 1,i_samples)
      i_sl = max(i_sl - 1,1)

      if(r_null .eq. -2004.)then
         r_null = r_cassini_null
      endif

c     Set the boundary of the polygon to be killed to the Null values

      i_cpp(1) = 0
      i_cpp(2) = 0
      i_read = 0

      write(6,*) ' '
      write(6,'(a)') 'Reading vertex data...'

      do i=1,i_numpts

         if(i .ne. i_numpts)then
            k = i+1
         else
            k = 1
         endif

         r_distance = 1.d0
         r_segdir(1,i) = (r_vertex(1,k)-r_vertex(1,i))/r_distance
         r_segdir(2,i) = (r_vertex(2,k)-r_vertex(2,i))/r_distance
         r_seg0(1,i) = r_vertex(1,i)
         r_seg0(2,i) = r_vertex(2,i)

         r_distance = sqrt((r_vertex(1,i)-r_vertex(1,k))**2 + (r_vertex(2,k)-r_vertex(2,i))**2)
         i_dis = nint(r_distance*100.)

         do m=0,i_dis

            r_f = float(m)/float(i_dis)
            r_omf = 1.d0 - r_f

            i_cp(1) = nint(r_omf*r_vertex(1,i) + r_f*r_vertex(1,k))
            i_cp(2) = nint(r_omf*r_vertex(2,i) + r_f*r_vertex(2,k))

            if(i .eq. 1 .and. m .eq. i_dis/2)then
               i_mp(1) = nint(r_omf*r_vertex(1,i) + r_f*r_vertex(1,k))
               i_mp(2) = nint(r_omf*r_vertex(2,i) + r_f*r_vertex(2,k))
            endif

            i_cp(1) = min(max(1,i_cp(1)),i_samples)

            if(i_cp(1) .ne. i_cpp(1) .or. i_cp(2) .ne. i_cpp(2))then !new point

               if(i_cp(2) .ne. i_cpp(2))then    !new record
                  if(i_read .ne. 0)then
                     write(10,rec=i_cpp(2)) (r_data_sl(j),j=1,i_samples)
                     i_read = 0
                  endif
                  read(10,rec=i_cp(2),err=950) (r_data_sl(j),j=1,i_samples)
                  i_read = 1
               endif

               r_data_sl(i_cp(1)) = r_null

               i_cpp(2) = i_cp(2)

            endif

         enddo   !loop over line

         write(10,rec=i_cp(2)) (r_data_sl(j),j=1,i_samples)

      enddo

      write(6,'(a,x,i4)') 'Vertices on Boundary: ',i_numpts

      r_difvec(1) = r_vertex(1,2) - r_vertex(1,1)
      r_difvec(2) = r_vertex(2,2) - r_vertex(2,1)

      r_perp(1) = r_difvec(2)
      r_perp(2) = -r_difvec(1)

      r_perp(1) = r_perp(1)/sqrt(r_difvec(1)**2 + r_difvec(2)**2)
      r_perp(2) = r_perp(2)/sqrt(r_difvec(1)**2 + r_difvec(2)**2)

      i_found = 0
      i_dir = 1

      i_ipp(1) = -1.
      i_ipp(2) = -1.

      write(6,*) ' '
      write(6,'(a,i10,x,i10)') 'Midpoint: ',i_mp(1),i_mp(2)

      do i=-300,300,50

c         type*, 'i = ',i

         r_dis = (float(i)/20.)*i_dir
         i_ip(1) = nint(r_dis*r_perp(1) + i_mp(1))
         i_ip(2) = nint(r_dis*r_perp(2) + i_mp(2))
         r_ip(1) = i_ip(1)
         r_ip(2) = i_ip(2)
c         write(66,*) r_ip(1),r_ip(2)

         if((i_ip(1) .ne. i_mp(1) .or. i_ip(2) .ne. i_mp(2)) .and. i_found .eq. 0)then

c            type*, ' '
c            type*, 'i_ip = ',i_ip
            
            if(i_ip(1) .ne. i_ipp(1) .or. i_ip(2) .ne. i_ipp(2))then !new point to try
               
               i_cnt = 0
               do k=1,i_numpts
c                  type*, ' '
c                  type*, 'k = ',k
                  call find_intersection(r_segdir(1,k),r_seg0(1,k),r_perp,r_ip,r_int,r_int2)
c                  type*, 'r_int = ',r_int,r_int2
c                  type*, 'inter ppoint ',r_segdir(1,k)*r_int + r_seg0(1,k),r_segdir(2,k)*r_int + r_seg0(2,k)
                  if(r_int .gt. 0.0 .and. r_int .le. 1.d0 .and. r_int2 .gt. 0)then      !Intersect in interior of line segment
c                     type*, 'k = ',k
c                     type*, 'r_int = ',r_int,r_int2
c                     type*, 'seg = ',r_segdir(1,k),r_segdir(2,k),r_seg0(1,k),r_seg0(2,k)
c                     type*, 'perp = ',r_perp(1),r_perp(2),r_ip(1),r_ip(2)
c                     type*, 'inter ppoint ',r_segdir(1,k)*r_int + r_seg0(1,k),r_segdir(2,k)*r_int + r_seg0(2,k)
                     i_cnt = i_cnt + 1
                  endif
               enddo

c               type*, 'i_cnt = ',i_cnt
               
               if(mod(i_cnt,2) .eq. 1)then     !Amen - Point is in the interior
                  i_found = 1
                  i_sseed = i_ip(1)
                  i_lseed = i_ip(2)
               endif

            endif

         endif

         i_ipp(1) = i_ip(1)
         i_ipp(2) = i_ip(2)

      enddo

c      close(66)

      if(i_found .eq. 0)then
         write(6,*) ' '
         write(6,'(a)') 'No interior point found - terminating!'
         stop
      else
         write(6,*) ' '
         write(6,*) 'Interior point at location: ',i_sseed,i_lseed
      endif

      if(1 .eq. 2)then
 950     write(6,'(a)') ' '
         write(6,'(a)') 'Point connecting vertex points not in file - terminating'
         stop
      endif

c     read in data around connected component to be killed

      write(6,*) ' '
      write(6,'(a)') 'Reading data...'

      do i=i_sl,i_el
         read(10,rec=i,err=999) (r_data(j,i-i_sl+1),j=1,i_samples)
      enddo

 999  i_el = i - 1

c     Data Null Mask - Set based on prescribed null value

      write(6,*) ' '
      write(6,'(a)') 'Generating data null mask...'

      i_sll = 1
      i_ell = i_el - i_sl + 1

      i_lseed = i_lseed - i_sl + 1

      do i=i_sll,i_ell
         do j=i_ss,i_es
            if(r_data(j,i) .le. r_null)then
               b_trees(j,i) = b_trees(j,i) .or. LCORR
            endif
         enddo
      enddo

      write(6,*) ' '
      write(6,'(a)') 'Killing data...'

      call grass(r_data,b_trees,i_sseed-1,i_lseed-1,i_ss-1,i_es-1,i_sll-1,i_ell-1,r_null,i_killed_points)

      write(6,*) ' '
      write(6,'(a,x,i10)') 'Points killed in connected component: ',i_killed_points

c     write data back to file with killed component

      write(6,*) ' '
      write(6,'(a)') 'Writing data...'

      do i=i_sl,i_el
         write(10,rec=i) (r_data(j,i-i_sl+1),j=1,i_samples)
      enddo

      write(6,*) ' '
      write(6,'(a)') 'Hang em high and bury them deep!'

      end  

c****************************************************************

      subroutine grass(r_data,trees,iseed,jseed,nr_start,nr_end,
     +     naz_start,naz_end,r_null,i_unw_ctr)

c****************************************************************
c**     
c**   FILE NAME: grass.f
c**     
c**   DATE WRITTEN: 6/30/97
c**     
c**   PROGRAMMER: Charles Werner, Paul Rosen, Scott Hensley
c**     
c**   FUNCTIONAL DESCRIPTION: This routine grows the grass after the 
c**   residues and trees have been generated. This means actually
c**   unwrapping the phase.
c**     
c**   ROUTINES CALLED:
c**     
c**   UPDATE LOG:
c**
c**   Date Changed        Reason Changed          CR # and Version #
c**   ------------       ----------------         ------------------
c**    28-Oct-97   incorrect count of points unwrapped	   
c**    11-Nov-97   removed connected components array      
c**    19-Jan-98   updated program format                  
c**
c*****************************************************************

      implicit none

c     PARAMETER STATEMENTS:

      integer MAXSAMPS,MAXLINES
      parameter(MAXSAMPS=6000,MAXLINES=6000)

      byte PLUS, MINUS, CHARGE, CUT, VISIT, LAWN, TREE, TWIG, NEUTRON, LCORR 
      parameter(PLUS=1,MINUS=2,CHARGE=3,CUT=4,VISIT=8,LAWN=16,TREE=16,TWIG=32)
      parameter(NEUTRON=64,LCORR=-128) 

      integer*4 MAX_GRASS
      parameter(MAX_GRASS=65535)		!max length of ping-pong lists for growing perimeter

c     INCLUDE FILES:

c     INPUT VARIABLES:

      real*4 r_data(0:MAXSAMPS-1, 0:MAXLINES-1) !Input data
      byte trees(0:MAXSAMPS-1, 0:MAXLINES-1)    !Data Null Mask
      integer*4 iseed, jseed			!starting seed point for phase unwrapping
      integer*4 nr_start, nr_end		!starting and ending range sample in the interferogram array
      integer*4 naz_start, naz_end		!starting and ending azimuth line
      real*4 r_null

c     OUTPUT VARIABLES:

      integer*4 i_unw_ctr			!number of points unwrapped

c     LOCAL VARIABLES:
	
      integer*4 ii(0:MAX_GRASS-1,0:1),jj(0:MAX_GRASS-1,0:1)	!ping-pong lists of the perimeter of the growing region
      integer*4 nn(0:1)				!array that contains lengths of ping-pong lists
      integer*4 isearch(0:3),jsearch(0:3)
      integer*4 i,j,k,l,m
      integer*4 i1,j1
      integer*4 nunw				!counter of the number of points unwrapped 

c     EQUIVALENCE STATEMENTS:

c     SAVE STATEMENTS:

      save 

c     PROCESSING STEPS:
 
      isearch(0) = 1				!offsets to adjacent samples for growing the grass
      jsearch(0) = 0
      isearch(1) = 0
      jsearch(1) = 1
      isearch(2) = -1
      jsearch(2) = 0
      isearch(3) = 0
      jsearch(3) = -1

      ii(0,0) = iseed				!initial element of list 0
      jj(0,0) = jseed
      nn(0) = 1					!initial length of list 0
      nn(1) = 0					!initial length of list 1
      m=0					!initialize ping-pong list pointer

      r_data(iseed,jseed) = r_null
      trees(iseed,jseed) = trees(iseed,jseed) .or. LAWN

      nunw = 1					!initialize counter of unwrapped points					
     
      do while(nn(m) .ne. 0)			!continue until list empty

        nn(1-m) = 0				!initialize length of the new list

        do k=0, nn(m)-1				!grow all elements of the current list

          i = ii(k,m)
          j = jj(k,m)
 
          do l=0,3				!search in all 4 directions

            i1 = i + isearch(l)			!look in the search direction
            j1 = j + jsearch(l)

            if((i1 .lt. nr_start) .or. (i1 .gt. nr_end)) goto 20	!test if candidate pixel outside of bounds
            if((j1 .lt. naz_start) .or. (j1 .gt. naz_end)) goto 20 

            if((trees(i1,j1).and.LAWN) .eq. LAWN) goto 20		!check if already unwrapped
            if((trees(i1,j1).and.LCORR) .eq. LCORR) goto 20		!check if below CORR threshold

            r_data(i1,j1) = r_null
            nunw = nunw + 1						!increment counter of unwrapped pixels
            trees(i1,j1) = trees(i1,j1) .or. LAWN			!mark pixel on the lawn

            if(nn(1-m) .lt. (MAX_GRASS-1)) then				!check length of new list
              ii(nn(1-m), 1-m) = i1					!add current element to the new list
              jj(nn(1-m), 1-m) = j1
              nn(1-m) = nn(1-m) + 1 					!increment new list pointer  
            else
              write(6,*) 'WARNING GRASS: Length of ping-pong lists exceeds list size allocation'
            endif

 20          continue 

           end do	!loop on search directions 

 40        continue

         end do 	!loop on current list elements
         m = 1-m	!switch to other list (ping-pong)

      end do		!grow while current list not empty

      if(nunw .eq. 1)then
         trees(iseed,jseed) = trees(iseed,jseed) .and. (.not.LAWN) 
         r_data(iseed,jseed) = r_null !reset phase of unwrapped points
         nunw = 0   
      else  
         i_unw_ctr = nunw       !return number of points unwrapped
      endif 

      return
      end         


c****************************************************************

      subroutine find_intersection(r_yhat,r_y0,r_shat,r_s0,r_int,
     +     r_int2)

c****************************************************************
c**     
c**   FILE NAME: kill_poly.f
c**     
c**   DATE WRITTEN: 9/29/01
c**     
c**   PROGRAMMER: Scott Hensley
c**     
c**   FUNCTIONAL DESCRIPTION: Computes the distance along a 
c**   ray corresponding to the intersection of the two
c**   input rays.
c**     
c**   ROUTINES CALLED:
c**     
c**   NOTES: 
c**     
c**   UPDATE LOG:
c**
c**   Date Changed        Reason Changed                  CR # and Version #
c**   ------------       ----------------                 -----------------
c**     
c*****************************************************************

      implicit none

c     PARAMETER STATEMENTS:

c     INPUT VARIABLES:

      real*8 r_yhat(2),r_y0(2),r_shat(2),r_s0(2)

c     OUTPUT VARIABLES:

      real*8 r_int,r_int2

c     LOCAL VARIABLES:

      real*8 r_dp,r_s0my0(2),r_shdyh,r_yhpsh(2),r_cos,r_s2,r_y2
      real*8 r_intpnt(2)

c     COMMON BLOCKS:

c     DATA STATEMENTS:

c     EQUIVALENCE STATEMENTS:

C     FUNCTION STATEMENTS:

c     SAVE STATEMENTS:

c     PROCESSING STEPS:

      r_s0my0(1) = r_s0(1) - r_y0(1)
      r_s0my0(2) = r_s0(2) - r_y0(2)

      r_shdyh = r_shat(1)*r_yhat(1) + r_shat(2)*r_yhat(2)

      r_y2 = r_yhat(1)*r_yhat(1) + r_yhat(2)*r_yhat(2)
      r_s2 = r_shat(1)*r_shat(1) + r_shat(2)*r_shat(2)

      r_cos = r_shdyh/sqrt(r_y2*r_s2)

      if(abs(r_cos) .eq. 1.d0)then       !parallel segements

         r_dp = r_s0my0(1)*r_yhat(2) - r_s0my0(2)*r_yhat(1)
         if(r_dp .ne. 0)then    !never intersect
            r_int = -99999.
         else
            r_int = 99999.      !many points on intersection
         endif

      else                            !not parallel

         r_yhpsh(1) = r_s2*r_yhat(1) - r_shat(1)*r_shdyh
         r_yhpsh(2) = r_s2*r_yhat(2) - r_shat(2)*r_shdyh

         r_dp = r_yhpsh(1)*r_s0my0(1) + r_yhpsh(2)*r_s0my0(2)

         r_int = r_dp/(r_s2*r_y2 - r_shdyh**2)

         r_intpnt(1) = r_int*r_yhat(1) + r_y0(1)
         r_intpnt(2) = r_int*r_yhat(2) + r_y0(2)

         r_int2 = ((r_intpnt(1) - r_s0(1))*r_shat(1) + (r_intpnt(2) - r_s0(2))*r_shat(2))/r_s2

      endif

      end

