!
!  Robust mean
!
!  Copyright © 2001 - 2017 F.Hroch (hroch@physics.muni.cz)
!
!  This file is part of Munipack.
!
!  Munipack is free software: you can redistribute it and/or modify
!  it under the terms of the GNU General Public License as published by
!  the Free Software Foundation, either version 3 of the License, or
!  (at your option) any later version.
!
!  Munipack is distributed in the hope that it will be useful,
!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!  GNU General Public License for more details.
!
!  You should have received a copy of the GNU General Public License
!  along with Munipack.  If not, see <http://www.gnu.org/licenses/>.
!
!

module robustmean

  ! This module provides rmean subroutine for estimation of robust mean.
  !
  ! rmean should be called as:
  !
  !  call rmean(x,t,dt,sig,reliable)
  !
  ! on input:
  !   x - array of data values to be estimated
  !
  ! on output are estimated:
  !   t - robust mean
  !   dt - standard error
  !   sig (optional) - standard deviation
  !   reliable (optional) - indicates reliability of result
  !
  ! The given results  means that a true value X of the sample x can be,
  ! with 70% probability, found in interval
  !
  !         t - dt  <  X  <  t + dt
  !
  ! and the distribution can be described by Normal distribution N(t,sig),
  ! but studentising is not applied.
  !
  !
  ! Robust estimators has been prepared on base of
  !  * Hogg in Launer, Wilkinson: Robustness in Statistics
  !  * Hubber: Robust Statistics
  !  * my experiences

  implicit none

  ! print debug information ?
  logical, parameter, private :: verbose = .false.
  logical, parameter, private :: debug = .false.

  ! estimate jacobian by differences or derivations ?
  logical, parameter, private :: analytic = .true.

  ! which method will be used?
  integer, parameter, private :: method = 5

  ! winsorize data?
  logical, parameter, private :: winsorize = .true.

  ! numerical precision of real numbers
  integer, parameter, private :: dbl = selected_real_kind(15)

  ! 50% quantil of N(0,1)
  real(dbl), parameter, private :: q50 = 0.6745

  ! dispersion of Hubber distribution
  real(dbl), parameter, private :: beta = 0.8200

  ! data buffers
  real(dbl), dimension(:), allocatable, private :: xdata,r,psi,dpsi
  real(dbl), private :: zsig

  integer, private :: nprint = 0

  ! this definition provides the interface described above
  interface rmean
     module procedure rmean_double, rmean_single
  end interface rmean

  private :: funder, fundif, difjac, loglikely, rmean_double, rmean_single, &
       funds, funis, difjac3, graph, zero_graph, zmean, zfun, rhalley, rnewton

contains

  subroutine rmean_double(x,t,dt,sig,reliable)

    ! simple interface for rmean

    real(dbl), dimension(:),intent(in) :: x
    real(dbl), intent(out) :: t,dt
    real(dbl), intent(out), optional :: sig
    logical, intent(out), optional :: reliable

    real(dbl), parameter :: macheps = epsilon(t)
    real(dbl) :: s, eps
    integer :: n,istat
    logical :: reli

    n = size(x)

    if( n == 0 ) then

       ! results are undefined
       t = 0
       s = 0
       dt = 0
       reli = .false.
       goto 666

    else if( n == 1 ) then

       ! dispersion and std.err. are undefined
       t = x(1)
       s = 0
       dt = 0
       reli = .false.
       goto 666

    else if( n == 2 ) then

       t = (x(1) + x(2))/2
       s = abs(x(1) - x(2))/2
       reli = .true.
       dt = s
       goto 666

    end if
    ! we are suppose that  n > 2  below...

    ! initial values
    call rinit(x,t,s)
    if( verbose ) write(*,*) "rinit: t, s= ",t, s

    if( abs(s) <= 2*macheps ) then
       ! If important part of data has identical values, within machine precision,
       ! their median is zero. The application of robust methods is uselles
       ! (and problematic while s == 0).
       !
       ! If all the data are identical, result can be considered as reliable.
       s = sum(abs(x-t)) / n
       reli = s < 10*macheps

       dt = s / sqrt(real(n))
       goto 666
    end if

    ! no-/print debug informations, parameter of lmder, lmdif subroutines
    if( verbose ) then
       nprint = 1
    else
       nprint = 0
    end if

    ! set-up required precision
    eps = 10*macheps*n*max(abs(t),1.0)

    ! allocate working arrays
    allocate(r(n),psi(n),dpsi(n))

    ! Robust solvers
    !
    ! Both the parameters t,s are initiated by rinit and the estimates
    ! are used by all the solvers. See also longer description
    ! given within comments of the routines.
    !
    ! Generally, only rmean5 (reliable, precise) is recommended
    ! for regular use. Please consider rmean[1-4], rnewton and rhalley
    ! as dead way alternatives.

    select case(method)

    case(0)
       ! Location is estimated by robust function, scale by MAD
       call rnewton(x,t,dt,s,eps,reli)

    case(1)
       ! Both location and scale is estimated by robust function
       call rmean1(x,t,dt,s,eps,istat)

    case(2)
       ! Both location and scale are estimated by maximum likelihood.
       call rmean2(x,t,dt,s,eps,istat)

    case(3)
       ! Location is estimated by maximum likelihood, scale by entropy.
       call rmean3(x,t,dt,s,eps,istat)

    case(4)
       ! Location is estimated by maximum likelihood, scale by entropy.
       call rmean4(x,t,dt,s,eps,istat)

    case(5)
       ! Reliable version of rmean4
       call rmean5(x,t,dt,s,eps,reli)

    case default
       ! No default is here, initial estimates will be used
       continue
       dt = s / sqrt(real(n))
       reli = .false.

    end select

    deallocate(r,psi,dpsi)

