!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! R. Lucas 2009-02-04
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
program grav_def
  use gildas_def
  use gkernel_interfaces
  include 'gbl_pi.inc'
  include 'clic_panels.inc'
  integer :: np, numPol, i, k, ii, jj, j, lenc, ier, l, ir, ip, iscr, is, &
       ix, iy, iz, lpar, lc, icol, next, idr, idz, np0, ielev, idn
  !integer, allocatable :: mPol(:), nPol(:)
  real, allocatable :: x(:), y(:), z(:), dz(:), dr(:), dp(:), dn(:,:), w(:), &
       pars(:), tp(:), dnn(:)
  integer, allocatable :: iring(:), ipanel(:), iscrew(:)
  real :: xx, yy, rr, aa, ff, sigmaz, sigmay, dydz, h0, h1, h2, &
       sigma1, sigma2, phi, &
       bz, by, dh, dmin, d, h, r, t, value, se, se2, sw, t2, sig(2)
  real :: cols(20)
  integer, parameter :: npar=6 
  character :: infile(2)*256, outfile*256, a*256, antennaType*256, par*20

  real :: taper, radius, sigma, elevation(2)
  logical :: def(mring, mpan, mm)
  !
  elevation(1) = 0.
  elevation(2) = 90.
  taper = 12.
  call gildas_open
  call gildas_char('INFILE_0$',infile(1))
  call gildas_char('INFILE_90$',infile(2))
  call gildas_char('ANTENNA_TYPE$',antennaType)
  call gildas_char('OUTFILE$',outfile)
  call gildas_close
  !
  ix = 2
  iy = 3
  idr = 4
  idz = 5
  idn = 6
  if (antennaType(1:13).eq.'MITSUBISHI_12') then
    call set_panels(type_melco12m)
  elseif (antennaType(1:9).eq.'VERTEX_12') then
    call set_panels(type_vx12m)
  endif
  !
  do ielev = 1, 2
    open (1, file=infile(ielev)(1:lenc(infile(ielev))))
    np = 0
    def = .false.
    do while (.true.)
      read (1,*, end=10) a
      if (a(1:1).ne.'!') np = np+1
    enddo
10  if (np.le.0) then
      call gagout('E-Grav_def,  No input data. ')
      goto 999
    endif
    print *, 'number of points : ',np
    if (ielev.eq.2) then
      if (np0.ne.np) then
        call gagout('E-Grav_def,  Wrong number of data points. ')
        goto 999
      endif
    else
      np0 = np
      allocate(x(np), y(np), z(np), w(np), dr(np), dz(np), dp(np), dn(np,2), &
           pars(npar), iring(np), ipanel(np), iscrew(np), tp(np), dnn(np), stat = ier)
      if (ier.ne.0) then
        call gagout('E-Grav_def,  Allocation error. ')
        goto 999
      endif
    endif
        

    rewind(1)
    k = 0
    sw = 0
    do while (.true.)
      read (1,'(a)', end=20) a
      if (a(1:1).ne.'!' .and. a(1:1).ne.'"') then
        k = k+1
        lc = lenc(a)
        call sic_blanc(a,lc)
        next = 1
        icol = 0
        !!print*, lc, a(1:lc)
        do while (next.le.lc)
          call sic_next(a(next:), par, lpar, next)
          if (lpar.gt.0) then
            read (par(1:lpar), *, iostat = ier) value
            !!print *, lar, par(1:lpar), next, value
            icol = icol+1
            ! clic works with y up, x to the right as seen from the back.
            ! the alma coord sys has y down.
            ! 
            if (icol.eq.ix) then
              x(k) = value
            elseif (icol.eq.iy) then
              y(k) = -value
            elseif (icol.eq.idz) then
              dz(k) = value/1e6
            elseif (icol.eq.idr) then
              dr(k) = value/1e6
            elseif (icol.eq.idn) then
              dnn(k) = value/1e6
            endif
          endif
        enddo
        if (ier.ne.0) then
          call gagout('E-Grav_def, Error reading data: ')
          call gagout(a)
          goto 999
        endif
        call xypanel(x(k),y(k),ir,ip,xx,yy)
        dmin = 1e10
        iscr = 0
        do is = 1, nscr(ir)
          ! x,y are local panel-centered coordinates
          d = sqrt((xscr(is,ir)-xx)**2+(yscr(is,ir)-yy)**2)
          if (d.lt.dmin) then
            dmin = d
            iscr = is
          endif
        enddo
        ! weight (area only)
        w(k) = pi*(ray(ir)+ray(ir-1))/nscr(ir)/npan(ir)/(ray(ir)-ray(ir-1))
        r = sqrt(x(k)**2+y(k)**2)
        t = r/2/focus
        ! normal deformation
        !dn = (dz(k) - t*dr(k)) / sqrt(1+t**2)
        ! correct for increasing path on the edges: 
        ! 1/2 path is dn*cos (alpha)
        !dp(k) = dn /  sqrt(1+t**2)
        ! result: 
        dp(k) = (dz(k) - t*dr(k)) / (1+t**2)
        radius = diameter/2
        
        ! weight (tapered)
        tp(k) = 1.-(1.-10**(-0.05*taper))*(r/radius)**2
        w(k) = w(k)*tp(k)
        w(k) = 1
        sw = sw + w(k)

        if (dmin.gt.0.01) then
          print 1000, '** ',x(k), y(k), xx, yy, xscr(iscr,ir), yscr(iscr,ir), dmin, ir, ip, iscr
