subroutine geo2rdr(latAcc,lonAcc,hgtAcc,azAcc,rgAcc,azoffAcc,rgoffAcc)
  use geo2rdrState
  use poly1dModule
  use geometryModule
  use orbitModule
  use linalg3Module
  use fortranUtils, ONLY: getPI,getSpeedOfLight
  
  implicit none
  include 'omp_lib.h'


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !! DECLARE LOCAL VARIABLES
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  integer stat,cnt
  integer*8 latAcc,lonAcc,hgtAcc
  integer*8 azAcc,rgAcc
  integer*8 azOffAcc,rgOffAcc
  real*8, dimension(:),allocatable :: lat
  real*8, dimension(:),allocatable :: lon
  real*8, dimension(:),allocatable :: dem
  real*8, dimension(:),allocatable :: rgm
  real*8, dimension(:),allocatable :: azt
  real*8, dimension(:),allocatable :: rgoff
  real*8, dimension(:),allocatable :: azoff
  real*4, dimension(:),allocatable :: distance

  !!!!Image limits
  real*8 tstart, tend, tline, tprev
  real*8 rngstart, rngend, rngpix

  !!!! Satellite positions
  real*8, dimension(3) :: xyz_mid, vel_mid, acc_mid
  real*8 :: tmid, rngmid, temp

  type(ellipsoidType) :: elp
  real*8 :: llh(3),xyz(3)
  real*8 :: satx(3), satv(3),sata(3)
  real*8 :: dr(3)
  integer :: pixel,line, ith

  integer :: i_type,k,conv
  real*8 :: dtaz, dmrg
  real*8 :: dopfact,fdop,fdopder

  real*8 :: c1,c2

  integer :: numOutsideImage

  real*4 :: timer0, timer1  

  !!Function pointer for orbit interpolation
  procedure(interpolateOrbit_f), pointer :: intp_orbit => null()

  ! declare constants
  real*8 pi,rad2deg,deg2rad,sol 
  real*8 fn, fnprime
  real*4 BAD_VALUE
  parameter(BAD_VALUE = -999999.0)


  !Doppler factor
  type(poly1dType) :: fdvsrng,fddotvsrng

  pi = getPi()
  sol = getSpeedOfLight()
  rad2deg = 180.d0/pi
  deg2rad = pi/180.d0

       !!!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


  ! get starting time
  timer0 = secnds(0.0)
  cnt = 0

  !$OMP PARALLEL
  !$OMP MASTER
  ith = omp_get_num_threads() !total num threads
  !$OMP END MASTER
  !$OMP END PARALLEL
  print *, "threads",ith


  elp%r_a= majorSemiAxis
  elp%r_e2= eccentricitySquared


  tstart = t0
  dtaz = Nazlooks / prf
  tend  = t0 + (length-1)* dtaz
  tmid = 0.5d0*(tstart+tend)

  print *, 'Starting Acquisition time: ', tstart
  print *, 'Stop Acquisition time: ', tend
  print *, 'Azimuth line spacing in secs: ', dtaz

  rngstart = rho0
  dmrg = Nrnglooks * drho
  rngend = rho0 + (width-1)*dmrg
  rngmid = 0.5d0*(rngstart+rngend)
  print *, 'Near Range in m: ', rngstart 
  print *, 'Far  Range in m: ', rngend
  print *, 'Range sample spacing in m: ', dmrg

  print *, 'Radar Image Lines: ', length
  print *, 'Radar Image Width: ', width



  ! allocate
  allocate(lat(demwidth))
  allocate(lon(demwidth))
  allocate(dem(demwidth))
  allocate(rgm(demwidth))
  allocate(azt(demwidth))
  allocate(rgoff(demwidth))
  allocate(azoff(demwidth))
  allocate(distance(demwidth))

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !! PROCESSING STEPS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  print *, "reading dem ..."

  print *, 'Geocoded Lines:  ', demlength
  print *, 'Geocoded Samples:', demwidth 



  !!!!Setup doppler polynomials
  call initPoly1D_f(fdvsrng, dopAcc%order)
  fdvsrng%mean = rho0 + dopAcc%mean * drho !!drho is original full resolution.
  fdvsrng%norm = dopAcc%norm * drho   !!(rho/drho) is the proper original index for Doppler polynomial

  !!!Coeff indexing is zero-based
  do k=1,dopAcc%order+1
     temp = getCoeff1d_f(dopAcc,k-1)
     temp = temp*prf
     call setCoeff1d_f(fdvsrng, k-1, temp)
  end do



  !!!Set up derivative polynomial
  if (fdvsrng%order .eq. 0) then
      call initPoly1D_f(fddotvsrng, 0)
      call setCoeff1D_f(fddotvsrng, 0, 0.0d0)
  else
      call initPoly1D_f(fddotvsrng, fdvsrng%order-1)
      fddotvsrng%mean = fdvsrng%mean
      fddotvsrng%norm = fdvsrng%norm

      do k=1,dopAcc%order
        temp = getCoeff1d_f(fdvsrng, k)
        temp = k*temp/fdvsrng%norm
        call setCoeff1d_f(fddotvsrng, k-1, temp)
      enddo
  endif


  print *, 'Dopplers: ', evalPoly1d_f(fdvsrng, rngstart), evalPoly1d_f(fdvsrng,rngend)

  !!!!Initialize satellite positions
  tline = tmid
  stat =  intp_orbit(orbit, tline, xyz_mid, vel_mid)

  if (stat.ne.0) then
      print *, 'Cannot interpolate orbits at the center of scene.'
      stop
  endif

  stat = computeAcceleration_f(orbit, tline, acc_mid)

  if (stat.ne.0) then
      print *, 'Cannot compute acceleration at the center of scene.'
      stop
  endif

  print *, "geo2rdr on ",ith,' threads...'
 
  numOutsideImage = 0