666 continue

    ! fill-up required items
    if( present(sig) ) sig = s
    if( present(reliable) ) reliable = reli

  end subroutine rmean_double

  subroutine rmean_single(x,t,dt,sig,reliable)

    ! this single precision integrace via double precision is a little bit slower,
    ! than genuine single one, although no code is doubled.

    real, dimension(:),intent(in) :: x
    real, intent(out) :: t,dt
    real, intent(out), optional :: sig
    logical, intent(out), optional :: reliable

    real(dbl), dimension(:), allocatable :: y
    real(dbl) :: d,dd,s
    logical :: reli

    allocate(y(size(x)))
    y = x
    call rmean_double(y,d,dd,s,reli)
    deallocate(y)
    t = real(d)
    dt = real(dd)
    if( present(sig) ) sig = real(s)
    if( present(reliable) ) reliable = reli

  end subroutine rmean_single


  !-------------------------------------------------------------------

  subroutine rinit(x,t,s)

    ! initial estimate of parameters

    use medians
    use qmeans

    real(dbl), dimension(:),intent(in) :: x
    real(dbl), intent(out) :: t,s

    real(dbl) :: mad,med
    integer :: n

    n = size(x)

    if( n > 50 ) then

       ! This choice of the threshold, used for median computation method,
       ! assumes that odd and even elements in sequence are the same (within
       ! requested precision). Note that qmed is the fastest known algorithm
       ! (~ 2*n) while median is simpler and slower (~ n*log(n)).

       med = qmedian(x)
       mad = qmedian(abs(x - med))

       ! correction for Normal distribution
       s = mad / q50
       t = med

    else if( n > 13 ) then

       ! correct way to compute median (two middle values)
       med = median(x)
       mad = median(abs(x - med))

       ! correction for Normal distribution
       s = mad / q50
       t = med

    else if( n > 2 ) then

       ! compute parameters by empirical CDF quantilies
       call qmean(x,t,s)

    else if( n == 2 ) then

       t = (x(1) + x(2))/2
       s = abs(x(1) - x(2))/2

    else if( n == 1 ) then

       t = x(1)
       s = 0

    else

       t = 0
       s = 0

    end if

  end subroutine rinit

  !-----------------------------------------------------------------------

  subroutine rnewton(x,t,dt,sig,tol,converge)

    ! One has been the default robust estimator for a long time.
    ! The main difference from other rmeans is that only the location
    ! parameter (t) is estimated (by Newton's iterations).
    ! The scale has its unchanged estimated value during all computations.
    ! This estimator may give less precise results (up to 10%), but one is
    ! significantly faster than all others rmedians (up to 10x to rmean5).
    !
    ! The reliability of result randomly depends on an initial estimate of t.
    ! There is no reliability if the estimate is out of convergence region
    ! of Newtons method or the scale sig is far from its right value. The region
    ! of convergence depends on robust function in use. Two most frequent
    ! cases are Huber's function which is monotone and converges in full range
    ! and Tukey's (or Hampel's) function which converges only between extremes
    ! of the function.
    !
    ! rmean5 is much more reliable and much more slower.

    use rfun

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t, sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    logical, intent(out) :: converge

    integer, parameter :: maxit = precision(t)
    ! Number of iterations is given by numerical precision.
    ! We belives, that one order is reached by each iteration.

    integer :: n,it
    real(dbl) :: d,s,f,f2,df

    converge = .false.
    dt = -1
    if( .not. (sig > 0) ) return

    n = size(x)
    s = sig

    ! iterations
    do it = 1, maxit

       r = (x - t)/s
       call tukeys(r,psi)
       call dtukeys(r,dpsi)
       f = sum(psi)
       df = sum(dpsi)

       ! df can be nearly zero when the important part of data has large
       ! deviations (above 3.4 for Hampel's piecewise or 1.6 for Tukey's
       ! bi-weight), this si "re-descending M-estimate" function problem.
       ! In this case, the computation is immediatelly interrupted and
       ! result should by considered as unreliable.
       if( df <= epsilon(df) ) exit

       ! corrector for mean
       d = s * f / df

       ! update location
       t = t + d

       if( debug ) &
            write(*,'(a,i2,1pg20.10,1p3g10.2)') "mean, inc., f, f': ",it,t,d,-f/s,df/s**2

       ! exit of iterations: the absolute errot must be at least |d| < tol
       converge = abs(d) < tol
       if( converge ) exit

    enddo

    ! estimation of standard deviation, K-factor is omited
    if( converge ) then
       f2 = sum(psi**2)
       if( df > 0 .and. f2 > 0 ) then
          sig = s !*
          dt = sig * sqrt(f2/df**2*n/(n-1))
       end if
    end if

  end subroutine rnewton


  subroutine rhalley(x,t,dt,sig,tol,converge)

    ! This is a faster version of Newton's method with cubicall
    ! convergention. It has simillar properties as rnewton().
    ! One is reccomended to be used only for fast determination
    ! of precise root.

    use rfun

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t, sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    logical, intent(out) :: converge

    integer, parameter :: maxit = precision(t)
    ! Number of iterations is given by numerical precision.
    ! We belives, that one order is reached by each iteration.

    integer :: n,it
    real(dbl) :: d,s,f,f2,df,d2f
    real(dbl), dimension(:), allocatable :: d2psi

    converge = .false.
    dt = -1
    if( .not. (sig > 0) ) return

    n = size(x)
    s = sig

    ! iterations
    allocate(d2psi(n))
    do it = 1, maxit

       r = (x - t)/s
       call tukeys(r,psi)
       call dtukeys(r,dpsi)
       call d2tukeys(r,d2psi)
       f = - sum(psi) / s
       df = sum(dpsi) / s**2
       d2f = - sum(d2psi) / s**3

       if( df <= epsilon(df) ) exit

       ! corrector for mean
       d = f / (df * (1 - (f*d2f)/(2*df)))

       ! update location
       t = t - d

       if( debug ) write(*,'(a,1pg15.5,1p3g12.3)') "mean,incr,f,df,d2f: ",t,d,df,d2f

       ! exit of iterations: the absolute errot must be at least |d| < tol
       converge = abs(d) < tol
       if( converge ) exit

    enddo

    ! estimation of standard deviation, K-factor is omited
    if( converge ) then
       f2 = sum(psi**2)
       if( df > 0 .and. f2 > 0 ) then
          df = df * s**2
          sig = s*sqrt(f2/df*n/(n-1))
          dt = sig / sqrt(df)
       end if
    end if

    deallocate(d2psi)

  end subroutine rhalley