1000      format(a, 7f10.4, 3i6)
        endif
        iring(k) = ir
        ipanel(k) = ip
        iscrew(k) = iscr
        def (ir, ip, iscr) = .true.
      endif
    enddo
    !
20  close(1)
    ! check
    do ir = 1, nring
      !print *, ir, npan(ir), nscr(ir)
      do ip = 1, npan(ir)
        do is = 1, nscr(ir)
          !print *, ir, is, ip, def(ir,ip,is)
          if (.not.def(ir,ip,is)) then
            print *, '** data missing for ring ', ir, ' panel ', ip, ' screw ', is
          endif
        enddo
      enddo
    enddo
    ! normalize weights
    do i=1, np
      w(i) = w(i) / sw
    enddo
    ! fit a parabola
    ! dp = 1. + 2.*x + 3 * y + 4. * (x**2+y**2)/6*2. + 5*(x**3*y)/6**4 + 6*(x*y**3)/6**4 ! test
    !print *, (dp(i), i=1, 20)
    !print *, (w(i), i=1, 20)

    call fit_grav(np, dp, w, x, y, focus, radius, npar, pars, sigma)
    !print *, (dp(i), i=1, 20)
    ! sigma is the raw rms. ...
    do i=4,6
      pars(i) = pars(i)*1e3
    enddo
    sig(ielev) = sigma*1e6
    
    ! save the normal deformation
    do i=1, np
      r = sqrt(x(i)**2+y(i)**2)
      t = r/2/focus
      dn(i, ielev) = dp(i) *  sqrt(1+t**2)
    enddo
    !print *,  sqrt(se2-se**2)/sw, sqrt(se2/sw-(se/sw)**2)*1e6
    !print *, pars
    print *, 'elevation, sigma, X, Y, Z'
    print 1001, elevation(ielev), sig(ielev), (pars(i), i=4, npar) 
1001 format (1x, f6.2, f6.2, 3(1x, f10.3))
  enddo
  ! 
  ! compute normal residuals: 
  ! output file format is number, X, Y, dN90, dn0 ...
  open (2, file=outfile(1:lenc(outfile)), status='unknown')
  write (2,2001) "! Antenna ", antennaType(1:10), " Elevations 0 and 90 "
2001 format(1x,a,a,a)
  write (2,*) "! ring, panel, screw, x(m), y(m), deformations(mum) "
  do i=1, np
!    write(2,2000) iring(i), ipanel(i), iscrew(i), x(i), y(i), &
!         dn(i,1)*1e6, dn(i,2)*1e6, w(i), tp(i), dnn(i)*1e6, dr(i), dz(i)
!2000 format(1x, 3i4, f10.6, f10.6, 2f10.1, 2f10.6, 3f13.6) 
    write(2,2000) iring(i), ipanel(i), iscrew(i), x(i), y(i), &
         dn(i,1)*1e6, dn(i,2)*1e6