!!  open(31, file='fndistance',access='direct',recl=4*demwidth,form='unformatted')
  do line = 1, demlength
     !!Initialize
     azt = BAD_VALUE
     rgm = BAD_VALUE
     rgoff = BAD_VALUE
     azoff = BAD_VALUE
     distance = BAD_VALUE

     !!Read in positions
     call getLineSequential(hgtAcc,dem,pixel)
     call getLineSequential(latAcc,lat,pixel)
     call getLineSequential(lonAcc,lon,pixel)

     if (mod(line,1000).eq.1) then
         print *, 'Processing line: ', line, numoutsideimage
     endif
     conv = 0

     !$OMP PARALLEL DO private(pixel,i_type,k)&
     !$OMP private(xyz,llh,rngpix,tline,satx,satv)&
     !$OMP private(c1,c2,tprev,dr,stat,fn,fnprime)&
     !$OMP private(dopfact,fdop,fdopder,sata) &
     !$OMP shared(length,width,demwidth) &
     !$OMP shared(rgm,azt,rgoff,azoff) &
     !$OMP shared(line,elp,ilrl,tstart,tmid,rngstart,rngmid) &
     !$OMP shared(xyz_mid,vel_mid,acc_mid,fdvsrng,fddotvsrng) &
     !$OMP shared(lat,lon,dem,dtaz,dmrg,deg2rad,bistatic,sol) &
     !$OMP shared(numOutsideImage,wvl,orbit,conv,distance) 
     do pixel = 1,demwidth 
       
        llh(1) = lat(pixel) * deg2rad
        llh(2) = lon(pixel) * deg2rad
        llh(3) = dem(pixel)

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


        !!!!Actual iterations
        tline = tmid
        satx = xyz_mid
        satv = vel_mid
        sata = acc_mid

        do k=1,51
            tprev = tline  

            dr = xyz - satx
            rngpix = norm(dr)

            dopfact = dot(dr,satv)
            fdop = 0.5d0 * wvl * evalPoly1d_f(fdvsrng,rngpix)
            fdopder = 0.5d0 * wvl * evalPoly1d_f(fddotvsrng,rngpix)

            fn = dopfact - fdop * rngpix

            c1 = (0.0d0 * dot(sata,dr) - dot(satv,satv))
            c2 = (fdop/rngpix + fdopder)

            fnprime = c1 + c2*dopfact

