!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! TELCAL routines related to focus
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine fit_xy_polynomial(line,error)
  use telcal_interfaces, except_this=>fit_xy_polynomial
  use fit_definitions
  use gildas_def
  use gkernel_interfaces
  use gbl_format
  use gkernel_types
  !----------------------------------------------------------------------
  ! @ private
  ! TELCAL support routine for command:
  !     POLYNOM Degree Xvar Yvar Wvar
  ! Purpose: Computes the weighted least-square polynomial fit of a given
  !          degree
  !----------------------------------------------------------------------
  character(len=*), intent(in)  :: line  ! Command line
  logical,          intent(out) :: error ! Error flag
  ! Local
  type(simple_1d) :: dat
  type(fit_fun) :: fun
  type(sic_descriptor_t) :: xinca,yinca
  integer(kind=address_length) :: ipx,ipy
  integer memory(1)
  integer nc,ndeg,ixy,inx,iny,ier,i
  logical found
  character(16) :: string,var
  character(80) :: chain
  !
  ! 0. Initialization
  !
  error = .false.
  !
  ! 1. Data vector setup
  !
  ! Degree(mandatory)
  call sic_i4(line,0,1,ndeg,.true.,error)
  if(error) return
  ! Xvar(mandatory)
  call sic_ch(line,0,2,var,nc,.true.,error)
  if(error) return
  call sic_descriptor(var,xinca,found)
  if(.not.found) then
     chain = 'E-POLYNOM,  Unknown variable '//var
     call gagout(chain)
     error = .true.
     return
  endif
  if (xinca%type.eq.fmt_r8) then
     inx = xinca%size/2
  elseif (xinca%type.eq.fmt_r4) then
     inx = xinca%size
  endif
  !
  ! Yvar(mandatory)
  call sic_ch(line,0,3,var,nc,.true.,error)
  if(error) return
  call sic_descriptor(var,yinca,found)
  if(.not.found) then
     chain = 'E-POLYNOM,  Unknown variable '//var
     call gagout(chain)
     error = .true.
     return
  endif
  if (yinca%type.eq.fmt_r8) then
     iny = yinca%size/2
  elseif (yinca%type.eq.fmt_r4) then
     iny = yinca%size
  endif
  if(iny.ne.inx) then
     call gagout('E-POLYNOM, Arrays do not match')
     error = .true.
     return
  endif
  ixy = inx
  !
  dat%n = ixy
  allocate(dat%x(dat%n),dat%y(dat%n),stat=ier)
  if(ier.ne.0) then
     error = .true.
     return
  endif
  !
  ipx = gag_pointer(xinca%addr,memory)
  ipy = gag_pointer(yinca%addr,memory)
  if (xinca%type.eq.fmt_r8) then
     call r8tor8(memory(ipx), dat%x, ixy)
  else
     call r4tor8(memory(ipx), dat%x, ixy)
  endif
  if (yinca%type.eq.fmt_r8) then
     call r8tor8(memory(ipy), dat%y, ixy)
  else
     call r4tor8(memory(ipy), dat%y, ixy)
  endif
  !
  ! Wvar(optional)
  allocate(dat%w(dat%n),stat=ier)
  if(ier.ne.0) then
     error = .true.
     return
  endif
  if(sic_present(0,4)) then
     print *,'Using input weights'
     call sic_ch(line,0,4,var,nc,.true.,error)
     if(error) return
     call sic_descriptor(var,yinca,found)
     if(.not.found) then
        chain = 'E-POLYNOM,  Unknown variable '//var
        call gagout(chain)
        error = .true.
        return
     endif
     if (yinca%type.eq.fmt_r8) then
        iny = yinca%size/2
     elseif (yinca%type.eq.fmt_r4) then
        iny = yinca%size
     endif
     if(iny.ne.inx) then
        call gagout('E-POLYNOM, Arrays do not match')
        error = .true.
        return
     endif
     ipy = gag_pointer(yinca%addr,memory)
     if(yinca%type.eq.fmt_r8) then
        call r8tor8(memory(ipy), dat%w, ixy)
     else
        call r4tor8(memory(ipy), dat%w, ixy)
     endif
  else
     print *,'Setting equal weights'
     dat%w = 1.0
  endif
  !
  ! 2. Fitting function setup
  !
  nullify(fun%par)
  call null_function(fun)
  fun%name   = 'POLYNOMIAL'
  fun%method = 'SLATEC'
  fun%npar   = ndeg+1
  allocate(fun%par(fun%npar),stat=ier)
  if(ier.ne.0) then
     error = .true.
     return
  endif
  ! Names
  do i =1, fun%npar
     call null_parameter(fun%par(i))
     write(string,'(a12,i2)') 'Coefficient ', i-1
     fun%par(i)%name = string
  enddo
  !
  ! 3. Fitting
  !
  call fit_polynomial(dat,fun)
  !
  ! 4. Freeing memory
  !
  deallocate(dat%x,dat%y,dat%w,fun%par)
  !
