!   G13EJF Example Program Text
!   Mark 26.1 Release. NAG Copyright 2016.

    Module g13ejfe_mod

!     G13EJF Example Program Module:
!     User-defined Routines

!     .. Use Statements ..
      Use nag_library, Only: nag_wp
!     .. Implicit None Statement ..
      Implicit None
!     .. Accessibility Statements ..
      Private
      Public                           :: f, h, read_problem_data
!     .. Parameters ..
      Integer, Parameter, Public       :: mx = 3, my = 2, nin = 5, nout = 6
!     .. Derived Type Definitions ..
      Type, Public                     :: g13ej_problem_data
        Real (Kind=nag_wp)             :: delta, a, r, d
        Real (Kind=nag_wp)             :: phi_rt, phi_lt
      End Type g13ej_problem_data
    Contains
      Subroutine f(n,xt,fxt,dat)

!       .. Scalar Arguments ..
        Type (g13ej_problem_data), Intent (In) :: dat
        Integer, Intent (In)           :: n
!       .. Array Arguments ..
        Real (Kind=nag_wp), Intent (Out) :: fxt(:,:)
        Real (Kind=nag_wp), Intent (In) :: xt(:,:)
!       .. Local Scalars ..
        Real (Kind=nag_wp)             :: t1, t3
        Integer                        :: i
!       .. Intrinsic Procedures ..
        Intrinsic                      :: cos, sin
!       .. Executable Statements ..
        Continue

        t1 = 0.5_nag_wp*dat%r*(dat%phi_rt+dat%phi_lt)
        t3 = (dat%r/dat%d)*(dat%phi_rt-dat%phi_lt)

        Do i = 1, n
          fxt(1,i) = xt(1,i) + cos(xt(3,i))*t1
          fxt(2,i) = xt(2,i) + sin(xt(3,i))*t1
          fxt(3,i) = xt(3,i) + t3
        End Do

        Return
      End Subroutine f
      Subroutine h(n,yt,hyt,dat)

!       .. Use Statements ..
        Use nag_library, Only: x01aaf
!       .. Scalar Arguments ..
        Type (g13ej_problem_data), Intent (In) :: dat
        Integer, Intent (In)           :: n
!       .. Array Arguments ..
        Real (Kind=nag_wp), Intent (Out) :: hyt(:,:)
        Real (Kind=nag_wp), Intent (In) :: yt(:,:)
!       .. Local Scalars ..
        Real (Kind=nag_wp)             :: tmp
        Integer                        :: i
!       .. Intrinsic Procedures ..
        Intrinsic                      :: cos, sin
!       .. Executable Statements ..
        Continue

        Do i = 1, n
          hyt(1,i) = dat%delta - yt(1,i)*cos(dat%a) - yt(2,i)*sin(dat%a)
          hyt(2,i) = yt(3,i) - dat%a

!         Make sure that the theta is in the same range as the observed
!         data, which in this case is [0, 2*pi)
          If (hyt(2,i)<0.0_nag_wp) Then
            hyt(2,i) = hyt(2,i) + 2*x01aaf(tmp)
          End If
        End Do

        Return
      End Subroutine h
      Subroutine read_problem_data(t,dat,read_ok)
!       Read in any data specific to the F and H subroutines

!       .. Scalar Arguments ..
        Type (g13ej_problem_data), Intent (Inout) :: dat
        Integer, Intent (In)           :: t
        Logical, Intent (Out)          :: read_ok
!       .. Local Scalars ..
        Integer                        :: tt
!       .. Executable Statements ..
        Continue

        If (t==0) Then
!         Read in the data that is constant across all time points
          Read (nin,*) dat%r, dat%d, dat%delta, dat%a
          read_ok = .True.
        Else
!         Read in data for time point t
          Read (nin,*) tt, dat%phi_rt, dat%phi_lt
          If (tt/=t) Then
!           Sanity check
            Write (nout,99999) 'Expected to read in data for time point ', t
            Write (nout,99999) 'Data that was read in was for time point ', tt
99999       Format (A,E22.15)
            read_ok = .False.
          Else
            read_ok = .True.
          End If
        End If
      End Subroutine read_problem_data
    End Module g13ejfe_mod

    Program g13ejfe

!     .. Use Statements ..
      Use g13ejfe_mod, Only: f, g13ej_problem_data, h, mx, my, nin, nout,      &
                             read_problem_data
      Use nag_library, Only: g13ejf, nag_wp
