!c  topocorrect - approximate topo correction
!c  Reference :
!http://earth-info.nga.mil/GandG/publications/tr8350.2/tr8350.2-a/Appendix.pdf

    subroutine topo(demAccessor, dopAccessor, slrngAccessor)
      use topozeroState
      use topozeroMethods
      use geometryModule
      use orbitModule
      use linalg3Module
      use fortranUtils, ONLY : getPI

      implicit none
      include 'omp_lib.h'
      integer*8 demAccessor, dopAccessor, slrngAccessor
      integer lineFile, stat
      !integer width, length
      real*8, allocatable ::lat(:),lon(:),z(:),zsch(:)
      real*4, allocatable :: losang(:), incang(:), elevang(:)
      real*4, allocatable :: distance(:)
      real*8, allocatable :: rho(:), dopline(:)
      real*4, allocatable :: dem(:,:), demline(:)

      integer*1, allocatable :: mask(:), omask(:)
      real*8, allocatable :: orng(:), ctrack(:)
      real*8, allocatable :: oview(:)
      real*8 ctrackmin, ctrackmax, dctrack
      real*8 sch(3),xyz(3),llh(3),delta(3)

      real*8 tline, rng, dopfact
      real*8 llh_prev(3), xyz_prev(3)
      real*8 xyzsat(3), velsat(3)
      real*8 schsat(3), llhsat(3)
      real*8 ltpsat(3), ltpvel(3)
      real*8 enu(3)
      real*8 n_img(3), n_img_enu(3), n_trg_enu(3)
      real*8 that(3), chat(3), nhat(3), vhat(3)
      real*8 enumat(3,3), xyz2enu(3,3)

!!      real*8 xyz2(3), vxyz2(3)

      integer, allocatable :: converge(:)
      integer totalconv, owidth, ofactor

      real*8 height, rcurv, vmag
      real*8 aa, bb,cc
      real*8 r2d,refhgt,hnadir
      integer pixel
      integer nearrangeflag
      real*8 beta, alpha, gamm
      real*8 costheta,sintheta,cosalpha
      real*8 arg,rminoraxis,rlatg,st,ct
      real*8 fraclat, fraclon
      real*4 z1,z2,demlat,demlon
      real*4 demmax
      real*8 cospsi
      integer line,iter,ind
      integer idemlat,idemlon,i_type,i,j!,i_cnt1,i_cnt2,i_loff,i_el,i_sl

      !!!Variables for cropped DEM
      integer udemwidth, udemlength
      integer ustartx, uendx
      integer ustarty, uendy
      double precision umin_lon, umax_lon
      double precision umin_lat, umax_lat
      double precision ufirstlat, ufirstlon
      double precision hgts(2)

      real*8 pi
      integer,parameter :: b1=1
      integer,parameter :: b2=1
      integer binarysearch

      !!Geometry objects
      type(ellipsoidType) :: elp
      type(pegType) :: peg
      type(pegtransType) :: ptm

      procedure(intpTemplate), pointer :: intp_dem => null()
      procedure(interpolateOrbit_f), pointer :: intp_orbit => null()


        !!!Set up DEM interpolation method
        if (method.eq.SINC_METHOD) then
          intp_dem => intp_sinc
        else if (method.eq.BILINEAR_METHOD) then
          intp_dem => intp_bilinear
        else if (method.eq.BICUBIC_METHOD) then
          intp_dem => intp_bicubic
        else if (method.eq.NEAREST_METHOD) then
          intp_dem => intp_nearest
        else if (method.eq.AKIMA_METHOD) then
          intp_dem => intp_akima
        else if (method.eq.BIQUINTIC_METHOD) then
            intp_dem => intp_biquintic
        else
          print *, 'Undefined interpolation method.'
          stop
        endif
        call prepareMethods(method)


        !!!Set up orbit interpolation method
        if (orbitmethod .eq. HERMITE_METHOD) then
            intp_orbit => interpolateWGS84Orbit_f

            if(orbit%nVectors .lt. 4) then
                print *, 'Need atleast 4 state vectors for using hermite polynomial interpolation'
                stop
            endif
            print *, 'Orbit interpolation method: hermite'
        else if (orbitmethod .eq. SCH_METHOD) then
            intp_orbit => interpolateSCHOrbit_f

            if(orbit%nVectors .lt. 4) then
                print *, 'Need atleast 4 state vectors for using SCH interpolation'
                stop
            endif
            print *, 'Orbit interpolation method: sch'
        else if (orbitmethod .eq. LEGENDRE_METHOD) then
            intp_orbit => interpolateLegendreOrbit_f

            if(orbit%nVectors .lt. 9) then
                print *, 'Need atleast 9 state vectors for using legendre polynomial interpolation'
                stop
            endif
            print *, 'Orbit interpolation method: legendre'
        else
            print *, 'Undefined orbit interpolation method.'
            stop
        endif


        ofactor = 2
        owidth = ofactor*width + 1
        pi = getPI()
        hgts(1) = MIN_H
        hgts(2) = MAX_H

        lineFile  = 0

        totalconv = 0

        height = 0.0d0
        min_lat = 10000.
        max_lat = -10000.
        min_lon = 10000.
        max_lon = -10000.

