! -------------------------------------------------------------------------
! Bernese GPS Software Version 5.1
! -------------------------------------------------------------------------

MODULE s_WGTKIN

  IMPLICIT NONE

#include "COM_CPU.inc"

CONTAINS

SUBROUTINE wgtkin(ip0,nPar,aNor,par,stName,clkhed,clkrec, &
                    DATUM,xStat,sigapr,nstwgt,istwgt,stWgt)

! -------------------------------------------------------------------------
! Purpose:    Compute covariance matrix for XYZ coordinate constraining
!             from NEU sigmas for kinematic positioning
!
! Remark:     For non-ground stations (markerType /= " ") stWgt contains
!             constraints in XYZ (per default one value).
!             If a kinematic input file (KININP) is available only epochs
!             with flag "K" are constrained.
!
! Author:     R. Dach
!
! Created:    21-Jul-2003
!
! Changes:    28-Jun-2013 SC: use M_BERN with ONLY
!             01-Dec-2016 KB: Add srName, call cpu_enter and cpu_exit
!             11-Apr-2017 ST: Move IMPLICIT NONE to module scope
!             11-Apr-2017 ST: Add #include "COM_CPU.inc"
!             11-Apr-2017 ST: Add IF(srfDebug) in front of CALL cpu_
!             10-Jul-2019 ST: Use LEN=* for srName
!             17-Aug-2018 AV: Replace locq (-> par) with t_par
!             19-Aug-2020 MK: use dXYZNEU
!             24-Aug-2020 MK: use t_datum
!             30-May-2024 EB: MARTYP not considered (TYPE 005 entries)
!
! Copyright:  Astronomical Institute
!             University of Bern
!             Switzerland
! -------------------------------------------------------------------------

! Modules
! -------
  USE m_bern,   ONLY: i4b, r8b, lfnerr, fileNameLength
  USE m_cpu,    ONLY: cpu_enter, cpu_exit
  USE d_clkrnx, ONLY: t_clkHead, t_clkRec
  USE d_par,    ONLY: t_par
  USE d_datum,  ONLY: t_datum
  USE f_ikf
  USE s_staflg
  USE s_readkin
  USE s_gtflna
  USE s_xyzell
  USE f_dXYZNEU


! List of arguments
! ------------------
! input:
  INTEGER(i4b)                   :: ip0         ! Start searching for kin param.
  INTEGER(i4b)                   :: nPar        ! Total number of parameters

! input/output
  REAL(r8b),    DIMENSION(*)     :: aNor        ! Normal equation

! input
  TYPE(t_par), DIMENSION(:)      :: par         ! parameter description
  CHARACTER(LEN=*), DIMENSION(*) :: stName      ! List of station names
  TYPE(t_datum)                  :: DATUM       ! geodetic DATUM
  REAL(r8b),    DIMENSION(3,*)   :: xStat       ! coordinates of station
  INTEGER(i4b)                   :: nstwgt      ! # stations with a priori
                                                ! weights for coordinates
  INTEGER(i4b), DIMENSION(*)     :: istwgt      ! station numbers for weights
  REAL(r8b),    DIMENSION(3,*)   :: stwgt       ! a priori weights for stations
  TYPE(t_clkHead)                :: clkhed ! %tFirst: MJD of first epoch param.
  TYPE(t_clkRec)                 :: clkrec ! %nEpo: # epochs with highest sampl.
                                           ! %epoch(1): last epoch (sec. since
                                           !   clkhed%tFirst)
  REAL(r8b)                      :: sigapr ! a priori sigma


! Local Variables
! ---------------
  CHARACTER(LEN=20)                   :: marTyp
  CHARACTER(LEN=fileNameLength), SAVE :: kininp

  INTEGER(i4b)                        :: ip
  INTEGER(i4b)                        :: iSta,jSta
  INTEGER(i4b)                        :: iEpo
  INTEGER(i4b)                        :: iFlag
  INTEGER(i4b)                        :: ii,kk,ik
  INTEGER(i4b),                  SAVE :: ircKin, ircPos

  REAL(r8b)                           :: dSec,tEpoch
  REAL(r8b), DIMENSION(3)             :: xKin,xEll
  REAL(r8b), DIMENSION(3,3)           :: covxyz, covplh, mC

  LOGICAL,                       SAVE :: first = .TRUE.

