module DeBoor_smooth

implicit none

private

integer,parameter :: SP = selected_real_kind(4)
integer,parameter :: DP = selected_real_kind(8)

public :: smooth

character(len=20),parameter,public :: DeBoor_smoothVer = "1.0" ! 2.3.2008

contains

subroutine setupq ( x, dx, y, npoint, v, qty )

	!! Originalni komentare zacinaji !C
	!C  from  * a practical guide to splines *  by c. de boor    
	!C  to be called in  s m o o t h
	!C  put  delx = x(.+1) - x(.)  into  v(.,4),
	!C  put  the three bands of  q-transp*d  into  v(.,1-3), and
	!C  put the three bands of  (d*q)-transp*(d*q)  at and above the diagonal
	!C  into  v(.,5-7) .
	!C  here,  q is  the tridiagonal matrix of order (npoint-2,npoint)
	!C  with general row  1/delx(i) , -1/delx(i) - 1/delx(i+1) , 1/delx(i+1)
	!C  and   d  is the diagonal matrix  with general row  dx(i) .


	integer,intent(in) :: npoint
	real(DP),dimension(npoint) :: dx,qty,x,y
	real(DP),dimension(npoint,7) :: v

	real(DP) :: diff,prev
	integer :: i,npm1

	npm1 = npoint - 1
    v(1,4) = x(2) - x(1)
    do i=2,npm1
        v(i,4) = x(i+1) - x(i)
        v(i,1) = dx(i-1)/v(i-1,4)
        v(i,2) = - dx(i)/v(i,4) - dx(i)/v(i-1,4)
		v(i,3) = dx(i+1)/v(i,4)
	end do
	v(npoint,1) = 0.
    do i=2,npm1
		v(i,5) = v(i,1)**2 + v(i,2)**2 + v(i,3)**2
	end do
	if (npm1 < 3)  goto 14
	do i=3,npm1
		v(i-1,6) = v(i-1,2)*v(i,1) + v(i-1,3)*v(i,2)
   	end DO
14  v(npm1,6) = 0.
    if (npm1 < 4)  goto 16
	do i=4,npm1
		v(i-2,7) = v(i-2,3)*v(i,1)
	END DO
16	v(npm1-1,7) = 0.
    v(npm1,7) = 0.
	!C	construct  q-transp. * y  in  qty.
	prev = (y(2) - y(1))/v(1,4)
    do i=2,npm1
        diff = (y(i+1)-y(i))/v(i,4)
        qty(i) = diff - prev
		prev = diff
	end do

end subroutine setupq

subroutine chol1d ( p, v, qty, npoint, u, qu )

	!c  from  * a practical guide to splines *  by c. de boor    
	!c to be called in  s m o o t h
	!C constructs the upper three diags. in v(i,j), i=2,,npoint-1, j=1,3, of
	!c  the matrix  6*(1-p)*q-transp.*(d**2)*q + p*r, then computes its
	!C  l*l-transp. decomposition and stores it also in v, then applies
	!c  forward and backsubstitution to the right side q-transp.*y in  qty
	!C  to obtain the solution in  u .

	integer,intent(in) :: npoint
    real(DP),intent(in) ::p
	real(DP),dimension(npoint),intent(in) :: qty
	real(DP),dimension(npoint,7),intent(inout) :: v
	real(DP),dimension(npoint),intent(out) :: qu,u

	integer :: i,npm1,npm2
	real(DP) :: prev,ratio,six1mp,twop	
    
	npm1 = npoint - 1
	!c  construct 6*(1-p)*q-transp.*(d**2)*q  +  p*r
    six1mp = 6.*(1.-p)
    twop = 2.*p
    do i=2,npm1
        v(i,1) = six1mp*v(i,5) + twop*(v(i-1,4)+v(i,4))
        v(i,2) = six1mp*v(i,6) + p*v(i,4)
		v(i,3) = six1mp*v(i,7)
	end do
    npm2 = npoint - 2
    if (npm2 >= 2) goto 10
    u(1) = 0.
    u(2) = qty(2)/v(2,1)
    u(3) = 0.
	goto 41
	!C  factorization