!$omp parallel
        if(omp_get_thread_num().eq.1) then
            write(6,*), 'Max threads used: ', omp_get_num_threads()
        end if
!$omp end parallel

        if ((slrngAccessor.eq.0).and.(r0.eq.0.0d0)) then
            print *, 'Both the slant range accessor and starting range are zero'
            stop
        endif

!c  allocate variable arrays
        allocate (lat(width))
        allocate (lon(width))
        allocate (z(width))
        allocate (zsch(width))
        allocate (rho(width))
        allocate (dopline(width))
        allocate (distance(width))
        allocate (losang(2*width))
        allocate (incang(2*width))
        allocate (elevang(width))
        allocate (converge(width)) !!PSA

        if (maskAccessor.gt.0) then
            allocate (omask(owidth))
            allocate (orng(owidth))
            allocate (mask(width))
            allocate (ctrack(owidth))
            allocate (oview(owidth))
        endif

!c  some constants
        refhgt=0
        r2d=180.d0/pi
        elp%r_a = major
        elp%r_e2 = eccentricitySquared


        !!!PSA - Keep track of near range issues
        nearrangeflag = 0

        !!!Determining the bbox of interest
        !!!For detailed explanation of steps - see main loop below
        line=1
        !!!Doppler for geometry (not carrier) is const / range variant only
        call getLine(dopAccessor, dopline, line)
        call getLine(slrngAccessor, rho, line)

        !!!First line
        do line=1,2
            tline = t0 + (line-1) * NAzlooks * (length-1.0d0)/prf
!!            stat = interpolateWGS84Orbit_f(orbit, tline, xyzsat, velsat)
            stat = intp_orbit(orbit, tline, xyzsat, velsat)
            if (stat.ne.0) then
                print *, 'Error getting statevector for bounds computation'
                exit
            endif
            vmag = norm(velsat)
            call unitvec(velsat, vhat)
            i_type = XYZ_2_LLH
            call latlon(elp, xyzsat, llhsat, i_type)
            height = llhsat(3)
            call tcnbasis(xyzsat, velsat, elp, that, chat, nhat)

            peg%r_lat = llhsat(1)
            peg%r_lon = llhsat(2)
            peg%r_hdg = peghdg
            call radar_to_xyz(elp, peg, ptm)
            rcurv = ptm%r_radcur


            do ind=1,2
                pixel = (ind-1)*(width-1) + 1
