program compute_gravitation_file
  use gkernel_interfaces
  implicit none
  !
  include 'clic_panels.inc'
  include 'gbl_pi.inc'
  !
  integer   :: lun1, lun2, i, iradnode, iaznode, iscrew, ir, ip,ipz
  integer   :: ir1, ir2, ircurrent, ipanel, nsector, is
  integer   :: ip1, ip2, ip3, ip4, k, nm, im, nmod
  real      :: x, y, z, dz1, dz2, az, r1, r0, t0, phi, r
  real      :: xoscr(5,7), yoscr(5,7), xx, yy, fmode, zpan, epan
  real*8    :: a1(mm,mm),a2(mm,mm), b1(mm), b2(mm), sig
  real      :: am1(mm,mring,mpan),am2(mm,mring,mpan)
  real      :: xpanel, ypanel,ae(mm,mring,mpan)
  real, allocatable :: def1(:,:,:), def2(:,:,:)
  character :: infile*256, file*256, chain*256, cpanel*5
  logical   :: error
  data xoscr/ &
       0.058, 0.058, -0.061, -0.061, 0., &
       0.037, 0.037, -0.035, -0.035, 0., &
       0.030, 0.030, -0.034, -0.034, 0., &
       0.031, 0.031, -0.043, -0.043, 0., &
       0.043, 0.043, -0.044, -0.044, 0., &
       0.032, 0.032, -0.034, -0.034, 0., &
       0.034, 0.034, -0.067, -0.067, 0./
  data yoscr/ &
       0.027, -0.027, 0.045, -0.045, 0., &
       0.025, -0.025, 0.048, -0.048, 0., &
       0.043, -0.043, 0.058, -0.058, 0., &
       0.024, -0.024, 0.031, -0.031, 0., &
       0.031, -0.031, 0.038, -0.038, 0., &
       0.038, -0.038, 0.045, -0.045, 0., &
       0.045, -0.045, 0.051, -0.051, 0./
  !
  ! set 30m defaults
  allocate(def1(mring,mpan,mm))
  allocate(def2(mring,mpan,mm))
  call set_panels(type_iram30m)
  !
  ! Open two deformation files
  error = sic_getlun(lun1).ne.1
  error = sic_getlun(lun2).ne.1
  if (error) then
    print *,'Get_rigging','No logical unit left'
    stop
  endif
  !
  ! Horizon file
  infile = 'graverr0.dat'
  if (.not.sic_query_file(infile,'data#dir:','',file)) then
    print *, 'Get_rigging','Deformation file not found'
    error = .true.
    stop
  endif
  open (lun1, file=file(1:lenc(file)))
  !
  ! Zenith file
  infile = 'graverr90.dat'
  if (.not.sic_query_file(infile,'data#dir:','',file)) then
    print *, 'Get_rigging','Deformation file not found'
    error = .true.
    stop
  endif
  open (lun2, file=file(1:lenc(file)))
!  read(lun,'(a)', err=20) chain
!  read(lun,'(a)', err=20) chain
  i = 1
  ircurrent = 1
  ipanel = 1
10 read (lun1, *, end=30, err=20) iradnode, iaznode, az, x, y, z, dz1
30 read (lun2, *, end=20, err=20) iradnode, iaznode, az, x, y, z, dz2
  if (iradnode.ne.ircurrent) then
    ipanel = 1
    ircurrent = iradnode
  endif
  dz1 = dz1 *1000.
  dz2 = dz2 *1000.
!  call xypanel(x,y,ir,ip,xpanel,ypanel)
  ir1 = iradnode-1
  ir2 = iradnode
  if (ir2.ge.1.and.ir2.le.nring) then
    ! Screw 2 on panel ipanel
    ip1 = ipanel
    def1(ir2,ip1,1) = dz1
    def2(ir2,ip1,1) = dz2
    ! Screw 1 on panel ipnanel-1
    ip3 = ipanel-1
    if (ip3.lt.1) ip3 = ip3 +npan(ir2)
    def1(ir2,ip3,2) = dz1
    def2(ir2,ip3,2) = dz2
  endif
  if (ir1.ge.1.and.ir1.lt.nring) then
    nsector = npan(ir2)/npan(ir1)
    if (mod(ipanel+1,nsector).eq.0) then
      ip2 = ipanel/nsector
      ip4 = ipanel/nsector-1
      if (ip4.le.0) ip4 = ip4+npan(ir1)
      def1(ir1,ip2,4) = dz1
      def1(ir1,ip4,3) = dz1
      def2(ir1,ip2,3) = dz2
      def2(ir1,ip4,3) = dz2 
    endif
  elseif(ir1.eq.nring) then
    nsector = 1 
    ip2 = ipanel/nsector
    ip4 = ipanel/nsector-1
    if (ip4.le.0) ip4 = ip4+npan(ir1)
    def1(ir1,ip2,3) = dz1
    def1(ir1,ip4,4) = dz1
    def2(ir1,ip2,3) = dz2
    def2(ir1,ip4,4) = dz2
  endif
  !
  
  ! Now assign deformation to "screws"
  !
  i = i+1
  ipanel = ipanel + 1
  goto 10
  !
  ! Close files