! ----------------------------------------------------------------------------


  subroutine rmean1(x,t,dt,sig,tol,istat)

    ! One estimates mean and standard deviation by minimizing
    ! both mean and scale together, the equations are solved
    ! by combination of the iteractions and Newton's method
    ! as reccomends Huber (1980), p. 147 (6.7 the computation
    ! of M-estimates)
    !
    ! t   is solution on output
    ! dt  is standard error of t
    !
    !
    ! IMPORTANT
    !
    ! The procedure works well when covariance matrix is nearly
    ! diagonal because minimizes scale and mean separately in
    ! theirs directions. By another words, the eigenvectors are
    ! orthogonal. The condition is sometimes violated when (perhaps)
    ! data has a distribution unlike the Normal. In this case,
    ! the solution can converge to a false point. This is reason
    ! for use rmean3 in any case! Second reason is the non-convergence
    ! when initial estimate is behind horizon of convergence.
    !
    ! Consider the implementation for study purposes only.
    ! It is unmaintained for long time.

    use rfun

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t,sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    integer, intent(out) :: istat

    integer, parameter :: maxit = 2*precision(t)
    integer :: n,i,it
    real(dbl) :: d,t0,s,s0,sum1,sum2,sum3,absd,xm,var,kcorr
    real(dbl), dimension(:), allocatable :: r,p,dp

    istat = 0
    n = size(x)
    s = sig

    ! 99% Winsorisation
    allocate(xdata(n),r(n),p(n),dp(n))
    if( winsorize ) then
       r = (x - t)/s
       where( abs(r) < 3.0 )
          xdata = x
       elsewhere
          xdata = t + sign(3.0*s,r)
       end where
    else
       xdata = x
    end if

    ! save intial estimate
    t0 = t
    s0 = s

    ! iterations
    it = 0
    do i = 1,maxit

       r = (x - t)/s
       call hubers(r,p)
       call dhubers(r,dp)
       sum1 = sum(p)
       sum2 = sum(dp)
       sum3 = sum(p**2)

       ! sum2 can be nearly zero or negative when the important part of data
       ! has large deviations (above 3.4 for Hampel's piecewise or 1.6 for Tukey's
       ! bi-weight), this si "re-descending M-estimate" function problem.
       ! In this case, and also when s*sum1 is smaller (which will indicate
       ! no convergence, perhaps), the computation is stoped and initial estimate
       ! of t,s is used.
       if( sum2 < tol .or. sum2 > s*sum1 ) then
          istat = 3
          exit
       end if

       ! corrector for mean
       d = s*sum1/sum2
       t = t + d

       ! corrector for scale
       s = sqrt(sum3*s**2/(n-1)/beta)

       if( verbose ) write(*,'(a,3g15.5)') "mean, increment, scale: ",t,d,s

       ! exit of iterations:
       absd = abs(d)
       if( absd/abs(t) < tol .or. absd < tol ) exit
       ! the relative error must be at least |d|/|t| < xrel
       ! the absolute errot must be at least |d| < xabs

       it = it + 1
    enddo

    if( istat == 3 ) then
       ! fail of convergence
       t = t0
       s = s0
    end if

    ! estimation of standard deviation, K-factor is included (~1 + 1/n*var/dmean)
    if( sum2 > tol ) then

       xm = sum2/n
       var = sum((dp - xm)**2)/n
       kcorr = (1 + var/xm**2/n)

       sig = s*sqrt(kcorr*sum3/sum2*n/(n-1))
       dt = sig / sqrt(sum2)

    else
       sig = s
       dt = sig/sqrt(real(n))
    end if

    deallocate(xdata,r,p,dp)

  end subroutine rmean1