!                rng=r0 + (pixel-1) * Nrnglooks *rspace
                rng = rho(pixel)
                dopfact = (0.5d0 * wvl * dopline(pixel)/vmag) * rng

                do iter=1,2

                    !!PSA - SWOT specific near range check
                    !!If slant range vector doesn't hit ground, pick nadir point
                    if (rng .le. (llhsat(3)-hgts(iter)+1.0d0)) then
                        llh = llhsat
                        print *, 'Possible near nadir imaging'
                        nearrangeflag = 1
                    else
                        zsch(pixel) = hgts(iter)
                        aa =  height + rcurv
                        bb = rcurv + zsch(pixel)
                        costheta = 0.5*((aa/rng) + (rng/aa) - (bb/aa)*(bb/rng))
                        sintheta = sqrt(1.0d0 - costheta*costheta)
                        gamm = costheta * rng
                        alpha  = (dopfact - gamm * dot(nhat,vhat)) / dot(vhat,that)
                        beta  = -ilrl * sqrt(rng*rng*sintheta*sintheta - alpha*alpha)
                        delta = gamm * nhat + alpha * that + beta * chat
                        xyz = xyzsat + delta
                        i_type=XYZ_2_LLH
                        call latlon(elp,xyz,llh,i_type)
                    endif

                    min_lat = min(min_lat, llh(1)*r2d)
                    max_lat = max(max_lat, llh(1)*r2d)
                    min_lon = min(min_lon, llh(2)*r2d)
                    max_lon = max(max_lon, llh(2)*r2d)
                end do
            end do
        end do

        !!!Account for margins
        min_lon = min_lon - MARGIN
        max_lon = max_lon + MARGIN
        min_lat = min_lat - MARGIN
        max_lat = max_lat + MARGIN



        print *,'DEM parameters:'
        print *,'Dimensions: ',idemwidth,idemlength
        print *,'Top Left: ',firstlon,firstlat
        print *,'Spacing: ',deltalon,deltalat
        print *, 'Lon: ', firstlon, firstlon+(idemwidth-1)*deltalon
        print *, 'Lat: ', firstlat+(idemlength-1)*deltalat, firstlat

        print *, ' '
        print *, 'Estimated DEM bounds needed for global height range: '
        print *, 'Lon: ', min_lon, max_lon
        print *, 'Lat: ', min_lat, max_lat


        !!!!Compare with what has been provided as input
        umin_lon = max(min_lon, firstlon)
        if (min_lon .lt. firstlon) then
            print *, 'Warning: west limit may be insufficient for global height range'
        endif

        umax_lon = min(max_lon, firstlon + (idemwidth-1)*deltalon)
        if (max_lon .gt. (firstlon + (idemwidth-1)*deltalon)) then
            print *, 'Warning: east limit may be insufficient for global height range'
        endif

        umax_lat = min(max_lat, firstlat)
        if (max_lat .gt. firstlat) then
            print *, 'Warning: north limit may be insufficient for global height range'
        endif

        umin_lat = max(min_lat, firstlat + (idemlength-1)*deltalat)
        if (min_lat .lt. (firstlat + (idemlength-1)*deltalat)) then
            print *, 'Warning: south limit may be insufficient for global height range'
        endif



        !!!!Usable part of the DEM limits
        ustartx = int((umin_lon - firstlon)/deltalon)+1
        if (ustartx .lt. 1) ustartx = 1

        uendx = int((umax_lon-firstlon)/deltalon + 0.5d0)+1
        if (uendx.gt.idemwidth) uendx = idemwidth

        ustarty = int((umax_lat-firstlat)/deltalat)+1
        if (ustarty.lt.1) ustarty=1

        uendy = int((umin_lat-firstlat)/deltalat + 0.5) + 1
        if (uendy.gt.idemlength) ustarty=idemlength

        ufirstlon = firstlon + deltalon * (ustartx-1)
        ufirstlat = firstlat + deltalat * (ustarty-1)

        udemwidth = uendx - ustartx + 1
        udemlength = uendy - ustarty + 1

        print *, ' '
        print *, 'Actual DEM bounds used: '
        print *,'Dimensions: ',udemwidth,udemlength
        print *,'Top Left: ',ufirstlon,ufirstlat
        print *,'Spacing: ',deltalon,deltalat
        print *, 'Lon: ', ufirstlon, ufirstlon + deltalon*(udemwidth-1)
        print *, 'Lat: ', ufirstlat + deltalat * (udemlength-1), ufirstlat
        print *, 'Lines: ', ustarty, uendy
        print *, 'Pixels: ', ustartx, uendx

!c  allocate dem array
        allocate (dem(udemwidth,udemlength))
        allocate (demline(idemwidth))

        !!!Read the useful part of the DEM
        do j=1,udemlength
            lineFile = j + ustarty - 1
!            print *, 'Line: ', lineFile
            call getLine(demAccessor,demline,lineFile)
            dem(:,j) = demline(ustartx:uendx)
        enddo

        demmax = maxval(dem)
        print *, 'Max DEM height: ', demmax

        print *, 'Primary iterations: ', numiter
        print *, 'Secondary iterations: ', extraiter
        print *, 'Distance threshold : ', thresh

        !!Initialize range values
!!        do pixel=1,width
!!           rho(pixel) = r0 + rspace*(pixel-1)*Nrnglooks
!!        enddo

        height = 0.0d0
        min_lat = 10000.
        max_lat = -10000.
        min_lon = 10000.
        max_lon = -10000.

      !!!File for debugging
!!      open(31, file='distance',access='direct',recl=4*width,form='unformatted')

      do line=1, length         !c For each line


         !!!!Set up the geometry
         !!Step 1: Get satellite position
         !!Get time
         tline = t0 + Nazlooks*(line - 1.0d0)/prf
         !!Get state vector

!!         stat = interpolateLegendreOrbit_f(orbit, tline, xyz2, vxyz2)
!!         print *, 'Line: ', line
!!         print *, tline, xyz2, vxyz2
!!         stat = interpolateWGS84Orbit_f(orbit, tline, xyzsat, velsat)
         stat = intp_orbit(orbit, tline, xyzsat, velsat)