10	do i=2,npm2
        ratio = v(i,2)/v(i,1)
        v(i+1,1) = v(i+1,1) - ratio*v(i,2)
        v(i+1,2) = v(i+1,2) - ratio*v(i,3)
        v(i,2) = ratio
        ratio = v(i,3)/v(i,1)
        v(i+2,1) = v(i+2,1) - ratio*v(i,3)
		v(i,3) = ratio
	end do
	
	!C  forward substitution
    u(1) = 0.
    v(1,3) = 0.
    u(2) = qty(2)
    do i=2,npm2
		u(i+1) = qty(i+1) - v(i,2)*u(i) - v(i-1,3)*u(i-1)
	end do
	!C	back substitution
    u(npoint) = 0.
    u(npm1) = u(npm1)/v(npm1,1)
    i = npm2    
40  u(i) = u(i)/v(i,1)-u(i+1)*v(i,2)-u(i+2)*v(i,3)
    i = i - 1
    if (i > 1)  goto 40
	!C  construct q*u
41	prev = 0.
    do i=2,npoint
        qu(i) = (u(i) - u(i-1))/v(i-1,4)
        qu(i-1) = qu(i) - prev
		prev = qu(i)
	end do
	qu(npoint) = -qu(npoint)
 
end subroutine chol1d


!******************************************************************************

subroutine smooth ( x, y, dy, npoint, s, v, a, sfp ,pp)
!*************************************************************************
!
!! SMOOTH constructs the cubic smoothing spline to given data.
!  Discussion:
!
!    The data is of the form
!      (x(i),y(i)), i=1,...,npoint, 
!    The cubic smoothing spline has as small a second derivative as 
!    possible while
!      s(f)=sum( ((y(i)-f(x(i)))/dy(i))**2 , i=1,...,npoint ) <= s .
!
!  Reference:
!    Carl DeBoor,
!    A Practical Guide to Splines,
!    Springer Verlag.
!
!  Parameters:
!  input 
!    Input, real X(NPOINT), the abscissas, assumed to be strictly
!    increasing .
!    Input, real Y(NPOINT), the corresponding ordinates.
!    dy(1),...,dy(npoint)     estimate of uncertainty in data, assumed 
!    to be positive .
!
!   npoint.....number of data points,  assumed greater than 1
!
!  s.....upper bound on the discrete weighted mean square distance of
!        the approximation  f  from the data .
!
!  work arrays:
!
!  v      of size (npoint,7)
!  a      of size (npoint,4)
!
!  output
!
!  a(.,1).....contains the sequence of smoothed ordinates .
!  a(i,j)=d**(j-1)f(x(i)), j=2,3,4, i=1,...,npoint-1 ,  i.e., the
!        first three derivatives of the smoothing spline  f  at the
!        left end of each of the data intervals .
!     Warning . . .   a  would have to be transposed before it
!        could be used in  ppvalu .
!
!  Method: 
!
!    the matrices  q-transp*d  and  q-transp*d**2*q  are constructed in
!    SETUPQ from  x  and  dy , as is the vector  qty=q-transp*y .
!    then, for given  p , the vector  u  is determined in  c h o l 1 d  as
!    the solution of the linear system
!               (6(1-p)q-transp*d**2*q+p*r)u =qty  .
!  from  u , the smoothing spline  f  (for this choice of smoothing par-
!  ameter  p ) is obtained in the sense that
!                        f(x(.)) = y-6(1-p)d**2*q*u        and
!                  (d**2)f(x(.)) = 6*p*u                      .
!
!  the smoothing parameter  p  is found (if possible) so that
!                sf(p) = s ,
!  with  sf(p)=s(f) , where  f  is the smoothing spline as it depends
!  on  p .  if  s=0, then p = 1 . if  sf(0) <= s , then p = 0 .
!  otherwise, the secant method is used to locate an appropriate  p  in
!  the open interval  (0,1) . specifically,
!                p(0)=0,  p(1) = (s-sf(0))/dsf
!  with  dsf=-24*u-transp*r*u  a good approximation to  d(sf(0)) = dsf
!  +60*(d*q*u)-transp*(d*q*u) , and  u  as obtained for  p=0 .
!  after that, for n=1,2,...  until sf(p(n)) <= 1.01*s, do....
!  determine  p(n+1)  as the point at which the secant to  sf  at the
!  points  p(n)  and  p(n-1)  takes on the value  s .
!  if  p(n+1) >= 1 , choose instead  p(n+1)  as the point at which
!  the parabola  sf(p(n))*((1-.)/(1-p(n)))**2  takes on the value  s.
!
!  Note that, in exact arithmetic, always  p(n+1) < p(n) , hence
!  sf(p(n+1)) < sf(p(n)) . therefore, also stop the iteration,
!  with final  p=1 , in case  sf(p(n+1)) >= sf(p(n)) .
!

	integer,intent(in) :: npoint
	real(kind=DP),dimension(npoint),intent(in) :: x,y,dy
	real(kind=DP),dimension(npoint,4),intent(out) :: a
	real(kind=DP),dimension(npoint,7),intent(inout) :: v
	real(kind=DP),intent(in) :: s
	real(kind=DP),intent(out) :: sfp
	real(DP),intent(out),optional :: pp
	real(kind=DP) :: change,p,prevp,prevsf,utru
	integer :: i

	call setupq ( x, dy, y, npoint, v, a(1,4) )
 
	if ( 0.0E+00 < s ) then
		go to 20
	end if

