c
c  program asprep - find repeat time for GPS constellation, as viewed from
c    a particular location (the "aspect repeat time")
c
c  $Id: asprep.f,v 1.2 2009/07/29 22:04:48 agnew Exp agnew $
c
c  $Log: asprep.f,v $
c  Revision 1.2  2009/07/29 22:04:48  agnew
c  Fixed initial setting of  jp  (thanks to Karla Edwards, OSU)
c
c
c  read GPS positions from .sp3 files (concatenated) and find repeat times from
c  a particular location by interpolating.
c
c  command line includes time to find repeats from, number of days to skip
c   forward, and station location.
c
c   calls findex and polyin (for interpolation), iscnsp and related routines
c    (for calendar computations).
c
c  D. C. Agnew, March/May 2006
c
c
      character*60 line
      character*1 mode
      double precision xp,yp,zp,time,ct
      double precision xl,yl,zl,tl
      double precision s0(3),pn(3),p(3),sn(3)
      double precision rlat,rlon,ht
      dimension isv(40),itime(4000),ist(6),lti(6),itref(6)
      dimension xp(40,4000),yp(40,4000),zp(40,4000),time(4000)
      dimension xl(12),yl(12),zl(12),tl(12)
      data iepc/1/,mode/'m'/,ifst/0/,lti/6*0/,iokst/0/,ioken/0/
      data stp/1./
      if(iargc().ne.10) then
         write(6,100)
 100     format(/,
     1  'Usage: cat file1.sp3 file2.sp3... | asprep y m d h m s ndays',
     3  ' lat long ht',
     4 /,' where the time given is the time to find the repeats from',/
     5 /,'This must be more than 2 hours after the start of the first ',
     6 '.sp3',/,' file; the repeat point, which will be ndays later'
     7 ,' must be',/,' more than 2 hours earlier than the end time of',
     8 ' the last .sp3 file',//,'Writes out SV number, repeat time ',
     9 '(normalized to one day), angular')
         write(6,101)
 101     format(' separation (millirad), elevation, azimuth, rate of ',
     1 'motion (millirad/sec),',/,' and separation/rate (sec)',/)
         stop
      endif
      do i=1,6
        call getarg(i,line)
        read(line,102) itref(i)
 102    format(i4)
      enddo
      call getarg(7,line)
      read(line,102) ndays
      isep=86155*ndays
      call getarg(8,line)
      read(line,103) rlat
 103  format(f8.0)
      call getarg(9,line)
      read(line,103) rlon
      call getarg(10,line)
      read(line,103) ht
      call xyz(rlat,rlon,ht,p,pn)
c
c  read start of file and get number of sats (assumed to be the same
c   for all days if concatenated)
c
 1    read(5,110) line
 110  format(a60)
      if(line(1:2).ne."+ ") go to 1
      read(line,112) ns,(isv(i),i=1,17)
 112  format(4x,i2,3x,17(1x,i2))
      read(5,110) line
      read(line,114) (isv(i),i=18,34)
 114  format(9x,17(1x,i2))
c
c  now read lines; if a *, get time (secs from start); then read positions
c
 3    read(5,110,end=13) line
      if(line(1:2).ne."* ") go to 3
      read(line,120) (lti(i),i=1,5)
 120  format(3x,i4,4i3)
      read(line,122) sec
 122  format(20x,f9.6)
      lti(6)=nint(sec)
c
c find time in seconds relative to start time, and decide if the time
c  should be read in; criteria are that it be within +-5 hours of
c  start time, or of that time ndays*86155 sec later
c
      lst=iscnsp(itref,lti,1,mode)
      iget=0
      if(lst.gt.-18000.and.lst.lt.18000) then
        iget=1
        iokst=iokst+1
      endif
      if(lst.gt.isep-18000.and.lst.lt.isep+18000) then
        iget=1
        ioken=ioken+1
      endif
      if(ifst.eq.0.and.iget.eq.1) then
        ifst=1
        do j=1,6
           ist(j)=lti(j)
        enddo
      endif
      itime(iepc)=iscnsp(ist,lti,1,mode)
      time(iepc)=itime(iepc)
      do i=1,ns
         read(5,110) line
         read(line,130) xp(i,iepc),yp(i,iepc),zp(i,iepc)
 130     format(4x,3f14.6)
      enddo