!!         print *, tline, xyzsat, velsat

         call unitvec(velsat, vhat)
         vmag = norm(velsat)
         !!vhat is unit vector along velocity
         !!vmag is the magnitude of the velocity



         !!Step 2: Get local radius of curvature along heading
         !!Convert satellite position to lat lon
         i_type = XYZ_2_LLH
         call latlon(elp, xyzsat, llhsat, i_type)
         height = llhsat(3)

!!         print *, 'Sat pos: ', line
!!         print *, llhsat(1)*r2d, llhsat(2)*r2d, llhsat(3)

         !!Step 3: Get TCN basis using satellite basis
         call tcnbasis(xyzsat, velsat, elp, that, chat, nhat)
         !!that is along local tangent to the planet
         !!chat is along the cross track direction
         !!nhat is along the local normal

         !!Step 4: Get Doppler information for the line
         !! For native doppler, this corresponds to doppler polynomial
         !! For zero doppler, its a constant zero polynomial
         call getLineSequential(dopAccessor, dopline, i_type)
!!         print *, 'VEL:', velsat
!!         print *, 'TCN:', xyzsat
!!         print *, that
!!         print *, chat
!!         print *, nhat
!!         print *, vhat
         !!Get the slant range
         call getLineSequential(slrngAccessor, rho, i_type)

         !!Step 4: Set up SCH basis right below the satellite
         peg%r_lat = llhsat(1)
         peg%r_lon = llhsat(2)
         peg%r_hdg = peghdg
         hnadir = 0.0d0

!!         print *, 'Heading: ', peghdg
         call radar_to_xyz(elp, peg, ptm)
         rcurv = ptm%r_radcur
         converge = 0
         z = 0.
         zsch = 0.

         if (mod(line,1000).eq.1) then
             print *, 'Processing line: ', line, vmag
             print *, 'Dopplers: ', dopline(1), dopline(width/2), dopline(width)
         endif

        !!Initialize lat,lon to middle of input DEM
        lat(:) = ufirstlat + 0.5d0*deltalat*udemlength
        lon(:) = ufirstlon + 0.05d0*deltalon*udemwidth


        !!PSA - SWOT specific near range check
        !!Computing nadir height
        if (nearrangeflag .ne. 0) then
            demlat=(llhsat(1)*r2d-ufirstlat)/deltalat+1
            demlon=(llhsat(2)*r2d-ufirstlon)/deltalon+1
            if(demlat.lt.1)demlat=1
            if(demlat.gt.udemlength-1)demlat=udemlength-1
            if(demlon.lt.1)demlon=1
            if(demlon.gt.udemwidth-1)demlon=udemwidth-1

            !!!!! This whole part can be put into a function
            idemlat=int(demlat)
            idemlon=int(demlon)
            fraclat=demlat-idemlat
            fraclon=demlon-idemlon
            hnadir = intp_dem(dem,idemlon,idemlat,fraclon,fraclat,udemwidth,udemlength)
        endif

         !!!!Start the iterations
         do iter=1,numiter+extraiter+1

            !$omp parallel do private(pixel,sch,beta,alpha,gamm) &
            !$omp private(i_type,llh,idemlat,idemlon,xyz,arg) &
            !$omp private(z1,z2,fraclat,fraclon,demlat,demlon) &
            !$omp private(llh_prev,xyz_prev,aa,bb,cc, rng) &
            !$omp private(costheta,sintheta,delta,dopfact)&
            !$omp shared(ufirstlat,ufirstlon,deltalat,deltalon)&
            !$omp shared(xyzsat,that,chat,nhat,vhat,peg,ptm)&
            !$omp shared(length,width,Nazlooks,height,r2d,dem) &
            !$omp shared(rcurv,rho,elp,lat,lon,z,zsch,line)&
            !$omp shared(extraiter,ilrl,iter,dopline,vmag,hnadir) &
            !$omp shared(distance,converge,thresh,numiter)&
            !$omp shared(udemwidth,udemlength,totalconv,wvl)
            do pixel=1,width
               rng = rho(pixel)
               dopfact = (0.5d0 * wvl * dopline(pixel)/vmag) * rng

               !!PSA - Check for near range issues
!!                if (nearrangeflag .ne. 0) then
!!                    if (rng .le. (llhsat(2)-hnadir+1.0d0)) then
!!                endif



               !! If pixel hasnt converged
               if(converge(pixel).eq.0) then

                   !!!!Use previous llh in degrees and meters
                   llh_prev(1) = lat(pixel)/r2d
                   llh_prev(2) = lon(pixel)/r2d
                   llh_prev(3) = z(pixel)