! Local Parameters
! ----------------
  CHARACTER(LEN=*), PARAMETER :: srName = 'wgtkin'

! DEBUG: entry
! ------------
  IF (srfDebug) CALL cpu_enter(srName)

! Get the name of a kinematic input file
! --------------------------------------
  IF (first) THEN
    CALL gtflna(0,'KININP',kininp,ircKin)
    first = .FALSE.
  ENDIF

! Loop all parameters
! -------------------
  DO ip = ip0,npar

! Find X-component of kinematic coordinates
! -----------------------------------------
    IF (par(ip)%locq(1) /= 21) CYCLE
    IF (par(ip)%locq(3) /=  1) CYCLE

! Check the other coordinate components
! -------------------------------------
    iSta = par(ip)%locq(2)
    iEpo = par(ip)%locq(4)

    IF (par(ip+1)%locq(1) /=   21 .OR. par(ip+2)%locq(1) /=   21 .OR. &
        par(ip+1)%locq(2) /= iSta .OR. par(ip+2)%locq(2) /= iSta .OR. &
        par(ip+1)%locq(3) /=    2 .OR. par(ip+2)%locq(3) /=    3 .OR. &
        par(ip+1)%locq(4) /= iEpo .OR. par(ip+2)%locq(4) /= iEpo) THEN
      WRITE(lfnerr,'(/,A,2(/,16X,A),/,16X,A,I10,/)')                 &
      ' ### SR WGTKIN: A kinematic coordinate component is missing.',&
                      '(may be due problems inverting the NEQ).',    &
                      'Station name:  '//TRIM(stName(iSta)),         &
                      'Epoch number:  ',iepo
      CYCLE
    ENDIF

! There are constraints for this stations?
! ----------------------------------------
    jSta = 0
    DO ii = 1,nstwgt
      IF (iSta == istwgt(ii)) jSta = ii
    ENDDO

    IF (jSta == 0) CYCLE

! Compute the epoch
! -----------------
    dsec   = DBLE(iEpo-1)*clkrec%epoch(1)/clkrec%nEpo
    tEpoch = clkhed%tFirst+dsec/86400.D0

! Constrain only with flag "K" in KININP
! --------------------------------------
    IF (ircKin == 0) THEN
      CALL readKin(kinInp,stName(iSta),tEpoch,1,0,xKin,ircPos)
      IF (ircPos == 1) CYCLE
    ENDIF

! Get the marker type
! -------------------
    CALL staFlg(stName(iSta),tEpoch,iFlag,marTyp)

! Terrestrial station: convert NEU->XYZ
! -------------------------------------
!    IF (marTyp == ' ') THEN

      ! Get the actual coordinates:
      IF (ircKin /= 0 .OR. ircPos /= 0)  xKin(1:3) = xStat(1:3,iSta)

      CALL xyzell(DATUM,xKin,xEll)

      covplh = 0.D0

      DO ii = 1,3
        IF (STWGT(ii,jSta) > 0D0) covplh(ii,ii) = (sigapr/STWGT(ii,jSta))**2
      ENDDO

      mC = dXYZNEU(xEll(1), xEll(2))
      covxyz = matmul(matmul(transpose(mC),covplh),mC)

      ! Put weights on normal equation
      DO ii = 1,3
        DO kk = 1,ii
          ik=IKF(ii+ip-1,kk+ip-1)
          ANOR(ik)=ANOR(ik)+covxyz(ii,kk)
        ENDDO
      ENDDO

! Other marker types: constraints are in XYZ
! ------------------------------------------
!    ELSE
!
!      DO ii =1,3
!        ik=IKF(ii+ip-1,ii+ip-1)
!        ANOR(ik)=ANOR(ik)+(sigapr/stWgt(ii,jSta))**2
!      ENDDO
!
!    ENDIF

  ENDDO ! Next parameter

! DEBUG: exit
! -----------
  IF (srfDebug) CALL cpu_exit(srName)

  RETURN
END SUBROUTINE wgtkin

END MODULE