!!            if (abs(fn) .le. 1.0d-5) then
!!                conv = conv + 1
!!                exit
!!            endif

            tline = tline - fn / fnprime

!!            print *, c1, c2, rngpix

            stat = intp_orbit(orbit,tline,satx,satv)

            if (stat.ne.0) then
                tline = BAD_VALUE
                rngpix = BAD_VALUE
                exit
            endif

!            stat = computeAcceleration_f(orbit,tline,sata)
!            if (stat.ne.0) then
!                tline = BAD_VALUE
!                rngpix = BAD_VALUE
!                exit
!            endif

            !!!Check for convergence
            if (abs(tline - tprev).lt.5.0d-9) then
                conv = conv + 1
                exit
            endif
        enddo


        if(tline.lt.tstart) then
            numOutsideImage = numOutsideImage + 1
            goto 100
        endif

        if(tline.gt.tend) then
            numOutsideImage = numOutsideImage + 1
            goto 100
        endif

        dr = xyz - satx
        rngpix = norm(dr)

        if(rngpix.lt.rngstart) then
            numOutsideImage = numOutsideImage + 1
            goto 100
        endif

        if(rngpix.gt.rngend) then
            numOutsideImage = numOutsideImage + 1
            goto 100
        endif

        if (bistatic) then
            tline = tline + 2.0d0*rngpix/sol 

            if(tline.lt.tstart) then
                numOutsideImage = numOutsideImage + 1
                goto 100
            endif

            if(tline.gt.tend) then
                numOutsideImage = numOutsideImage + 1
                goto 100
            endif

            !!!!Interpolate orbit to new position
            stat = intp_orbit(orbit,tline,satx,satv)
            if (stat.ne.0) then
                tline = BAD_VALUE
                rngpix = BAD_VALUE
            endif

            if (tline.eq.BAD_VALUE) then
                numOutsideImage = numOutsideImage + 1
                goto 100
            endif

            dr = xyz-satx
            rngpix = norm(dr)

            if(rngpix.lt.rngstart) then
                numOutsideImage = numOutsideImage + 1
                goto 100
            endif

            if(rngpix.gt.rngend) then
                numOutsideImage = numOutsideImage + 1
                goto 100
            endif
        endif

        cnt = cnt + 1
        rgm(pixel) = rngpix
        azt(pixel) = tline

        rgoff(pixel) = ((rngpix - rngstart)/dmrg) - 1.0d0*(pixel-1)
        azoff(pixel) = ((tline - tstart)/dtaz) - 1.0d0*(line-1)
        distance(pixel) = tline - tprev

100        continue


        enddo
        !$OMP END PARALLEL DO



        ! write output file
        if (azAcc.gt.0) then
            call setLineSequential(azAcc,azt)
        endif

        if (rgAcc.gt.0) then
            call setLineSequential(rgAcc,rgm)
        endif

        if (azoffAcc.gt.0) then
            call setLineSequential(azoffAcc,azoff)
        endif

        if (rgoffAcc.gt.0) then
            call setLineSequential(rgoffAcc,rgoff)
        endif
!!        write(31,rec=line)(distance(pixel),pixel=1,demwidth)
    enddo


    print *, 'Number of pixels outside the image: ', numOutsideImage
    print *, 'Number of pixels with valid data:   ', cnt
    print *, 'Number of pixels that converged:    ', conv

    !!!!Clean polynomials
    call cleanpoly1d_f(fdvsrng)
    call cleanpoly1d_f(fddotvsrng)

!!    close(31) 
    deallocate(lat,lon,dem)
    deallocate(azt,rgm)
    deallocate(azoff,rgoff)
    deallocate(distance)

     timer1 = secnds(timer0)
     print *, 'elapsed time = ',timer1,' seconds'
end 
        
