!! Routines to implement Kriging interpolation
!|author:  <a href="mailto:giovanni.ravazzani@polimi.it">Giovanni Ravazzani</a>
! license: <a href="http://www.gnu.org/licenses/">GPL</a>
!    
!### History
!   
! current version  1.0 - 1st July 2018   
!
! | version  |  date       |  comment |
! |----------|-------------|----------|
! | 1.0      | 01/Jul/2018 | Original code |
!
!### License  
! license: GNU GPL <http://www.gnu.org/licenses/>
!
!### Module Description 
!   routines to implement Kriging interpolation. Firstly the 
!   semivariogram is generated from a given sample. 
!   The sample point pairs are ordered into even-width bin,
!   separated by the euclidean distance of the point pairs.
!   The Semivariance in the bin is calculated by the Matheron estimator.
!   If number of lags and lag max distance are not given
!   they are automatically computed or set to default value.    
!   Empirical semivariogram is then automatically fitted
!   to the best fitting model among Spherical, Exponential,
!   Gaussian, and Power, considering the nugget effect.
!   Fitting algorithm is adapted from the VARFIT model 
!   by Pardo-Iguzquiza (1999). The VARFIT algorithm 
!   performs a nonlinear minimization of the weighed 
!   squared differences between the experimental
!   variogram and the model. The weighting function
!   is the inverse of the estimation variance.
!   VARFIT uses a non-linear minimisation method
!   that does not require (explicit) initial values for the
!   parameters in order to initialise the minimisation al-
!   gorithm. Jian et al. (1996) report problems with con-
!   vergence if the initial guess is poor.
!   Solution is found with the simplex method of function
!   minimization of Nelder and Mead (1965)
!   This ingenious method is efficient for non-linear mini-
!   mization in a multiparameter space but is still easy to
!   program and only requires evaluations of the objective
!   function. The program stops the iterations whenever
!   one of the two following criteria is reached:
!   1. Convergence is reached
!   2. The maximum number of iterations  is reached.
!   The best fitted semivariogram model is used 
!   to interpolate a regular grid of  data.
!   Code is adapted from the Geostats program
!   written by Luke Spadavecchia, Biosphere Atmosphere 
!   Modelling Group, University of Edinburgh, November 2006.
!   A number of closest points are used to buils 
!   the covariance matrix used to predict value at
!   location where value is unknown. Matrix inversion
!   uses the the Gauss-Jordan method. 
!   An n*2,n work array is assembled, with the array 
!   of interest on the left half, and the identity matrix 
!   on the right half. A check is made to ensure the matrix is 
!   invertable, and the matrix inverse is returned, providing 
!   the matrix is not singular. The matrix is invertable if, 
!   after pivoting and row reduction, the identity matrix 
!   shifts to the left half of the work array.
!   
! References:
!
! Pardo-Iguzquiza, E. VARFIT: a fortran-77 program 
!  for fitting variogram models by weighted least squares. 
!  Computers & Geosciences, 25, 251-261, 1999.
!
! Nelder, J.A., Mead, R., 1965. A simplex method for 
!  function minimization. Computer Journal 7, 308-313.
! 
! Jian, X., Olea, R.A., Yu, Y., 1996. Semivariogram modelling
!  by weighted least squares. Computers & Geosciences 22 (3),
!  387-397.
!
MODULE Kriging


USE DataTypeSizes, ONLY:&
  !Imported parameters:
  float, double, short

USE LogLib, ONLY: &
  !Imported routines:
  Catch

USE GeoLib, ONLY: &
  !Imported variables:
  point1, point2, &
  !Imported routines:
  Distance , DirectionAngle, &
  !Imported definition:
  Coordinate, &
  !Imported operators:
  ASSIGNMENT( = )

USE ObservationalNetworks, ONLY: &
  !Imported definitions:
  ObservationalNetwork, &
  !Imported routines:
   ActualObservations, &
  !Imported definition:
  Observation

USE GridLib, ONLY: &
    !Imported definition:
    grid_real

USE GridOperations, ONLY: &
    !Imported routines:
    GetXY

USE Units, ONLY: &
  !Imported parameters
  pi, degToRad, radToDeg

IMPLICIT NONE


! Global Procedures:
PUBLIC :: Krige

!Public parameters:
INTEGER, PARAMETER, PUBLIC :: SPHERICAL = 1  !!fit spherical semivariogram model
INTEGER, PARAMETER, PUBLIC :: EXPONENTIAL = 2 !!fit exponential semivariogram model
INTEGER, PARAMETER, PUBLIC :: GAUSSIAN = 3 !!fit gaussian semivariogram model
INTEGER, PARAMETER, PUBLIC :: POWER = 4  !!fit power semivariogram model
INTEGER, PARAMETER, PUBLIC :: AUTOFIT = 5 !!automatic fitting among spherical, exponential and gaussian

!Private procedures
PRIVATE :: PairDistance
PRIVATE :: DistMatrix
PRIVATE :: Semivariogram
PRIVATE :: KrigingInitialize
PRIVATE :: Varfit
PRIVATE :: Simplex
PRIVATE :: Valf
PRIVATE :: Focompu
PRIVATE :: Variogr
PRIVATE :: Fconv
PRIVATE :: Separation
PRIVATE :: Sorted
PRIVATE :: Sort
PRIVATE :: Covariance
PRIVATE :: Covariance2D
PRIVATE :: Interpolate


! private declarations:

TYPE pairs
	INTEGER (KIND = short) :: p1, p2 !!id of points pair
	REAL (KIND = float)    :: h !! pair distance 
    REAL (KIND = float)    :: theta !!pair angle
END TYPE pairs
PRIVATE pairs

TYPE site
	INTEGER (KIND = short) :: oid  !! identification code
	REAL (KIND = float)    :: value !!observed value
    REAL (KIND = float)    :: h !! distance (m)
    REAL (KIND = float)    :: theta !!anistropy angle
    TYPE (Coordinate)      :: xyz  !!position
END TYPE site
PRIVATE site

!variables used for fitting variogram
INTEGER (KIND = short), PRIVATE :: ieva !number of evaluations of the objective function
!INTEGER (KIND = short), PUBLIC :: ieva !number of evaluations of the objective function

INTEGER (KIND = short), PRIVATE :: npep !if the nugget effect is estimated  XXXXXX deve essere settato da qualche parte
INTEGER (KIND = short), PRIVATE :: ndir !!number of directions of the variogram

INTEGER (KIND = short), PRIVATE :: nvar ! if nvar =1 it is imposed that variance = experimental variance
INTEGER (KIND = short), PRIVATE :: ipara !! number of parameters to estimate
INTEGER (KIND = short), PRIVATE :: objectiveFunction !!kind of objective function to minimize
INTEGER (KIND = short), PRIVATE :: minpairs = 30!!minimum number of pairs in the lag to be considered valid for semivariogram fitting.


REAL (KIND = float),    PRIVATE :: nugget !!nugget effect (C0)
REAL (KIND = float),    PRIVATE :: range !!range
REAL (KIND = float),    PRIVATE :: partialSill !!partial sill (C1) sill of the variogram


REAL (KIND = float),    PRIVATE :: anisotropyAngle !!anisotropy angle 
REAL (KIND = float),    PRIVATE :: anisotropyRatio !!anisotropy ratio >= 1
REAL (KIND = float),    PRIVATE :: var !! variance
REAL (KIND = float),    PRIVATE :: expVar !! experimental variance
REAL (KIND = float),    PRIVATE :: parRangeMin (5) !!minimum values of parameters
REAL (KIND = float),    PRIVATE :: parRangeMax (5) !!maximum values of parameters
REAL (KIND = float),    PRIVATE :: lagTolerance (4) !! How much the distance between pairs can differ from the exact lag distance and still be included in the lag calculations (m)

REAL (KIND = float),    PRIVATE :: hfina !!value of the objective function at the minimum

INTEGER (KIND = short), ALLOCATABLE, PRIVATE :: lagNumber (:) !!number of lags for each direction
INTEGER (KIND = short), ALLOCATABLE, PRIVATE :: pairNumber (:,:) !!number of pairs for each direction and lag

REAL (KIND = float),    ALLOCATABLE, PRIVATE :: dist (:,:) !!average distance for each direction and lag RIMETTERE PRIVATE
REAL (KIND = float),    ALLOCATABLE, PRIVATE :: empVariogram (:,:) !!experimental (empirical) variogram for each direction and lag  !RIMETTERE PRIVATE
REAL (KIND = float),    ALLOCATABLE, PRIVATE :: modVariogram (:,:) !!modelled (empirical) variogram for each direction and lag  !RIMETTERE PRIVATE

REAL (KIND = float),    ALLOCATABLE, PRIVATE :: dist_pairs (:,:)  !!distance  between pairs 
REAL (KIND = float),    ALLOCATABLE, PRIVATE :: dir_pairs (:,:)  !!direction angle between pairs
REAL (KIND = float),    ALLOCATABLE, PRIVATE :: dir (:) !!angle (radians) between the direction of the variogram and the
                                                    !! x axis, measured from the x axis anti-clockwise

!variables used for kriging interpolation
TYPE(pairs), DIMENSION (:,:), ALLOCATABLE, PRIVATE :: obsdist !!distance matrix
TYPE(site), DIMENSION (:), ALLOCATABLE, PRIVATE :: controlpts !!subset of closest points used for interpolation 
TYPE(pairs), DIMENSION (:,:), ALLOCATABLE :: hobs
REAL (KIND = float), DIMENSION (:), ALLOCATABLE, PRIVATE :: cvest, hest, weights
REAL (KIND = float), DIMENSION (:,:), ALLOCATABLE, PRIVATE :: cvobs
INTEGER (KIND = short), DIMENSION(:), ALLOCATABLE, PRIVATE :: last