!!                  print *, 'ITER: ', iter
!!                  print *, 'PREV: ', lat(pixel), lon(pixel), z(pixel)

                  !!!!Solve for new position at height zsch
                  aa =  height + rcurv
                  bb = rcurv + zsch(pixel)

!!                  print *, aa, bb, rng
                  !!!!Normalize reasonably to avoid overflow
                  costheta = 0.5*((aa/rng) + (rng/aa) - (bb/aa)*(bb/rng))
                  sintheta = sqrt(1.0d0 - costheta*costheta)

!!                  print *, costheta, sintheta
                  !!Components along unit vectors

                  !!Vector from satellite to point on ground can be written as
                  !! vec(dr) = alpha * vec(that) + beta * vec(chat) + gamma *
                  !! vec(nhat)
                  gamm = costheta * rng
                  alpha  = (dopfact - gamm * dot(nhat,vhat)) / dot(vhat,that)
                  beta  = -ilrl * sqrt(rng*rng*sintheta*sintheta - alpha*alpha)
!!                  print *, alpha, beta, gamm

                  !!! xyz position of target
                  delta = gamm * nhat + alpha * that + beta * chat
                  xyz = xyzsat + delta

                  i_type=XYZ_2_LLH
                  call latlon(elp,xyz,llh,i_type)

!!                  print *, 'NOW:', llh(1)*r2d, llh(2)*r2d, llh(3)
                  !c  convert lat, lon, hgt to xyz coordinates
                  lat(pixel)=llh(1)*r2d
                  lon(pixel)=llh(2)*r2d
                  demlat=(lat(pixel)-ufirstlat)/deltalat+1
                  demlon=(lon(pixel)-ufirstlon)/deltalon+1
                  if(demlat.lt.1)demlat=1
                  if(demlat.gt.udemlength-1)demlat=udemlength-1
                  if(demlon.lt.1)demlon=1
                  if(demlon.gt.udemwidth-1)demlon=udemwidth-1

                  !!!!! This whole part can be put into a function
                  idemlat=int(demlat)
                  idemlon=int(demlon)
                  fraclat=demlat-idemlat
                  fraclon=demlon-idemlon
!!!                  z1=dem(idemlon,idemlat)*(1-fraclon)+dem(idemlon+1,idemlat)*fraclon
!!!                  z2=dem(idemlon,idemlat+1)*(1-fraclon)+dem(idemlon+1,idemlat+1)*fraclon
                  !!!Can change this to Akima
!!!                  z(pixel)=z1*(1-fraclat)+z2*fraclat

                  z(pixel) = intp_dem(dem,idemlon,idemlat,fraclon,fraclat,udemwidth,udemlength)
                  !!!!!! This whole part can be put into a function



                  if(z(pixel).lt.-500.0)z(pixel)=-500.0

                  ! given llh, where h = z(pixel,line) in WGS84, get the SCH height
                  llh(1) = lat(pixel)/r2d
                  llh(2) = lon(pixel)/r2d
                  llh(3) = z(pixel)

!!                  print *, 'UPDATED: ', lat(pixel), lon(pixel), z(pixel)
                  i_type = LLH_2_XYZ
                  call latlon(elp,xyz,llh,i_type)

                  i_type = XYZ_2_SCH
                  call convert_sch_to_xyz(ptm,sch,xyz,i_type)
                  ! print *, 'after = ', sch
!!                  print *, 'ZSCH:' , zsch(pixel), sch(3)
                  zsch(pixel) = sch(3)

                  !!!!Absolute distance
                  distance(pixel) = sqrt((xyz(1)-xyzsat(1))**2 +(xyz(2)-xyzsat(2))**2 + (xyz(3)-xyzsat(3))**2) - rng
