!
!  Robust minimization functions
!
!  Copyright © 2011 - 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 rfun

  implicit none

  ! M-estimator functions
  ! Hogg in Launer, Wilkinson: Robustness in Statistics

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

  ! parameters of robust functions
  real(dbl),parameter,private :: huber_a = 1.349_dbl, huber_a2 = huber_a**2/2.0_dbl
  real(dbl),parameter :: huber_sigcorr = 1.15
  real(dbl),parameter,private :: hampel_a=1.7_dbl,hampel_b=3.4_dbl,hampel_c=8.5_dbl
  real(dbl),parameter,private :: Andrews_a = 2.1_dbl
  real(dbl),parameter,private :: Tukey_a = 6.0_dbl, Tukey_a2 = 36.0_dbl / 2.0_dbl
  real(dbl),parameter,private :: pi = 3.141592653589793115997963468544185161590576171875_dbl
  real(dbl),parameter :: winscut = 1.618
  real(dbl),parameter,private :: Andrews_pi = Andrews_a*pi

contains

  !--------------------------------------------------------------
  !
  ! Robust functions
  !

  ! Huber

  function huber(x)

    real(dbl) :: huber
    real(dbl),intent(in) :: x

    if( abs(x) < huber_a )then
       huber = x
    else
       huber = sign(huber_a,x)
    endif

  end function huber


  function dhuber(x)

    real(dbl) :: dhuber
    real(dbl),intent(in) :: x

    if( abs(x) < huber_a )then
       dhuber = 1.0_dbl
    else
       dhuber = 0.0_dbl
    endif

  end function dhuber

  function ihuber(x)

    real(dbl) :: ihuber
    real(dbl), intent(in) :: x

    if( abs(x) < huber_a ) then
       ihuber = x**2/2.0_dbl
    else
       ihuber = huber_a*abs(x) - huber_a2
    end if

  end function ihuber


  subroutine hubers(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < huber_a )
       y = x
    elsewhere
       y = sign(huber_a,x)
    end where

  end subroutine hubers

  subroutine dhubers(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < huber_a )
       y = 1.0_dbl
    elsewhere
       y = 0.0_dbl
    end where

  end subroutine dhubers

  subroutine ihubers(x,y)

    real(dbl), dimension(:), intent(in) :: x
    real(dbl), dimension(:), intent(out) :: y

    where( abs(x) < huber_a )
       y = x**2/2.0_dbl
    elsewhere
       y = huber_a*abs(x) - huber_a2
    end where

  end subroutine ihubers


  ! Hampell

  function hampel(x)

    real(dbl) :: hampel
    real(dbl),intent(in) :: x

    if( abs(x) < hampel_a )then
       hampel = x
    elseif( hampel_a <= abs(x) .and. abs(x) < hampel_b )then
       hampel = sign(hampel_a,x)
    elseif( hampel_b <= abs(x) .and. abs(x) < hampel_c )then
       hampel = hampel_a*(x - sign(hampel_c,x))/(hampel_b - hampel_c)
    elseif(  abs(x) >= hampel_c )then
       hampel = 0.0_dbl
    else
       hampel = 0.0_dbl
    endif

  end function hampel

  function dhampel(x)

    real(dbl) :: dhampel
    real(dbl),intent(in) :: x

    if( abs(x) < hampel_a )then
       dhampel = 1.0_dbl
    elseif( hampel_b <= abs(x) .and. abs(x) < hampel_c )then
       dhampel = hampel_a/(hampel_b - hampel_c)
    else
       dhampel = 0.0_dbl
    endif

  end function dhampel

  subroutine hampels(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < hampel_a )
       y = x
    elsewhere( hampel_a <= abs(x) .and. abs(x) < hampel_b )
       y = sign(hampel_a,x)
    elsewhere( hampel_b <= abs(x) .and. abs(x) < hampel_c )
       y = hampel_a*(x - sign(hampel_c,x))/(hampel_b - hampel_c)
    elsewhere(  abs(x) >= hampel_c )
       y = 0.0_dbl
    elsewhere
       y = 0.0_dbl
    endwhere

  end subroutine hampels


  subroutine dhampels(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < hampel_a )
       y = 1.0_dbl
    elsewhere( hampel_b <= abs(x) .and. abs(x) < hampel_c )
       y = hampel_a/(hampel_b - hampel_c)
    elsewhere
       y = 0.0_dbl
    endwhere

  end subroutine dhampels

  ! Andrews

  function andrews(x)

    real(dbl) :: andrews
    real(dbl),intent(in) :: x

    if( abs(x) < andrews_a*pi )then
       andrews = sin(x/andrews_a)
    else
       Andrews = 0.0_dbl
    endif

  end function andrews

  function dandrews(x)

    real(dbl) :: dandrews
    real(dbl),intent(in) :: x

    if( abs(x) < andrews_a*pi )then
       dandrews = cos(x/andrews_a)/andrews_a
    else
       dandrews = 0.0_dbl
    endif

  end function dandrews

  subroutine iAndrews(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < Andrews_pi )
       y = Andrews_a*(1 - cos(x/Andrews_a))
    elsewhere
       y = 2*Andrews_a
    end where

  end subroutine iAndrews


  ! Tukey

  function tukey(x)

    real(dbl) :: tukey
    real(dbl),intent(in) :: x
    real(dbl) :: t

    if( abs(x) < tukey_a )then
       t = (x/tukey_a)**2
       tukey = x*(1.0_dbl - t)**2
    else
       tukey = 0.0_dbl
    endif

  end function tukey

  function dtukey(x)

    real(dbl) :: dTukey
    real(dbl),intent(in) :: x
    real(dbl) :: t

    if( abs(x) < tukey_a )then
       t = (x/tukey_a)**2
       dtukey = (1.0_dbl - t)*(1.0_dbl - 5.0_dbl*t)
    else
       dtukey = 0.0_dbl
    endif

  end function dTukey

  function iTukey(x)

    real(dbl) :: itukey
    real(dbl),intent(in) :: x
    real(dbl) :: t

    if( abs(x) < tukey_a )then
       t = (x/tukey_a)**2
       iTukey = (x**2/2.0_dbl)*(1.0_dbl + t*(t/3.0_dbl - 1.0_dbl))
    else
       iTukey = tukey_a
    endif

  end function iTukey


  subroutine tukeys(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

!    integer :: i

!    do i = 1, size(x)
!       if( abs(x(i)) < tukey_a ) then
!          y(i) = x(i)*(1.0_dbl - (x(i)/tukey_a)**2)**2
!       else
!          y(i) = 0.0_dbl
!       end if
!    end do

    where( abs(x) < tukey_a )
       y = x*(1.0_dbl - (x/tukey_a)**2)**2
    elsewhere
       y = 0.0_dbl
    end where

  end subroutine tukeys

  subroutine dtukeys(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    integer :: i
    real(dbl) :: t

    do i = 1, size(x)
       if( abs(x(i)) < tukey_a ) then
          t = (x(i) / tukey_a)**2
          y(i) = (1.0_dbl - t)*(1.0_dbl - 5.0_dbl*t)
       else
          y(i) = 0.0_dbl
       end if
    end do

!    where( abs(x) < tukey_a )
!       y = (x / tukey_a)**2
!       y = (1.0_dbl - y)*(1.0_dbl - 5.0_dbl*y)
!    elsewhere
!       y = 0
!    end where

  end subroutine dtukeys


  subroutine d2tukeys(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    where( abs(x) < tukey_a )
       y = (x / tukey_a)**2
       y = 4*x/tukey_a**2 * (5*y - 3)
    elsewhere
       y = 0.0_dbl
    end where

  end subroutine d2tukeys


  subroutine iTukeys(x,y)

    real(dbl),dimension(:),intent(in) :: x
    real(dbl),dimension(:),intent(out) :: y

    integer :: i
    real(dbl) :: t

    do i = 1, size(x)
       if( abs(x(i)) < tukey_a ) then
          t = (x(i) / tukey_a)**2
          y(i) = tukey_a2*t*(1.0_dbl + t*(t/3.0_dbl - 1.0_dbl))
       else
          y(i) = tukey_a
       end if
    end do

    !    where( abs(x) < tukey_a )
!       y = (x / tukey_a)**2
!       y = tukey_a2*y*(1.0_dbl + y*(y/3.0_dbl - 1.0_dbl))
!    elsewhere
!       y = tukey_a
!    end where


  end subroutine iTukeys



end module rfun