! ----------------------------------------------------------------------------

  subroutine rmean2(x,t,dt,sig,tol,istat)

    ! minimizes parameters of a robust function with joint estimate of mean and scale
    ! the parameters maximises likelihood getting the most probable solution
    ! This is givest the best solution for outliers free data. The winsorizing
    ! is used for correcting the outliers.
    !
    ! t   is estimation of mean
    ! dt  is standard error of t
    ! sig is standard deviation of sample
    !
    ! rmean3 works faster, more robust and no winsorizing is required!

    use rfun
    use minpacks
    use neldermead

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(inout) :: t,sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    integer, intent(out) :: istat

    ! limit to detect non-convergent series, reccomended values: >100
    real(dbl), parameter :: siglim = 1000.0

    integer :: n,m,info,nprint,icount,numres,ifault,i
    real(dbl) :: s,c,sum2,sum3,bcorr,smin,reqmin
    real(dbl), dimension(:), allocatable :: r,p,dp
    real(dbl), dimension(2) :: u,u0,du

    istat = 0
    n = size(x)
    s = sig

    allocate(xdata(n),r(n))

    ! winsorizing, outliers are replaced by x-sigma values
    if( winsorize ) then
       r = (x - t)/s
       c = winscut*s
       where( abs(r) < winscut )
          xdata = x
       elsewhere
          xdata = t + sign(c,r)
       end where
       if( verbose ) then
          write(*,'(a,f0.2)') "Winsorising at cut: ",winscut
          write(*,*) " #, original,  replacement, sig"
          m = 0
          do i = 1,n
             if( abs(xdata(i) - x(i)) > 10*epsilon(x) ) then
                m = m + 1
                !             write(*,'(i4,2g15.4,g10.3)') i,x(i),xdata(i),r(i)
             end if
          end do
          write(*,'(a,i0,a,f0.1,a)') 'Replacements: ',m,'x (',100*float(m)/float(n),'%)'
       end if
    else
       xdata = x
    end if

    ! Location of proper minimum. The parameters are initiated by a non-M-estimate
    ! method and we are expecting a deviated estimate. The M-estimation
    ! is derived in two steps:
    !  1. approximate location of minimum of negative log(likelihood) function
    !     without use of derivates
    !  2. precise estimate by a gradient method
    !
    ! The gradient method can't be used directly because the estimation of scale
    ! frequently lies near (or above) horizon of convergence.

    ! Approximate location of -log(L), the function has one global minimum
    reqmin = s**2
    u = (/t,s/)
    u0 = u
    du = (/ s, s /)
    call nelmin(loglikely,size(u),u0,u,smin,reqmin,du,10,10000,icount,numres,ifault)
    if( verbose ) write(*,'(a,2i3,3g15.5)') 'Approximate solution: ',ifault,icount,u

    if( ifault /= 0 ) then
       ! no global convergence occured

       if( verbose ) &
            write(*,*) "Warning: Finished prematurely without likelihood convergence."

       t = u0(1)
       sig = huber_sigcorr*u0(2)
       dt = sig/sqrt(real(n))
       goto 666

    end if
    u0 = u

    ! Precise solution, a gradient method leads to two minimums: the right, in infinity.
    ! We are suppose, that the previous estimation of u is very nearly to the right.
    if( analytic ) then
       call lmder2(funder,u,tol,nprint,info)
    else
       call lmdif2(fundif,u,tol,nprint,info)
    end if

    if( abs(u(2))/s < siglim .and. info /= 5 ) then
       ! the minimum localised successfully, update estimates
       t = u(1)
       s = u(2)

    else
       ! No convergence detected, so the initial values are used.
       ! The approach is less precise but works better for any unexpected data:
       !  * many same values and one outlier, we are in doubts what is the right result

       if( verbose ) &
            write(*,*) "Warning: Finished prematurely without convergence in gradient."
       t = u0(1)
       sig = huber_sigcorr*u0(2)
       dt = sig/sqrt(real(n))
       goto 666

    end if


    ! Estimation of dispersion (variance) and uncertainty of mean
    ! With the extimations, two notes are related:
    !
    !  * the variance estimated by scale parameter of robust function is
    !    usually smaller than standard deviations because the second
    !    moments of the functions are smaller. For Huber's function,
    !    the second moment is 1.18945 (computed as int(p(normal)*x**2))
    !    To get asymptotical estimation of dispersion from scale, we are
    !    appling the factor.
    !
    !  * The estimation of uncertainity is by Hubber(1981), formula (8.8)
    !    The sum2 has meaning: "which fraction of values lies inside -1.345...
    !    .. 1.345 interval" (in Huber's function). The sum3 is robust equivalent
    !    of mean of square deviations.


    allocate(p(n),dp(n))
    r = (x - t)/s
    call hubers(r,p)
    call dhubers(r,dp)
    bcorr = real(n)/real(n-1) ! Bessel's correction, real() to prevent numerical overflow
    sum2 = sum(dp)
    sum3 = sum(p**2)
    deallocate(p,dp)

    sig = s

    if( sum2 > tol ) then
       ! Huber(1981), Section 7.6 - Asymptotic convariances ....
       dt = sig*sqrt(bcorr*sum3/sum2**2)
    else
       ! the alternative for the scale nearly to machine epsilon
       dt = sig/sqrt(real(n))
    end if