end subroutine fit_xy_polynomial
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine solve_focus(line,error)
  use telcal_interfaces, except_this=>solve_focus
  use fit_definitions
  use focus_definitions
  !----------------------------------------------------------------------
  ! @ public
  ! TELCAL support routine for command
  !    FOCUS [Polynomial|Lorentzian|Gaussian]
  !----------------------------------------------------------------------
  character(*), intent(in)  :: line  ! Command line
  logical,      intent(out) :: error ! Error flag
  !
  ! Local variables
  character(1) :: method
  integer :: idir,ndir,i,n
  real :: min,max
  !
  method = zfocus%fun(1)%name
  call sic_ke(line,0,1,method,n,.false.,error)
  if(error) return
  !
  ndir = 2
  !
  do idir=1,ndir
    if(method.eq.'L') then
      zfocus%fun(idir)%name   = 'LORENTZIAN'
      zfocus%fun(idir)%par(1)%guess = 1.0
      zfocus%fun(idir)%par(2)%guess = 0.0
      zfocus%fun(idir)%par(3)%guess = 3.0
    else if(method.eq.'G') then
      zfocus%fun(idir)%name   = 'GAUSSIAN'
      zfocus%fun(idir)%par(1)%guess = 1.0
      zfocus%fun(idir)%par(2)%guess = 0.0
      zfocus%fun(idir)%par(3)%guess = 3.0
    else ! if(method.eq.'P') then
      zfocus%fun(idir)%name   = 'POLYNOMIAL'
    endif
     if(zfocus%fun(idir)%flag.ne.-1000) then
        call fit_1d(zfocus%dat(idir),zfocus%fun(idir),.true.)
        min = zfocus%dat(idir)%x(1)
        max = zfocus%dat(idir)%x(zfocus%dat(idir)%n)
        do i=1,zfocus%sol(idir)%n
           zfocus%sol(idir)%x(i) = min+(max-min)*(i-1)/(zfocus%sol(idir)%n-1)
        enddo
        call get_profile(zfocus%fun(idir),zfocus%sol(idir))
     endif
  enddo
  call exec_program('sic\@ plot-focus.telcal')
  !
end subroutine solve_focus
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine init_focus(ndat)
  use telcal_interfaces, except_this=>init_focus
  use focus_definitions
  !----------------------------------------------------------------------
  ! @ public
  ! TELCAL support routine for focus
  ! Purpose:
  !----------------------------------------------------------------------
  integer, intent(in) :: ndat ! Number of data points
  !
  ! Local variables
  integer   :: ndir,npar,nsol,i,idir,ier
  character :: string*16
  !
  ndir = 2
  npar = 3   ! Parabola, Lorentzian or Gaussian fitting
  nsol = 100 ! Arbitrary value
  !
  zfocus%sys = 0
  zfocus%dir(1) = '+FCZ'
  zfocus%dir(2) = '-FCZ'
  !
  do idir=1,ndir
     !
     call null_simple_1d(zfocus%dat(idir))
     zfocus%dat(idir)%n = ndat
     allocate(zfocus%dat(idir)%x(ndat),zfocus%dat(idir)%y(ndat),zfocus%dat(idir)%w(ndat),stat=ier)
     zfocus%dat(idir)%x = 0.
     zfocus%dat(idir)%y = 0.
     zfocus%dat(idir)%w = 1.
     !
     call null_simple_1d(zfocus%sol(idir))
     zfocus%sol(idir)%n = nsol
     allocate(zfocus%sol(idir)%x(nsol),zfocus%sol(idir)%y(nsol),zfocus%sol(idir)%w(nsol),stat=ier)
     zfocus%sol(idir)%x = 0.
     zfocus%sol(idir)%y = 0.
     zfocus%sol(idir)%w = 1.
     !
     call null_function(zfocus%fun(idir))
     zfocus%fun(idir)%name   = 'LORENTZIAN'  ! or 'POLYNOMIAL', or 'GAUSSIAN'
     zfocus%fun(idir)%method = 'SLATEC'
     zfocus%fun(idir)%npar = npar
     allocate(zfocus%fun(idir)%par(npar),stat=ier)
     do i=1,npar
        call null_parameter(zfocus%fun(idir)%par(i))
        write(string,'(a12,i2)') 'COEFFICIENT ', i-1
        zfocus%fun(idir)%par(i)%name = string
     enddo
     !
  enddo
  !