c only advance index if time is in windows; otherwise will be overwritten
      if(iget.eq.1) iepc=iepc+1
c
c loop back to read (we assume) next time--or next header, which is just
c   skipped
c
      go to 3
c
c done with read--after checking that we have data covering the times needed,
c  find the first time, and get the positions; then
c  interpolate over a 300-s period n days later to get the repeats
c
 13   iepc=iepc-1
      if(iokst.lt.35.or.ioken.lt.35) then
        if(iokst.lt.35) write(6,132)
 132    format(' No orbit data around start time ')
        if(ioken.lt.35) write(6,134)
 134    format(' No orbit data around end time ')
        stop
      endif
      jlo=iscnsp(ist,itref,1,mode)
      do i=1,ns
         ct=jlo
         call findex(time,iepc,ct,jp)
         do k=1,12
          tl(k)=time(k+jp-5)
          xl(k)=xp(i,k+jp-5)
          yl(k)=yp(i,k+jp-5)
          zl(k)=zp(i,k+jp-5)
         enddo
         jpo=jp
c
c  find the station-satellite vector, and normalize it
c
         call polyin(tl,xl,12,ct,s0(1))
         call polyin(tl,yl,12,ct,s0(2))
         call polyin(tl,zl,12,ct,s0(3))
         do k=1,3
           s0(k)=1000*s0(k)-p(k)
         enddo
         r=sqrt(s0(1)**2+s0(2)**2+s0(3)**2)
         do k=1,3
           s0(k)=s0(k)/r
         enddo
c
c shift start time of search by 86155 seconds/day, with 155*ndays subtracted;
c  then search over a 300-s window in 1 s steps--this will capture all
c  but some very rare situations, and those missed will be far from repeating
c  anyway
c
         jl=jlo+ndays*86155 - 155*ndays
         nspan=(300+310*(ndays-1))/stp
         dol=0.
         dol2=0.
         do j=jl,jl+nspan
           ct=jl+stp*(j-jl)
           jp=0
           call findex(time,iepc,ct,jp)
           if(jp.ne.jpo) then
c fill arrays for interpolation
             do k=1,12
               tl(k)=time(k+jp-5)
               xl(k)=xp(i,k+jp-5)
               yl(k)=yp(i,k+jp-5)
               zl(k)=zp(i,k+jp-5)
             enddo
           endif
           jpo=jp
           call getvec(tl,xl,yl,zl,ct,p,s0,sn,d,sky)
c test to see if we have passed the minimum
           if(d.gt.dol.and.j.gt.jl) then
c from current and last two values, find time of minimum assuming
c parabolic behavior, and step size of 1
              d1=dol-d
              d2=dol2-d
              dt=(4*d1-d2)/(2*(d2-2*d1))
c rescale for step size and find actual time
              ct=ct+stp*dt
              call getvec(tl,xl,yl,zl,ct,p,s0,sn,d,tmp)
c find az and elevation
              dot=0
              do k=1,3
                dot=dot+sn(k)*pn(k)
              enddo
              el = 90. - 180.d0*acos(dot)/3.1415922635898d0
              az = 180.*azim(sn,pn)/3.14159265359
              if(az.lt.0) az=az+360
c find values in milliradians, and millirad/sec
              skysep=1000*asin(d)
              rate=(1000*asin(sky))/stp
              tr=skysep/rate
c
c  write out SV number, repeat time, angle in sky, elevation, azimuth,
c  rate of motion (millirad/sec) and derived time for this rate to cover
c  the separation
c
c      change in time computation (to correct a 1-sec error) made 9 Oct 2006
c
              reptime=(ct-jlo)/ndays
              if(j-jl.gt.2)
     1           write(6,140) isv(i),reptime,skysep,el,az,rate,tr
 140          format(i2,f10.2,f8.3,2f8.1,f9.4,f6.1)
              if(j-jl.eq.2)
     1           write(6,141) isv(i)
 141          format(i2,'  minimum outside of window searched')