!     .. Implicit None Statement ..
      Implicit None
!     .. Local Scalars ..
      Type (g13ej_problem_data)        :: dat
      Integer                          :: i, ifail, irevcm, ldfxt, ldlx, ldly, &
                                          ldst, ldxt, licomm, lrcomm, lropt,   &
                                          n, ntime, t
      Logical                          :: read_ok
!     .. Local Arrays ..
      Real (Kind=nag_wp), Allocatable  :: fxt(:,:), lx(:,:), ly(:,:),          &
                                          rcomm(:), ropt(:), st(:,:), x(:),    &
                                          xt(:,:), y(:)
      Integer, Allocatable             :: icomm(:)
!     .. Intrinsic Procedures ..
      Intrinsic                        :: abs, max, repeat
!     .. Executable Statements ..
      Write (nout,*) 'G13EJF Example Program Results'
      Write (nout,*)

!     Skip heading in data file
      Read (nin,*)

!     Using default optional arguments
      lropt = 0
      Allocate (ropt(lropt))

!     Allocate arrays
      n = 2*mx + 1
      If (lropt>=1) Then
        If (abs(ropt(1)-2.0_nag_wp)<=0.0_nag_wp) Then
          n = n + 2*mx
        End If
      End If
      ldlx = mx
      ldly = my
      ldst = mx
      ldxt = mx
      ldfxt = max(mx,my)
      licomm = 30
      lrcomm = 30 + my + mx*my + 2*max(mx,my)
      Allocate (lx(ldlx,mx),ly(ldly,my),x(mx),st(ldst,mx),xt(ldxt,max(my,      &
        n)),fxt(ldfxt,n+max(mx,my)),icomm(licomm),rcomm(lrcomm),y(my))

!     Read in the Cholesky factorization of the covariance matrix for the
!     process noise
      Do i = 1, mx
        Read (nin,*) lx(i,1:i)
      End Do

!     Read in the Cholesky factorization of the covariance matrix for the
!     observation noise
      Do i = 1, my
        Read (nin,*) ly(i,1:i)
      End Do

!     Read in the initial state vector
      Read (nin,*) x(1:mx)

!     Read in the Cholesky factorization of the initial state covariance
!     matrix
      Do i = 1, mx
        Read (nin,*) st(i,1:i)
      End Do

!     Read in the number of time points to run the system for
      Read (nin,*) ntime

!     Read in any problem specific data that is constant
      Call read_problem_data(0,dat,read_ok)
      If (.Not. read_ok) Then
        Go To 100
      End If

!     Title for first set of output
      Write (nout,*) ' Time   ', repeat(' ',(11*mx-16)/2), 'Estimate of State'
      Write (nout,*) repeat('-',7+11*mx)

!     Loop over each time point
      irevcm = 0
      Do t = 1, ntime

!       Read in any problem specific data that is time dependent
        Call read_problem_data(t,dat,read_ok)
        If (.Not. read_ok) Then
          Go To 100
        End If

!       Read in the observed data for time t
        Read (nin,*) y(1:my)

!       Call Unscented Kalman Filter routine
ukf_lp: Do
          ifail = 0
          Call g13ejf(irevcm,mx,my,y,lx,ldlx,ly,ldly,x,st,ldst,n,xt,ldxt,fxt,  &
            ldfxt,ropt,lropt,icomm,licomm,rcomm,lrcomm,ifail)
          Select Case (irevcm)
          Case (1)
!           Evaluate F(X)
            Call f(n,xt,fxt,dat)

          Case (2)
!           Evaluate H(X)
            Call h(n,xt,fxt,dat)

          Case Default
!           IREVCM = 3, finished
            Exit ukf_lp
          End Select
        End Do ukf_lp

!       Display the some of the current state estimate
        Write (nout,99999) t, x(1:mx)
      End Do

      Write (nout,*)
      Write (nout,*) 'Estimate of Cholesky Factorization of the State'
      Write (nout,*) 'Covariance Matrix at the Last Time Point'
      Do i = 1, mx
        Write (nout,99998) st(i,1:i)
      End Do

100   Continue

99999 Format (1X,I3,4X,10(1X,F10.3))
99998 Format (10(1X,E10.3))
    End Program g13ejfe