666 continue

    deallocate(xdata,r)

  end subroutine rmean2


  function loglikely(p)

    use rfun

    real(dbl), dimension(:), intent(in) :: p
    real(dbl) :: loglikely
    real(dbl), dimension(:), allocatable :: r,f
    real(dbl) ::t,s
    integer :: n

    t = p(1)
    s = p(2)

    if( s < epsilon(s) ) then
        loglikely = 1e-3*huge(loglikely)
       return
    end if

    n = size(xdata)
    allocate(r(n),f(n))

    r = (xdata - t)/s
    call ihubers(r,f)
!    f = r**2/2
    loglikely = sum(f) + n*log(s)*beta

    deallocate(r,f)

  end function loglikely


  subroutine funder(m,np,p,fvec,fjac,ldfjac,iflag)

    use rfun

    integer, intent(in) :: m,np,ldfjac
    integer, intent(inout) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(m), intent(out) :: fvec
    real(dbl), dimension(ldfjac,np), intent(out) :: fjac
    real(dbl), dimension(:), allocatable :: r,f,df,rd
    real(dbl), dimension(2) :: fv
    real(dbl), dimension(2,2) :: dfjac
    real(dbl) :: s
    integer :: n

    if( iflag == 0 ) then

       write(*,'(4g15.5)') p,fvec

       if( debug ) then
          write(*,*) 'fjac:',fjac(1,:)
          write(*,*) 'fjac:',fjac(2,:)

          call difjac(p(1),p(2),dfjac)
!          write(*,*) 'djac:',dfjac(1,:)
!          write(*,*) 'djac:',dfjac(2,:)
       end if

       return
    end if

    n = size(xdata)
    allocate(r(n),f(n))

    s = p(2)
    r = (xdata - p(1))/s
    call hubers(r,f)
!    f = r

    fv(1) = sum(f)
    fv(2) = sum(f*r) - n*beta

    if( iflag == 1 ) then

       fvec = fv / s

    else if( iflag == 2 ) then

       allocate(df(n),rd(n))
       call dhubers(r,df)
!       df = 1

       rd = df*r

       fjac(1,1) = sum(df)
       fjac(1,2) = fv(1) + sum(rd)
       fjac(2,1) = fjac(1,2)
       rd = rd*r
       fjac(2,2) = 2*fv(2) + sum(rd) + n*beta

       fjac = - fjac / s**2

       deallocate(rd,df)

    end if


    deallocate(r,f)

  end subroutine funder


  subroutine fundif(m,np,p,fvec,iflag)

    use rfun

    integer, intent(in) :: m,np
    integer, intent(in out) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(m), intent(out) :: fvec
    real(dbl), dimension(:), allocatable :: r,f
    real(dbl), dimension(2,2) :: jac
    real(dbl), dimension(2) :: fv
    integer :: n
    real(dbl) :: s

    if( iflag == 0 ) then

       write(*,'(4g15.5)') p,fvec

       if( debug ) then
          n = 2
          call funder(2,2,p,fv,jac,2,n)
          write(*,*) ' jac:',jac(1,:)
          write(*,*) ' jac:',jac(2,:)
       end if

       return
    end if

    n = size(xdata)
    allocate(r(n),f(n))

    s = p(2)
    r = (xdata - p(1))/s
    call hubers(r,f)

    fvec(1) = sum(f)
    fvec(2) = sum(f*r) - n*beta

    fvec = fvec / s

    deallocate(r,f)

  end subroutine fundif

  subroutine difjac(t,s,jac)

    ! numerical approximation of jacobian

    real(dbl), intent(in) :: t,s
    real(dbl), dimension(:,:), intent(out) :: jac
    real(dbl), dimension(2) :: fv1,fv2
    real(dbl) :: d = 1e-4
    integer :: iflag

    iflag = 1

    if( abs(t) > epsilon(t) ) then
       d = sqrt(epsilon(d))*abs(t)
    else
       d = sqrt(epsilon(d))
    end if
    call fundif(2,2,(/t+d,s/),fv1,iflag)
    call fundif(2,2,(/t-d,s/),fv2,iflag)
    jac(1,:) = (fv1 - fv2)/(2*d)

    d = sqrt(epsilon(d))
    call fundif(2,2,(/t,s+d/),fv1,iflag)
    call fundif(2,2,(/t,s-d/),fv2,iflag)
    jac(2,:) = (fv1 - fv2)/(2*d)

  end subroutine difjac