c
c leave loop over time, go to next satellite
c
              go to 17
           endif
           dol2=dol
           dol=d
         enddo
 17      if(d.lt.do) write(6,141) isv(i)
      enddo
      stop
      end
      subroutine getvec(tl,xl,yl,zl,ct,p,s0,sn,d,sky)
c
c  given an array of times tl, positions xyz, current time ct,
c    station vector p, and previous day's topocentric vector s0,
c    finds the current topocentric unit vector sn, the separation
c    of this from the previous days (d), and the rate of motion (sky)
c   
      save sno
      double precision tl,xl,yl,zl,ct,p,s0,sn
      double precision dn,sno,skysn,s
      dimension xl(*),yl(*),zl(*),tl(*),p(*),s0(*)
      dimension sn(*)
      dimension dn(3),sno(3),skysn(3),s(3)
      call polyin(tl,xl,12,ct,s(1))
      call polyin(tl,yl,12,ct,s(2))
      call polyin(tl,zl,12,ct,s(3))
      do k=1,3
        s(k)=1000*s(k)-p(k)
      enddo
      r=sqrt(s(1)**2+s(2)**2+s(3)**2)
      do k=1,3
        sn(k)=s(k)/r
      enddo
c find cross-product of s0 and sn: minimum of this gives time when
c satellite is in the same direction
      dn(1)= s0(2)*sn(3) - s0(3)*sn(2)
      dn(2)= s0(3)*sn(1) - s0(1)*sn(3)
      dn(3)= s0(1)*sn(2) - s0(2)*sn(1)
      d=sqrt(dn(1)*dn(1)+dn(2)*dn(2)+dn(3)*dn(3))
c find cross-product of sn with its previous value, to get angular motion in
c the last second
      skysn(1)= sno(2)*sn(3) - sno(3)*sn(2)
      skysn(2)= sno(3)*sn(1) - sno(1)*sn(3)
      skysn(3)= sno(1)*sn(2) - sno(2)*sn(1)
      sky=sqrt(skysn(1)*skysn(1)+skysn(2)*skysn(2)+
     1   skysn(3)*skysn(3))
      do k=1,3
         sno(k)=sn(k)
      enddo
      return
      end
      subroutine polyin(xa,ya,n,x,y)
c
c given an array of y values in ya, at x values in xa (both of length n),
c  and an x-value x, returns a y-value using the interpolating polynomial
c
      double precision xa,ya,x,y,cy,c,d,df,dfx
      dimension xa(*),ya(*)
      dimension c(13),d(13)
      ns=1
      df=abs(x-xa(1))
      do i=1,n
        dfx=abs(x-xa(i))
        if (dfx.lt.df) then
          ns=i
          df=dfx
        endif
        c(i)=ya(i)
        d(i)=ya(i)
      enddo
      y=ya(ns)
      ns=ns-1
      do m=1,n-1
        do i=1,n-m
          ho=xa(i)-x
          hp=xa(i+m)-x
          w=c(i+1)-d(i)
          den=ho-hp
          if(den.eq.0.) then
             write(6,100)
 100         format(' Interpolation error')
          endif
          den=w/den
          d(i)=hp*den
          c(i)=ho*den
        enddo
        if (2*ns.lt.n-m)then
          cy=c(ns+1)
        else
          cy=d(ns)
          ns=ns-1
        endif
        y=y+cy
      enddo
      return
      end
      subroutine findex(x,n,cx,jo)
c
c   given an array x, of length n, and a value cx, find the index jo that
c    corresponds to the term just below it; we assume that the x's are in
c    increasing order. The procedure starts by assuming that jo is the right value
c
      double precision x,cx
      dimension x(*)
      jl=jo
c
c starting value off end of array, go to bisection
c
      if(jl.le.0.or.jl.gt.n)then
        jl=1
        jh=n
        go to 7
      endif