end subroutine init_focus
!
subroutine free_focus
  use telcal_interfaces, except_this=>free_focus
  use focus_definitions
  !----------------------------------------------------------------------
  ! @ private
  ! TELCAL support routine for focus
  ! Purpose:
  !----------------------------------------------------------------------
  !
  ! Local variables
  integer   :: ndir,idir
  !
  ndir = 2
  !
  do idir=1,ndir
     call null_simple_1d(zfocus%dat(idir))
     call null_simple_1d(zfocus%sol(idir))
     call null_function(zfocus%fun(idir))
  enddo
  !
end subroutine free_focus
!
subroutine define_sic_focus
  use telcal_interfaces, except_this=>define_sic_focus
  use focus_definitions
  use gildas_def
  use gkernel_interfaces
  !----------------------------------------------------------------------
  ! @ private
  ! TELCAL support routine for focus
  ! Purpose:
  !----------------------------------------------------------------------
  !
  ! Local variables
  integer :: idir,ipar,ndat,nsol,ndir,npar
  character(1) :: cdir,cpar
  logical :: error
  !
  ndir = 2
  !
  if (.not.sic_varexist('zfocus')) then
     call sic_defstructure('zfocus%',.true.,error)
     call sic_def_inte('zfocus%sys',zfocus%sys,0,0,.false.,error)
     call sic_def_charn('zfocus%dir',zfocus%dir,1,2,.false.,error)
     do idir=1,ndir
        write(cdir,1000) idir
        call sic_defstructure('zfocus%dat'//cdir,.true.,error)
        call sic_defstructure('zfocus%sol'//cdir,.true.,error)
        call sic_defstructure('zfocus%fun'//cdir,.true.,error)
        !
        ndat = zfocus%dat(idir)%n
        call sic_def_inte('zfocus%dat'//cdir//'%n',zfocus%dat(idir)%n,0,0,.false.,error)
        call sic_def_dble('zfocus%dat'//cdir//'%x',zfocus%dat(idir)%x,1,ndat,.false.,error)
        call sic_def_dble('zfocus%dat'//cdir//'%y',zfocus%dat(idir)%y,1,ndat,.false.,error)
        call sic_def_dble('zfocus%dat'//cdir//'%w',zfocus%dat(idir)%w,1,ndat,.false.,error)
        !
        nsol = zfocus%sol(idir)%n
        call sic_def_inte('zfocus%sol'//cdir//'%n',zfocus%sol(idir)%n,0,0,.false.,error)
        call sic_def_dble('zfocus%sol'//cdir//'%x',zfocus%sol(idir)%x,1,nsol,.false.,error)
        call sic_def_dble('zfocus%sol'//cdir//'%y',zfocus%sol(idir)%y,1,nsol,.false.,error)
        call sic_def_dble('zfocus%sol'//cdir//'%w',zfocus%sol(idir)%w,1,nsol,.false.,error)
        !
        npar = zfocus%fun(idir)%npar
        call sic_def_char('zfocus%fun'//cdir//'%name',  zfocus%fun(idir)%name,.true.,error)
        call sic_def_char('zfocus%fun'//cdir//'%method',zfocus%fun(idir)%method,.true.,error)
        call sic_def_real('zfocus%fun'//cdir//'%chi2',  zfocus%fun(idir)%chi2,0,0,.false.,error)
        call sic_def_real('zfocus%fun'//cdir//'%rms',   zfocus%fun(idir)%rms,0,0,.false.,error)
        call sic_def_inte('zfocus%fun'//cdir//'%flag',  zfocus%fun(idir)%flag,0,0,.false.,error)
        call sic_def_inte('zfocus%fun'//cdir//'%ncall', zfocus%fun(idir)%ncall,0,0,.false.,error)
        call sic_def_inte('zfocus%fun'//cdir//'%npar',  zfocus%fun(idir)%npar,0,0,.true.,error)
        do ipar=1,npar
           write(cpar,1000) ipar
           call sic_def_char('zfocus%fun'//cdir//'%par'//cpar//'%name', zfocus%fun(idir)%par(ipar)%name,.true.,error)
           call sic_def_dble('zfocus%fun'//cdir//'%par'//cpar//'%value',zfocus%fun(idir)%par(ipar)%value,0,0,.false.,error)
           call sic_def_dble('zfocus%fun'//cdir//'%par'//cpar//'%error',zfocus%fun(idir)%par(ipar)%error,0,0,.false.,error)
           call sic_def_dble('zfocus%fun'//cdir//'%par'//cpar//'%mini', zfocus%fun(idir)%par(ipar)%mini,0,0,.false.,error)
           call sic_def_dble('zfocus%fun'//cdir//'%par'//cpar//'%maxi', zfocus%fun(idir)%par(ipar)%maxi,0,0,.false.,error)
           call sic_def_logi('zfocus%fun'//cdir//'%par'//cpar//'%fixed',zfocus%fun(idir)%par(ipar)%fixed,.false.,error)
        enddo
     enddo
  endif
  !
1000 format(i1)
end subroutine define_sic_focus
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