10	continue

	p = 1.0_DP
	call chol1d ( p, v, a(1,4), npoint, a(1,3), a(1,1) )
	sfp = 0.0_DP
	goto 70

20  continue
 
	p = 0.0_DP
	call chol1d ( p, v, a(1,4), npoint, a(1,3), a(1,1) )
	sfp = 0.0_DP
	do i = 1, npoint
		sfp = sfp + ( a(i,1) * dy(i) )**2
	end do
	sfp = sfp * 36.0_DP

	if ( sfp <= s ) then
		go to 70
	end if
  
	prevp = 0.0_DP
	prevsf = sfp

	utru = 0.0_DP
	do i = 2, npoint
		utru = utru+v(i-1,4)*(a(i-1,3)*(a(i-1,3)+a(i,3))+a(i,3)**2)
	end do

	p = (sfp-s)/(24.0_DP*utru)
	!
	!  Secant iteration for the determination of p starts here.
	!
! 50 continue
	
	skok50: do
		call chol1d ( p, v, a(1,4), npoint, a(1,3), a(1,1) )

		sfp = 0.0_DP
		do i = 1, npoint
			sfp = sfp + ( a(i,1) * dy(i) )**2
		end do
		sfp = sfp * 36.0_DP * ( 1.0_DP - p )**2

		if ( sfp <= 1.01_DP * s ) then
			go to 70    ! exit skok50
		end if

		if ( prevsf <= sfp ) then
			p = 1.0_DP
			call chol1d ( p, v, a(1,4), npoint, a(1,3), a(1,1) )
			sfp = 0.0_DP    !exit skok50
			go to 70
		end if

		change = ( p - prevp ) / ( sfp - prevsf ) * ( sfp - s )
		prevp = p
		p = p - change
		prevsf = sfp

!! vypis hledani p
!~ print *,"p = ",p

		if ( 1.0_DP <= p ) then
			p = 1.0_DP - sqrt ( s / prevsf ) * ( 1.0_DP - prevp )
		end if
	
	end do skok50

	!  The correct value of p has been found.
	!  Compute polynomial coefficients from  q*u (in a(.,1)).
	!

70 continue

	do i = 1, npoint
		a(i,1) = y(i) - 6.0_DP * ( 1.0_DP - p ) * dy(i)**2 * a(i,1)
	end do

	do i = 1, npoint
		a(i,3) = 6.0_DP * a(i,3) * p
	end do

	do i = 1, npoint-1
		a(i,4) = (a(i+1,3)-a(i,3))/ v(i,4)
		a(i,2) = (a(i+1,1)-a(i,1))/v(i,4) &
				-(a(i,3)+a(i,4)/3.0_DP*v(i,4))/2.0_DP*v(i,4)
	end do
	
	if(present(pp)) pp=p
 
end subroutine smooth



end module DeBoor_smooth