!!                  print *, 'DIST: ', distance(pixel)
                  if(abs(distance(pixel)).le.thresh) then
                     zsch(pixel) = sch(3)
                     converge(pixel) = 1
                     totalconv = totalconv+1

                 else if(iter.gt.(numiter+1)) then

                    i_type=LLH_2_XYZ
                    call latlon(elp, xyz_prev,llh_prev,i_type)

                     xyz(1) = 0.5d0*(xyz_prev(1)+xyz(1))
                     xyz(2) = 0.5d0*(xyz_prev(2)+xyz(2))
                     xyz(3) = 0.5d0*(xyz_prev(3)+xyz(3))

                     !!!!Repopulate lat,lon,z
                     i_type=XYZ_2_LLH
                     call latlon(elp,xyz,llh,i_type)
                     lat(pixel) = llh(1)*r2d
                     lon(pixel) = llh(2)*r2d
                     z(pixel) = llh(3)

                     i_type=XYZ_2_SCH
                     call convert_sch_to_xyz(ptm,sch,xyz,i_type)
                     zsch(pixel) = sch(3)
                     !!!!Absolute distance
                     distance(pixel) = sqrt((xyz(1)-xyzsat(1))**2 +(xyz(2)-xyzsat(2))**2 + (xyz(3)-xyzsat(3))**2) - rng
                 endif
             endif

            end do
            !$omp end parallel do

         end do


         !!!!Final computation.
         !!!! The output points are exactly at range pixel
         !!!!distance from the satellite


         !$omp parallel do private(pixel,cosalpha) &
         !$omp private(xyz,llh,delta,rng,i_type,sch,aa,bb) &
         !$omp private(n_img,n_img_enu,n_trg_enu,cospsi) &
         !$omp private(alpha,beta,gamm,costheta,sintheta,dopfact) &
         !$omp private(enumat,xyz2enu,enu) &
         !$omp private(demlat,demlon,idemlat,idemlon,fraclat,fraclon)&
         !$omp shared(zsch,line,rcurv,rho,height,losang,width,velsat) &
         !$omp shared(peghdg,r2d,ilrl,lat,lon,z,xyzsat,distance,incang)&
         !$omp shared(elp,ptm,that,chat,vhat,nhat,vmag,dopline,wvl,dem)&
         !$omp shared(udemwidth,udemlength,ufirstlat,ufirstlon)&
         !$omp shared(deltalat,deltalon,elevang)
         do pixel=1,width

            rng = rho(pixel)
            dopfact = (0.5d0 * wvl * dopline(pixel)/vmag) * rng

            !!!!Solve for new position at height zsch
            aa =  height + rcurv
            bb = rcurv + zsch(pixel)

            costheta = 0.5*((aa/rng) + (rng/aa) - (bb/aa)*(bb/rng))
            sintheta = sqrt(1.0d0 - costheta*costheta)

            gamm = costheta * rng
            alpha  = (dopfact -gamm * dot(nhat,vhat)) / dot(vhat,that)
            beta  = -ilrl * sqrt(rng*rng*sintheta*sintheta - alpha*alpha)

            !!! xyz position of target
            delta = gamm * nhat + alpha * that + beta * chat
            xyz = xyzsat + delta

            i_type=XYZ_2_LLH
            call latlon(elp,xyz,llh,i_type)

            !!!!Copy into output arrays
            lat(pixel) = llh(1)*r2d
            lon(pixel) = llh(2)*r2d
            z(pixel)   = llh(3)

!!            distance(pixel) = ((xyz(1)-xyzsat(1))* velsat(1)+(xyz(2)-xyzsat(2))*velsat(2) + (xyz(3)-xyzsat(3))*velsat(3)) - dopfact * vmag
            distance(pixel) = sqrt((xyz(1)-xyzsat(1))**2 + (xyz(2)-xyzsat(2))**2 + (xyz(3)-xyzsat(3))**2) - rng

            !!!Computations in ENU coordinates around target
            call enubasis(llh(1), llh(2), enumat)
            xyz2enu = transpose(enumat)
            enu = matmul(xyz2enu,delta)

            cosalpha = abs(enu(3)) / norm(enu)

            !!!!LOS vectors
            losang(2*pixel-1) = acos(cosalpha)*r2d
            losang(2*pixel) = (atan2(-enu(2), -enu(1))-0.5*pi)*r2d
            elevang(pixel) = acos(costheta)*r2d

            !!!ctrack gets stored in zsch
            zsch(pixel) = rng * sintheta

            !!!!Get local incidence angle

            demlat=(lat(pixel)-ufirstlat)/deltalat+1
            demlon=(lon(pixel)-ufirstlon)/deltalon+1
            if(demlat.lt.2)demlat=2
            if(demlat.gt.udemlength-1)demlat=udemlength-1
            if(demlon.lt.2)demlon=2
            if(demlon.gt.udemwidth-1)demlon=udemwidth-1

            !!!!! This whole part can be put into a function
            idemlat=int(demlat)
            idemlon=int(demlon)
            fraclat=demlat-idemlat
            fraclon=demlon-idemlon

            !!!Slopex
            aa = intp_dem(dem,idemlon-1,idemlat,fraclon,fraclat,udemwidth,udemlength)
            bb  = intp_dem(dem,idemlon+1,idemlat,fraclon,fraclat,udemwidth,udemlength)
            gamm = lat(pixel)/r2d
            alpha = (bb-aa)* r2d / (2.0d0 * reast(elp, gamm) * deltalon)

            !!!Slopey
            aa = intp_dem(dem,idemlon,idemlat-1,fraclon,fraclat,udemwidth,udemlength)
            bb  = intp_dem(dem,idemlon,idemlat+1,fraclon,fraclat,udemwidth,udemlength)
            beta = (bb-aa)*r2d/(2.0d0 * rnorth(elp,gamm)*deltalat)

            enu = enu / norm(enu)
            costheta = (enu(1)*alpha + enu(2)*beta-enu(3))/sqrt(1.0d0+alpha*alpha+beta*beta)
            incang(2*pixel) = acos(costheta)*r2d

            !!!! Calculate psi angle between image plane and local slope
            call cross(delta, velsat, n_img)
            call unitvec(n_img, n_img)
            n_img_enu = matmul(xyz2enu, -ilrl*n_img)
            n_trg_enu = [-alpha, -beta, 1.0d0]
            cospsi = dot(n_trg_enu, n_img_enu) / (norm(n_trg_enu)*norm(n_img_enu))
            incang(2*pixel-1) = acos( cospsi )*r2d

            !!! Temporary hack needed by dense baseline in the
            !!! derivative computation. Todo: create two new layers
            !incang(2*pixel-1) = alpha                                                                                                                                                            !incang(2*pixel) = beta


         end do
         !$omp end parallel do


         !c Maybe add hmin and hmax?
         min_lat = min(minval(lat), min_lat)
         max_lat = max(maxval(lat), max_lat)
         min_lon = min(minval(lon), min_lon)
         max_lon = max(maxval(lon), max_lon)