! ----------------------------------------------------------------------------

  subroutine rmean3(x,t,dt,sig,tol,istat)

    ! minimizes parameters of a robust function with joint estimate of mean and scale
    ! The location estimate maximises likelihood while scale is estimated
    ! from maximum of entropy (free energy). The subroutine gives the best
    ! estimation of scale. No winsorising is applied.
    !
    ! t   is estimation of mean,
    ! dt  is standard error of t
    ! sig is standard deviation of sample
    !
    ! rnewton is significantly faster.

    use rfun
    use minpacks

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t,sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    integer, intent(out) :: istat

    integer :: n,info
    real(dbl) :: s,sum2,sum3
    real(dbl), dimension(:), allocatable :: r,f,df
    real(dbl), dimension(2) :: u

    istat = 0
    n = size(x)
    s = sig

    allocate(xdata(n))
    xdata = x

    u = (/ t,s /)
    if( analytic ) then
       call lmder2(funds,u,tol,nprint,info)
    else
       call lmdif2(funis,u,tol,nprint,info)
    end if

    if( info /= 5 ) then
       ! the minimum localised successfully, update estimates
       t = u(1)
       s = u(2)

    else
       ! No convergence occured, so the initial values are used.

       if( verbose ) &
            write(*,*) "Warning: Finished prematurely without convergence in gradient."

       dt = sig/sqrt(real(n))
       goto 666

    end if

    ! Estimation of dispersion (variance) and uncertainty of mean
    !
    !  * The estimation of uncertainity is by Hubber(1981), formula (8.8)
    !    The sum2 has meaning: "which fraction of values lies inside -1.349...
    !    .. 1.349 interval" (in Huber's function). The sum3 is robust equivalent
    !    of mean of square deviations.


    allocate(r(n),f(n),df(n))
    r = (x - t)/s
    call hubers(r,f)
    call dhubers(r,df)
    sum2 = sum(df)
    sum3 = sum(f**2)

    sig = s

    if( sum2 > tol ) then
       dt = sig*sqrt(sum3/sum2**2)
    else
       ! the alternative for the scale nearly to machine epsilon
       dt = sig/sqrt(real(n))
    end if
    deallocate(r,f,df)

666 continue

    deallocate(xdata)

  end subroutine rmean3


  subroutine funds(m,np,p,fvec,fjac,ldfjac,iflag)

    use rfun

    integer, intent(in) :: m,np,ldfjac
    integer, intent(inout) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(m), intent(out) :: fvec
    real(dbl), dimension(ldfjac,np), intent(out) :: fjac
    real(dbl), dimension(:), allocatable :: rs,f,df,rho,tmp
    real(dbl), dimension(2) :: fv
    real(dbl), dimension(2,2) :: dfjac
    real(dbl) :: s
    integer :: n

    if( iflag == 0 ) then

       write(*,'(4g15.5)') p,fvec

       if( debug ) then
          call difjac3(p(1),p(2),dfjac)
          write(*,*) 'djac:',dfjac(1,:)*p(2)
          write(*,*) 'djac:',dfjac(2,:)*p(2)
       end if

       return
    end if

    n = size(xdata)
    allocate(rs(n),f(n),rho(n))

    s = p(2)
    rs = (xdata - p(1))/s   ! r = xdata - p(1)
    call hubers(rs,f)
    call ihubers(rs,rho)

    ! f = rs
    ! rho = rs**2/2

    ! maximum likelihood
    fv(1) = sum(f)

    ! update for entropy
    f = 2*f
    rho = 2*rho

    fv(2) = sum(f*rs*exp(-rho)*(1-rho))/s

    if( iflag == 1 ) then

       fvec = fv

    else if( iflag == 2 ) then

       allocate(df(n),tmp(n))
       call dhubers(rs,df)
       !       df = 1

       ! maximum likelihood
       fjac(1,1) = sum(df)
       fjac(1,2) = sum(df*rs)

       ! the update for entropy
       df = 2*df
       tmp = exp(-rho)*((f+df*rs)*(1-rho) + rs*f**2*(rho-2))

       ! rho
       fjac(2,1) = sum(tmp)/s
       fjac(2,2) = sum(tmp*rs)/s + fv(2)/s

       if( debug ) then
          write(*,*) fjac(1,:)
          write(*,*) fjac(2,:)
       end if

       fjac = - fjac / s

       deallocate(df,tmp)

    end if

    deallocate(rs,f,rho)

  end subroutine funds

  subroutine funis(m,np,p,fvec,iflag)

    use rfun

    integer, intent(in) :: m,np
    integer, intent(inout) :: iflag
    real(dbl), dimension(np), intent(in) :: p
    real(dbl), dimension(m), intent(out) :: fvec
    real(dbl), dimension(:), allocatable :: rs,f
    real(dbl), dimension(:), allocatable :: rho
    real(dbl), dimension(2,2) :: dfjac
    real(dbl) :: s
    integer :: n

    if( iflag == 0 ) then

       write(*,'(4g15.5)') p,fvec

       if( debug ) then
          call difjac3(p(1),p(2),dfjac)
          write(*,*) 'djac:',dfjac(1,:)*p(2)
          write(*,*) 'djac:',dfjac(2,:)*p(2)
       end if

       return
    end if

    n = size(xdata)
    allocate(rs(n),f(n),rho(n))

    s = p(2)
    rs = (xdata - p(1))/s
    call hubers(rs,f)
    call ihubers(rs,rho)