2000 format(1x, 3i4, f10.6, f10.6, 2f10.1) 
  enddo
  close(2)
  ! compute best rigging angle
  ! where sig(2)^2*(sin(90)-sin(h))^2 + sig(1)^2*(cos(90)-sin(h))^2
  ! == sig(2)^2*(sin(0)-sin(h))^2 + sig(1)^2*(cos(0)-sin(h))^2
  print *, sig
  phi = atan((sig(2)/sig(1))**2)
  
  h0 = acos((cos(phi)-sin(phi))/2)-phi
  print *, 'Best rigging angle h0: ', h0*180./pi, ' degrees.'
  print *, 'Max. rms with h0 setting: ',&
       sqrt(sig(2)**2*(sin(h0))**2+sig(1)**2*(1-cos(h0))**2)
  h1 = 20.*pi/180.
  h2 = 90.*pi/180.
  print *, 'Max. rms with [20.,90.] setting: ', &
       sqrt(sig(2)**2*((sin(h1)-sin(h2))/2)**2 + &
       sig(1)**2*((cos(h1)-cos(h2))/2)**2) 
  !
  call gagout('F-Grav_def: Done.') 
  !
  !
  stop
  !
999 call gagout('F-Grav_def:  Failed.')

  !
end program grav_def
!------------------------------------------------------------------------- 
subroutine fit_grav(np, dp, w, x, y, focus, radius, npar, par,  sigma)
  use gildas_def
  integer :: np, npar 
  real focus, sigma, par(npar), radius
  real dp(np), w(np), x(np), y(np)
  real*8, allocatable :: a(:,:), b(:)               !
  real*8 :: sig, x2, y2, r2, sw, sp, sp2, t2
  integer k, i, nd, j, l
  ! Global
  include 'gbl_pi.inc'
  ! Local
  !
  !
  k = 0
  allocate (a(np,npar), b(np))
  do i=1, np 
    if (w(i).gt.0) then
      k = k+1
      x2 = x(i)*x(i)
      y2 = y(i)*y(i)
      r2 = x2+y2
      a(k,1) = 1.d0
      a(k,2) = x(i)/radius
      a(k,3) = y(i)/radius
!      a(k,4) = 1-(1-r2/4/focus**2)/sqrt(r2/focus**2+(1.-r2/4/focus**2)**2)
!      a(k,5) = x(i)/focus*(1.-1./sqrt(r2/focus**2+(1.-r2/4/focus**2)**2))
!      a(k,6) = y(i)/focus*(1.-1./sqrt(r2/focus**2+(1.-r2/4/focus**2)**2))
      t2 = r2/4/focus**2
      a(k,4) = 2*t2/(1+t2)
      a(k,5) = x(i)/focus*t2/(1+t2)
      a(k,6) = y(i)/focus*t2/(1+t2)
      b(k) = dp(i)
      !do j=1, npar
      !  a(k,j) = a(k,j)*w(i)
      !enddo
      !b(k) = dp(i)*w(i)
        
      !print 1000, k, (a(k,j),j=1,npar)
!1000  format (i3, 6(1pg11.3))
    endif
  enddo
  nd = k
  !
  ! Solve linear system
  call mth_fitlin ('fit_grav',nd,npar,a,b,np,sig)
  do l=1, npar
    par(l) = b(l)
  enddo
  sigma = sig
  ! return the residuals 
  k = 0
  sw = 0
  sp = 0
  sp2 = 0
  do i=1, np 
    if (w(i).gt.0) then
      k = k+1
      x2 = x(i)*x(i)
      y2 = y(i)*y(i)
      r2 = x2+y2
      a(k,1) = 1.d0
      a(k,2) = x(i)/radius
      a(k,3) = y(i)/radius
      !a(k,4) = 1-(1-r2/4/focus**2)/sqrt(r2/focus**2+(1.-r2/4/focus**2)**2)
      !a(k,5) = x(i)/focus*(1.-1./sqrt(r2/focus**2+(1.-r2/4/focus**2)**2))
      !a(k,6) = y(i)/focus*(1.-1./sqrt(r2/focus**2+(1.-r2/4/focus**2)**2))
      t2 = r2/4/focus**2
      a(k,4) = 2*t2/(1+t2)
      a(k,5) = x(i)/focus*t2/(1+t2)
      a(k,6) = y(i)/focus*t2/(1+t2)
      do l=1, 6
        dp(i) = dp(i)-a(k,l)*par(l) 
      enddo
      ! test rms of dn
      sw = sw + w(i)
      sp = sp + w(i)*dp(i)
      sp2 = sp2 + w(i)*dp(i)**2
    endif
  enddo
  sp = sp / sw
  sp2 = sp2 /sw
  sig = sqrt(sp2-sp**2)
  
  !print *, 'sw, sp, sp2, sqrt(sp2-sp**2), sig'
  !print *, sw, sp, sp2, sqrt(sp2-sp**2), sig
  
end subroutine fit_grav