!!         write(31,rec=line)(distance(j),j=1,width)
         call setLineSequential(latAccessor, lat)
         call setLineSequential(lonAccessor, lon)
         call setLineSequential(heightAccessor, z)
         if(losAccessor.gt.0) then
             call setLineSequential(losAccessor,losang)
         endif

         if (incAccessor.gt.0) then
             call setLineSequential(incAccessor, incang)
         endif


         if (maskAccessor.gt.0) then
             ctrackmin = minval(zsch) - demmax
             ctrackmax = maxval(zsch) + demmax
             dctrack = (ctrackmax-ctrackmin)/(owidth-1.0d0)

             !!!Sort lat / lon by ctrack
             call InsertionSort(zsch, lat, lon, width)

             !!!Interpolate heights to regular ctrack grid

             !$omp parallel do private(pixel,llh,xyz,rng,aa,bb,i_type)&
             !$omp private(demlat,demlon,idemlat,idemlon,fraclat,fraclon)&
             !$omp shared(ctrackmin,ctrackmax,dctrack,dem,r2d)&
             !$omp shared(orng,owidth,lat,lon,ufirstlat,ufirstlon)&
             !$omp shared(deltalat,deltalon,udemlength,udemwidth)&
             !$omp shared(xyzsat,elp,ctrack,oview,nhat)
             do pixel=1,owidth
                aa = ctrackmin + (pixel-1)*dctrack
                ctrack(pixel) = aa
                i_type = binarysearch(zsch, width, aa)