!=======         
    CONTAINS
!======= 
! Define procedures contained in this module. 

    
!==============================================================================
!| Description:
!  Interpolate irregularly spaced data using Kriging. The subroutine
!  generates an empirical semivariogram (anisotropy is assumed), 
!  fits the best model among spherical, exponential and gaussian.
!
! References:
!
!  Matheron, G. (1965) Les variables régionalisées et leur estimation: 
!  Une application de la theorie des fonctions aléatoires aux sciences 
!  de la nature. Masson, Paris.
!  
SUBROUTINE Krige &
!
(sitedata, anisotropy, nlags, maxlag, neighbors, varModel, grid, gridvar, &
 points, predictions, predictionsVar)


IMPLICIT NONE

!Arguments with intent (in):
TYPE (ObservationalNetwork), INTENT(IN) :: sitedata
INTEGER (KIND = short), INTENT(IN) :: anisotropy !! if = 1, anisotropy is considered 
INTEGER (KIND = short), INTENT(IN) :: nlags !! the number of discrete distances used for comparing data, if 0 it is set to default value
REAL (KIND = float), INTENT(IN) :: maxlag !! maximum distance to consider for semivariogram assessment (m), if 0 it is automatically computed
INTEGER (KIND = short), INTENT(IN) :: neighbors  !! number of neighbors to consider for interpolation
INTEGER (KIND = short), INTENT(IN) :: varModel !!semivariogram model, autofit for atomatic selecting the best among spherical, exponential, and gaussian
TYPE (Coordinate), OPTIONAL, INTENT(IN) :: points (:) !!list of coordinates for prediction (alternative to regular grid)

!Arguments with intent(inout):
TYPE (grid_real), OPTIONAL, INTENT (INOUT) :: grid  !!interpolated grid
TYPE (grid_real), OPTIONAL, INTENT (INOUT) :: gridvar  !!variance of interpolated grid
REAL ( KIND = float), OPTIONAL, DIMENSION (:), INTENT (INOUT) :: predictions  !interpolated values on not regular spaced points
REAL ( KIND = float), OPTIONAL, DIMENSION (:), INTENT (INOUT) :: predictionsVar !variance of interpolated values on not regular spaced points

!Local declarations
TYPE(ObservationalNetwork) :: active_stations
INTEGER (KIND = short) :: count
INTEGER (KIND = short) :: nclosest !!number of closest points to include in interpolation 
INTEGER (KIND = short) :: i,j  
TYPE (site) :: prediction_point
REAL (KIND = float) :: x, y
TYPE(site), DIMENSION (:), ALLOCATABLE ::  estdist
REAL (KIND = float) :: est !estimated value 
REAL (KIND = float) :: variance
INTEGER (KIND = short) :: outmodel !!semivariogram model selected by varfit
!---------------------------end of declarations--------------------------------

  !first check optional input data
  IF (PRESENT (grid) .AND. PRESENT (points) ) THEN
       CALL Catch ('error', 'Kriging interpolation',  &
          'only grid or points should be set for interpolation, they are mutual exclusive' )
  END IF
  
  
  !create a subset of active stations
  CALL ActualObservations (sitedata, count, active_stations)
  
  !compute pair distance
  CALL PairDistance ( active_stations )
  
  !compute direction angle when anisotropy analysis is required
  IF ( anisotropy == 1 ) THEN
      CALL PairDirection (active_stations )
  END IF
  
  !initialize Kriging 
  CALL KrigingInitialize (anisotropy, nlags, maxlag, count, neighbors, nclosest)
  
  !generate empirical semivariogram
  CALL Semivariogram ( active_stations)  
  
  
  !automatic fitting of empirical semivariogram to the best model
  CALL VarFit (varModel, outModel)
  
  
  !Interpolate
  IF (PRESENT (grid) .OR. PRESENT(points) ) THEN
          !populate distance matrix
          CALL DistMatrix (active_stations)
  
          !set coordinate reference system of prediction point 
          IF (PRESENT (grid)) THEN
              prediction_point % xyz % system = grid % grid_mapping
          ELSE
              prediction_point % xyz % system = points(1) % system
          END IF
  
          !allocate estdist
          ALLOCATE ( estdist ( count ) )
  
  
          !Loop through prediction locations
          IF (PRESENT (grid)) THEN  !interpolate regular spaced points
                  DO i = 1, grid % idim
                      DO j = 1, grid % jdim
                          IF ( grid % mat (i,j) /= grid % nodata) THEN
              
                              !set prediction point
                              CALL GetXY (i, j, grid, x, y)
                              prediction_point % xyz % easting = x
                              prediction_point % xyz % northing = y
              
                              !Build Estimation distance Vector  estdist   
		                       CALL Separation (prediction_point, active_stations, estdist)
               
                              !Sort estdist to find the nearest neighbors of estimation location, and return
		                      !the prediction h vector and h matrix.
                              CALL Sorted (estdist, nclosest) 
              
                              !Calculate est covariance from semivariance model, using the estimate to data
		                      !distance vector
                              CALL Covariance(controlpts%h,controlpts%theta,cvest(1:nclosest), sill = partialsill, range = range, varmodel = outModel)
              
                              !Add extra element for lagrangian minimisation
		                      cvest (nclosest + 1 ) = 1.
              
                              !If control points for this datum are the same as for the last, the next steps
		                      !can be skipped over: Since distances between the observations are unchanged
		                      !the observation covariace matrix and its inverse are also unchanged. 
		                      IF (ANY(last .NE. controlpts%oid)) THEN
			                      !Calculate observation covariance matrix from specified semivariance model, 
			                      !using the observation distance matrix (hobs)
                                  CALL Covariance2d(hobs%h, hobs%theta, cvobs(1:nclosest,1:nclosest), sill = partialsill, range = range, varmodel = outModel)
  
			                      !Add extra row and column for lagrangian minimisation
                                  cvobs ( nclosest + 1, : ) = 1.
			                      cvobs ( :, nclosest + 1 ) = 1.
			                      cvobs ( nclosest + 1, nclosest + 1 ) = 0.
  
			                      !Invert Observation covariance Matrix
                                  CALL Matinv ( cvobs, cvobs )
                  
                              END IF
              
                              !Produce estimate for location
                              CALL Interpolate (cvobs, cvest, controlpts%value, est, variance, nclosest, sill = (partialsill + nugget) )
                              grid % mat (i,j) = est
                      
                              IF (PRESENT(gridvar)) THEN
                                 gridvar % mat (i,j) = variance
                              END IF
              
                  
                              !store last set of control points
		                      last = controlpts % oid
              
                          END IF
                      END DO
                  END DO
  
          ELSE !interpolate not regular spaced points
      
              DO i = 1, SIZE(predictions) 
          
                   prediction_point % xyz % easting = points(i) % easting
                   prediction_point % xyz % northing = points(i) % northing
          
                   !Build Estimation distance Vector  estdist   
		            CALL Separation (prediction_point, active_stations, estdist)
               
                    !Sort estdist to find the nearest neighbors of estimation location, and return
		            !the prediction h vector and h matrix.
                    CALL Sorted (estdist, nclosest) 
              
                    !Calculate est covariance from semivariance model, using the estimate to data
		            !distance vector
                    CALL Covariance(controlpts%h,controlpts%theta,cvest(1:nclosest), sill = partialsill, range = range, varmodel = outModel)
              
                    !Add extra element for lagrangian minimisation
		            cvest (nclosest + 1 ) = 1.
              
                    !If control points for this datum are the same as for the last, the next steps
		            !can be skipped over: Since distances between the observations are unchanged
		            !the observation covariace matrix and its inverse are also unchanged. 
		            IF (ANY(last .NE. controlpts%oid)) THEN
			            !Calculate observation covariance matrix from specified semivariance model, 
			            !using the observation distance matrix (hobs).
                        CALL Covariance2d(hobs%h, hobs%theta, cvobs(1:nclosest,1:nclosest), sill = partialsill, range = range, varmodel = outModel)
  
			            !Add extra row and column for lagrangian minimisation
                        cvobs ( nclosest + 1, : ) = 1.
			            cvobs ( :, nclosest + 1 ) = 1.
			            cvobs ( nclosest + 1, nclosest + 1 ) = 0.
  
			            !Invert Observation covariance Matrix
                        CALL Matinv ( cvobs, cvobs )
                  
                    END IF
              
                    !Produce estimate for location
                    !sill = nugget + partial sill = C0 + C1
		            CALL Interpolate (cvobs, cvest, controlpts%value, est, variance, nclosest, sill = (partialsill + nugget) )
            
                    predictions (i) = est
            
                    IF (PRESENT (predictionsVar)) THEN     
                       predictionsVar (i) = variance
                    END IF
              
                  
                    !store last set of control points
		            last = controlpts%oid
          
              END DO
          END IF
  END IF
  
  !free memory
  DEALLOCATE ( active_stations % obs )
  DEALLOCATE ( dist_pairs )
  IF (ALLOCATED (dir_pairs) ) DEALLOCATE ( dir_pairs )
  DEALLOCATE ( dir )
  DEALLOCATE ( lagNumber ) 
  DEALLOCATE ( dist ) 
  DEALLOCATE ( empVariogram ) 
  DEALLOCATE ( modVariogram)
  DEALLOCATE ( pairNumber ) 
  IF (ALLOCATED (obsdist) )  DEALLOCATE ( obsdist )
  DEALLOCATE ( weights )
  DEALLOCATE ( hest )
  DEALLOCATE ( cvest )
  IF (ALLOCATED ( estdist) )   DEALLOCATE ( estdist )
  DEALLOCATE ( controlpts )
  DEALLOCATE ( hobs )
  DEALLOCATE ( cvobs )
  DEALLOCATE ( last )

  RETURN