20  close(lun1)
  close(lun2)
  call sic_frelun(lun1)
  call sic_frelun(lun2)
  !
  ! Compute support position (optionnaly)
  do ir = 1, nring
    nscr(ir) = 4      ! 4 corners
    r1 = (ray(ir)-ray(ir-1))/2
    r0 = (ray(ir)+ray(ir-1))/2
    t0 = r0 / 2 / focus
    do is = 1, nscr(ir)
       ! coordinates of corner (or center if offset is zero)
       r = r0
       phi = 0
       if (xoscr(is,ir).lt.0) then
         r = ray(ir)
       elseif (xoscr(is,ir).gt.0) then
         r = ray(ir-1)
       endif
       if (yoscr(is,ir).lt.0) then
         phi = pi/npan(ir)
       elseif (yoscr(is,ir).gt.0) then
         phi = -pi/npan(ir)
       endif
       xscr(is,ir) = (r*cos(phi)-r0+(r**2-r0**2)/4/focus*t0)   &
         /sqrt(1+t0**2) !+ xoscr(is,ir)
       yscr(is,ir) = r*sin(phi) !+ yoscr(is,ir)
1999    format (a,i1,a,i1,a,f10.6)
    enddo
  enddo
  ! 
  ! Fit mode to derived panels
  nmod = 4
  do ir = 1, nring
    do ip=1,npan(ir)
      do is = 1, nscr(ir)
        xx = xscr(is,ir)
        yy = yscr(is,ir)       
        nm = nmod
        do im=1, nm
          a1(is,im) = fmode(im,ir,xx,yy)
          a2(is,im) = fmode(im,ir,xx,yy)
        enddo
        b1(is) = def1(ir, ip, is)
        b2(is) = def2(ir, ip, is)
      enddo
      call mth_fitlin ('MODE',nscr(ir),nm,a1,b1,mm,sig)
      call mth_fitlin ('MODE',nscr(ir),nm,a2,b2,mm,sig)
      do k=1,nm
        am1(k,ir,ip) = b1(k)
        am2(k,ir,ip) = b2(k)
      enddo
    enddo
  enddo
  !
  ! Now compute deformation at actual position
  call set_panels(type_iram30m)
  do ir = 1, nring
    do ip = 1, npan(ir)
      do is = 1, nscr(ir)
        call zpanel(xscr(is,ir),yscr(is,ir),ir,ip,am1,ae,zpan,epan,nmod)
        def1(ir,ip,is) = zpan
        call zpanel(xscr(is,ir),yscr(is,ir),ir,ip,am2,ae,zpan,epan,nmod)       
        def2(ir,ip,is) = zpan

      enddo
    enddo
  enddo
  error = sic_getlun(lun1).ne.1
  !
  ! Now write correctly formatted file
  open(lun1, file='gravDef_30m.dat',status='UNKNOWN')
  write(lun1,*) "! computed with compute_gravitation_file"
  write(lun1,*) "! iring ipanel iscrew x y dz0 dz90"
  do ir = 1, nring
    do ip = 1, npan(ir)
      do is = 1, nscr(ir)
        write(lun1,'(i2,1x,i3,1x,i1,1x,1pg3.1,1x,1pg3.1,1x,1pg11.3,1x,1pg11.3)') ir, ip, is, 1, 1, def1(ir,ip,is), def2(ir,ip,is)
      enddo
    enddo 
  enddo
  close(lun1)
  call sic_frelun(lun1)
  stop
end program