!!                print *, line, pixel, aa, i_type
                if (i_type.eq.width) i_type = width-1
                if (i_type.eq.0) i_type=1

                !!!!Simple bi-linear interpolation
                fraclat = (aa - zsch(i_type)) / (zsch(i_type+1) - zsch(i_type))
                demlat = lat(i_type) + fraclat*(lat(i_type+1)-lat(i_type))
                demlon = lon(i_type) + fraclat*(lon(i_type+1)-lon(i_type))

                llh(1) = demlat/r2d
                llh(2) = demlon/r2d

                demlat=(demlat-ufirstlat)/deltalat+1
                demlon=(demlon-ufirstlon)/deltalon+1
                if(demlat.lt.2)demlat=2
                if(demlat.gt.udemlength-1)demlat=udemlength-1
                if(demlon.lt.2)demlon=2
                if(demlon.gt.udemwidth-1)demlon=udemwidth-1

                !!!!! This whole part can be put into a function
                idemlat=int(demlat)
                idemlon=int(demlon)
                fraclat=demlat-idemlat
                fraclon=demlon-idemlon
                llh(3) = intp_dem(dem,idemlon,idemlat,fraclon,fraclat,udemwidth,udemlength)
                i_type=LLH_2_XYZ
                call latlon(elp,xyz,llh,i_type)

                xyz = xyz - xyzsat
                bb = norm(xyz)
                orng(pixel) = bb
                aa = abs(sum(nhat*xyz))
                oview(pixel) = acos(aa/bb)*r2d
            end do
            !$omp end parallel do


            !!!Again sort in terms of slant range
            call InsertionSort(orng, ctrack, oview, owidth)

            mask = 0
            omask = 0
            aa = elevang(1)
            do pixel=2,width
                bb=elevang(pixel)
                if (bb.le.aa) then
                    mask(pixel) = 1
                else
                    aa = bb
                endif
            end do

            aa = elevang(width)
            do pixel=width-1,1,-1
                bb = elevang(pixel)
                if (bb.ge.aa) then
                    mask(pixel) = 1
                else
                    aa = bb
                endif
            end do

            !!!!If we wanted to work with shadow
            !!!!in cross track sorted coords
            !aa = oview(1)
            !do pixel=2,owidth
                !bb = oview(pixel)
                !if (bb.le.aa) then
                    !omask(pixel) = 1
                !else
                    !aa = bb
                !endif
            !enddo

            !aa = oview(width)
            !do pixel=width-1,1,-1
                !bb = oview(pixel)
                !if (bb.ge.aa) then
                    !omask(pixel) = 1
                !else
                    !aa = bb
                !endif
            !end do


            aa = ctrack(1)
            do pixel=2,width
                bb = ctrack(pixel)
                if ((bb.le.aa).and.(omask(pixel).lt.2)) then
                    omask(pixel) = omask(pixel) + 2
                else
                    aa = bb
                endif
            end do

            aa = ctrack(owidth)
            do pixel=owidth-1,1,-1
                bb = ctrack(pixel)
                if ((bb.ge.aa).and.(omask(pixel).lt.2)) then
                    omask(pixel) = omask(pixel) + 2
                else
                    aa = bb
                endif
            end do


            do pixel=1, owidth
                if (omask(pixel).gt.0) then
!!                    idemlat = nint((orng(pixel) - r0)/ (rspace * Nrnglooks))+1
                    idemlat = binarysearch(rho, width, orng(pixel))
                    if ((idemlat.ge.1) .and. (idemlat.le.width)) then
                        if (mask(idemlat) .lt. omask(pixel)) then
                            mask(idemlat) = mask(idemlat) + omask(pixel)
                        endif
                    endif
                endif
            enddo

            !!!!!If using shadow from ctrack coords
            !do pixel=1, owidth
                !if (omask(pixel).gt.0) then
                    !idemlat = nint((orng(pixel) - r0)/ (rspace * Nrnglooks))+1
                    !if ((idemlat.ge.1) .and. (idemlat.le.width)) then
                            !mask(idemlat) = omask(pixel)
                    !endif
                !endif
            !enddo



            call setLineSequential(MaskAccessor, mask)
        endif
      end do

      print *, 'Total convergence:', totalconv, ' out of ', width*length
      call unprepareMethods(method)
!!       close(31)

       if (maskAccessor.gt.0) then
          deallocate(omask)
          deallocate(orng)
          deallocate(mask)
          deallocate(ctrack)
          deallocate(oview)
       endif

       deallocate (demline)
       deallocate (converge)
       deallocate (distance)
       deallocate (lat)
       deallocate (lon)
       deallocate (z)
       deallocate (zsch)
       deallocate (rho)
       deallocate (dem)
       deallocate (losang)
       deallocate (incang)
       deallocate (elevang)
      end


      SUBROUTINE InsertionSort(a,b,c,num)
        REAL*8, DIMENSION(num) :: a,b,c
        REAL*8 :: tempa,tempb,tempc
        INTEGER :: i, j, num

        DO i = 2, num
            j = i - 1
            tempa = a(i)
            tempb = b(i)
            tempc = c(i)
            DO WHILE (j>=1 .AND. a(j)>tempa)
                a(j+1) = a(j)
                b(j+1) = b(j)
                c(j+1) = c(j)
                j = j - 1
            END DO
            a(j+1) = tempa
            b(j+1) = tempb
            c(j+1) = tempc
        END DO
     END SUBROUTINE InsertionSort


    function binarysearch(array, length, val)
        implicit none
        integer :: length
        real*8, dimension(length) :: array
        real*8 :: val

        integer :: binarysearch, ind

        integer :: left, middle, right


        left = 1
        right = length
        do
            if (left > right) then
                exit
            endif
            middle = nint((left+right) / 2.0)

            if (left .eq. (right-1)) then
                binarySearch = left
                return
            elseif (array(middle).le.val) then
                left = middle
            elseif (array(middle).gt.val) then
                right = middle
            end if
        end do

        binarysearch = left
    end function binarysearch