END SUBROUTINE Krige


!==============================================================================
!| Description:
!  A subroutine to assemble the global distance. This operation
!  is only carried out once; kriging observations matricies are subsetted from
!  this lookup table to speed up processing.
!
SUBROUTINE DistMatrix &
!
(stations)


IMPLICIT NONE

!Arguments with intent (in):
TYPE (ObservationalNetwork), INTENT(IN) :: stations

!Local declarations
INTEGER (KIND = short) :: i, j, n
TYPE (pairs) ::  comp
  
  
!---------------------------end of declarations--------------------------------
!allocate square matrix where n is the number of available observation points
n = stations % countobs
ALLOCATE(obsdist(n,n))

!set points coordinate reference system
point1 % system = stations % mapping
point2 % system = stations % mapping

DO i=1,n
	DO j=1,n
        !set point1
        point1 % northing = stations % obs (i) % xyz % northing  
        point1 % easting = stations % obs (i) % xyz % easting 
        
        !set point2  
        point2 % northing = stations % obs (j) % xyz % northing  
        point2 % easting = stations % obs (j) % xyz % easting
        
        !prepare pair
        comp % p1 = i
        comp % p2 = j
        comp % h = Distance (point1,point2) 
        
        !populate distance matrix
        obsdist(i,j) = comp
        
	END DO
		
END DO
  
RETURN
END SUBROUTINE DistMatrix



!==============================================================================
!| Description:
!  Compute distance between pairs 
!
SUBROUTINE PairDistance &
!
(stations)


IMPLICIT NONE

!Arguments with intent (in):
TYPE (ObservationalNetwork), INTENT(IN) :: stations

!Local declarations
  INTEGER (KIND = short) :: i, j
  REAL (KIND = float) :: average
  
!---------------------------end of declarations--------------------------------

  
!allocate variables
ALLOCATE ( dist_pairs(stations % countObs,stations % countObs))


! Compute distance among stations and experimental variance
average = 0.
dist_pairs = 0.

DO i = 1, stations % countObs
    average = average + stations % obs (i) % value
    DO j = 1, stations % countObs
        
        IF ( i == j) CYCLE !skip same point pairs, distance is zero
        
        IF ( j < i) CYCLE !this pair is already in the vector; pair(4,2) is the same as pair(2,4)
        
        point1 % northing = stations % obs (i) % xyz % northing  
        point1 % easting = stations % obs (i) % xyz % easting 
          
        point2 % northing = stations % obs (j) % xyz % northing  
        point2 % easting = stations % obs (j) % xyz % easting
                
        dist_pairs(i,j) = Distance (point1,point2)       
        
    END DO
END DO 

average = average / stations % countObs

!compute experimental variance
expVar = 0.
DO i = 1, stations % countObs
    expVar = expVar + ( stations % obs (i) % value - average ) ** 2.
END DO
expVar = expVar / stations % countObs
  
RETURN
END SUBROUTINE PairDistance


!==============================================================================
!| Description:
!  Compute direction angle between pairs (radians) measured from the x axis 
!  anti-clockwise
!
SUBROUTINE PairDirection &
!
(stations)


IMPLICIT NONE

!Arguments with intent (in):
TYPE (ObservationalNetwork), INTENT(IN) :: stations

!Local declarations
  INTEGER (KIND = short) :: i, j
  
!---------------------------end of declarations--------------------------------

  
!allocate variables
ALLOCATE ( dir_pairs (stations % countObs,stations % countObs) )

! Compute direction angle between stations 
dir_pairs = 0.
DO i = 1, stations % countObs
    DO j = 1, stations % countObs
        
        IF ( i == j) CYCLE !skip same point pairs, distance is zero
        
        IF ( j < i) CYCLE !this pair is already in the vector; pair(4,2) is the same as pair(2,4)
        
        point1 % northing = stations % obs (i) % xyz % northing  
        point1 % easting = stations % obs (i) % xyz % easting 
          
        point2 % northing = stations % obs (j) % xyz % northing  
        point2 % easting = stations % obs (j) % xyz % easting
                
        dir_pairs(i,j) = DirectionAngle (point1,point2)   
        
        !direction angle is measured from North clockwise.
        !change convention to angle measured from x- axis (east) anticlockwise.
        IF ( dir_pairs (i,j) <= pi / 2. ) THEN
            dir_pairs (i,j) = pi /2. - dir_pairs (i,j)
        ELSE
            !dir_pairs (i,j) = 3./2.*pi - dir_pairs (i,j)
            dir_pairs (i,j) =   pi + dir_pairs (i,j)
        END IF
       
    END DO
END DO 

RETURN
END SUBROUTINE PairDirection



!==============================================================================
!| Description:
!  generate a Semivariogram from a given sample.
!  The sample point pairs are ordered into even-width bin,
!  separated by the euclidean distance of the point pairs.
!  The Semivariance in the bin is calculated by the Matheron estimator.
!  If number of lags and lag max distance are not given
!  they are automatically computed or set to default value.
!
! References:
!
!  Matheron, G. (1965) Les variables régionalisées et leur estimation: 
!  Une application de la théorie des fonctions aléatoires aux sciences 
!  de la nature. Masson, Paris.
!  
SUBROUTINE Semivariogram &
!
(stations)

IMPLICIT NONE

!Arguments with intent (in):
TYPE (ObservationalNetwork), INTENT(IN) :: stations

!Local declarations
INTEGER (KIND = short) :: i, j, k, m, index
REAL (KIND = float) :: zj, zk !!observation at point j, and k
REAL (KIND = float), ALLOCATABLE :: empVariogramCopy (:,:), distCopy (:,:) 
INTEGER (KIND = short), ALLOCATABLE :: pairNumberCopy (:,:)

!---------------------------end of declarations--------------------------------

!generate empirical semivariogram

DO j = 1, stations % countObs !loop through pairs
   DO k = 1, stations % countObs
      IF ( j == k ) CYCLE !skip same point pairs, distance is zero  
      IF ( k <  j ) CYCLE !this pair is already in the vector; pair(4,2) is the same as pair(2,4)
     
      DO m = 1, ndir
         DO i = 1, lagNumber (m) !loop through lags
                          
                IF ( ndir == 1 ) THEN !isotrophic analysis, direction is not considered
            
                    IF (dist_pairs (j,k) >= dist (m,i) - lagTolerance (m) .AND. &
                        dist_pairs (j,k) < dist (m,i) + lagTolerance (m) ) THEN
                 
                         zj = stations % obs (j) % value
                         zk = stations % obs (k) % value
                 
                         pairNumber (m,i) = pairNumber (m,i) + 1
                         empVariogram (m,i) = empVariogram (m,i) + ( zj - zk )**2.
                
                    END IF
                        
                ELSE !anisotrophy analysis, create bins considering direction
                    IF ( dist_pairs (j,k) >= dist (m,i) - lagTolerance (m) .AND. &
                            dist_pairs (j,k) < dist (m,i) + lagTolerance (m) ) THEN
                        
                        
                       IF (  ( m == 1 .AND. dir_pairs (j,k) < pi / 8. )   .OR. &
                          ( m == 1 .AND. dir_pairs (j,k) >= 15./8. * pi ) .OR. &
                          ( m == 2 .AND. dir_pairs (j,k) >= pi / 8. .AND. dir_pairs (j,k) < 3./8. * pi )    .OR. &
                          ( m == 3 .AND. dir_pairs (j,k) >= 3./8. * pi .AND. dir_pairs (j,k) <= 1./2. * pi ) .OR. &
                          ( m == 3 .AND. dir_pairs (j,k) > 3./2. * pi .AND. dir_pairs (j,k) < 13./8. * pi ) .OR. &
                          ( m == 4 .AND. dir_pairs (j,k) >= 13./8. * pi .AND. dir_pairs (j,k) < 15./8. * pi ) ) THEN
                        
                             zj = stations % obs (j) % value
                             zk = stations % obs (k) % value
                             pairNumber (m,i) = pairNumber (m,i) + 1
                             empVariogram (m,i) = empVariogram (m,i) + ( zj - zk )**2.
                    
                        END IF
                     END IF  
                        
                END IF 
            
         END DO
      ENDDO     
   END DO
END DO


DO m = 1, ndir !loop through directions
  DO i = 1, lagNumber (m) !loop through lags
     empVariogram (m,i) = empVariogram (m,i) / ( 2. * pairNumber (m,i) )
  END DO
END DO

RETURN

END SUBROUTINE Semivariogram


!==============================================================================
!| Description:
!   Fit model to experimental variogram
!
SUBROUTINE Varfit &
!
(varModel, modelselected)

IMPLICIT NONE

!arguments with intent (in):
INTEGER (KIND = short), INTENT(IN) :: varModel

!arguments with intent(out):
INTEGER (KIND = short), INTENT(OUT) :: modelselected !!if automodel is passed as varModel, the optimal model is returned

!local declarations:
REAL (KIND = float) :: ami1, ama1, ami2, ama2, ami3, ama3 
REAL (KIND = float) :: ami4, ama4, ami5, ama5
REAL (KIND = float) :: gmax, gmin, dmin, dmax
REAL (KIND = float) :: fac, fape
INTEGER :: i,j, k
INTEGER (KIND = short) :: nmodels, model
REAL (KIND = float) :: sillmodel (3), rangemodel (3), nuggetmodel (3), of (3), iterations (3)
REAL (KIND = float) :: minimumOF !!minimum value of objective function
!-----------------------------------end of declarations-----------------------!