c
c increase search area to cover value sought
c
      incr=1
      if(cx.ge.x(jl))then
 3      jh=jl+incr
        if(jh.gt.n) jh=n
        if(cx.ge.x(jh)) then
          jl=jh
          incr=2*incr
          go to 3
        endif
      endif
      if(cx.lt.x(jl))then
        jh=jl
 5      jl=jh-incr
        if(jlo.lt.1) jl=1
        if(cx.lt.x(jl)) then
          jh=jl
          incr=2*incr
          go to 5
        endif
      endif
c find the index by bisection
 7    if(jh-jl.eq.1)return
      jm=(jh+jl)/2
      if(cx.gt.x(jm)) jl=jm
      if(cx.le.x(jm)) jh=jm
      jo=jl
      go to 7
      end
      function iscnsp(it1,it2,isamp,mode)
c  finds the number of scans (terms, data points, . . .) separating times
c  specified in the arrays it1 and it2, for a sample interval of isamp
c  seconds. it2 is taken to be the later time; if it is actually earlier,
c  the function will return an appropriately negative value.  if the
c  dates are the same, 0 is returned (note that this value therefore
c  differs from the usual scan count)
c   mode sets the way in which the dates are given; if it is 'd' the dates
c  are year, day, hour, minute, second; if it is 'm' the dates are year,
c  month, day, hour, minute, second.
c
c  $$$$calls juldat,toymd,muldiv
c
      character*1 mode
      dimension it1(*),it2(*)
      dimension il1(6),il2(6)
      common /scanrem/irc
c convert times to Julian Date, and seconds of day
      if(mode.eq.'d') then
        call toymd(it1,il1)
        call toymd(it2,il2)
c  since seconds not used, do not need this copying
c       do i=3,5
c         il1(i+1)=it1(i)
c         il2(i+1)=it2(i)
c       enddo
        jd1 = juldat(il1)
        jd2 = juldat(il2)
        js1=60*(60*it1(3)+it1(4))+it1(5)
        js2=60*(60*it2(3)+it2(4))+it2(5)
      endif
      if(mode.ne.'d') then
        jd1 = juldat(it1)
        jd2 = juldat(it2)
        js1=60*(60*it1(4)+it1(5))+it1(6)
        js2=60*(60*it2(4)+it2(5))+it2(6)
      endif
      idir=1
c     if time-2 earlier than time-1, note this and swap the times
      if(jd2.lt.jd1.or.jd2.eq.jd1.and.is2.lt.is1) then
        j=jd2
        jd2=jd1
        jd1=j
        j=js2
        js2=js1
        js1=j
        idir=-1
      endif
c If necessary adjust the end date to a day earlier, to make the number of
c seconds for the later day greater than the the number on the start day
      if(js2.lt.js1) then
        js2=js2+86400
        jd2=jd2-1
      endif
      js=js2-js1
      jd=jd2-jd1
c Time span is jd days plus js seconds. Convert this to
c  scans, plus a remainder if it is not an even number.
      call muldiv(jd,86400,isamp,iscnsp,irc)
c add in the seconds part
      iscnsp = iscnsp + js/isamp
c and combine the two remainders and add them in as well
      irc = irc + mod(js,isamp)
      iscnsp = iscnsp + irc/isamp
      irc = mod(irc,isamp)
c scans are < 0 if time-2 is before time-1
      iscnsp = idir*iscnsp
      return
      end
      subroutine toymd(it1,it2)
      dimension it1(*),it2(*)
c
c  converts times in it1 given as year and day to year-month-day in it2
c
      idn(m) = mod((367*(m-2-12*((m-14)/12)))/12+29,365)
      mon(jj,m) = (12*(jj-29-m))/367 + 2 + (jj-200)/169
      it2(1)=it1(1)
      it2(2)=mon(it1(2),leap(it1(1)))
      it2(3) = it1(2) - idn(it2(2)) - leap(it2(1))*((9+it2(2))/12)
      return
      end
      function leap(iy)