!    f = 2*rs
!    rho = rs**2

    ! maximum likelihood
    fvec(1) = sum(f)

    ! the update for entropy
    f = 2*f
    rho = 2*rho

    fvec(2) = sum(f*rs*exp(-rho)*(1-rho))/s

    deallocate(rs,f,rho)

  end subroutine funis


  subroutine difjac3(t,s,jac)

    ! numerical approximation of jacobian of rmean3

    real(dbl), intent(in) :: t,s
    real(dbl), dimension(:,:), intent(out) :: jac
    real(dbl), dimension(2) :: fv1,fv2
    real(dbl) :: d = 1e-4
    integer :: iflag

    iflag = 1

    if( abs(t) > epsilon(t) ) then
       d = sqrt(epsilon(d))*abs(t)
    else
       d = sqrt(epsilon(d))
    end if
    call funis(2,2,(/t+d,s/),fv1,iflag)
    call funis(2,2,(/t-d,s/),fv2,iflag)
    jac(:,1) = (fv1 - fv2)/(2*d)

    d = sqrt(epsilon(d))
    call funis(2,2,(/t,s+d/),fv1,iflag)
    call funis(2,2,(/t,s-d/),fv2,iflag)
    jac(:,2) = (fv1 - fv2)/(2*d)

  end subroutine difjac3


  ! --------------------------------------------------------


  subroutine rmean4(x,t,dt,sig,tol,istat)

    ! One minimizes mean by Huber function with scale estimated by entropy.
    ! The location is estimated by maximum of likelihood while scale is
    ! estimated from extreme of entropy. The subroutine gives the best
    ! estimation of scale. No winsorising is applied.
    !
    ! t   is estimation of mean,
    ! dt  is standard error of t
    ! sig is standard deviation of sample
    !
    ! rnewton is significantly faster.

    use rfun

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t,sig
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    integer, intent(out) :: istat

    integer, parameter :: maxiter = precision(t)
    ! Number of iterations is limited by numerical precision.
    ! We belives in properties of Newton's method so,
    ! one order is reached by each iteration, at least.

    logical :: convergent
    integer :: n,iter
    real(dbl) :: s,sum3,sum2,sum1,d,fs,dfs
    real(dbl), dimension(:), allocatable :: r,rs,f,df,rho,erho

    ! allocate working arrays
    n = size(x)
    allocate(r(n),rs(n),f(n),df(n),rho(n),erho(n))

    ! try to improve estimation of scale by entropy extreme
    s = sig
    convergent = .false.
    do iter = 1, maxiter

       ! check of scale on positiveness
       if( .not. (s > 0) ) exit

       ! solution of f(s) = 0, with f(s) = d [sum(rho*exp(-2*rho))] / ds
       r = x - t
       rs = r / s
       call ihubers(rs,rho)
       call hubers(rs,f)
       call dhubers(rs,df)
!       rho = rs**2/2
!       f = rs
!       df = 1

       erho = exp(-2*rho)
       sum1 = sum(f*r*(1-2*rho)*erho)
       sum2 = sum(((1-2*rho)*(2*f**2-df) + 2*f**2)*r**2*erho)

       fs = -sum1 / s**2
       dfs = 2*sum1 / s**3 - sum2 / s**4

       ! Note. If we have good initial estimate, than sum1 (=fs) is near to zero
       ! and sum1 / sum2 * s**2 is numerically equivalent, but little bit
       ! faster, than fs/dfs. Theirs numerical differences are negligible.

       if( .not. (abs(dfs) > 0) ) exit

       ! Newton's step for scale
       d = fs / dfs
       s = s - d

       !if( debug ) write(*,'(a,i3,4g15.5)') "scale,f,f',incr.",iter,s,sum1,sum2,d

       convergent = abs(d) < tol

       ! exit immediately when required precision is reached
       if( convergent ) exit
    end do

    ! no convergence detected, fall-back to initials
    if( .not. convergent ) s = sig

    ! precision of mean improved with the updated scale
    convergent = .false.
    do iter = 1, maxiter

       ! solution of f(t) = 0, where f(t) = sum(phi((x-t)/s)
       rs = (x - t)/s
       call hubers(rs,f)
       call dhubers(rs,df)
       sum1 = sum(f)
       sum2 = sum(df)

       if( sum2 < tol ) exit

       ! corrector for mean
       d = s*sum1/sum2

       ! update location
       t = t + d

       convergent = abs(d) < tol

       ! exit of iterations: the absolute errot must be at least |d| < tol
       if( convergent ) exit

       if( debug ) write(*,'(a,3g15.5)') "mean, increment, scale: ",t,d,s
    enddo

    ! estimation of standard deviation
    if( convergent ) then

       sum3 = sum(f**2)
       dt = s*sqrt(sum3/sum2**2*n/(n-1))
       sig = s
       istat = 0

    else
       ! this alternative branch is for:
       !  * parameters are poorly estimated, all phi' are zeros
       !  * no convergence
       !
       !  As the consequence, results should be incorrect!
       sig = s
       dt = sig/sqrt(real(n))
       istat = 5
    end if

    deallocate(r,rs,f,df,rho,erho)

  end subroutine rmean4


  subroutine rmean5(x,t,dt,s,tol,reli)

    ! One minimizes mean by Huber function with scale estimated by entropy.
    ! The location is estimated by minimum of likelihood while scale is
    ! estimated from maximum of entropy.
    !
    ! t   is estimation of mean,
    ! dt  is standard error of t
    ! sig is standard deviation of sample
    !
    ! rnewton is significantly faster but less reliable

    use rfun
    use entropyscale

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), intent(in out) :: t,s
    real(dbl), intent(out) :: dt
    real(dbl), intent(in) :: tol
    logical, intent(out) :: reli

    integer :: n
    real(dbl) :: s0,t0
    logical :: converge, treli, sreli

    converge = .false.
    dt = -1

    n = size(x)
    allocate(xdata(n))

    ! keep initial estimation
    t0 = t
    s0 = s

    if( debug ) call zero_graph(x,t,s)

    call zmean(x,t,s,treli)
    call escale(x-t,s,sreli)
    reli = treli .and. sreli

    if( debug ) write(*,'(a,1pg20.10,1pg12.3,1x,2l1)') 'zmean, escale:',t,s,treli,sreli

    if( reli ) then

       ! keep the solution
       t0 = t
       s0 = s

       ! Newton's iterations are only used to get more precise result and
       ! to estimate of std.err. by  derivative.
       call rnewton(x,t,dt,s,tol,converge)