!!find minimum and maximum values
dmin = MINVAL ( dist, MASK = dist > 0.0 )
dmax = MAXVAL ( dist)
gmin = MINVAL ( empVariogram, MASK = empVariogram > 0.0   )
gmax = MAXVAL ( empVariogram  )

ipara = 2

IF ( ndir > 1 ) ipara = 4
IF ( nvar == 1 ) ipara = ipara - 1
IF ( npep == 1 ) ipara = ipara + 1

!set number of sevivariogram models to fit
IF (varmodel == AUTOFIT) THEN
    nmodels = 2
ELSE
    nmodels = 1
END IF

DO k = 1, nmodels
    
    !reset variables
    ieva = 0 
    nugget = 0.0
    range = 0.0
    partialSill = 0.0
    anisotropyAngle = 0.0
    anisotropyRatio = 1.0
    var = expVar
    
    IF (nmodels == 1) THEN
        model = varmodel
    ELSE
        model = k
    END IF
    
        !     ALCANCE MAYOR
              AMI1=0.1*DMIN
              AMA1=1.5*DMAX
        !     ANGULO DE ANISOTROPIA
              AMI2=0.0
              AMA2=PI
        !     RATIO DE ANISOTROPIA
              AMI3=1.0
              AMA3=20.0
        !     MESETA
              AMI4=0.1*VAR
              AMA4=2.0*VAR
        !     EFECTO DE PEPITA
              AMI5=0.0
              AMA5=VAR
              IF (model == 4) THEN
                FAC=2.0*GMAX/DMAX
                FAPE=2.0*GMIN
              END IF
        !        WRITE (6,*)'PARAMETERS TO ESTIMATE: ',IPARA
                parRangeMin(1)=AMI1
                parRangeMax(1)=AMA1
                IF (IPARA.EQ.2) THEN
                  IF (NPEP.EQ.1) THEN
                    parRangeMin(2)=AMI5
                    parRangeMax(2)=AMA5
                  ELSE
                    parRangeMin(2)=AMI4
                    parRangeMax(2)=AMA4
                    IF (varModel.EQ.4) THEN
                      parRangeMin(2)=0.00001
                      parRangeMax(2)=FAC
                    END IF
                  END IF
                ELSE IF (IPARA.EQ.3) THEN
                  IF (NPEP.EQ.1) THEN
                    parRangeMin(2)=AMI4
                    parRangeMax(2)=AMA4
                    parRangeMin(3)=AMI5
                    parRangeMax(3)=AMA5
                    IF (model.EQ.4) THEN
                      parRangeMin(2)=0.00001
                      parRangeMax(2)=FAC
                      parRangeMin(3)=0.0
                      parRangeMax(3)=FAPE
                    END IF
                  ELSE
                    parRangeMin(2)=AMI2
                    parRangeMax(2)=AMA2
                    parRangeMin(3)=AMI3
                    parRangeMax(3)=AMA3
                  END IF
                ELSE IF (IPARA.EQ.4) THEN
                  parRangeMin(2)=AMI2
                  parRangeMax(2)=AMA2
                  parRangeMin(3)=AMI3
                  parRangeMax(3)=AMA3
                  IF (NPEP.EQ.1) THEN
                    parRangeMin(4)=AMI5
                    parRangeMax(4)=AMA5
                  ELSE
                    parRangeMin(4)=AMI4
                    parRangeMax(4)=AMA4
                    IF (model.EQ.4) THEN
                      parRangeMin(4)=0.00001
                      parRangeMax(4)=FAC
                    END IF
                  END IF
                ELSE
                  parRangeMin(2)=AMI2
                  parRangeMax(2)=AMA2
                  parRangeMin(3)=AMI3
                  parRangeMax(3)=AMA3
                  parRangeMin(4)=AMI4
                  parRangeMax(4)=AMA4
                  parRangeMin(5)=AMI5
                  parRangeMax(5)=AMA5
                  IF (model.EQ.4) THEN
                    parRangeMin(4)=0.00001
                    parRangeMax(4)=FAC
                    parRangeMin(5)=0.0
                    parRangeMax(5)=FAPE
                  END IF
                END IF

        !!     VARIANCE = SILL + NUGGET

              IF (model.EQ.4) THEN
                 parRangeMin(1)=0.00001
                 parRangeMax(1)=1.9
              END IF   

              CALL Simplex (model)
              
              sillmodel (k) = partialSill
              rangemodel (k) = range
              nuggetmodel (k) = nugget
              of (k) = hfina
              iterations (k) = ieva
END DO



IF (nmodels > 1) THEN !set the best model
    minimumOF = 10E6
     DO i = 1, nmodels
          IF ( of (i) < minimumOF ) THEN
              minimumOF = of (i)
              modelselected = i
              partialSill = sillmodel (i)
              range = rangemodel (i)
              nugget = nuggetmodel (i)
              ieva = iterations (i)
              
          END IF
     END DO
ELSE
    modelselected = varModel
END IF

        
IF (nmodels > 1) THEN
    hfina = minimumOF
END IF

RETURN
END SUBROUTINE Varfit



!==============================================================================
!| Description:
!   This subroutine implements the simplex method of
!   function minimization of Nelder and Mead (1965).
!   This ingenious method is efficient for non-linear mini-
!   mization in a multiparameter space but is still easy to
!   program and only requires evaluations of the objective
!   function. The program stops the iterations whenever
!   one of the two following criteria is reached:
!   1. Convergence is reached
!   2. The maximum number of iterations  is reached.
!   
!   Output:
!   hfina: objective function at the minimum
!   parameters of the estimated variogram
!
! References:
!
!   Nelder, J.A., Mead, R., 1965. A simplex method for function
!   minimization. Computer Journal 7, 308-313.
!
SUBROUTINE Simplex &
!
(varModel)


USE Units, ONLY: &
    !imported parameters:
    pi

IMPLICIT NONE

!Arguments with intent(in):
INTEGER (KIND = short), INTENT (IN) :: varModel !!type of variogram: 1 = spherical, 2 = exponential, 3 = gaussian, 4 = power


!local declarations:
INTEGER, PARAMETER :: maxiter = 200 !maximum number of iteration
REAL (KIND = float), PARAMETER :: eps = 0.01 !convergence criterion
REAL (KIND = float), PARAMETER :: refl = 0.5 !reflection factor
REAL (KIND = float), PARAMETER :: expa = 2.0 !expansion factor
REAL (KIND = float), PARAMETER :: cont = 0.5 !contraction factor
REAL (KIND = float) :: pu (7,6)
REAL (KIND = float) :: p1 (6), p2 (6), pf (6)
REAL (KIND = float) :: c (6)
REAL (KIND = float) :: y (7)
REAL (KIND = float) :: val, y1, y2
REAL (KIND = float) :: ymax, ymin, bmax
INTEGER (KIND = short) :: imax, imin
INTEGER (KIND = short) :: i, j

!--------------------------------end of declarations---------------------------

! set initial values
SELECT CASE (ipara)
    CASE (1)
        pu (1,1) = parRangeMin (1)
        pu (2,1) = parRangeMax (1)
    CASE (2) 
        pu (1,1) = parRangeMin (1)
        pu (1,2) = parRangeMin (2)
        pu (2,1) = parRangeMin (1)
        pu (2,2) = parRangeMax (2)
        pu (3,1) = parRangeMax (1)
        pu (3,2) = parRangeMax (2)
    CASE (3)
        pu (1,1) = parRangeMin (1)
        pu (1,2) = parRangeMin (2)
        pu (1,3) = parRangeMin (3)
        pu (2,1) = parRangeMin (1)
        pu (2,2) = parRangeMin (2)
        pu (2,3) = parRangeMax (3)
        pu (3,1) = parRangeMin (1)
        pu (3,2) = parRangeMax (2)
        pu (3,3) = parRangeMin (3)
        pu (4,1) = parRangeMax (1)
        pu (4,2) = parRangeMax (2)
        pu (4,3) = parRangeMax (3)
        IF ( varModel == 4) THEN
            pu (4,1) = parRangeMax (1) / 2.0
            pu (4,2) = parRangeMax (2) / 2.0
            pu (4,3) = parRangeMin (3)
        END IF   
    CASE (4)
         pu (1,1) = parRangeMin (1)
         pu (1,2) = parRangeMin (2)
         pu (1,3) = parRangeMin (3)
         pu (1,4) = parRangeMin (4)
         pu (2,1) = parRangeMin (1)
         pu (2,2) = parRangeMin (2)
         pu (2,3) = parRangeMin (3)
         pu (2,4) = parRangeMax (4)
         pu (3,1) = parRangeMin (1)
         pu (3,2) = parRangeMin (2)
         pu (3,3) = parRangeMax (3)
         pu (3,4) = parRangeMin (4)
         pu (4,1) = parRangeMin (1)
         pu (4,2) = parRangeMax (2)
         pu (4,3) = parRangeMin (3)
         pu (4,4) = parRangeMin (4)
         pu (5,1) = parRangeMax (1)
         pu (5,2) = parRangeMax (2)
         pu (5,3) = parRangeMax (3)
         pu (5,4) = parRangeMax (4)
      CASE DEFAULT 
         pu (1,1) = parRangeMin (1)
         pu (1,2) = parRangeMin (2)
         pu (1,3) = parRangeMin (3)
         pu (1,4) = parRangeMin (4)
         pu (1,5) = parRangeMin (5)
         pu (2,1) = parRangeMin (1)
         pu (2,2) = parRangeMin (2)
         pu (2,3) = parRangeMin (3)
         pu (2,4) = parRangeMin (4)
         pu (2,5) = parRangeMax (5)
         pu (3,1) = parRangeMin (1)
         pu (3,2) = parRangeMin (2)
         pu (3,3) = parRangeMin (3)
         pu (3,4) = parRangeMax (4)
         pu (3,5) = parRangeMin (5)
         pu (4,1) = parRangeMin (1)
         pu (4,2) = parRangeMin (2)
         pu (4,3) = parRangeMax (3)
         pu (4,4) = parRangeMin (4)
         pu (4,5) = parRangeMin (5)
         pu (5,1) = parRangeMin (1)
         pu (5,2) = parRangeMax (2)
         pu (5,3) = parRangeMin (3)
         pu (5,4) = parRangeMin (4)
         pu (5,5) = parRangeMin (5)
         pu (6,1) = parRangeMax (1)
         pu (6,2) = parRangeMax (2)
         pu (6,3) = parRangeMax (3)
         pu (6,4) = parRangeMax (4)
         pu (6,5) = parRangeMin (5)
 END SELECT