c
c returns 1 if year is a leap year, by Clavian (Gregorian) rule
c
      leap = 1 - (mod(iy,4)+3)/4
      if(mod(iy,100).eq.0.and.mod(iy,400).ne.0) leap=0
      return
      end
      function juldat(it)
      dimension it(*)
c
c  Julian Date from Gregorian date, Algorithm from p. 604, Explanatory
c    Supplement Amer Ephemeris & Nautical Almanac (cf Comm CACM, 11, 657 (1968)
c    and 15, 918 (1972)) Valid for all positive values of Julian Date
c
      juldat=(1461*(it(1)+4800+(it(2)-14)/12))/4
     1     + (367*(it(2)-2-12*((it(2)-14)/12)))/12
     2     - (3*((it(1)+4900+(it(2)-14)/12)/100))/4+it(3)-32075
      return
      end
      subroutine muldiv(n1,n2,n3,np,nr)
c
c   finds the integer part and remainder of the quantity (n1*n2)/n3
c    without forming the product n1*n2, to minimize the chance of overflow
c      Algorithm by R. L. Parker, modified by D. C. Agnew
c
c    Basic idea is that we can decompose an integer n2 to be j1 mutliples
c       of n3, plus a remainder k1
c            n2 = j1*n3 + k1   whence in real arithmetic
c          n2/n3 = j1 + k1/n3  and this means that
c          n1*n2/n3 = n1*j1 + n1*k1/n3 and we can take the integer part
c    and remainder of n1*k1/n3 to get the integer part and remainder of
c    n1*n2/n3, with k1 being bounded as n2 is not. A second application  
c    gives the algorithm below.
c
      j2=n2/n3
      k2=mod(n2,n3)
      j1=n1/n3
      k1=mod(n1,n3)
c product nn has max value of n3*n3, so for n3*n3 < max value, will not
c  overflow unless another product is actually too large
      nn=k1*k2
      np=n1*j2 + j1*k2 + nn/n3
      nr=mod(nn,n3)
      return
      end
      subroutine xyz(rlat,rlong,ht,p,pn)
c
C     Routine to convert the geodetic co-latitudes, longitude and
C     elipsoidal height to the Cartesian XYZ coordinates of
C     the site.
      save f,e2
      double precision rlat,rlong,ht,p,pn,f,rad,pi,rc
      double precision clat,clong,slat,slong
      dimension p(*),pn(*)
      data rad/6378137.d0/
      data f/298.257223563d0/
      data pi/3.1415926535898d0/
      data ifst/0/
      if(ifst.eq.0) then
        f=1./f
        e2=f*(2.d0-f)
        ifst=1
      endif
      clat=cos(pi*(rlat)/180.d0)
      slat=sin(pi*(rlat)/180.d0)
      clong=cos(pi*(rlong)/180.d0)
      slong=sin(pi*(rlong)/180.d0)
      rc=rad/(sqrt(1.d0 - e2*slat*slat))
      p(1) = (rc+ht)*clat*clong
      p(2) = (rc+ht)*clat*slong
      p(3) = (rc*(1-e2)+ht)*slat
      dist=sqrt(p(1)**2+p(2)**2+p(3)**2)
      do i=1,3
        pn(i)=p(i)/dist
      enddo
      return
      end
      function azim(sn,pn)
c
c   sn is unit vector towards satellite from station;
c   pn towards station (geocentric)
c
      double precision sn,pn
      dimension sn(*),pn(*)
C      data pi/3.1415926535898d0/
      csb=pn(3)
      ssb=sqrt(1-csb**2)
      stalo=atan2(pn(2),pn(1))
      csc=sn(3)
      ssc=sqrt(1-csc**2)
      satlo=atan2(sn(2),sn(1))
      caa=cos(satlo-stalo)
      saa=sin(satlo-stalo)
      csa = csb*csc + ssb*ssc*caa
      sida = acos(csa)
      ssa = sin(sida)
      sac = saa*ssc/ssa
      cac = (csc -csb*csa)/(ssb*ssa)
      azim = atan2(sac,cac)
      return
      stop
      end