!       call rhalley(x,t,dt,s,tol,converge)
       reli = converge .and. (minval(x) <= t .and. t <= maxval(x))

       if( debug ) write(*,'(a,1pg20.10,1p2g17.7,l3)') 'Newton:',t,dt,reli

    end if

    if( .not. reli ) then
       ! Failback for:
       !  * parameters are poorly estimated, all psi'(.) are zero
       !  * no convergence
       !  * final result is out of range of the zmean result
       !
       !  As the consequence, results are unreliable.

       t = t0
       s = s0
       if( dt < 0 ) dt = s/sqrt(real(n))

       if( debug ) write(*,*) 'rmean5 (not reliable):',t,t0,s,s0,dt,converge

    end if

    if( verbose ) write(*,'(a,1g20.10,2g12.3,l2)') 'rmean5:',t,dt,s,reli
    deallocate(xdata)

  end subroutine rmean5

  subroutine zmean(x,t,s,reli)

    ! Zero mean estimates mean of robust function by Brent's method
    ! which is combination of bisection and inverse quadratic interpolation.
    ! It is important that it works without derivatives so there is
    ! no potential risk of divergence due to "re-descending" property
    ! of Tukey's function as in rnewton() with an imprecise initial.

    use fmm

    real(dbl), dimension(:),intent(in) :: x
    real(dbl), intent(in) :: s
    real(dbl), intent(in out) :: t
    logical, intent(out) :: reli

    real(dbl) :: tol, tmin, tmax, d
    integer :: n

    n = size(x)
    tol = 0.001 * s / sqrt(real(n))

    tmin = t - s
    tmax = t + s

    xdata = x
    zsig = s
    t = zeroin(tmin,tmax,zfun,tol)

    ! d gives precision of localisation on base of std.dev.
    ! Number of calls of fhuber will approximately ln((tmax-tmin)/d).

    d = 2*tol
    reli = abs(t - tmin) > d .and. abs(tmax - t) > d

    ! results lying too close to endpoints looks very bad...

  end subroutine zmean

  function zfun(t)

    use rfun

    real(dbl) :: zfun
    real(dbl), intent(in) :: t

    call tukeys((xdata -t)/zsig,psi)
    !    psi = r/zsig

    zfun = sum(psi) / size(psi)

  end function zfun



  !-------------------------------------
  !
  ! diagnostics routines
  !

  subroutine zero_graph(x,t0,s)

    use rfun

    real(dbl), dimension(:),intent(in) :: x
    real(dbl), intent(in) :: t0,s

    integer :: i
    real(dbl) :: t

    xdata = x
    zsig = s

    open(1,file='/tmp/t')
    do i = -500,500,10
       t = t0 + s * i / 100.0
       write(1,*) t,zfun(t)
    end do
    close(1)

  end subroutine zero_graph



  ! an interface for plotting of functions in minimum (not used for computations)
  subroutine graph(x,t,sig,type,fvec)

    real(dbl), dimension(:), target, intent(in) :: x
    real(dbl), intent(in) :: t,sig
    character(len=*), intent(in) :: type
    real(dbl), dimension(:) :: fvec
    integer :: iflag, n

    n = size(x)
    allocate(xdata(n))
    xdata = x

    if( type == "grad" ) then
       iflag = 1
       call fundif(2,2,(/t,sig/),fvec,iflag)
    else if( type == "like" ) then
       fvec(1) = loglikely((/t,sig/))
    end if

    deallocate(xdata)

  end subroutine graph


end module robustmean