DO  i = 1, ipara + 1
  DO j = 1, ipara
     pf (j) = pu (i,j)
  END DO
  y (i) = Valf (pf, varmodel)
END DO  

! simplex algorithm
200 ymax = -1.0E+15
ymin = 1.0E+15
bmax = ymax

DO i = 1, ipara + 1
    
    IF ( y(i) > ymax ) THEN
        ymax = y(i)
        imax = i
    END IF
    
    IF ( y(i) < ymin ) THEN
        ymin = y(i)
        imin = i
    END IF
END DO

DO  i = 1, ipara + 1
    IF (i /= imax) THEN
       IF (y(i) > bmax) bmax = y(i)
    END IF
END DO


!     200 ITERATIONS MAXIMUM

      IF ( ieva > maxiter ) GOTO 300

!    CONTROID

DO  I=1,IPARA
    VAL=0.0
    DO  J=1,IPARA+1
        IF (J.NE.IMAX) VAL=VAL+PU(J,I)
    END DO
    C(I)=VAL/IPARA
END DO

!     P*
  
DO  I=1,IPARA
     P1(I)=(1.0+REFL)*C(I)-REFL*PU(IMAX,I)
END DO
      Y1=VALF(P1, varmodel)
      IF (Y1.GT.YMIN.AND.Y1.LT.BMAX) THEN
         DO  I=1,IPARA
            PU(IMAX,I)=P1(I)
         END DO
         Y(IMAX)=Y1
         IF (FCONV(Y,IPARA).LT.EPS) GOTO 300
         GOTO 200
      END IF
      IF (Y1.LT.YMIN) THEN
         DO 11 I=1,IPARA
            P2(I)=(1.0+EXPA)*P1(I)-EXPA*C(I)
 11      CONTINUE
         Y2=VALF(P2, varmodel)
         IF (Y2.LT.YMIN) THEN
            DO 12 I=1,IPARA
               PU(IMAX,I)=P2(I)
 12         CONTINUE
            Y(IMAX)=Y2
            IF (FCONV(Y,IPARA).LT.EPS) GOTO 300
            GOTO 200
         ELSE
            DO 13 I=1,IPARA
               PU(IMAX,I)=P1(I)
 13         CONTINUE
            Y(IMAX)=Y1
            IF (FCONV(Y,IPARA).LT.EPS) GOTO 300
            GOTO 200
         END IF
      END IF

      IF (Y1.LT.YMAX) THEN
         DO 15 I=1,IPARA
            PU(IMAX,I)=P1(I)
 15      CONTINUE
         Y(IMAX)=Y1
      END IF

      DO 16 I=1,IPARA
         P2(I)=CONT*PU(IMAX,I)+(1.0-CONT)*C(I)
 16   CONTINUE
      Y2=VALF(P2, varmodel)
      IF (Y2.GT.YMAX) THEN
         DO 17 I=1,IPARA+1
            DO 18 J=1,IPARA
               PU(I,J)=(PU(I,J)+PU(IMIN,J))/2.0
 18         CONTINUE
            DO 19 J=1,IPARA
               PF(J)=PU(I,J)
 19         CONTINUE
            Y(I)=VALF(PF, varmodel)
 17      CONTINUE
         IF (FCONV(Y,IPARA).LT.EPS) GOTO 300
         GOTO 200
      ELSE
         DO 1 I=1,IPARA
            PU(IMAX,I)=P2(I)
 1       CONTINUE
         Y(IMAX)=Y2
         IF (FCONV(Y,IPARA).LT.EPS) GOTO 300
         GOTO 200
      END IF

300    IF (IPARA.EQ.1) THEN
          range=PU(IMIN,1)
          anisotropyAngle=0.0
          anisotropyRatio=1.0
          partialSill=VAR
          nugget=0.0
       ELSE IF (IPARA.EQ.2) THEN
          IF (NPEP.EQ.1) THEN
            range=PU(IMIN,1)
            nugget=PU(IMIN,2)
            partialSill=VAR-nugget
            anisotropyAngle=0.0
            anisotropyRatio=1.0
          ELSE
            range=PU(IMIN,1)
            partialSill=PU(IMIN,2)
            nugget=0.0
            anisotropyAngle=0.0
            anisotropyRatio=1.0
            VAR=partialSill
          END IF
       ELSE IF (IPARA.EQ.3) THEN
          IF (NPEP.EQ.1) THEN
            range=PU(IMIN,1)
            partialSill=PU(IMIN,2)
            nugget=PU(IMIN,3)
            VAR=partialSill+nugget
            anisotropyAngle=0.0
            anisotropyRatio=1.0
          ELSE
            range=PU(IMIN,1)
            anisotropyAngle=PU(IMIN,2)
            anisotropyRatio=PU(IMIN,3)
            nugget=0.0
            partialSill=VAR
          END IF
       ELSE IF (IPARA.EQ.4) THEN
          IF (NPEP.EQ.1) THEN
            range=PU(IMIN,1)
            anisotropyAngle=PU(IMIN,2)
            anisotropyRatio=PU(IMIN,3)
            nugget=PU(IMIN,4)
            partialSill=VAR-nugget
          ELSE
            range=PU(IMIN,1)
            anisotropyAngle=PU(IMIN,2)
            anisotropyRatio=PU(IMIN,3)
            partialSill=PU(IMIN,4)
            nugget=0.0
            VAR=partialSill
          END IF
       ELSE
          range=PU(IMIN,1)
          anisotropyAngle=PU(IMIN,2)
          anisotropyRatio=PU(IMIN,3)
          partialSill=PU(IMIN,4)
          nugget=PU(IMIN,5)
          VAR=partialSill+nugget
       END IF
        HFINA=YMIN


RETURN
END SUBROUTINE Simplex



!==============================================================================
!| Description:
!   function called by Simplex
!
REAL FUNCTION Valf &
!
(a, varmodel)


IMPLICIT NONE

!Arguments with intent in:
REAL (KIND = float), INTENT (IN) :: a (:)
INTEGER (KIND = short), INTENT(IN) :: varmodel


!local declarations:
REAL (KIND = float) :: hoo
INTEGER (KIND = short) :: i

!---------------------------end of declarations--------------------------------

DO i = 1, ipara
    IF ( a (i) < parRangeMin (i) .OR. a (i) > parRangeMax (i) ) THEN
        Valf = 1.0E+15
        RETURN
    END IF
END DO
              
ieva = ieva + 1
SELECT CASE (ipara)
  CASE (1)
    range = a(1)
    anisotropyAngle = 0.0
    anisotropyRatio = 1.0
    partialSill = var
    nugget = 0.0
  CASE (2)
    IF (npep == 1) THEN
        range = a(1)
        nugget = a(2)
        partialSill = var - nugget 
        anisotropyAngle = 0.0
        anisotropyRatio = 1.0
    ELSE
        range = a(1)
        partialSill = a(2)
        nugget = 0.0
        anisotropyAngle = 0.0
        anisotropyRatio = 1.0
    END IF
  CASE (3)
    IF (npep == 1) THEN
        range = a(1)
        partialSill = a(2)
        nugget = a(3)
        anisotropyAngle = 0.0
        anisotropyRatio = 1.0
    ELSE
        range = a(1)
        anisotropyAngle = a(2)
        anisotropyRatio = a(3)
        nugget = 0.0
        partialSill = var
    END IF
  CASE (4)
    IF (npep == 1) THEN
        range = a(1)
        anisotropyAngle = a(2)
        anisotropyRatio = a(3)
        nugget = a(4)
        partialSill = var - nugget 
    ELSE
        range = a(1)
        anisotropyAngle = a(2)
        anisotropyRatio = a(3)
        partialSill = a(4)
        nugget = 0.0
    END IF
 CASE DEFAULT
    range = a(1)
    anisotropyAngle = a(2)
    anisotropyRatio = a(3)
    partialSill = a(4)
    nugget = a(5)
END SELECT

CALL Focompu (varmodel, hoo)

Valf = hoo

RETURN
END FUNCTION Valf

!==============================================================================
!| Description:
!   function called by Simplex
!
REAL FUNCTION Fconv &
!
(b, ipara)

IMPLICIT NONE

!Arguments with intent (in):
REAL (KIND = float), INTENT(IN) :: b (:)
INTEGER (KIND = short), INTENT(IN) :: ipara

!local declarations:
REAL (KIND = float) :: xmed, xval
INTEGER (KIND = short) :: i

!--------------------------------------end of declarations---------------------


xmed = 0.0

DO  i = 1, ipara + 1
    xmed = xmed + b (i)
END DO

xmed = xmed / (ipara + 1.0)
xval = 0.0
DO  i = 1, ipara + 1
    xval = xval + (b(i) - xmed )**2
END DO
Fconv = SQRT( xval / (ipara + 1.0) )
       
RETURN
END FUNCTION Fconv




!==============================================================================
!| Description:
!   evaluation of the objective function
!
SUBROUTINE Focompu &
!
(varmodel, hoo)

IMPLICIT NONE

!arguments with intent(in):
INTEGER (KIND = short), INTENT(IN) :: varmodel

!arguments with intent out
REAL (KIND = float), INTENT(OUT) :: hoo

!local declaration:
INTEGER (KIND = short) :: i, j
REAL (KIND = float) :: alpha
REAL (KIND = float) :: h
REAL (KIND = float) :: gam
REAL (KIND = float) :: dif2
REAL (KIND = float) :: wei

!----------------------------------end of declarations-------------------------

hoo = 0.0
DO  i = 1, ndir
    alpha = dir (i)
    DO  j = 1, lagNumber (i)
        h = dist (i,j)
        
        IF (h > 0.0) THEN
            CALL Variogr (varModel, h, alpha, gam)
            dif2 = ( empVariogram(i,j) - gam ) ** 2.
            IF (objectiveFunction == 1) THEN
              wei = 1.0
            ELSE IF (objectiveFunction == 2) THEN
              wei = pairNumber (i,j)
            ELSE IF (objectiveFunction == 3) THEN
              wei = 1.0 / ( gam * gam )
            ELSE IF (objectiveFunction == 4) THEN
              wei = pairNumber (i,j) / ( gam * gam )
            ELSE
              wei = pairNumber(i,j) / ( dist(i,j) * dist(i,j) )
            END IF
            hoo = hoo + wei * dif2
        END IF
        
    END DO
END DO

RETURN
END SUBROUTINE Focompu


!==============================================================================
!| Description:
!   calculation of the variogram value in 2D
!
SUBROUTINE Variogr &
!
(varModel, h, alpha, gam)

IMPLICIT NONE

!arguments with intent(in):
REAL (KIND = float), INTENT(IN) :: alpha
INTEGER (KIND = short), INTENT (IN) :: varmodel

!arguments with intent(out)
REAL (KIND = float), INTENT(OUT) :: gam

!arguments with intent(inout)
REAL (KIND = float), INTENT(INOUT) :: h

!local declaration:
REAL (KIND = float) :: zero
REAL (KIND = float) :: caa, cab, cac, dc2, ds2, x, y, dx, dy, w

!-------------------------------------end of declarations----------------------

zero = 0.000001
gam = 0.0

IF (h < zero) RETURN
gam = nugget

caa = 1.0
cab = 1.0
cac = 0.0

IF ( anisotropyRatio > 1.0 ) THEN
    dc2 = COS(anisotropyAngle)**2
    ds2 = SIN(anisotropyAngle)**2
    caa = dc2 + anisotropyRatio * ds2
    cab = ds2 + anisotropyRatio * dc2
    cac = ( 1.0 - anisotropyRatio ) * SIN(anisotropyAngle) * COS(anisotropyAngle)
END IF

x = h * COS(alpha)
y = h * SIN(alpha)
dx = x * caa + y * cac
dy = x * cac + y * cab
h = SQRT ( dx * dx + dy * dy )

IF ( varModel == 1) THEN !Spherical
    w = 1.5 * h / range - 0.5 * h * h * h / ( range * range * range )
    IF (h > range) w = 1.0
    
ELSE IF ( varModel == 2) THEN !Exponential
     w = 1.0 - EXP ( -3*h / range )
     !w = 1.0 - EXP ( - h / range )
    
ELSE IF ( varModel == 3) THEN !Gaussian
    w = 1.0 - EXP ( -10*h * h / (range * range) )
    
ELSE IF ( varModel == 4) THEN !Power
    w = h ** range
    
END IF   

gam = gam + w * partialSill

RETURN
END SUBROUTINE Variogr



!==============================================================================
!| Description:
!   Initialize variables for kriging
!
SUBROUTINE KrigingInitialize &
!
(anisotropy, nlags, maxlag, count, neighbors, nclosest)
    
IMPLICIT NONE

!Arguments with intent(in):
INTEGER (KIND = short), INTENT(IN) :: anisotropy !! if = 1, anisotropy is considered
INTEGER (KIND = short), INTENT(IN) :: nlags !! the number of discrete distances used for comparing data, if 0 it is set to default value
REAL (KIND = float), INTENT(IN) :: maxlag !! maximum distance to consider for semivariogram assessment (m), if 0 it is automatically computed
INTEGER (KIND = short), INTENT(IN) :: count !!total number of available observations
INTEGER (KIND = short), INTENT(IN) :: neighbors !! number of neighbors to consider for interpolation

!Arguments with intent(out)::
INTEGER (KIND = short), INTENT(OUT) :: nclosest

!local declarations:
INTEGER (KIND = short) :: lagnumbers !!number of lags used in computing semivariogram, default = 15
REAL (KIND = float) :: varcoverage !! variogram coverage, distance to consider for semivariogram assessment, (m), anisotropy case
REAL (KIND = float) :: varcoverageEW !! variogram coverage in East-South direction (0°)
REAL (KIND = float) :: varcoverageNESW !! variogram coverage in NorthEast-SouthWest direction (45°)
REAL (KIND = float) :: varcoverageNS !! variogram coverage in North-Sout direction (90°)
REAL (KIND = float) :: varcoverageNWSE !! variogram coverage in NorthWest-SoutEast direction (315°)
REAL (KIND = float) :: width (4)  !! distance between lags (m)
INTEGER (KIND = short) :: i, j, m, k

!----------------------------------end of declarations-------------------------

!set number of lags and allocate vectors
IF (nlags > 0) THEN
    lagnumbers = nlags
ELSE
    lagnumbers = 15 !default
END IF


IF (anisotropy == 1) THEN 
    ndir = 4
ELSE !only one direction is considered
    ndir = 1   
END IF

!allocate memory for semivariogram
ALLOCATE ( dir (ndir) ) !angle between the direction of the variogram and the x axis
ALLOCATE ( lagNumber (ndir) ) !number of lags for each direction
ALLOCATE ( dist (ndir, lagnumbers) ) !distance for each direction and lag
ALLOCATE ( empVariogram (ndir, lagnumbers) ) !experimental variogram for each direction and lag
ALLOCATE ( modVariogram (ndir, lagnumbers) ) !modelled variogram for each direction and lag
ALLOCATE ( pairNumber (ndir, lagnumbers) ) !number of pairs for each direction and lag

!set distance for semivariogram computation
IF (maxlag > 0.) THEN
    varcoverage = maxlag
ELSE !set to maximum distance between points
    IF (anisotropy == 0) THEN
         varcoverage = 2. ** 0.5 * MAXVAL (dist_pairs) / 2.  !sqrt(2) *1/2 Maximum point distance
    ELSE !search for maximum distance along  directions
       varcoverageEW   = 0.
       varcoverageNESW = 0.
       varcoverageNS   = 0.
       varcoverageNWSE = 0.
       DO j = 1, count !loop through pairs
           DO k = 1, count
               !EW direction
               IF ( dir_pairs (j,k) < pi / 8. .OR. dir_pairs (j,k) >= 15./8. * pi ) THEN 
                   IF ( dist_pairs (j,k) > varcoverageEW ) varcoverageEW = dist_pairs (j,k)
               END IF
               
               !NE-SW direction
               IF ( dir_pairs (j,k) >= pi / 8. .AND. dir_pairs (j,k) < 3./8. * pi  ) THEN 
                   IF ( dist_pairs (j,k) > varcoverageNESW ) varcoverageNESW = dist_pairs (j,k)
               END IF
               
               !N-S direction
               IF ( dir_pairs (j,k) >= 3./8. * pi .AND. dir_pairs (j,k) <= 1./2. * pi .OR. &
                    dir_pairs (j,k) > 3./2. * pi .AND. dir_pairs (j,k) < 13./8. * pi  ) THEN 
                   IF ( dist_pairs (j,k) > varcoverageNS ) varcoverageNS = dist_pairs (j,k)
               END IF
                    
               !NW-SE direction
               IF ( dir_pairs (j,k) >= 13./8. * pi .AND. dir_pairs (j,k) < 15./8. * pi  ) THEN 
                   IF ( dist_pairs (j,k) > varcoverageNWSE ) varcoverageNWSE = dist_pairs (j,k)
               END IF     
               
           END DO
       END DO
                          
    END IF
END IF

varcoverageEW = 2. ** 0.5 * varcoverageEW / 2.
varcoverageNESW = 2. ** 0.5 * varcoverageNESW / 2.
varcoverageNS = 2. ** 0.5 * varcoverageNS / 2.
varcoverageNWSE = 2. ** 0.5 * varcoverageNWSE / 2.



!set distance between lags (m). Assume even spaced lags
IF (anisotropy == 0) THEN
    width = varcoverage / lagnumbers
ELSE
    width (1) = varcoverageEW / lagnumbers
    width (2) = varcoverageNESW / lagnumbers
    width (3) = varcoverageNS / lagnumbers
    width (4) = varcoverageNWSE / lagnumbers
END IF

!populate lags vectors
DO i = 1, ndir
    DO j = 1, lagnumbers
       dist (i,j) = width (i) * j
    END DO
END DO

!set tolerance
IF (anisotropy == 0) THEN
  lagTolerance = width (1) / 2. !by default tolerance is half lag width
ELSE
    DO i = 1, ndir
        lagTolerance (i) = width (i) / 2.
    END DO
END IF
!initialize
DO i = 1, ndir !number of lags is the same for each direction
  lagNumber (i) = lagnumbers
END DO

IF (anisotropy) THEN
    !set 4 directions: E-W (0°), NE-SW (45°), N-S (90°), NW-SE (315°)
    dir (1) = 0. !E-W
    dir (2) = 45. * degToRad ! NE-SW
    dir (3) = 90. * degToRad ! N-S
    dir (4) = 315. * degToRad ! NW-SE
ELSE
    !direction is not used
    dir (1) = 0. 
END IF

pairNumber = 0
empVariogram = 0.

ieva = 0
nugget = 0.0
range = 0.0
partialSill = 0.0
anisotropyAngle = 0.0
anisotropyRatio = 1.0
objectiveFunction = 4 !set objective function to minimize

npep = 1 !1 = nugget is estimated
nvar = 0 !1 means variance = experimental variance

!find the dimension of matrixes for interpolation
IF (neighbors > 0) THEN
    IF ( neighbors > count ) THEN !too many neighbors respect to available observations
         nclosest = count !limit dimension to available observations number
    ELSE
         nclosest = neighbors
    END IF
ELSE !neighbors = 0 => include all observations
    nclosest = count
END IF

!Allocate memory for interpolation
ALLOCATE ( weights ( nclosest + 1 ) )
ALLOCATE ( hest ( nclosest ) )
ALLOCATE ( cvest ( nclosest + 1 ) )
ALLOCATE ( controlpts ( nclosest ) )
ALLOCATE ( hobs ( nclosest,nclosest ) )
ALLOCATE ( cvobs( nclosest + 1, nclosest + 1 ) )
ALLOCATE ( last (nclosest) )


RETURN
END SUBROUTINE KrigingInitialize



!==============================================================================
!| Description:
!   Subroutine to find the distances between prediction point and
!	observations. Also returns angles if anisotropy present
!
SUBROUTINE Separation &
!!
(prediction, observations, estdist)

IMPLICIT NONE

!Arguments with intent(in):
TYPE (site), INTENT(IN) :: prediction
TYPE (ObservationalNetwork), INTENT(IN) :: observations

!Arguments with intent(out):
TYPE( site), DIMENSION(:), INTENT(out) :: estdist

!local declarations:
INTEGER (KIND = short) :: i

!----------------------------------------end of declarations-------------------

DO i = 1, observations % countObs
  estdist (i) % h = Distance (prediction % xyz, observations % obs (i) % xyz)
  estdist (i) % oid = i
  estdist (i) % value = observations % obs (i) % value
  estdist (i) % xyz % easting = observations % obs (i) % xyz % easting
  estdist (i) % xyz % northing = observations % obs (i) % xyz % northing
  !Return the angle between points if anisotropy is present 
  IF ( anisotropyAngle > 0.) THEN
     estdist (i) % theta = pi / 2. - DirectionAngle (prediction % xyz, observations % obs (i) % xyz)
     !estdist (i) % theta = ATAN ( (observations % obs (i) % xyz % northing - prediction % xyz % northing) /  &
     ! (observations % obs (i) % xyz % easting - prediction % xyz % easting)  )  
  ELSE
     estdist (i) % theta = 0.
  END IF
  
END DO

RETURN
END SUBROUTINE Separation


!==============================================================================
!| Description:
! A subroutine to retrieve the nearest neighbors of the estimation location
! referred to as control points. The observations separation matrix is
! returned (as a precursor to producing the observations covariance matrix)
! along with the vector of nearest neighbbors.
!
SUBROUTINE Sorted &
!
( estdist, neighbors )
	
IMPLICIT NONE

!Arguments with intent(in):
INTEGER (KIND = short), INTENT(IN) :: neighbors

!Arguments with intent(inout):
TYPE(site),DIMENSION(:),INTENT(inout) :: estdist
	
!Local variable declarations
INTEGER :: i,j,p1,p2

!------------------------------------end of declarations-----------------------
	
!Sort estdist by h (distance) to find the nearest neighbors of the prediction location
CALL Sort (estdist,0)

!Select control points for kriging: the n nearest neighbors
controlpts = estdist(1:neighbors)

!Construct observations distance matrix (hobs) from the control points
DO i=1,neighbors
	DO j=1,neighbors
		p1=controlpts(i)%oid
		p2=controlpts(j)%oid
		hobs(i,j)=obsdist(p1,p2)
	END DO
END DO

END SUBROUTINE Sorted


!==============================================================================
!| Description:
! Subroutine to sort a vector of points using the Heap-sort algorithm. 
! Data are sorted by value if flag =1, or by separation distance if flag = 0.
! Actually, only a pointer is sorted, as this is more efficient. Subroutine
! Adapted from Numerical Recipes in Fortran 90: Press, Teukolsky, Vetterling 
! and Flannery (1996) pg. 1171 
!
SUBROUTINE Sort &
!
(input, flag)
	
IMPLICIT NONE
	
!Arguments with intent(in):
INTEGER,INTENT(in) :: flag

!Arguments with intent(inout) 
TYPE(site), DIMENSION(:), INTENT(inout) :: input

	
!Local variable declarations
INTEGER :: i,n
TYPE(site), DIMENSION(:), POINTER :: work
TYPE(site) :: dummy
!----------------------------------end of declarations-------------------------
	
n=SIZE(input)
ALLOCATE(work(n))
work=input

DO i=n/2,1,-1
	!Loop down the left range of the sift-down: The element to be sifted
	!is decremented to n/2 to 1 during "Hiring" in heap creation
	CALL sift_down(i,n,flag)
END DO

DO i=n,2,-1
	!Loop down the right range of the sift-down: Decremented from n-1 to
	!1 during the "Retirement and promotion" stage of heap creation
		
	!Clear space at the top of the array and retire the top of the heap into it
	dummy=work(1)
	work(1)=work(i)
	work(i)=dummy

	CALL sift_down(1,i-1,flag)
END DO

input=work

DEALLOCATE (work)

CONTAINS

SUBROUTINE sift_down(l,r,flag)
	IMPLICIT NONE
	!Dummy argument declaration
	INTEGER,INTENT(in) :: l,r,flag
	
	!Local variable declarations
	INTEGER :: j,old
	Type(site) :: a
	
	!Carry out sift-down on element array(l) to maintain heap structure
		
	!Get element to sift
	a=work(l)
	old=l
	j=l+l
		
	SELECT CASE(flag)

	CASE(0)
		!If flag = 0, sort by h
		!Do while j<=r
		DO
			IF(j>r) EXIT
			IF(j<r) THEN
				!Compare to the better underling
				IF(work(j)%h<work(j+1)%h) j=j+1
			END IF

			!If found a's level, terminate the sift-down, else demote and continue
			IF(a%h >= work(j)%h) EXIT
			work(old) = work(j)
			old=j
			j=j+j
		END DO
		
	CASE(1)
		!If flag = 1, sort by value
		!Do while j<=r
		DO
			IF(j>r) EXIT
			IF(j<r) THEN
				!Compare to the better underling
				IF(work(j)%value<work(j+1)%value) j=j+1
			END IF

			!If found a's level, terminate the sift-down, else demote and continue
			IF(a%value >= work(j)%value) EXIT
			work(old) = work(j)
			old=j
			j=j+j
		END DO
		
	END SELECT

	!Put into its slot
	work(old)=a
END SUBROUTINE sift_down

END SUBROUTINE Sort




!==============================================================================
!| Description:
! Subroutine to calculate a covariance vector from a semivariogram model
! and a vector of separation distances.
!
SUBROUTINE Covariance &
!
(dist, theta, cov, sill, range, varmodel)

IMPLICIT NONE

!Arguments with intent(in)
REAL (KIND = float), INTENT(in) ::  sill  !! partial sill (total sill - nugget) from automatic fitting
REAL (KIND = float), INTENT(in) :: range  !! range from automatic fitting
REAL (KIND = float), DIMENSION(:), INTENT(IN) :: dist !!separation distance vector
REAL (KIND = float), DIMENSION(:), INTENT(IN) :: theta !!anisotropy angle ??
INTEGER (KIND = short), INTENT(in) :: varmodel  !!semivariogram model: 1 = spherical, 2 = exponential, 3 = gaussian, 4 = power

!Arguments with intent(out):
REAL (KIND = float), DIMENSION(:), INTENT(OUT) :: cov !!covariance vector


!Local variable declarations
REAL (KIND = float), DIMENSION(SIZE(dist)) :: h_prime
INTEGER (KIND = short) :: i

!------------------------------end of declarations-----------------------------

!h_prime = dist ! da correggere con anisotropia quando sarà implementata
    
!Check for anisotropy
IF (anisotropyAngle == 0.) THEN
	h_prime = dist
ELSE
	!Transform distance vector by applying anisotropy correction factor:
	!The minor axis of variation reaches sill before major axis of variation;
	!this is achieved by adding extra distance to the point separations 
	!to 'pull back' the range towards the origin. Separation distances 
	!are reprojected into a transformed coordinate system h'. 
	!First theta is rotated to the major axis of semivariance lies on the horizontal.
	!The anisotropy correction factor is calculated by multiplying the SIN of the 
	!angle between the major axis of variation and the point separation angle by the 
	!eccentricity of the anisotropy ellipsoid (major semiaxis/minor semiaxis). 
	!The transformed coordinate system h' is then the product of the separation 
	!and the correction factor.
    
	!h_prime = dist + dist * ABS( SIN( theta - rotation(i) ) * (theta3(i)/theta2(i)) )
    h_prime = dist + dist * ABS( SIN( theta - anisotropyAngle ) * anisotropyRatio )
END IF
        
!covariance = nugget + partialsill - semivariogram(h)
!since semivariogram(h) = nugget + partialsill * f(h,range) => covariance = partialsill * ( 1- f(h,range) )
SELECT CASE(varmodel)
    CASE (1) !Spherical
		WHERE(h_prime < range)
        cov = sill * (1. - ( 1.5 * (h_prime / range) - 0.5 * (h_prime / range)**3. ) )
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (2) !Exponential
        WHERE(h_prime < range)
        !cov = sill * (1. - ( 1. - EXP (- h_prime / range) ) )
        cov = sill * (1. - ( 1. - EXP (- 3 * h_prime / range) ) )
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (3) !Gaussian: MUST have a nugget effect to function without instability
        WHERE(h_prime < range)
        !cov = sill * (1. - ( 1. - EXP (- h_prime * h_prime / (range * range) ) ) ) 
        cov = sill * (1. - ( 1. - EXP (- 10 * h_prime * h_prime / (range * range) ) ) ) 
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (4) !Power
        WHERE(h_prime < range)
        cov = sill * (1. - (h_prime / range) ) 
		ELSEWHERE
        cov = 0.
        END WHERE

END SELECT 

RETURN

END SUBROUTINE Covariance


!==============================================================================
!| Description:
!  Subroutine to calculate a covariance matrix of observations 
!  from a semivariogram model and a vector of separation distances.
!
SUBROUTINE Covariance2D &
!
(dist, theta, cov, sill, range, varmodel)

IMPLICIT NONE

!Arguments with intent(in)
REAL (KIND = float), INTENT(in) ::  sill  !! partial sill (total sill - nugget) from automatic fitting
REAL (KIND = float), INTENT(in) :: range  !! range from automatic fitting
REAL (KIND = float), DIMENSION(:,:), INTENT(IN) :: dist !!separation distance vector
REAL (KIND = float), DIMENSION(:,:), INTENT(IN) :: theta !!anisotropy angle ??
INTEGER (KIND = short), INTENT(in) :: varmodel  !!semivariogram model: 1 = spherical, 2 = exponential, 3 = gaussian, 4 = power

!Arguments with intent(out):
REAL (KIND = float), DIMENSION(:,:), INTENT(OUT) :: cov  !!covariance matrix

!Local variable declarations
REAL (KIND = float), DIMENSION(SIZE(dist,1),SIZE(dist,2)) :: h_prime 
INTEGER (KIND = short) :: i

!---------------------------------end of declarations--------------------------

!h_prime = dist ! da correggere con anisotropia quando sarà implementata

	
!Check for anisotropy
IF (anisotropyAngle == 0.) THEN
	h_prime = dist
ELSE
	!Transform distance vector by applying anisotropy correction factor
 	h_prime = dist + dist * ABS( SIN( theta - anisotropyAngle ) * anisotropyRatio )
END IF

!covariance = nugget + partialsill - semivariogram(h)
!since semivariogram(h) = nugget + partialsill * f(h,range) => covariance = partialsill * ( 1- f(h,range) )
SELECT CASE(varmodel)
	CASE (1) !Spherical
		WHERE(h_prime < range)
        cov = sill * (1. - ( 1.5 * (h_prime / range) - 0.5 * (h_prime / range)**3. ) )
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (2) !Exponential
        WHERE(h_prime < range)
        !cov = sill * (1. - ( 1. - EXP (- h_prime / range) ) )
        cov = sill * (1. - ( 1. - EXP (- 3*  h_prime / range) ) )
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (3) !Gaussian: MUST have a nugget effect to function without instability
        WHERE(h_prime < range)
        !cov = sill * (1. - ( 1. - EXP (- h_prime * h_prime / (range * range) ) ) ) 
        cov = sill * (1. - ( 1. - EXP (- 10 * h_prime * h_prime / (range * range) ) ) ) 
		ELSEWHERE
        cov = 0.
        END WHERE
             
    CASE (4) !Power
        WHERE(h_prime < range)
        cov = sill * (1. - (h_prime / range) ) 
		ELSEWHERE
        cov = 0.
        END WHERE

END SELECT 

RETURN	
END SUBROUTINE Covariance2D



!==============================================================================
!| A generic subroutine to invert a square 2D matrix by pivoting 
! ("Gauss-Jordan" method). An n*2,n work array is assembled, with the array 
! of interest on the left half, and the identity matrix on the right half. 
! A check is made to ensure the matrix is invertable, and the matrix inverse 
! is returned, providing the matrix is not singular. The matrix
! is invertable if, after pivoting and row reduction, the identity matrix 
! shifts to the left half of the work array. Initially the code was written 
! in integer form to avoid fractions in the work array, but numbers rapidly 
! become inconcevably large. The implemented solution therefore works on 
! fractional pivot rows, with pivots factored to leading ones. 
! Double precision arrays have been used to maintain numerical stability.
!
! Copyright (C) 2006  Luke Spadavecchia
! This program 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 2 of the License, or
! (at your option) any later version.
! This program 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 this program; if not, write to the Free Software Foundation, Inc.,
! 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
!
SUBROUTINE Matinv &
!
(input, output)
	
IMPLICIT NONE

!Arguments with intent(in):
REAL (KIND = float), DIMENSION(:,:), INTENT(in) :: input

!Arguments with intent(out):
REAL (KIND = float), DIMENSION(:,:), INTENT(out) :: output

!Local variable declarations
INTEGER (KIND = short) :: i, row ,n, column
INTEGER (KIND = short), DIMENSION ( SIZE (input,1), SIZE(input,2) ) :: identity
REAL (KIND = double) :: a, b, pivot
REAL (KIND = double), DIMENSION ( SIZE(input,1)*2, SIZE(input,2) ) :: work
!---------------------------end of declarations--------------------------------

!Get size of input
n=SIZE(input,1)

!Build identity matrix
identity=0
	
!Make diagonals = 1
DO i=1,n
	identity(i,i)=1
END DO

!Build work array:	
!Input array on the left half. 
work(1:n,:)=input

!Identity matrix on the right half
work(n+1:n*2,:) = identity

!Step through the work array rows
DO row=1,n
	!Step through row j's columns
	DO i=1,n*2
		!Find first non-zero element of row and record it as the pivot
		IF (work(i,row) .NE. 0) THEN
			pivot = work(i,row)
			column = i
			EXIT
		END IF
	END DO

	!Divide pivot row by pivot to rescale for leading 1
	work(:,row)=work(:,row)/pivot

	!Loop over rows to 'Clear' the pivot column using the pivot value
	DO i=1,n
		!Skip over if i is the pivot row
		IF (i == row) CYCLE

		!Get clearance row scaling factor
		b = work(column,i)
				
		!Replace row with the following:
		!Difference between clearance row and (b * pivot row)
		work(:,i) = work(:,i) - b*work(:,row)
	END DO 
END DO

!Validate that the inversion is successful, by checking left side of work array
!is equal to the identity matrix.
IF (ANY(work(1:n,:) .NE. identity)) THEN
	PRINT '("Inversion not possible: Matrix is degenerate!"/)'
	PAUSE
	STOP
END IF

!Output the inverse of input array (right hand side of work array).
output = work(n+1:2*n,:) 

RETURN
END SUBROUTINE Matinv



!==============================================================================
!| A subroutine to predict the data value of an unsampled location, 
! using geostatistical methods. Interpolation is carried out using 
! the 'Ordinary Kriging' method. 
! 
SUBROUTINE Interpolate &
!
(cvobs, cvest, controlpts, est, var, neighbors, sill)
	
IMPLICIT NONE

!Arguments with intent(in):
INTEGER (KIND = short),              INTENT(IN) :: neighbors
REAL (KIND = float),                 INTENT(IN) :: sill
REAL (KIND = float), DIMENSION(:),   INTENT(IN) :: cvest
REAL (KIND = float), DIMENSION(:,:), INTENT(IN) :: cvobs
REAL (KIND = float), DIMENSION(:),   INTENT(IN) :: controlpts

!arguments with intent(out):
REAL (KIND = float), INTENT(OUT) :: est !!estimated value
REAL (KIND = float), INTENT(OUT) :: var !!variance of estimated value

!Local variable declarations
INTEGER (KIND = short) :: n
!REAL,DIMENSION(neighbors+1) :: musv
REAL (KIND = float) :: ck
!--------------------------end of declarations---------------------------------

n = SIZE(controlpts)

!Derive Weights
weights = MATMUL(cvobs,cvest)
		
!Check weights sum to 1 (n=1 th element is the lagrange parameter)
ck = SUM(weights(1:neighbors))
IF (NINT(ck) .NE. 1) THEN
    CALL Catch ('warning', 'Kriging interpolation',  &
          'Kriging weights do not sum to 1' )
END IF
			 
!Estimate datum value sum(weights*observations)
est = SUM ( weights(1:neighbors) * controlpts )

!Estimate the local mean: derive new set of weights from the observtions covariance
!matrix and an estimateion covariance vector which is equal to zero. This filters
!the residual component of the estimate, returning the local mean.
!musv=0
!musv(neighbors+1)=1
!musv=MATMUL(cvobs,musv)
!localmu=SUM(musv(1:neighbors)*controlpts)

!Derive Estimation Variance: ssum(weights*semivariances) + lagrangian
var = sill - SUM(weights(1:n)*cvest(1:n)) + weights(n+1)

IF ( var < 0. ) THEN
    CALL Catch ('warning', 'Kriging interpolation',  &
          'Negative variance produced! Check validity &
            of semivariogram model' )
END IF

RETURN
END SUBROUTINE Interpolate
    
END MODULE Kriging