!==============================================================================!
  subroutine Calculate_Geometry(grid, real_run)
!------------------------------------------------------------------------------!
!   Calculates geometrical quantities of the grid.                             !
!------------------------------------------------------------------------------!
!----------------------------------[Modules]-----------------------------------!
  use Const_Mod, only: HUGE, ONE_THIRD
  use Math_Mod
  use Gen_Mod,   only: face_c_to_c
  use Grid_Mod,  only: Grid_Type
!------------------------------------------------------------------------------!
  implicit none
!---------------------------------[Arguments]----------------------------------!
  type(Grid_Type)     :: grid
  logical, intent(in) :: real_run
!-----------------------------------[Locals]-----------------------------------!
  integer :: c, c1, c2, m, n, s, n_per, nn, nf, c1o, c2o, so
  real    :: loc_x_node(4), loc_y_node(4), loc_z_node(4)
  real    :: x_cell_tmp, y_cell_tmp, z_cell_tmp
  real    :: xs2, ys2, zs2
  real    :: dsc1, dsc2          !  for the interpolation factors
  real    :: t, tot_surf
  real    :: xc1, yc1, zc1, xc2, yc2, zc2
  integer :: f4n(6,4)
  integer :: f3n(4,3)
!==============================================================================!
!                                                                              !
!                                n3                                            !
!                 +---------------!---------------+                            !
!                /|              /|              /|                            !
!               / |             / |             / |                            !
!              /  |          n2/  |            /  |                            !
!             +---------------!---------------+   |                            !
!             |   |           |   |           |   |                            !
!             |   |     o---- | s-------o     |   |                            !
!             |   +---- c1 ---|   !---- c2 ---|   +                            !
!             |  /            |  /n4          |  /                             !
!             | /             | /             | /                              !
!             |/              |/              |/                               !
!             +---------------!---------------+                                !
!                            n1                                                !
!                                                                              !
!   Notes:                                                                     !
!                                                                              !
!     ! faec s is oriented from cell center c1 to cell center c2               !
!     ! c2 is greater then c1 inside the domain or smaller then 0              !
!       on the boundary                                                        !
!     ! nodes are denoted with n1 - n4                                         !
!                                                                              !
!            c3                                                                !
!             \  4-----3                                                       !
!              \/ \  . |                                                       !
!              /   \  +---c2                                                   !
!             /  .  \  |                                                       !
!            / .     \ |                                                       !
!           /.        \|                                                       !
!          1-----------2                                                       !
!                   |                                                          !
!                   c1                                                         !
!                                                                              !
!                                n3                                            !
!                 +---------------!-------+                                    !
!                /|            n2/|      /|                                    !
!               / |             !-------+ |                                    !
!              /  |            /|s|  c2 | |                                    !
!             +---------------+ | !-----| +                                    !
!             |   |           | |/n4    |/                                     !
!             |   |     c1    | !-------+                                      !
!             |   +-----------|n1 +                                            !
!             |  /            |  /                                             !
!             | /             | /                                              !
!             |/              |/                                               !
!             +---------------+                                                !
!                            n1                                                !
!                                                                              !
!------------------------------------------------------------------------------!
!   Generaly:                                                                  !
!                                                                              !
!   the equation of plane reads: A * x + B * y + C * z + D = 0                 !
!                                                                              !
!   and the equation of line:  x = x0 + t*rx                                   !
!                              y = y0 + t*ry                                   !
!                              z = z0 + t*rz                                   !
!- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -       !
!   In our case:                                                               !
!                                                                              !
!     line is a connection between the two cell centers:                       !
!                                                                              !
!     x = xc(c1) + t*(xc(c2)-xc(c1)) = xc(c1) + t*rx                           !
!     y = yc(c1) + t*(yc(c2)-yc(c1)) = yc(c1) + t*ry                           !
!     z = zc(c1) + t*(zc(c2)-zc(c1)) = zc(c1) + t*rz                           !
!                                                                              !
!                                                                              !
!     plane is a cell face:                                                    !
!                                                                              !
!     sx * x + sy * y + Sz * z = sx * xsp(s) + sy * ysp(s) + sz * zsp(s)       !
!                                                                              !
!     and the intersection is at:                                              !
!                                                                              !
!         sx*(xsp(s)-xc(c1)) + sy*(ysp(s)-yc(c1) + sz*(zsp(s)-zc(c1))          !
!     t = -----------------------------------------------------------          !
!                           rx*sx + ry*sy + rz*sz                              !
!                                                                              !
!------------------------------------------------------------------------------!
  data    f4n / 1, 1, 2, 4, 3, 5,     &
                2, 5, 6, 8, 7, 7,     &
                4, 6, 8, 7, 5, 8,     &
                3, 2, 4, 3, 1, 6  /

  data    f3n / 1,  1,  2,  3,        &
                2,  4,  4,  4,        &
                3,  2,  3,  1 /

    ! Without the following six lines, this procedure works for any grid
    do c = 1, grid % n_cells
      grid % cells_n_nodes(c)=8
    end do
    do s = 1, grid % n_faces
      grid % faces_n_nodes(s)=4
    end do

    !-----------------------------------------!
    !   Calculate the cell centers            !
    !-----------------------------------------!
    !   => depends on: x_node,y_node,z_node   !
    !   <= gives:      xc,yc,zc c>0           !
    !-----------------------------------------!
    do c = 1, grid % n_cells
      grid % xc(c)=0.0
      grid % yc(c)=0.0
      grid % zc(c)=0.0
      do n = 1, grid % cells_n_nodes(c)
        grid % xc(c) = grid % xc(c) + grid % xn(grid % cells_n(n,c))  &
                     / (1.0*grid % cells_n_nodes(c))
        grid % yc(c) = grid % yc(c) + grid % yn(grid % cells_n(n,c))  &
                     / (1.0*grid % cells_n_nodes(c))
        grid % zc(c) = grid % zc(c) + grid % zn(grid % cells_n(n,c))  &
                     / (1.0*grid % cells_n_nodes(c))
      end do
    end do

    !-----------------------------------------------------!
    !   Calculate:                                        !
    !      components of cell faces, cell face centers.   !
    !-----------------------------------------------------!
    !   => depends on: x_node,y_node,z_node               !
    !   <= gives:      sx,sy,sz,xsp,yzp,zsp               !
    !-----------------------------------------------------!
    do s = 1, grid % n_faces
      do n = 1, grid % faces_n_nodes(s)  ! for quadrilateral an triangular faces
        loc_x_node(n) = grid % xn(grid % faces_n(n,s))
        loc_y_node(n) = grid % yn(grid % faces_n(n,s))
        loc_z_node(n) = grid % zn(grid % faces_n(n,s))
      end do

      ! Cell side components
      if( grid % faces_n_nodes(s) .eq. 4 ) then
        grid % sx(s)= 0.5 * ((loc_y_node(2)-loc_y_node(1))  &
                           * (loc_z_node(2)+loc_z_node(1))  &
                           + (loc_y_node(3)-loc_y_node(2))  &
                           * (loc_z_node(2)+loc_z_node(3))  &
                           + (loc_y_node(4)-loc_y_node(3))  &
                           * (loc_z_node(3)+loc_z_node(4))  &
                           + (loc_y_node(1)-loc_y_node(4))  &
                           * (loc_z_node(4)+loc_z_node(1)) )
        grid % sy(s)= 0.5 * ((loc_z_node(2)-loc_z_node(1))  &
                           * (loc_x_node(2)+loc_x_node(1))  &
                           + (loc_z_node(3)-loc_z_node(2))  &
                           * (loc_x_node(2)+loc_x_node(3))  &
                           + (loc_z_node(4)-loc_z_node(3))  &
                           * (loc_x_node(3)+loc_x_node(4))  &
                           + (loc_z_node(1)-loc_z_node(4))  &
                           * (loc_x_node(4)+loc_x_node(1)) )
        grid % sz(s)= 0.5 * ((loc_x_node(2)-loc_x_node(1))  &
                           * (loc_y_node(2)+loc_y_node(1))  &
                           + (loc_x_node(3)-loc_x_node(2))  &
                           * (loc_y_node(2)+loc_y_node(3))  &
                           + (loc_x_node(4)-loc_x_node(3))  &
                           * (loc_y_node(3)+loc_y_node(4))  &
                           + (loc_x_node(1)-loc_x_node(4))  &
                           * (loc_y_node(4)+loc_y_node(1)) )
      else if( grid % faces_n_nodes(s) .eq. 3 ) then
        grid % sx(s)= 0.5 * ((loc_y_node(2)-loc_y_node(1))  &
                           * (loc_z_node(2)+loc_z_node(1))  &
                           + (loc_y_node(3)-loc_y_node(2))  &
                           * (loc_z_node(2)+loc_z_node(3))  &
                           + (loc_y_node(1)-loc_y_node(3))  &
                           * (loc_z_node(3)+loc_z_node(1)) )
        grid % sy(s)= 0.5 * ((loc_z_node(2)-loc_z_node(1))  &
                           * (loc_x_node(2)+loc_x_node(1))  &
                           + (loc_z_node(3)-loc_z_node(2))  &
                           * (loc_x_node(2)+loc_x_node(3))  &
                           + (loc_z_node(1)-loc_z_node(3))  &
                           * (loc_x_node(3)+loc_x_node(1)) )
        grid % sz(s)= 0.5 * ((loc_x_node(2)-loc_x_node(1))  &
                           * (loc_y_node(2)+loc_y_node(1))  &
                           + (loc_x_node(3)-loc_x_node(2))  &
                           * (loc_y_node(2)+loc_y_node(3))  &
                           + (loc_x_node(1)-loc_x_node(3))  &
                           * (loc_y_node(3)+loc_y_node(1)) )
      else
        print *, '# Compute_Grid_Geometry: something horrible has happened !'
        stop
      end if

      ! Barycenters
      if(grid % faces_n_nodes(s) .eq. 4) then
        grid % xf(s) = (   loc_x_node(1)+loc_x_node(2)          &
                         + loc_x_node(3)+loc_x_node(4) ) / 4.0
        grid % yf(s) = (   loc_y_node(1)+loc_y_node(2)          &
                         + loc_y_node(3)+loc_y_node(4) ) / 4.0
        grid % zf(s) = (   loc_z_node(1)+loc_z_node(2)          &
                         + loc_z_node(3)+loc_z_node(4) ) / 4.0
      else if(grid % faces_n_nodes(s) .eq. 3) then
        grid % xf(s) = (loc_x_node(1)+loc_x_node(2)+loc_x_node(3)) / 3.0
        grid % yf(s) = (loc_y_node(1)+loc_y_node(2)+loc_y_node(3)) / 3.0
        grid % zf(s) = (loc_z_node(1)+loc_z_node(2)+loc_z_node(3)) / 3.0
      end if

    end do ! through faces

    !--------------------------------------!
    !   Calculate boundary cell centers    !
    !--------------------------------------!
    !   => depends on: xc,yc,zc,sx,sy,sz   !
    !   <= gives:      xc,yc,zc for c<0    !
    !--------------------------------------!
    do s = 1, grid % n_faces
      c1 = grid % faces_c(1,s)
      c2 = grid % faces_c(2,s)

      tot_surf = sqrt(grid % sx(s)*grid % sx(s) +  &
                      grid % sy(s)*grid % sy(s) +  &
                      grid % sz(s)*grid % sz(s))

      if(c2  < 0) then
        t = (   grid % sx(s) * (grid % xf(s)-grid % xc(c1))           &
              + grid % sy(s) * (grid % yf(s)-grid % yc(c1))           &
              + grid % sz(s) * (grid % zf(s)-grid % zc(c1)) ) / tot_surf
        grid % xc(c2) = grid % xc(c1) + grid % sx(s)*t / tot_surf
        grid % yc(c2) = grid % yc(c1) + grid % sy(s)*t / tot_surf
        grid % zc(c2) = grid % zc(c1) + grid % sz(s)*t / tot_surf
      end if
    end do ! through faces

    !---------------------------------------------!
    !   Find the faces on the periodic boundary   !
    !---------------------------------------------!
    !   => depends on: xc,yc,zc,sx,sy,sz          !
    !   <= gives:      dx,dy,dz                   !
    !---------------------------------------------!
    if(real_run) then

      ! Initialize variables for grid periodicity
      n_per = 0
      grid % per_x = 0.0
      grid % per_y = 0.0
      grid % per_z = 0.0

      do s = 1, grid % n_faces

        ! Initialize
        grid % dx(s) = 0.0
        grid % dy(s) = 0.0
        grid % dz(s) = 0.0

        c1 = grid % faces_c(1, s)
        c2 = grid % faces_c(2, s)
        if(c2 > 0) then

          ! Scalar product of the side with line c1 - c2 is a good ...
          ! ... criterion to check if face is on periodic boundary
          if( (grid % sx(s) * (grid % xc(c2) - grid % xc(c1) ) +  &
               grid % sy(s) * (grid % yc(c2) - grid % yc(c1) ) +  &
               grid % sz(s) * (grid % zc(c2) - grid % zc(c1) )) < 0.0 ) then

            n_per = n_per + 1

            ! Find the coordinates of ...
            m = face_c_to_c(s,2)

            if(grid % faces_n_nodes(s) .eq. 4) then

              ! Coordinates of the shadow face
              xs2=.25*(  grid % xn(grid % cells_n(f4n(m,1), c2))  &
                       + grid % xn(grid % cells_n(f4n(m,2), c2))  &
                       + grid % xn(grid % cells_n(f4n(m,3), c2))  &
                       + grid % xn(grid % cells_n(f4n(m,4), c2)))

              ys2=.25*(  grid % yn(grid % cells_n(f4n(m,1), c2))  &
                       + grid % yn(grid % cells_n(f4n(m,2), c2))  &
                       + grid % yn(grid % cells_n(f4n(m,3), c2))  &
                       + grid % yn(grid % cells_n(f4n(m,4), c2)))

              zs2=.25*(  grid % zn(grid % cells_n(f4n(m,1), c2))  &
                       + grid % zn(grid % cells_n(f4n(m,2), c2))  &
                       + grid % zn(grid % cells_n(f4n(m,3), c2))  &
                       + grid % zn(grid % cells_n(f4n(m,4), c2)))

              ! Store the shadow face nodes
              nn = grid % faces_n_nodes(s)
              nf = grid % n_faces + n_per   ! new face number
              grid % faces_n_nodes(nf) = nn
              grid % faces_n(1:nn, nf) = grid % cells_n(f4n(m,1:nn), c2)

            else if(grid % faces_n_nodes(s) .eq. 3) then

              ! Coordinates of the shadow face
              xs2 = ONE_THIRD * (grid % xn(grid % cells_n(f3n(m,1), c2))  &
                               + grid % xn(grid % cells_n(f3n(m,2), c2))  &
                               + grid % xn(grid % cells_n(f3n(m,3), c2)) )

              ys2 = ONE_THIRD * (grid % yn(grid % cells_n(f3n(m,1), c2))  &
                               + grid % yn(grid % cells_n(f3n(m,2), c2))  &
                               + grid % yn(grid % cells_n(f3n(m,3), c2)) )

              zs2 = ONE_THIRD * (grid % zn(grid % cells_n(f3n(m,1), c2))  &
                               + grid % zn(grid % cells_n(f3n(m,2), c2))  &
                               + grid % zn(grid % cells_n(f3n(m,3), c2)) )

              ! Store the shadow face nodes
              nn = grid % faces_n_nodes(s)
              nf = grid % n_faces + n_per   ! new face number
              grid % faces_n_nodes(nf) = nn
              grid % faces_n(1:nn, nf) = grid % cells_n(f3n(m,1:nn), c2)

            end if

            ! Work out the cell distance for periodic faces
            grid % dx(s) = grid % xf(s) - xs2  !------------------------!
            grid % dy(s) = grid % yf(s) - ys2  ! later: xc2 = xc2 + dx  !
            grid % dz(s) = grid % zf(s) - zs2  !------------------------!

            ! Store a few geometrical quantities for the new shadow face
            ! (Change the sign for surface areas (as the shadow face
            !  is oriented differently) but keep face distance the same.)
            grid % xf(nf) =  xs2
            grid % yf(nf) =  ys2
            grid % zf(nf) =  zs2
            grid % sx(nf) = -grid % sx(s)
            grid % sy(nf) = -grid % sy(s)
            grid % sz(nf) = -grid % sz(s)
            grid % dx(nf) =  grid % dx(s)
            grid % dy(nf) =  grid % dy(s)
            grid % dz(nf) =  grid % dz(s)

            ! Store cells surrounding the face and each other's shadows
            grid % faces_c(1, nf) = c1
            grid % faces_c(2, nf) = c2
            grid % faces_s(s)  = nf
            grid % faces_s(nf) = s

            grid % per_x = max(grid % per_x, abs(grid % dx(s)))
            grid % per_y = max(grid % per_y, abs(grid % dy(s)))
            grid % per_z = max(grid % per_z, abs(grid % dz(s)))

          end if !  s*(c2-c1) < 0.0
        end if  !  c2 > 0
      end do    !  faces

      grid % n_shadows = n_per

      print '(a38,i7)',   '# Number of periodic faces:          ', n_per
      print '(a38,f8.3)', '# Periodicity in x direction         ', grid % per_x
      print '(a38,f8.3)', '# Periodicity in y direction         ', grid % per_y
      print '(a38,f8.3)', '# Periodicity in z direction         ', grid % per_z
    end if

  !----------------------------------!
  !   Calculate the cell volumes     !
  !----------------------------------!
  !   => depends on: xc,yc,zc,       !
  !                  dx,dy,dz,       !
  !                  xsp, ysp, zsp   !
  !   <= gives:      volume          !
  !----------------------------------!
  if(real_run) then
    do c = 1, grid % n_cells
      grid % vol(c)=0.0
    end do

    do s = 1, grid % n_faces
      c1 = grid % faces_c(1,s)
      c2 = grid % faces_c(2,s)

      do n = 1, grid % faces_n_nodes(s)  ! for quadrilateral an triangular faces
        loc_x_node(n) = grid % xn(grid % faces_n(n,s))
        loc_y_node(n) = grid % yn(grid % faces_n(n,s))
        loc_z_node(n) = grid % zn(grid % faces_n(n,s))
      end do

      ! First cell
      x_cell_tmp = grid % xc(c1)
      y_cell_tmp = grid % yc(c1)
      z_cell_tmp = grid % zc(c1)
      dsc1 = Math_Mod_Distance(x_cell_tmp,   y_cell_tmp,   z_cell_tmp,    &
                               grid % xf(s), grid % yf(s), grid % zf(s))
      grid % vol(c1) = grid % vol(c1)                                      &
        + Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),      &
                              loc_x_node(1),loc_y_node(1),loc_z_node(1),   &
                              loc_x_node(2),loc_y_node(2),loc_z_node(2),   &
                              x_cell_tmp,y_cell_tmp,z_cell_tmp)
      grid % vol(c1) = grid % vol(c1)                                      &
        + Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),      &
                              loc_x_node(2),loc_y_node(2),loc_z_node(2),   &
                              loc_x_node(3),loc_y_node(3),loc_z_node(3),   &
                              x_cell_tmp,y_cell_tmp,z_cell_tmp)
      if(grid % faces_n_nodes(s) .eq. 4) then
        grid % vol(c1) = grid % vol(c1)                                    &
          + Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),    &
                                loc_x_node(3),loc_y_node(3),loc_z_node(3), &
                                loc_x_node(4),loc_y_node(4),loc_z_node(4), &
                                x_cell_tmp,y_cell_tmp,z_cell_tmp)
        grid % vol(c1) = grid % vol(c1)                                    &
          + Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),    &
                                loc_x_node(4),loc_y_node(4),loc_z_node(4), &
                                loc_x_node(1),loc_y_node(1),loc_z_node(1), &
                                x_cell_tmp,y_cell_tmp,z_cell_tmp)
      else if(grid % faces_n_nodes(s) .eq. 3) then
        grid % vol(c1) = grid % vol(c1)                                    &
          + Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),    &
                                loc_x_node(3),loc_y_node(3),loc_z_node(3), &
                                loc_x_node(1),loc_y_node(1),loc_z_node(1), &
                                x_cell_tmp,y_cell_tmp,z_cell_tmp)
      end if

      ! Second cell
      if(c2  > 0) then
        x_cell_tmp = grid % xc(c2) + grid % dx(s)
        y_cell_tmp = grid % yc(c2) + grid % dy(s)
        z_cell_tmp = grid % zc(c2) + grid % dz(s)
        dsc2=Math_Mod_Distance(x_cell_tmp,   y_cell_tmp,   z_cell_tmp,    &
                               grid % xf(s), grid % yf(s), grid % zf(s))
        grid % vol(c2) = grid % vol(c2)                                    &
          - Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),    &
                                loc_x_node(1),loc_y_node(1),loc_z_node(1), &
                                loc_x_node(2),loc_y_node(2),loc_z_node(2), &
                                x_cell_tmp,y_cell_tmp,z_cell_tmp)
        grid % vol(c2) = grid % vol(c2)                                    &
          - Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),    &
                                loc_x_node(2),loc_y_node(2),loc_z_node(2), &
                                loc_x_node(3),loc_y_node(3),loc_z_node(3), &
                                x_cell_tmp,y_cell_tmp,z_cell_tmp)
        if(grid % faces_n_nodes(s) .eq. 4) then
          grid % vol(c2) = grid % vol(c2)                                  &
            - Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),  &
                           loc_x_node(3),loc_y_node(3),loc_z_node(3),      &
                           loc_x_node(4),loc_y_node(4),loc_z_node(4),      &
                           x_cell_tmp,y_cell_tmp,z_cell_tmp)
          grid % vol(c2) = grid % vol(c2)                                  &
            - Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),  &
                           loc_x_node(4),loc_y_node(4),loc_z_node(4),      &
                           loc_x_node(1),loc_y_node(1),loc_z_node(1),      &
                           x_cell_tmp,y_cell_tmp,z_cell_tmp)
        else if(grid % faces_n_nodes(s) .eq. 3) then
          grid % vol(c2) = grid % vol(c2)                                  &
            - Math_Mod_Tet_Volume(grid % xf(s),grid % yf(s),grid % zf(s),  &
                           loc_x_node(3),loc_y_node(3),loc_z_node(3),      &
                           loc_x_node(1),loc_y_node(1),loc_z_node(1),      &
                           x_cell_tmp,y_cell_tmp,z_cell_tmp)
        end if
      else
        dsc2 = 0.0
      end if

    end do

    grid % min_vol =  HUGE
    grid % max_vol = -HUGE
    grid % tot_vol = 0.0
    do c = 1, grid % n_cells
      grid % tot_vol = grid % tot_vol + grid % vol(c)
      grid % min_vol = min(grid % min_vol, grid % vol(c))
      grid % max_vol = max(grid % max_vol, grid % vol(c))
    end do
    print '(a45,es12.5)', ' # Minimal cell volume is:                   ',  &
          grid % min_vol
    print '(a45,es12.5)', ' # Maximal cell volume is:                   ',  &
          grid % max_vol
    print '(a45,es12.5)', ' # Total domain volume is:                   ',  &
          grid % tot_vol
    print *, '# Cell volumes calculated !'
  end if

  !------------------------------------------------------------!
  !   Calculate the interpolation factors for the cell faces   !
  !------------------------------------------------------------!
  if(real_run) then
    do s = 1, grid % n_faces
      c1 = grid % faces_c(1,s)
      c2 = grid % faces_c(2,s)

      ! First cell
      xc1 = grid % xc(c1)
      yc1 = grid % yc(c1)
      zc1 = grid % zc(c1)
      dsc1 = Math_Mod_Distance(xc1, yc1, zc1,  &
                               grid % xf(s), grid % yf(s), grid % zf(s))

      ! Second cell (pls. check if xsi=xc on the boundary)
      xc2 = grid % xc(c2) + grid % dx(s)
      yc2 = grid % yc(c2) + grid % dy(s)
      zc2 = grid % zc(c2) + grid % dz(s)
      dsc2 = Math_Mod_Distance(xc2, yc2, zc2,  &
                               grid % xf(s), grid % yf(s), grid % zf(s))

      ! Interpolation factor
      grid % f(s) = dsc2 / (dsc1 + dsc2)
    end do

  end if

  end subroutine


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! DEXROU (Double EXample Known EigenValues)
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine chooses N random eigenvalues on the unit
! circle, constructs a unitary matrix with prescribed
! eigenvalues, and computes the N eigenvalues by solving
! a corresponding unitary eigenvalue problem two different
! ways.
!
! 1) Form upper Hessenberg permutation matrix and compute its
!    eigenvalues using DOHFQR
!
! 2) Construct the factorization directly and compute its
!    eigenvalues using DOFFQR
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
program DEXKEV

  implicit none

  ! compute variables
  integer, parameter :: N = 16
  integer :: ii, INFO, cpair
  real(8) :: WORK(4*N), Q(2*(N-1)), D(2*N)
  real(8) :: H(N,N), Z
  integer :: ITS(N-1), ind, jj
  double precision :: rm,rp,pi = 3.141592653589793239d0
  double precision :: temp(2,2), he, diff
  double precision :: nrm,b1(2),b2(2),b3(2),tb(2),tol

  ! real and imag part of eigenvalues
  double precision :: rev(N), iev(N)

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! print banner
  print*,""
  print*,"DEXKEV: Double EXample Known EigenValues"
  print*,""

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! initialize Q and D to be an identity matrix
  Q = 0d0
  D = 0d0
  do ii=1,n-1
     Q(2*ii-1) = 1d0
     D(2*ii-1) = 1d0
  end do
  D(2*n-1) = 1d0

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! random eigenvalues, on the unit circle
  do ii = 1,n,2
     call random_number(rm)
     rev(ii) = sin(2*pi*rm)
     iev(ii) = cos(2*pi*rm)
     ! project on the unit circle
     nrm = rev(ii)**2+iev(ii)**2
     if (abs(nrm-1)<tol) then
        nrm = sqrt(nrm)
        rev(ii) = rev(ii)/nrm
        iev(ii) = iev(ii)/nrm
     end if
     Q(2*ii-1) = rev(ii)
     Q(2*ii)   = iev(ii)
     rev(ii+1) = rev(ii)
     iev(ii+1) = -iev(ii)
  end do

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! inverse eigenvalue problem
  do ii=n-3,1,-2
     ! generate random bulge
     call random_number(rp)
     call random_number(rm)
     call DARCG22(sqrt(1-rp**2),rp,b1(1),b1(2),NRM,INFO)
     call DARCG22(sqrt(1-rm**2),rm,b2(1),b2(2),NRM,INFO)
     ! fuse on the top of the current Q sequence
     tb(1) = b2(1)
     tb(2) = -b2(2)
     b3(1) = b1(1)
     b3(2) = -b1(2)
     ind = 2*(ii-1)
     call DARGTO(tb,b3,Q((ind+1):(ind+2)),INFO)
     call DARFGR('R',b3,Q((ind+3):(ind+4)),INFO)
     b3 = Q((ind+1):(ind+2))
     Q((ind+1):(ind+2)) = tb
     ! main chasing loop
     do jj=ii,(n-3)
        ind = 2*(jj-1)
        ! set indices
        call DARGTO(Q((ind+3):(ind+4)),Q((ind+5):(ind+6)),b1,INFO)
        call DARGTO(Q((ind+1):(ind+2)),Q((ind+3):(ind+4)),b2,INFO)
        call DARGTO(b3,b1,b2,INFO)
        ! update bulges
        tb = b2
        b2 = b3
        b3 = b1
        b1 = tb
     end do
     ind = 2*(n-3)
     ! fusion at bottom
     call DARFGR('L',Q((ind+3):(ind+4)),b1,INFO)
     call DARGTO(Q((ind+1):(ind+2)),Q((ind+3):(ind+4)),b2,INFO)
     call DARFGR('L',b3,b2,INFO)
     call DARFGR('L',Q((ind+3):(ind+4)),b3,INFO)
  end do

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! initialize H to be an upper Hessenberg permutation matrix
  H = 0d0
  do ii=1,N
     H(ii,ii)=1d0
  end do
  H(1,1) = Q(1)
  H(1,2) = Q(2)
  H(2,1) = -Q(2)
  H(2,2) = Q(1)
  do ii=2,N-1
     temp(1,1) = Q(2*ii-1)
     temp(1,2) = Q(2*ii)
     temp(2,1) = -Q(2*ii)
     temp(2,2) = Q(2*ii-1)
     do jj = 1,ii+1
        he = H(jj,ii)*temp(1,1) + H(jj,ii+1)*temp(2,1)
        H(jj,ii+1) = H(jj,ii)*temp(1,2) + H(jj,ii+1)*temp(2,2)
        H(jj, ii) = he
     end do
  end do

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! call dohfqr
  call DOHFQR('N',N,H,Z,ITS,WORK,INFO)

  ! check INFO
  if (INFO.NE.0) then
    print*,"DOHFQR failed."
    print*,"INFO:",INFO
  end if

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  print*,"Eigenvalues:"
  do ii=1,N
     print*, rev(ii), iev(ii)
  end do
  print*,""

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  ! print diag of H
  cpair = 0
  print*,"Eigenvalues computed using DOHFQR: (real part, imag part, distance to closest exact eigenvalue)"
  do ii=1,N
    if (cpair.EQ.0) then
      if (H(ii,ii+1).EQ.0) then
         diff = abs(cmplx(H(ii,ii)-rev(1),iev(1),kind=8))
         do jj=2,N
            he = abs(cmplx(H(ii,ii)-rev(jj),iev(jj),kind=8))
            if (he < diff) then
               diff = he
            end if
         end do
         print*,H(ii,ii), 0d0, diff
      else
         diff = sqrt(abs(H(ii,ii)-rev(1))**2+abs(H(ii,ii+1)-iev(1))**2)
         do jj=2,N
            he = sqrt(abs(H(ii,ii)-rev(jj))**2+abs(H(ii,ii+1)-iev(jj))**2)
            if (he < diff) then
               diff = he
            end if
         end do
         print*,H(ii,ii),H(ii,ii+1),diff
         diff = sqrt(abs(H(ii+1,ii+1)-rev(1))**2+abs(H(ii+1,ii)-iev(1))**2)
         do jj=2,N
            he = sqrt(abs(H(ii+1,ii+1)-rev(jj))**2+abs(H(ii+1,ii)-iev(jj))**2)
            if (he < diff) then
               diff = he
            end if
         end do
         print*,H(ii+1,ii+1),H(ii+1,ii),diff
         cpair = 1
      end if
    else
      cpair = 0
    end if
  end do
  print*,""

  ! call doffqr
  call DOFFQR('N',N,Q,D,Z,ITS,INFO)

  ! check INFO
  if (INFO.NE.0) then
    print*,"DOFFQR failed."
    print*,"INFO:",INFO
  end if

  ! print D
  print*,"Eigenvalues computed using DOFFQR: (real part, imag part, distance to closest exact eigenvalue)"
  do ii=1,N
     diff = sqrt(abs(D(2*ii-1)-rev(1))**2+abs(D(2*ii)-iev(1))**2)
     do jj=2,N
        he = sqrt(abs(D(2*ii-1)-rev(jj))**2+abs(D(2*ii)-iev(jj))**2)
        if (he < diff) then
           diff = he
        end if
     end do
     print*,D(2*ii-1),D(2*ii), diff
  end do
  print*,""

end program DEXKEV


!---------------------------------------------------!
! Copyright (c) 2016 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
!-------10--------20--------30--------40--------50--------60--------70--------80--------90
subroutine energy(Eex)
  use global_variables
  implicit none
  real(8) :: Eex
  integer :: ikr,ikz

  Eex = 0d0
!$omp parallel
!$omp do private(ikz, ikr) reduction(+:Eex)
  do ikz = -NKz,NKz
  do ikr = 1,NKr
    Eex = Eex+ deps(ikr,ikz)*abs(zCt(2,ikr,ikz))**2*kr(ikr)
  end do
  end do
!$omp end parallel
  Eex = Eex*2d0/((2d0*pi)**3)*(2d0*pi*dkr*dkz)

  return
end subroutine energy


program main
  use LoggerClass
  use fem
  implicit none

  type(FEMDomain_) :: cube
  type(Logger_) :: Bob
  type(Math_) :: math
  integer(int32) :: i

  ! create an object
  call cube%create("Cube3D")

  ! set channels
  call Bob%set("Node3_x",cube%mesh%nodcoord(3,1))
  call Bob%set("Node12_y",cube%mesh%nodcoord(12,2))
  call Bob%set("Node9_z",cube%mesh%nodcoord(3,3))

  ! start logger
  call Bob%start()
  do i=1,100
    call cube%move(x=sin(dble(i)/2.0d0/Math%PI ) )
    call cube%rotate(x=sin(dble(i)/2.0d0/Math%PI ) )
    call Bob%save()
  enddo
  call Bob%reset()
  do i=1,100
    call cube%rotate(x=sin(dble(i)/2.0d0/Math%PI ) )
    call Bob%save()
  enddo
end program main


      SUBROUTINE DRFT (A, MODE, M, ND, MS, S)
c>> 1997-03-31 DFFT Krogh  Increased KEDIM, more sine table checks.
c>> 1996-01-23 DRFT Krogh  Changes to simplify conversion to C.
c>> 1994-10-20 DRFT Krogh  Changes to use M77CON
c>> 1990-01-23 DRFT CLL Removed extraneous label 80.
c>> 1989-06-16 FTK Fix error message on MODE.
c>> 1989-05-10 FTK & CLL
c>> 1989-05-07 FTK & CLL
c>> 1989-04-21 FTK & CLL
c Copyright (c) 1996 California Institute of Technology, Pasadena, CA.
c ALL RIGHTS RESERVED.
c Based on Government Sponsored Research NAS7-03001.
c
c This subroutine computes Fourier transforms of real data in up to 6
c dimensions using the Cooley-Tukey fast Fourier transform.
c
c     Programmed by Fred F. Krogh at the Jet Propulsion Laboratory,
c     Pasadena, Calif.   August 1, 1969.
c     Revised for portability by Krogh -- February 2, 1988
c
c     Variables in the calling sequence have the following types
      DOUBLE PRECISION A(*), S(*)
      INTEGER          ND, M(ND), MS
      CHARACTER        MODE
c
c     Values for A(), MODE, M(), ND, and MS must be specified before
c     calling the subroutine.
c
c     In describing the usage the following notation is used
c     NDA = ND
c     N(K) = 2 ** M(K)
c     MA = M(1) + M(2) + ... + M(ND)
c     NA = 2 ** MA
c     MX = MAX(M(1),M(2), ..., M(ND))
c     NX = 2**MX
c     WK = EXP(2*PI*I/N(K)), WHERE I = SQRT(-1) AND PI = 3.14159...
c
c     The usage is as follows
c
c A() on input is an array of function values, X, if one is doing
c   Fourier analysis, and is an array of Fourier coefficients, B, if one
c   is doing Fourier synthesis.  On output, A contains B if doing
c   analysis, and contains X if doing synthesis.  In our description
c   here, we assume that A has been declared by the user as a real array
c   with dimension A(N(1), N(2), ..., N(NDA)) when A contains the real
c   data, and A is a complex array, C, with dimension C(N(1)/2,N(2),...,
c   N(NDA)), when A contains complex Fourier coefficients.  (B(k1, k2,
c   ..., kNDA) for k1 > N(1)/2 need not be saved since B(N(1)-k1,
c   N(2)-k2, ..., N(NDA)-kNDA) = conjugate of B(k1, k2, ..., kNDA),
c   where the L-th subscripts are to be interpreted modulo N(L).  It is
c   assumed that the imaginary part of a complex number is stored
c   in the cell immediately following its real part, except that
c   if k1 = 0 and all of the remaining ki satisfy ki = 0 or ki = N(i)/2,
c   then B(0,k2,...,kNDA) and B(N(1)/2,k2,...,kNDA) are real, and
c   real part of C(1,k2+1,...,kNDA+1) = B(0,k2,...,kNDA) and
c   imag. part of C(0,k2+1,...,kNDA+1) = B(N(1)/2,k2,...,kNDA).
c   If k1 = 0 and kL is the first ki that does not satisfy ki = 0 or
c   ki = N(i)/2 then C(1,k2+1,...,kNDA+1) = B(0,k2,...,kNDA) for
c   kL .LT. N(L)/2, and C(1,k2+1,...,kNDA+1) = B(N(1)/2,k2,...,kNDA) for
c   kL.GT.N(L)/2.  Of course the storage required for A can be
c   reserved by the user in any way that works.
c
c MODE  A character variable to select Analysis or Synthesis.
c   'A' or 'a' selects Analysis, and 'S' or 's' selects Synthesis.
c   For Analysis the subroutine computes
c      B(k1,k2,...,kNDA) = sum over 0 .le. j1 .le. N(1)-1, 0 .le. j2
c      .le. N(2)-1,..., 0 .le. jNDA .le. N(NDA)-1, of X(j1,j2,...,jNDA)
c      * T(1,j1,k1) * T(1,j2,k2) * ... * T(NDA,jNDA,kNDA), 0 .le.
c      k1 .le. N(1)-1,..., 0 .le. kNDA .le. N(NDA)-1
c      with T(L,j,k) = (1/N(L)) * WL ** (-j*k).
c   For Synthesis the subroutine computes
c      X(j1,j2,...,jNDA) = sum over 0 .le. k1 .le. N(1)-1, 0 .le. k2
c      .le. N(2)-1,..., 0 .le. kNDA .le. N(NDA)-1, of B(k1,k2,...,kNDA)
c      * T(1,j1,k1) * T(1,j2,k2) * ... * T(NDA,jNDA,kNDA), 0 .le.
c      j1 .le. N(1)-1,..., 0 .le. jNDA .le. N(NDA)-1
c      with T(L,j,k) = WL**(j*k).
c
c M() is a vector used to indicate N(k) = 2**M(k), the number of
c  real points in the k-th variable.  M must be such that 0 .le. M(k)
c  .le. 21, and if M(1)=0, then M(k) must be 0 for k =1, ..., NDA.
c
c ND gives the dimension of (i.e. the number of subscripts in)
c    the array A.  ND must satisfy 1 .le. ND .le. 6.
c
c MS gives the state of the sine table.  If MS > 0, there are NT =
c    2 ** (MS-2) good entries in the sine table.  On the initial call,
c    MS must be set to 0, and when the return is made, it will be set
c    to MX, which is the value of MS required for computing a
c    transform of size NX.  If MS = -1, the sine table will be computed
c    as for the case MS = 0, and then a return to the user will be made
c    with MS set as before, but no transform will be computed.  This
c    option is useful if the user would like access to the sine table
c    before computing the FFT.
c
c S is a vector, S(j) = sin(pi*j/2*NT)), j = 1, 2, ..., NT-1, where
c  NT is defined in the description of MS above.  S is computed by the
c  subroutine if MX .gt. MS.  (If S is altered, set MS=0 so that S
c  is recomputed.)
c     ------------------------------------------------------------------
c                Notes on COMMON, PARAMETER's, and local variables
c
c     NDMAX = the maximum value for ABS(ND), and MAXM = the maximum
c     permitted for M(1), ..., M(ND)
c
c     The dimension of KE must be at least as large as MAXM-1.
c
c     The named common CDFFTC is used for communication between this
c     subroutine and the subroutine DFFT which computes a one
c     dimensional complex Fourier transform and computes the sine table.
c     The use of the variables in CDFFTC is contained in the listing
c     of DFFT.
c
c     NF(1) = 1, NF(K+1) = 2**(M(1) + M(2) + ... + M(K)), K = 1,...,NDA
c
c     MU is used in computing the index associated with a given index.
c     (The indices (k1,k2,...,kNDA) and (j1,j2,...,jNDA) are associated
c     if k1 = (N(1)/2)-j1 (modulo (N(1)/2)) and ki = N(i)-j1 (modulo ni)
c     i = 2,...,ND.)
c
c     ANAL = .TRUE. when doing Fourier analysis, and .FALSE. otherwise
c
c     N1 = 2**M(1)
c     ------------------------------------------------------------------
c--D replaces "?": ?RFT, ?FFT, C?FFTC
c     Both versions use IERM1
c     ------------------------------------------------------------------
      INTEGER MAXM, NDMAX
      PARAMETER (NDMAX = 6)
      INTEGER I, I1, I2, II, II1, II2, IIB, IIC
      INTEGER IR, IR1, IR2, IRB, IRC
      INTEGER J, JDIF, JJ, JL
      INTEGER K, K1, K1L, K1N, KB
      INTEGER KK, KN2, KN2S, KSM
      INTEGER L, LB
      INTEGER M1, MA, MMAX, MSI, MU(NDMAX-1)
      INTEGER N1, NDA, NF(NDMAX+1), NTOT2
      LOGICAL ANAL
      CHARACTER MSG1*19

      DOUBLE PRECISION FN, HALF
      DOUBLE PRECISION SPI4, T, TI, TT, TTI, TWO, WI, WR
c
      PARAMETER (SPI4 = .70710 67811 86547 52440 08443 62104 8490D0)
      EQUIVALENCE (NF(2), N1)
c
c Common variables
      INTEGER KEDIM
      PARAMETER (KEDIM=30)
      LOGICAL NEEDST
      INTEGER MT, NT, MM, KS, ILAST, KE(KEDIM), KEE(KEDIM+1)
c Note that KEE(1) is equivalent to ILAST.
      EQUIVALENCE (KE(1), KEE(2))
      COMMON /CDFFTC/ NEEDST, MT, NT, MM, KS, ILAST, KE
      SAVE /CDFFTC/

      PARAMETER(MAXM = KEDIM + 1)
      PARAMETER (HALF = .5D0)
      PARAMETER (TWO = 2.D0)
      DATA NF(1) / 1 /
      DATA MSG1 / 'Bad MODE.  MODE =  ' /
c     ------------------------------------------------------------------
c
      if( MODE .eq. 'A' .or. MODE .eq. 'a') then
         ANAL = .true.
      else if( MODE .eq. 'S' .or. MODE .eq. 's') then
         ANAL = .false.
      else
         MSG1(19:19) = MODE
         call ERMSG('DRFT', 2, 2, MSG1, '.')
         MS = -2
         return
      end if
      NDA = ND
      IF ((NDA .EQ. 0) .OR. (NDA .GT. NDMAX)) THEN
c                               Fatal error, default is to stop in IERM1
         CALL IERM1 ('DRFT', 1, 2, 'BAD ND', 'ND', ND, '.')
         MS = -2
         RETURN
      END IF
      MA = 0
      MMAX = -1
      DO 10 K = 1, NDA
         MM = M(K)
         IF (MM .LT. 0) GO TO 400
         IF (MM .GT. MMAX) THEN
c                if M(1)=0, then so must all of the rest.
            IF ((MMAX .EQ. 0) .OR. (MM .GT. MAXM)) GO TO 400
            MMAX = MM
         END IF
         MA = MA + MM
         NF(K+1) = NF(K) * 2 ** MM
   10 CONTINUE
      MSI = MS
      NEEDST = MMAX .GT. MSI
      if (.NOT. NEEDST) then
c  Check internal parameters to catch certain user errors.
         if (MT .lt. KEDIM) then
            if (MMAX .le. MT + 2) then
c              Skip sine table computation if all appears O.K.
               if (MT .le. 0) go to 20
               if (abs(S(NT/2) - SPI4) .le. 1.D-7) go to 20
            end if
         end if
         NEEDST = .true.
         call ERMSG('DRFT1', 3, 1,
     1      'Invalid sine table (re)computed', '.')
      end if
      MS = MMAX
      MT = MMAX - 2
      CALL DFFT (A, A, S)
      IF (MSI .EQ. -1) RETURN
c                   All setup for computation now
   20 IF (MMAX .EQ. 0) RETURN
      NTOT2 = NF(NDA+1)
      M1 = M(1)
c
c     Compute constants associated with N1.
      KN2 = N1 / 2
      KN2S = KN2
      IF (ANAL) KN2S = -KN2S
      K1L = KN2 - 1
      JDIF = (4 * NT) / N1
c
      IF (.NOT. ANAL) THEN
c                     Set flags for Fourier synthesis
         IRC = 0
         IIC = 1
         GO TO 160
      END IF
c
c     Set flags for Fourier analysis
      IRC = 1
      IIC = 0
      FN = HALF / DBLE(NTOT2)
c     Doing Fourier analysis, so multiply by 2**(-M(1)-M(2)-...-M(ND)-1)
      DO 90 I = 1, NTOT2
         A(I) = A(I) * FN
   90 CONTINUE
c     Beginning of loop for computing multiple sum
c     multi-dimensional complex Fourier synthesis or analysis
  100 DO 140 K = 1, NDA
         MM = M(K)
         KS = NF(K)
         IF (K.NE.1) GO TO 110
         MM = MM - 1
         KS = 2
  110    KSM = KS - 1
         ILAST = NF(K+1)
         DO 120 L = 1, MM
            KEE(L+1) = KEE(L) / 2
  120    CONTINUE
         DO 130 J = 1, NTOT2, ILAST
            JL = J + KSM
            DO 125 I = J, JL, 2
               IR = I + IRC
               II = I + IIC
               CALL DFFT (A(IR), A(II), S)
  125       CONTINUE
  130    CONTINUE
  140 CONTINUE
c     End of loop for doing multi-dimensional complex Fourier transforms
c
      IF (.NOT. ANAL) RETURN
c
c
c     Beginning of calculations relating coefficients of real data
c     with coefficients of associated complex data
  160 KB = 0
      L = 1
      LB = 1
c     L = LB occurs when the coefficients of the real data which are
c     computed in the case k1 = 0, are real.
c
c     Coefficients with index L + k1 and LB + N1 - k1 (k1.gt.0) are
c     associated in the sense described above where MU is dimensioned.
c     When K.GT.1, LB = KB - L.
c
      K = 1
c
c     Special case -- K1 = N1/4  (L = LB)
  170 IF (M1.ne.1) then
         I1 = L + KN2
         A(I1) = TWO * A(I1)
         A(I1+1) = -TWO * A(I1+1)
      end if
c
c     Special case --  K1 = 0    (L = LB)
      T = A(L) + A(L+1)
      TI = A(L) - A(L+1)
      IF (ANAL) THEN
         T = TWO * T
         TI = TWO * TI
      END IF
      A(L) = T
      A(L+1) = TI
      GO TO 220
c
c     Special case -- K1 = N1/4  (L.NE.LB)
  200 IF (M1.NE.1) THEN
         I1 = L + KN2
         I2 = LB + KN2
         T = TWO * A(I2)
         A(I2) = TWO * A(I1)
         A(I1) = T
         T = -TWO * A(I2+1)
         A(I2+1) = -TWO * A(I1+1)
         A(I1+1) = T
      END IF
c        SET UP BASE INDICES
      IR = L
      IRB = LB
      IF (ANAL) THEN
         IR = IRB
         IRB = L
      END IF
      II = IR + 1
      IIB = IRB + 1
c
c     SPECIAL CASE-- K1 = 0     (L.NE.LB)
      T = A(IR) + A(IRB)
      TT = A(IIB) + A(II)
      TI = A(II) - A(IIB)
      TTI = A(IR) - A(IRB)
      A(IR) = T - TT
      A(IRB) = T + TT
      A(II) = TI + TTI
      A(IIB) = TTI - TI
c
  220 IF (M1.LE.2) GO TO 260
      J = 0
c
c     USUAL CASE -- K1.NE.0  AND  K1.NE.N1/4
      DO 250 K1 = 2, K1L, 2
         K1N = N1 - K1
         IF (ANAL) THEN
            IR1 = LB + K1N
            IR2 = L + K1
         ELSE
            IR1 = L + K1
            IR2 = LB + K1N
         END IF
         II2 = IR2 + 1
         II1 = IR1 + 1
         J = J + JDIF
         WI = S(J)
         JJ = NT - J
         WR = S(JJ)
         T = A(IR1) - A(IR2)
         TI = A(II1) + A(II2)
         TT = T * WI + TI * WR
         TTI = T * WR - TI * WI
         T = A(IR1) + A(IR2)
         TI = A(II1) - A(II2)
         A(IR1) = T - TT
         A(IR2) = T + TT
         A(II1) = TTI + TI
         A(II2) = TTI - TI
         IF (L .NE. LB) THEN
            IR1 = IR1 + KN2S
            IR2 = IR2 - KN2S
            II2 = IR2 + 1
            II1 = IR1 + 1
            T = A(IR1) - A(IR2)
            TI = A(II1) + A(II2)
            TT = T * WR - TI * WI
            TTI = T * WI + TI * WR
            T = A(IR1) + A(IR2)
            TI = A(II1) - A(II2)
            A(IR1) = T - TT
            A(IR2) = T + TT
            A(II1) = TI - TTI
            A(II2) = -TI - TTI
         END IF
  250 CONTINUE
c
  260 IF (K .LT. 2) GO TO 310
  270 L = L + N1
      KK = 1
      IF (K .GT. 2) THEN
c             Logic used to find index associated with a given index.
  280    IF (MU(KK).EQ.0) KB = KB + NF(KK+2)
         MU(KK) = MU(KK) + NF(KK+1)
         IF (MU(KK) .GE. NF(KK+2)) THEN
            MU(KK) = 0
            KK = KK + 1
            KB = KB - NF(KK+1)
            IF (KK.LE.(K - 2)) GO TO 280
         END IF
      END IF
  300 LB = KB - L
      IF (LB .GE. L) THEN
         IF (LB .GT. L) GO TO 200
         GO TO 170
      END IF
      IF (KK .LE. (K-2)) GO TO 270
  310 IF (K.GE.NDA) GO TO 330
      DO 320 I = 1, K
  320 MU(I) = 0
      K = K + 1
      KB = NF(K+1) + 2
      L = NF(K) + 1
      GO TO 300
c
  330 IF (.NOT. ANAL) GO TO 100
      RETURN
c
c               Fatal error, default is to stop in IERM1
  400 call ERMSG('DRFT',2,2,'M() specified improperly','.')
      MS = -2
      RETURN
c
      END


! subroutine CalDdatSigma(dall,obst,cbst,sigmaT,meandeltaT,stddeltaT)
subroutine CalDdatSigma(dall, obst, cbst, sigmaT, meandeltaT)
implicit none
! abandoned
! used in Yunnan
! INPUT
integer dall
real obst(dall),cbst(dall)
! OUTPUT
real  sigmaT(dall)
real meandeltaT,stddeltaT
! PARAMETER
integer jj,kk,ii
integer i,j,k
real twostdratio
real :: deltaT(dall)
! sigma(i)=std(t_res/t_obs)*t_obs
deltaT=0
meandeltaT=0

do i = 1,dall
    deltaT(i)=abs(cbst(i)/obst(i))
    meandeltaT=meandeltaT+deltaT(i)
enddo
meandeltaT=meandeltaT/dall
! meandeltaT=sum(deltaT(1:dall))/dall
stddeltaT=0
do i=1,dall
    stddeltaT=stddeltaT+(deltaT(i)-meandeltaT)**2
enddo
stddeltaT=sqrt(stddeltaT/dall)
! stddeltaT=sqrt(sum((deltaT(1:dall)-meandeltaT)**2))
do i=1,dall
    twostdratio=abs(deltaT(i)/(1.5*stddeltaT))
    if (twostdratio.gt.1.0)then
        sigmaT(i)=stddeltaT*obst(i)*exp(twostdratio-1)
    else
        sigmaT(i)=stddeltaT*obst(i)
    endif
enddo
end subroutine

! Residual Norm !
subroutine CalVsReslNorm(maxvp,dall,GVs,VsInv,datweight,Tdata,fwdTvs,resbst)
    use  lsmrblasInterface, only : dnrm2
    implicit none
    ! INPUT
    integer,intent(in):: dall,maxvp
    real,intent(in):: GVs(dall,maxvp)
    ! real,intent(in):: dv(maxvp),datweight(dall),Tdata(dall)
    real,intent(in):: VsInv(maxvp),datweight(dall),Tdata(dall)
    ! OUTPUT
    real:: resbst(dall)

    real:: resW2Nm,res2Nm
    real:: PreRes
    ! PARAMETER

    real fwdTgc(dall),fwdTgs(dall), fwdTvs(dall),fwdt(dall)
    real fwdTaa(dall),res(dall),resW(dall),per_res(dall)
    integer jj,kk,ii
    integer i,j,k
    !--------------------------------------------------------------------!
    ! Target:
    !  Calculate traveltime resiudal between observe traveltime
    !  and forward traveltime which is generated by inversion result
    !  using G from ref  model.
    !   r=Gm-d
    !--------------------------------------------------------------------!
    ! VsInv=dv(1:maxvp)
    !--------------------------------------------------------------------!
    ! forward residual
    fwdTvs=matmul(GVs,VsInv)
    fwdt=0
    res=0
    resW=0
    per_res=0
    resbst=0
    do i=1,dall
        resbst(i)=Tdata(i)-fwdTvs(i)
        resW(i)=resbst(i)*datweight(i) ! here, 1/sigma is for
        per_res(i)=resbst(i)/Tdata(i)
    enddo
    resW2Nm=dnrm2(dall,resW,1)
    res2Nm=dnrm2(dall,resbst,1)
    PreRes=sum(abs(per_res(1:dall)))/dall

    write(6 ,'(a,2f12.3)') '  dVs:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    write(66,'(a,2f12.3)') '  dVs:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    ! write(6,*) ' abs mean Res(Vs)/Res  (%)',PreRes*100
    ! write(66,*)' abs mean Res(Vs)/Res  (%)',PreRes*100
end subroutine

subroutine CalGcsReslNorm(maxvp,dall,GGc,GGs, GcInv, GsInv, datweight,Tdata,fwdTaa,resbst)
    use  lsmrblasInterface, only : dnrm2
    implicit none
    ! resW2Nm: predicated ||W(Gm-d)||2
    ! res2Nm: predicated ||(Gm-d)||2
    ! INPUT
    integer,intent(in):: dall,maxvp
    real,intent(in):: GGc(dall,maxvp),GGs(dall,maxvp)
    real,intent(in):: GcInv(maxvp), GsInv(maxvp), datweight(dall),Tdata(dall)
    ! OUTPUT
    real:: resbst(dall)
    real:: res2Nm,resW2Nm
    real:: PreRes
    ! PARAMETER
    ! real VsInv(maxvp),GcInv(maxvp),GsInv(maxvp)
    real fwdTgc(dall),fwdTgs(dall), fwdTvs(dall),fwdt(dall)
    real fwdTaa(dall),res(dall),resW(dall),per_res(dall)
    integer jj,kk,ii
    integer i,j,k
    real norm2,norm2W
    !--------------------------------------------------------------------!
    ! Target:
    !  Calculate traveltime resiudal between observe traveltime
    !  and forward traveltime which is generated by inversion result
    !  using G from ref  model.
    !   r=Gm-d
    !--------------------------------------------------------------------!
    ! GcInv=dv(1:maxvp)
    ! GsInv=dv(1+maxvp:2*maxvp)
    !--------------------------------------------------------------------!
    ! forward residual
    fwdTgc=matmul(GGc,GcInv)
    fwdTgs=matmul(GGs,GsInv)
    fwdTaa=0
    res=0
    resW=0
    per_res=0
    resbst=0
    norm2=0
    norm2W=0
    do i=1,dall
        fwdTaa(i)=fwdTgs(i)+fwdTgc(i)
        resbst(i)=Tdata(i)-fwdTaa(i)
        resW(i)=resbst(i)*datweight(i) ! here, 1/sigma is for data weight
        if (Tdata(i) .gt. 0)   per_res(i)=resbst(i)/Tdata(i)
        ! norm2=norm2+resbst(i)**2
        ! norm2W=norm2W+resW(i)**2
        ! write(6,*)'res=',resbst(i),'weight=',datweight(i),'resW=',resW(i),'norm2=',norm2,'norm2W=',norm2W
    enddo
    resW2Nm=dnrm2(dall,resW,1)
    res2Nm=dnrm2(dall,resbst,1)
    PreRes=sum(abs(per_res(1:dall)))/dall
    write(6 ,'(a,2f12.3)') '  Gcs:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    write(66,'(a,2f12.3)') '  Gcs:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    ! write(6,*) ' abs mean Res(aa)/Res  (%)',PreRes*100
    ! write(66,*)' abs mean Res(aa)/Res  (%)',PreRes*100
end subroutine

subroutine CalReslNormJoint(maxvp, dall, GVs, GGc, GGs, VsInv, GcInv, GsInv, datweight, Tdata, fwdTvs, fwdTaa, resbst)
    use  lsmrblasInterface, only : dnrm2
    implicit none
    ! resW2Nm: predicated ||W(Gm-d)||2
    ! res2Nm: predicated ||(Gm-d)||2
    ! INPUT
    integer,intent(in):: dall,maxvp
    real,intent(in):: GVs(dall,maxvp), GGc(dall,maxvp), GGs(dall,maxvp)
    real,intent(in):: VsInv(maxvp), GcInv(maxvp), GsInv(maxvp), datweight(dall),Tdata(dall)
    ! OUTPUT
    real:: resbst(dall)
    real:: fwdTvs(dall),fwdTaa(dall)
    ! PARAMETER
    ! real VsInv(maxvp),GcInv(maxvp),GsInv(maxvp)
    real fwdTgc(dall),fwdTgs(dall),fwdt(dall)
    real res(dall),resW(dall),per_res(dall)
    integer jj,kk,ii
    integer i,j,k
    real norm2,norm2W
    real:: res2Nm,resW2Nm
    real:: PreRes
    real:: mean
    !--------------------------------------------------------------------!
    ! Target:
    !  Calculate traveltime resiudal between observe traveltime
    !  and forward traveltime which is generated by inversion result
    !  using G from ref  model.
    !   r=Gm-d
    !--------------------------------------------------------------------!
    ! GcInv=dv(1:maxvp)
    ! GsInv=dv(1+maxvp:2*maxvp)
    !--------------------------------------------------------------------!
    ! forward residual
    fwdTvs=matmul(GVs,VsInv)
    fwdTgc=matmul(GGc,GcInv)
    fwdTgs=matmul(GGs,GsInv)
    fwdTaa=0
    res=0
    resW=0
    per_res=0
    resbst=0
    norm2=0
    norm2W=0
    do i=1,dall
        fwdTaa(i)=fwdTgs(i)+fwdTgc(i)
        resbst(i)=Tdata(i)-fwdTaa(i)-fwdTvs(i)
        resW(i)=resbst(i)*datweight(i) ! here, 1/sigma is for

        ! norm2=norm2+resbst(i)**2
        ! norm2W=norm2W+resW(i)**2
        ! write(6,*)'res=',resbst(i),'weight=',datweight(i),'resW=',resW(i),'norm2=',norm2,'norm2W=',norm2W
    enddo
    resW2Nm=dnrm2(dall,resW,1)
    res2Nm=dnrm2(dall,resbst,1)

    write(6 ,'(a,2f12.3)') '  All:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    write(66,'(a,2f12.3)') '  All:  ||(Gm-d)||^2  and ||W(Gm-d)||^2: ', res2Nm, resW2Nm
    write(66,'(a)')'  '
    write(6 ,'(a)')'  '
    ! analysis residual
    mean=sum(abs(fwdTaa(1:dall)))/dall
    write(66,'(a,f12.4,a)')'  ABS Mean T(AA): ',mean,'s'
    write(6 ,'(a,f12.4,a)')'  ABS Mean T(AA): ',mean,'s'
    mean=sum(abs(fwdTvs(1:dall)))/dall
    write(66,'(a,f12.4,a)')'  ABS Mean T(dVs):',mean,'s'
    write(6 ,'(a,f12.4,a)')'  ABS Mean T(dVs):',mean,'s'
end subroutine

! Model Norm !

subroutine Calmodel2Norm(nar1,nar,maxvp,count3,rw,col,dv,weight)
    use  lsmrblasInterface, only : dnrm2
    implicit none
    ! works for both Vs and Gcs inversion.
    integer,intent(in) ::nar1,nar,maxvp,count3
    real rw(*)
    integer col(*)
    real,intent(in):: dv(*)
    real,intent(in):: weight
    ! OUTPUT
    real Mnorm2,MwNorm2
    ! PARAMETER
    integer jj,kk,ii
    integer i,j,k
    real Lm(count3),LmWeight(count3)
    integer numC
    ! LOGICAL:: isTikh
    !--------------------------------------------------------------------!
    ! count3: row number of L
    ! Formula: Lm=[a12(m1-m2)
    !                              a13(m1-m3)]
    ! nar1: number of  nonzero value of G
    ! nar: number of nonzero value of Gbar-E
    ! dv: inversion result
    ! rw: value of Gbar
    ! iw2: row number of Gbar, not contain column
    ! col: column number of Gbar
    ! Mnorm2: 2 norm of Lm
    ! numC: count index of  nonzero L
    ! weight: in L, weight has added to Gbar, so divide weight
    ! Regularization Type: (T) 1st order Tikhonov ;(F) Gaussian
    Lm=0
    LmWeight=0

    do i=1,count3
      numC=nar1+i
      Lm(i)=rw(numC)*dv(col(numC))/weight
      LmWeight(i)=rw(numC)*dv(col(numC))
    enddo
    Mnorm2=dnrm2(count3,Lm,1)
    MwNorm2=dnrm2(count3,LmWeight,1)

    ! open(90,file='Lnorm.txt')
    ! do i=1,count3
    ! numC=nar1+i*2-1
    ! write(90,*)numC,col(numC),col(numC+1),dv(col(numC)),dv(col(numC+1)),rw(numC),rw(numC+1)
    ! enddo
    ! close(90)
    !
    ! open(90,file='dv_output.txt')
    ! do i=1,maxvp
    ! write(90,*)dv(i),dv(i+maxvp),dv(i+maxvp*2)
    ! enddo
    ! close(90)

    write(6 ,'(a,2f12.3)') '  dVs:  ||Lm||^2      and ||wLm||^2    : ', Mnorm2, MwNorm2
    write(66,'(a,2f12.3)') '  dVs:  ||Lm||^2      and ||wLm||^2    : ', Mnorm2, MwNorm2
end subroutine

subroutine Calmodel2NormJoint(nar1,nar,maxvp, NreVs, Nre, rw,col,dv, lameGcs, lameVs)
    use  lsmrblasInterface, only : dnrm2
    implicit none
    ! INPUT
    ! NreVs: number of Vs regularization in G
    ! Nre  : number of regularization in G
    integer,intent(in) ::nar1,nar,maxvp, Nre, NreVs
    real rw(*)
    integer col(*)
    real,intent(in):: dv(*)
    real,intent(in):: lameGcs,lameVs
    ! OUTPUT
    real Mnorm2,MwNorm2
    real VsNorm2,VswNorm2
    real GcsNorm2,GcswNorm2
    ! PARAMETER
    integer jj,kk,ii
    integer i,j,k
    real Lm(Nre), LmWeight(Nre)
    integer numC
    ! LOGICAL:: isTikh
    !--------------------------------------------------------------------!
    ! count3: row number of L
    ! Formula: Lm=[a12(m1-m2)
    !                              a13(m1-m3)]
    ! nar1: number of  nonzero value of G
    ! nar: number of nonzero value of Gbar-E
    ! dv: inversion result
    ! rw: value of Gbar
    ! iw2: row number of Gbar, not contain column
    ! col: column number of Gbar
    ! Mnorm2: 2 norm of Lm
    ! numC: count index of  nonzero L
    ! weight: in L, weight has added to Gbar, so divide weight
    ! Regularization Type: (T) 1st order Tikhonov ;(F) Gaussian
    Lm=0
    LmWeight=0

    do i=1, NreVs
      numC=nar1+i
      Lm(i)=rw(numC)*dv(col(numC))/lameVs
      LmWeight(i)=rw(numC)*dv(col(numC))
    enddo
    VsNorm2=dnrm2(NreVs, Lm(1:NreVs), 1)
    VswNorm2=dnrm2(NreVs, LmWeight(1:NreVs), 1)

    do i=NreVs+1, Nre
      numC=nar1+i
      Lm(i)=rw(numC)*dv(col(numC))/lameGcs
      LmWeight(i)=rw(numC)*dv(col(numC))
    enddo

    GcsNorm2 = dnrm2(Nre-NreVs, Lm(NreVs+1:Nre), 1)
    GcswNorm2= dnrm2(Nre-NreVs, LmWeight(NreVs+1:Nre), 1)

    Mnorm2=dnrm2(Nre, Lm, 1)
    MwNorm2=dnrm2(Nre, LmWeight, 1)

    write(6 ,'(a,2f12.3)') '  dVs:  ||Lm||^2   and   ||wLm||^2     : ', VsNorm2, VswNorm2
    write(66,'(a,2f12.3)') '  dVs:  ||Lm||^2   and   ||wLm||^2     : ', VsNorm2, VswNorm2
    write(6 ,'(a,2f12.3)') '  Gcs:  ||Lm||^2   and   ||wLm||^2     : ', GcsNorm2, GcswNorm2
    write(66,'(a,2f12.3)') '  Gcs:  ||Lm||^2   and   ||wLm||^2     : ', GcsNorm2, GcswNorm2
    write(6 ,'(a,2f12.3)') '  All:  ||Lm||^2   and   ||wLm||^2     : ', Mnorm2, MwNorm2
    write(66,'(a,2f12.3)') '  All:  ||Lm||^2   and   ||wLm||^2     : ', Mnorm2, MwNorm2
    write(66,'(a)')'  '
    write(6 ,'(a)')'  '
end subroutine


C
C***********************************************************************
C                                                                      *
C            NACHOS II - A Finite Element Computer Program             *
C                        for Incompressible Flow Problems              *
C                                                                      *
C     Copyright (c) 1986,2019   National Technology & Engineering      *
C                               Solutions of Sandia, LLC (NTESS)       *
C                                                                      *
C                            All rights reserved.                      *
C                                                                      *
C     This software is distributed under the BSD 3-Clause License.     *
C                                                                      *
C***********************************************************************
C
      SUBROUTINE SHAPE8 (NGAUSS,GS1,GS2)
C
C     ******************************************************************
C
C     SUBROUTINE TO CALCULATE THE VALUES OF THE EIGHT NODE 'SERENDIPITY'
C     SHAPE FUNCTIONS AND THEIR DERIVATIVES AT THE INTEGRATION POINTS
C     (N X N GAUSS INTEGRATION RULE)
C
C     ******************************************************************
C
      COMMON /QUAD8/ F8Q(8,9),F8L(4,9),DF8QDS(8,9),DF8QDT(8,9)
C
      DIMENSION GS1(*), GS2(*)
      DIMENSION SI(8), TI(8)
C
      DATA (SI(I),I=1,8)/-1.,1.,1.,-1.,0.,1.,0.,-1./
      DATA (TI(I),I=1,8)/-1.,-1.,1.,1.,-1.,0.,1.,0./
C
C     STATEMENT FUNCTIONS FOR THE SHAPE FUNCTIONS AND THEIR DERIVATIVES
C
      FC(I,S,T)=.25*(1.+S*SI(I))*(1.+T*TI(I))*(S*SI(I)+T*TI(I)-1.)
      FM(I,S,T)=.50*((1.-S**2)*(1.+T*TI(I))*TI(I)**2+(1.-T**2)*(1.+S*SI
     1(I))*SI(I)**2)
      G(I,S,T)=.25*(1.+S*SI(I))*(1.+T*TI(I))
      DFCDS(I,S,T)=.25*(1.+T*TI(I))*(2.*S*SI(I)+T*TI(I))*SI(I)
      DFMDS(I,S,T)=.50*((1.+T*TI(I))*(-2.*S)*TI(I)**2+(1.-T**2)*SI(I))
      DFCDT(I,S,T)=.25*(1.+S*SI(I))*(2.*T*TI(I)+S*SI(I))*TI(I)
      DFMDT(I,S,T)=.50*((1.+S*SI(I))*(-2.*T)*SI(I)**2+(1.-S**2)*TI(I))
C
C     ******************************************************************
C
C     EVALUATE FUNCTIONS
C
      DO 10 I=1,4
      L=1
      DO 10 K=1,NGAUSS
      DO 10 J=1,NGAUSS
      DF8QDS(I,L)=DFCDS(I,GS1(J),GS2(K))
      DF8QDT(I,L)=DFCDT(I,GS1(J),GS2(K))
      F8Q(I,L)=FC(I,GS1(J),GS2(K))
      F8L(I,L)=G(I,GS1(J),GS2(K))
      L=L+1
   10 CONTINUE
      DO 20 I=5,8
      L=1
      DO 20 K=1,NGAUSS
      DO 20 J=1,NGAUSS
      DF8QDS(I,L)=DFMDS(I,GS1(J),GS2(K))
      DF8QDT(I,L)=DFMDT(I,GS1(J),GS2(K))
      F8Q(I,L)=FM(I,GS1(J),GS2(K))
      L=L+1
   20 CONTINUE
C
      RETURN
      END


MODULE m_sat_ini_vet

      IMPLICIT NONE
      !SAVE

Contains

 SUBROUTINE sat_ini_vet (fname_sp3, ini_vet)

! ----------------------------------------------------------------------
! SUBROUTINE: sat_ini_vet
! ----------------------------------------------------------------------
! Purpose: Generate the initial state vector for multi-GNSS satellites
!          using the interpolatior of Lagrange polynomials
! ----------------------------------------------------------------------
! Input arguments:
! - fname_sp3:  	Orbit file in sp3 format which contains only position vector
!
! Output arguments:
! - ini_vet:    	Array to restore the initial state vectors in ITRF
! ----------------------------------------------------------------------
! Note 1:
!  The input orbit file (sp3) is considered to include satellite position vector only
!  as this is the IGS standard product
!  During the orbit interpolation, based on Lagrange polynomials, position
!  and velocity vectors for the selected satellite (PRN) are computed and
!  provided in the output files.
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou, Cooperative Research Centre for Spatial Information, Australia
! Created:	5 August 2016
! ----------------------------------------------------------------------
! Last modified:
! - Dr. Thomas Papanikolaou, 14 November 2017:
!	Upgraded from Fortran 90 to Fortran 2003 for considering the
!	Fortran 2003 adavantages in dynamic memory allocation
!
! - Dr. Tzupang Tseng, 28 August 2018:
!       Enable the function of processing the multi-GNSS satellites
!       in order to obtain the initial state vector which will be used for
!       the further steps, such as the numerical integration or the orbit determination.
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      USE m_gnssp3
      USE m_lagrange
      !USE mdl_arr
      !USE mdl_write
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      CHARACTER (LEN=300), INTENT(IN) :: fname_sp3
      !CHARACTER (LEN=300), INTENT(IN) :: fname_orbint
!      INTEGER (KIND = prec_int8), INTENT(IN) :: interv, NPint
	  !INTEGER (KIND = 4) :: PRN
      !CHARACTER (LEN=3), INTENT(IN) :: PRN
! OUT
!      REAL (KIND = prec_q), INTENT(OUT), DIMENSION(:,:), ALLOCATABLE :: orbint
!      REAL (KIND = prec_q), INTENT(OUT), DIMENSION(:,:,:), ALLOCATABLE :: ini_vet
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int8) :: Nepochs, epoch_i, sz1, sz2
      INTEGER (KIND = prec_int8) :: data_rate, Nepoch_int, Norb_int, NPint_1, NPint_2, Nlimit
	  INTEGER (KIND = prec_int8) :: i, j, i1, i2, ti
	  REAL (KIND = prec_d) :: SecDay_i, MJD_i
	  REAL (KIND = prec_d) :: MJD_tint, tint, Xint, Xint_1, Yint, Yint_1, Zint, Zint_1
	  REAL (KIND = prec_d) :: X, Y, Z, Vx, Vy, Vz
      INTEGER (KIND = prec_int2) :: AllocateStatus, DeAllocateStatus
      CHARACTER (LEN=300) :: fname_write
      INTEGER (KIND = 4) :: PRN,NEPO,zz
      INTEGER (KIND = 4) :: Ofile
      INTEGER (KIND = prec_int8) :: interv, NPint
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: orbint
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: orbsp3
      REAL (KIND = prec_q), DIMENSION(:,:,:), ALLOCATABLE :: orbeph
      REAL (KIND = prec_q), DIMENSION(:,:,:), ALLOCATABLE :: ini_vet
      REAL (KIND = prec_q), DIMENSION(:), ALLOCATABLE :: X_interp
      REAL (KIND = prec_q), DIMENSION(:), ALLOCATABLE :: Y_interp
! ----------------------------------------------------------------------

! Initialize varbiles of interv and NPint
! ---------------------------------------
       interv = 900 ! in second
       NPint  = 12
       Ofile  = 88
      OPEN (unit = 88, file= "gnss-t0-vector.txt")

	  ! Read IGS sp3 orbit data file (position only): orbsp3
      Call gnssp3 (fname_sp3, orbeph)
	  sz1 = size(orbeph, DIM = 2)

      ALLOCATE ( orbsp3(size(orbeph, DIM = 2), size(orbeph, DIM = 3)), STAT = AllocateStatus)
      ALLOCATE ( ini_vet(size(orbeph, DIM = 1),1, 8), STAT = AllocateStatus)

      DO PRN = 1 , size(orbeph, DIM = 1) ! satellite PRN (BIG LOOP for PROCESSING MULTI-GNSS SATELLITES)

	               DO NEPO = 1, size(orbeph, DIM = 2) ! number of epochs

        IF (orbeph(PRN,NEPO,3)==0 .and.orbeph(PRN,NEPO,4)==0.and. orbeph(PRN,NEPO,5)==0)THEN
                        orbsp3 (NEPO , 1) = 0.0d0
                        orbsp3 (NEPO , 2) = 0.0d0
                        orbsp3 (NEPO , 3) = 0.0d0
                        orbsp3 (NEPO , 4) = 0.0d0
                        orbsp3 (NEPO , 5) = 0.0d0

                        EXIT
        ELSE
                        orbsp3 (NEPO , 1) = orbeph(PRN,NEPO,1)
                        orbsp3 (NEPO , 2) = orbeph(PRN,NEPO,2)
                        orbsp3 (NEPO , 3) = orbeph(PRN,NEPO,3)
                        orbsp3 (NEPO , 4) = orbeph(PRN,NEPO,4)
                        orbsp3 (NEPO , 5) = orbeph(PRN,NEPO,5)
        END IF
                       End DO

! ----------------------------------------------------------------------
! Interpolation epoch limits (24h orbit arc or less):
! 1. Interpolation is applied only within the data points (15 min less than 24h arc):  Set Nlimit to 1
! 2. Interpolation is applied also after the last data point's epoch (23h 45min 00sec) in order to cover a 24h arc:  Set Nlimit to 0
Nlimit = 1
! ----------------------------------------------------------------------

	  ! Data rate of sp3 (in sec)
      DO i = 1 , sz1-1
		if ( INT(orbsp3(i+1,1)) - INT(orbsp3(i,1)) == 0 ) then
			data_rate = INT( orbsp3(2,2) - orbsp3(1,2) )
			EXIT
		end if
	  End Do

      ! Number of points to be interpolated during the span between two epochs of the sp3 file (15min, 5min)
	  Nepoch_int = data_rate / interv - 1

      ! Number of epochs of interpolated orbit (excluding extrapolation at the last point i.e. the start of the next day's sp3: 00h 00min 00sec)
      !Norb_int = (data_rate / interv) * (sz1-1) + 1
      Norb_int = (data_rate / interv) * (sz1 - Nlimit) + Nlimit
	  !Nlimit=1
      !Norb_int = 1 + (data_rate / interv) * (sz1 - Nlimit)
	  !Nlimit=0
      !Norb_int = 1 + (data_rate / interv) * (sz1 - Nlimit) - 1

! Allocatable arrays for Lagrange Interpolation (declared in mdl_arr.f90)
      ALLOCATE (X_interp(NPint), STAT = AllocateStatus)
      ALLOCATE (Y_interp(NPint), STAT = AllocateStatus)

! Allocatable arrays  for the interpolated orbit array
      ALLOCATE (orbint(Norb_int, 8) , STAT = AllocateStatus)

! ----------------------------------------------------------------------
! Number of data points (used for the interpolation) before and after of each epoch of sp3 file
      NPint_1 = INT(NPint/2)
      NPint_2 = NPint - NPint_1

	  epoch_i = 0
      DO i = 1 , sz1
		epoch_i  = epoch_i + 1
		MJD_i    = orbsp3(i,1)
		SecDay_i = orbsp3(i,2)
 IF (orbsp3(i,1)==0 .and. orbsp3(i,2)==0) EXIT

! ----------------------------------------------------------------------
! Form arrays with data points to be used in the interpolation
		if ( i < NPint_1 ) then
			i1 = i
			i2 = i + (NPint - 1)
			!print *,"sz1, i, epoch_i, i1, i2", sz1, i, epoch_i, i1, i2
		else if ( sz1 - i < NPint_2 ) then
			i2 = sz1
			i1 = i2 - (NPint-1)
			!print *,"sz1, i, epoch_i, i1, i2", sz1, i, epoch_i, i1, i2
		else
			i1 = i - (NPint_1 - 1)
			i2 = i + NPint_2
			!print *,"i,epoch_i,i1,i2", i, epoch_i, i1, i2
		end if

        ! Time array of the data points used for the interpolation: X_interp
!		ti = 1
!		X_interp(1) = orbsp3(i1, 2)
!		Do ti = 2 , NPint
!			X_interp(ti) = X_interp(1) + (ti - 1) * data_rate * 1.D0
!		End Do

		ti = 0
		Do j = i1 , i2
		ti = ti + 1
			X_interp(ti) = SecDay_i + (j-i) * data_rate * 1.D0  !(ti - 1) * data_rate * 1.D0
		End Do
	    j = 0
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Data Point: Velocity computation based on interpolation
! ----------------------------------------------------------------------
		! Position Vector: value from the IGS sp3 file
		orbint(epoch_i, 1:5) = orbsp3(i, 1:5)
        X = orbsp3(i,3)
		Y = orbsp3(i,4)
		Z = orbsp3(i,5)

		! Velocity Vector: computation via Lagrange interpolation
		tint = SecDay_i

		! Vx
		Y_interp = orbsp3(i1:i2, 3)
		!Call interp_lag(tint-1, Xint_1)
		Call interp_lag(tint-1, X_interp, Y_interp, Xint_1)
        Vx = X - Xint_1
		! Vy
		Y_interp = orbsp3(i1:i2, 4)
		!Call interp_lag(tint-1, Yint_1)
		Call interp_lag(tint-1, X_interp, Y_interp, Yint_1)
        Vy = Y - Yint_1
		! Vz
		Y_interp = orbsp3(i1:i2, 5)
		!Call interp_lag(tint-1, Zint_1)
		Call interp_lag(tint-1, X_interp, Y_interp, Zint_1)
        Vz = Z - Zint_1

		! Write Velocity to array orbint
		orbint(epoch_i, 6) = Vx
		orbint(epoch_i, 7) = Vy
		orbint(epoch_i, 8) = Vz
! ----------------------------------------------------------------------
! ----------------------------------------------------------------------
! Output the initial state vector at the first epoch (added by Tzupang Tseng
! 28-08-2018)
! ----------------------------------------------------------------------

      ini_vet (PRN, 1, 1) = orbint(1, 1)
      ini_vet (PRN, 1, 2) = orbint(1, 2)
      ini_vet (PRN, 1, 3) = orbint(1, 3)
      ini_vet (PRN, 1, 4) = orbint(1, 4)
      ini_vet (PRN, 1, 5) = orbint(1, 5)
      ini_vet (PRN, 1, 6) = orbint(1, 6)
      ini_vet (PRN, 1, 7) = orbint(1, 7)
      ini_vet (PRN, 1, 8) = orbint(1, 8)

      End Do

      WRITE (UNIT = 88, FMT = "(I4,5X,F8.2,5X,F3.1,3(5X,F15.4),3(5X,F14.8))") &
      & PRN, ini_vet (PRN, 1, 1), ini_vet (PRN, 1, 2), ini_vet (PRN, 1, 3), &
      &      ini_vet (PRN, 1, 4), ini_vet (PRN, 1, 5), ini_vet (PRN, 1, 6), &
      &      ini_vet (PRN, 1, 7), ini_vet (PRN, 1, 8)

!print*,"PRN=",PRN
!print*, ini_vet (PRN, 1, 1), ini_vet (PRN, 1, 2), ini_vet (PRN, 1, 3), &
!      & ini_vet (PRN, 1, 4), ini_vet (PRN, 1, 5), ini_vet (PRN, 1, 6), &
!      & ini_vet (PRN, 1, 7), ini_vet (PRN, 1, 8)

      END DO ! BIG LOOP for PROCESSING MULTI-GNSS SATELLITES

	  Deallocate (X_interp)
	  Deallocate (Y_interp)

      CLOSE (UNIT = 88)
End subroutine

END Module


! DART software - Copyright UCAR. This open source software is provided
! by UCAR, "as is", without charge, subject to all terms of use at
! http://www.image.ucar.edu/DAReS/DART/DART_download

!> Test to evaluate how to look_for_bias in prior ensemble distribution
!> and observation in observation space.
!>
!> See work from 13 October, 2003.

program sys_sim301

use random_seq_mod, only : random_seq_type, init_random_seq, random_gaussian, &
   twod_gaussians, random_uniform

implicit none

type (random_seq_type) :: r
double precision :: y_o, sigma_y_o, y_p, sigma_y_p, dist_sq, sample_mean
double precision :: dist_sq_sum, mean_dist_sq
integer :: i, j, n, num, n_samples, count

! Initialize repeatable random sequence
call init_random_seq(r)

count = 0

! For now have a set of free parameters that may be too large
write(*, *) 'Input prior variance of observation variable'
read(*, *) sigma_y_p
write(*, *) 'Input variance of observing instrument'
read(*, *) sigma_y_o
write(*, *) 'Input ensemble size'
read(*, *) n

write(*, *) 'Input number of samples for statistics'
read(*, *) n_samples
sample_mean = 0.0

! Loop through the number of samples
do i = 1, n_samples

! Generate the observation
   y_o = random_gaussian(r, dble(0.0), sqrt(sigma_y_o))

   dist_sq_sum = 0.0
   do j = 1, n
      y_p = random_gaussian(r, dble(0.0), sqrt(sigma_y_p))
! Compute the distance between the samples and the obs
      dist_sq = (y_p - y_o) ** 2
!!!      dist_sq = abs(y_p - y_o)
      dist_sq_sum = dist_sq_sum + dist_sq
   end do
   mean_dist_sq = dist_sq_sum / n

   sample_mean = sample_mean + mean_dist_sq

end do

write(*, *) 'mean squared distance is ', sample_mean / n_samples

end program sys_sim301


      subroutine chunkintf(f,fint,ndim,k,nch,chunks,ders,hs)
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
c
c     Computes the anti-derivative with respect to arc-length
c     of the real function f (which can be vector valued)
c
c
c
cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
      implicit none
c     global variables
      integer ndim, k, nch
      real *8 f(ndim,k,*), fint(ndim,k,*), chunks(2,k,*), ders(2,k,*)
      real *8 hs(*)
c     local variables
      real *8, allocatable :: ainte(:), adiff(:), x(:), whts(:)
      real *8, allocatable :: ftemp2(:,:), ftemp(:,:), work(:)
      real *8, allocatable :: sums(:), endinter(:)
      integer i, j, l, lwork
      integer korder, itype
      real *8 dsdt

      korder = k-1

      lwork = 3*k**2 + 2*k + 500

      allocate(ainte(k*k),adiff(k*k),x(k),whts(k),ftemp(k,ndim),
     1     ftemp2(k,ndim),work(lwork),sums(ndim),endinter(k))

      itype = 1
      call legeinmt(k,ainte,adiff,x,whts,endinter,itype,work)

      do l = 1,ndim
         sums(l) = 0.0d0
      enddo

      do i = 1,nch
         do j = 1,k
            dsdt = dsqrt(ders(1,j,i)**2+ders(2,j,i)**2)*hs(i)
            do l = 1,ndim
               ftemp2(j,l) = f(l,j,i)*dsdt
            enddo
         enddo
         do l = 1,ndim
c     get spectral anti-derivative
            call amatchunkint(ainte,ftemp2(1,l),ftemp(1,l),k)
         enddo
         do j = 1,k
            do l = 1,ndim
               fint(l,j,i) = ftemp(j,l) + sums(l)
            enddo
         enddo
         do l = 1,ndim
            do j = 1,k
               sums(l) = sums(l) + whts(j)*ftemp2(j,l)
            enddo
         enddo
      enddo

      return
      end

      subroutine amatchunkint(a,x,y,n)
c     evaluates y = a*x
      implicit none
      real *8 a(n,n), x(*), y(*)
      integer n, i, j

      do i = 1,n
         y(i) = 0.0d0
      enddo

      do j = 1,n
         do i = 1,n
            y(i) = y(i)+a(i,j)*x(j)
         enddo
      enddo

      return
      end


        INTEGER  FUNCTION  TIME2SEC ( TIME )

C***********************************************************************
C Version "$Id: time2sec.f 1 2017-06-10 18:05:20Z coats $"
C EDSS/Models-3 I/O API.
C Copyright (C) 1992-2002 MCNC and Carlie J. Coats, Jr.,
C (C) 2003-2010 by Baron Advanced Meteorological Systems.
C Distributed under the GNU LESSER GENERAL PUBLIC LICENSE version 2.1
C See file "LGPL.txt" for conditions of use.
C.........................................................................
C  function body starts at line  40
C
C  FUNCTION:  convert time difference format HHMMSS to integer seconds
C
C  RETURN VALUE:  seconds
C
C  PRECONDITION:  integer TIME formatted HHMMSS
C
C  REVISION  HISTORY:
C      Prototype  5/1992 by CJC
C      Version    2/1993 by CJC for CRAY, etc.
C       Modified 03/2010 by CJC: F9x changes for I/O API v3.1
C***********************************************************************

      IMPLICIT NONE

C...........   ARGUMENTS and their descriptions:

        INTEGER, INTENT(IN   ) :: TIME

C...........   SCRATCH LOCAL VARIABLES and their descriptions:

        INTEGER      	ABST

C***********************************************************************
C   begin body of function  TIME2SEC

        IF ( TIME .GE. 0 ) THEN		!  div-mod arithmetic OK
            TIME2SEC = MOD( TIME, 100 )  +
     &                      60 * ( MOD( TIME / 100, 100 )  +
     &                             60 * ( TIME / 10000 ) )
        ELSE				!  work with absolute values:
            ABST     = - TIME
            TIME2SEC = - ( MOD( ABST, 100 )  +
     &                          60 * ( MOD( ABST / 100, 100 )  +
     &                                 60 * ( ABST / 10000 ) ) )
        END IF

        RETURN

        END FUNCTION TIME2SEC


        SUBROUTINE GDGHIR ( inttyp, glat, glon, np, maxx, maxy, grid,
     +			    grdo, iret )
C************************************************************************
C* GDGHIR								*
C*									*
C* This subroutine interplates the data in the input grid to the	*
C* output grid whose positions relative to the input grid are given	*
C* in GLAT and GLON as I and J positions.				*
C*									*
C* GRID and GRDO may be the same array.					*
C*									*
C* GDGHIR ( INTTYP, GLAT, GLON, NP, MAXX, MAXY, GRID, GRDO, IRET )	*
C*									*
C* Input parameters:							*
C*	INTTYP		INTEGER		Interpolation type		*
C*					  0 = linear interpolation	*
C*					  1 = nearest point assignment	*
C*	GLAT (NP)	REAL		GRID relative I positions	*
C*	GLON (NP)	REAL		GRID relative J positions	*
C*	NP		INTEGER		Number of output points		*
C*	MAXX		INTEGER		Maximum I dimension of GRID	*
C*	MAXY		INTEGER		Maximum J dimension of GRID	*
C*	GRID (MAXX,MAXY)REAL		Grid of data			*
C*									*
C* Output parameters:							*
C*	GRDO (*)	REAL		Output grid of interplated data *
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					-18 = invalied interp type	*
C**									*
C* Log:									*
C* K. Brill/HPC		 2/00						*
C* T. Lee/SAIC		 7/03		Changed LLMXGD -> LLMXTG	*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
	REAL 		grid (*), grdo (*), glat (*), glon (*)
C*
	REAL		gout (LLMXTG)
C*
	INCLUDE		'ERMISS.FNC'
	INDXQ ( lx, ly, kx ) = ( ly - 1 ) * kx + lx
C------------------------------------------------------------------------
	iret = 0
C
C*	Interpolate to output grid at valid points.
C
	IF ( inttyp .eq. 0 ) THEN
C
C*	    Bilinear interpolation.  Note:  GR_INTP is not used
C*	    because it does no check for missing coordinates.
C
	    DO ii = 1, np
		IF ( .not.
     +		  ( ERMISS ( glat (ii) ) .or. ERMISS ( glon (ii) )
     +		 .or. glat (ii) .lt. 1 .or. glon (ii) .lt. 1
     +		 .or. glat (ii) .gt. maxx .or. glon (ii) .gt. maxy ) )
     +		THEN
		    icol = glat (ii)
		    irow = glon (ii)
		    IF ( icol .eq. maxx ) icol = icol - 1
		    IF ( irow .eq. maxy ) irow = irow - 1
		    icolp1 = icol + 1
		    irowp1 = irow + 1
		    c = glat (ii) - FLOAT (icol)
		    r = glon (ii) - FLOAT (irow)
		    omc = 1. - c
		    omr = 1. - r
		    IF ( ERMISS (grid(INDXQ(icol,irow,maxx)))
     +			 .or.
     +			 ERMISS (grid(INDXQ(icolp1,irow,maxx)))
     +			 .or.
     +			 ERMISS (grid(INDXQ(icol,irowp1,maxx)))
     +			 .or.
     +			 ERMISS (grid(INDXQ(icolp1,irowp1,maxx))) )
     +		    THEN
			gout (ii) = RMISSD
		    ELSE
			gout (ii) =
     +			( grid (INDXQ(icol,irow,maxx)) * omc +
     +			  grid (INDXQ(icolp1,irow,maxx)) * c ) * omr +
     +		        ( grid (INDXQ(icol,irowp1,maxx)) * omc +
     +		          grid (INDXQ(icolp1,irowp1,maxx) ) * c ) * r
		    END IF
		ELSE
		    gout (ii) = RMISSD
		END IF
	    END DO
	ELSE IF ( inttyp .eq. 1 ) THEN
C
C*	    Nearest value interpolation (NVI).
C
	    DO ii = 1, np
		IF ( .not.
     +		  ( ERMISS ( glat (ii) ) .or. ERMISS ( glon (ii) )
     +		 .or. glat (ii) .lt. 1 .or. glon (ii) .lt. 1
     +		 .or. glat (ii) .gt. maxx .or. glon (ii) .gt. maxy ) )
     +		THEN
		    nrx = NINT ( glat (ii) )
		    nry = NINT ( glon (ii) )
		    gout (ii) = grid (INDXQ(nrx,nry,maxx))
		ELSE
		    gout (ii) = RMISSD
		END IF
	    END DO
	ELSE
	    iret = -18
	    RETURN
	END IF
	DO ii = 1, np
	    grdo (ii) = gout (ii)
	END DO
C*
	RETURN
	END


      subroutine OPERATORLAP4(
     &           lofphi
     &           ,ilofphilo0,ilofphilo1
     &           ,ilofphihi0,ilofphihi1
     &           ,nlofphicomp
     &           ,phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           ,alpha
     &           ,beta
     &           )
      implicit none
      integer nlofphicomp
      integer ilofphilo0,ilofphilo1
      integer ilofphihi0,ilofphihi1
      REAL*8 lofphi(
     &           ilofphilo0:ilofphihi0,
     &           ilofphilo1:ilofphihi1,
     &           0:nlofphicomp-1)
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      REAL*8 alpha
      REAL*8 beta
      REAL*8 dxinv, lap
      integer n,ncomp
      integer i,j
      ncomp = nphicomp
      if(ncomp .ne. nlofphicomp) then
         call MAYDAYERROR()
      endif
      dxinv = (1.0d0)/(dx*dx)
      do n = 0, ncomp-1
      do j = iregionlo1,iregionhi1
      do i = iregionlo0,iregionhi0
          lap = (
     &   (16.0d0)*phi(i-1,j  ,n) - phi(i-2,j  ,n)
     & + (16.0d0)*phi(i+1,j  ,n) - phi(i+2,j  ,n)
     & + (16.0d0)*phi(i  ,j-1,n) - phi(i  ,j-2,n)
     & + (16.0d0)*phi(i  ,j+1,n) - phi(i  ,j+2,n)
     &                     -((30.0d0)*2)*phi(i,j,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lofphi(i,j,n) = alpha*phi(i,j,n)+beta*lap
      enddo
      enddo
      enddo
      return
      end
      subroutine OPERATORLAPRES4(
     &           r
     &           ,irlo0,irlo1
     &           ,irhi0,irhi1
     &           ,nrcomp
     &           ,phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0,irhslo1
     &           ,irhshi0,irhshi1
     &           ,nrhscomp
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           ,alpha
     &           ,beta
     &           )
      implicit none
      integer nrcomp
      integer irlo0,irlo1
      integer irhi0,irhi1
      REAL*8 r(
     &           irlo0:irhi0,
     &           irlo1:irhi1,
     &           0:nrcomp-1)
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0,irhslo1
      integer irhshi0,irhshi1
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           irhslo1:irhshi1,
     &           0:nrhscomp-1)
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      REAL*8 alpha
      REAL*8 beta
      REAL*8 dxinv, lap, lhs
      integer n,ncomp
      integer i,j
      ncomp = nphicomp
      dxinv = (1.0d0)/(dx*dx)
      do n = 0, ncomp-1
      do j = iregionlo1,iregionhi1
      do i = iregionlo0,iregionhi0
          lap = (
     &   (16.0d0)*phi(i-1,j  ,n) - phi(i-2,j  ,n)
     & + (16.0d0)*phi(i+1,j  ,n) - phi(i+2,j  ,n)
     & + (16.0d0)*phi(i  ,j-1,n) - phi(i  ,j-2,n)
     & + (16.0d0)*phi(i  ,j+1,n) - phi(i  ,j+2,n)
     &                     -((30.0d0)*2)*phi(i,j,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lhs = alpha*phi(i,j,n) + beta*lap
          r(i,j,n) = rhs(i,j,n) - lhs
      enddo
      enddo
      enddo
      return
      end
      subroutine RESTRICTRES4(
     &           res
     &           ,ireslo0,ireslo1
     &           ,ireshi0,ireshi1
     &           ,nrescomp
     &           ,phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0,irhslo1
     &           ,irhshi0,irhshi1
     &           ,nrhscomp
     &           ,alpha
     &           ,beta
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           )
      implicit none
      integer nrescomp
      integer ireslo0,ireslo1
      integer ireshi0,ireshi1
      REAL*8 res(
     &           ireslo0:ireshi0,
     &           ireslo1:ireshi1,
     &           0:nrescomp-1)
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0,irhslo1
      integer irhshi0,irhshi1
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           irhslo1:irhshi1,
     &           0:nrhscomp-1)
      REAL*8 alpha
      REAL*8 beta
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      REAL*8 denom,dxinv,lofphi,lap
      integer n,ncomp
      integer i,j
      integer ii,jj
      ncomp = nphicomp
      dxinv = (1.0d0) / (dx*dx)
      denom = 2  *2
      do n = 0, ncomp-1
      do j = iregionlo1,iregionhi1
      do i = iregionlo0,iregionhi0
          ii = i/2
          jj = j/2
          lap = (
     &   (16.0d0)*phi(i-1,j  ,n) - phi(i-2,j  ,n)
     & + (16.0d0)*phi(i+1,j  ,n) - phi(i+2,j  ,n)
     & + (16.0d0)*phi(i  ,j-1,n) - phi(i  ,j-2,n)
     & + (16.0d0)*phi(i  ,j+1,n) - phi(i  ,j+2,n)
     &                     -((30.0d0)*2)*phi(i,j,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          lofphi = alpha*phi(i,j,n) + beta*lap
          res(ii,jj,n) = res(ii,jj,n)
     &                            + (rhs(i,j,n) - lofphi) / denom
      enddo
      enddo
      enddo
      return
      end
      subroutine GSRBLAPLACIAN4(
     &           phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0,irhslo1
     &           ,irhshi0,irhshi1
     &           ,nrhscomp
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           ,tmp
     &           ,itmplo0,itmplo1
     &           ,itmphi0,itmphi1
     &           ,ntmpcomp
     &           ,redBlack
     &           )
      implicit none
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0,irhslo1
      integer irhshi0,irhshi1
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           irhslo1:irhshi1,
     &           0:nrhscomp-1)
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      integer ntmpcomp
      integer itmplo0,itmplo1
      integer itmphi0,itmphi1
      REAL*8 tmp(
     &           itmplo0:itmphi0,
     &           itmplo1:itmphi1,
     &           0:ntmpcomp-1)
      integer redBlack
      REAL*8 dx2t, thD
      integer i,j
      integer n,ncomp,indtot,imin,imax,red,black
      red = 0
      black = 1
      dx2t = (12.0d0)*dx*dx
      thD  = (1.000d0 / 30.000d0)/2
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
         if (redBlack .eq. red) then
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               tmp(i,j,n) = thD*(
     &           (16.0d0)*phi(i+1,j,n) - phi(i+2,j,n)
     &         + (16.0d0)*phi(i-1,j,n) - phi(i-2,j,n)
     &         + (16.0d0)*phi(i,j+1,n) - phi(i,j+2,n)
     &         + (16.0d0)*phi(i,j-1,n) - phi(i,j-2,n)
     &         - dx2t*rhs(i,j,n) )
            enddo
          enddo
        else if (redBlack .eq. black) then
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+black, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,j,n) = thD*(
     &           (16.0d0)*tmp(i+1,j,n) - tmp(i+2,j,n)
     &         + (16.0d0)*tmp(i-1,j,n) - tmp(i-2,j,n)
     &         + (16.0d0)*tmp(i,j+1,n) - tmp(i,j+2,n)
     &         + (16.0d0)*tmp(i,j-1,n) - tmp(i,j-2,n)
     &         - dx2t*rhs(i,j,n) )
            enddo
          enddo
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,j,n) = tmp(i,j,n)
            enddo
          enddo
        else
           call MAYDAYERROR()
        end if
      enddo
      return
      end
      subroutine SORLAPLACIAN4(
     &           phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0,irhslo1
     &           ,irhshi0,irhshi1
     &           ,nrhscomp
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           )
      implicit none
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0,irhslo1
      integer irhshi0,irhshi1
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           irhslo1:irhshi1,
     &           0:nrhscomp-1)
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      REAL*8 dx2t, thD, tmp, omega
      integer i,j
      integer n,ncomp
      dx2t = (12.0d0)*dx*dx
      thD  = (1.000d0 / 30.000d0)/2
      omega = 0.47
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
      do j = iregionlo1,iregionhi1
      do i = iregionlo0,iregionhi0
         tmp = thD*(
     &        (16.0d0)*phi(i+1,j,n) - phi(i+2,j,n)
     &        + (16.0d0)*phi(i-1,j,n) - phi(i-2,j,n)
     &        + (16.0d0)*phi(i,j+1,n) - phi(i,j+2,n)
     &        + (16.0d0)*phi(i,j-1,n) - phi(i,j-2,n)
     &        - dx2t*rhs(i,j,n) )
         phi(i,j,n) = omega*tmp
     &        + ((1.0d0)-omega)*phi(i,j,n)
      enddo
      enddo
      enddo
      return
      end
      subroutine GSRBHELMHOLTZ4(
     &           phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,rhs
     &           ,irhslo0,irhslo1
     &           ,irhshi0,irhshi1
     &           ,nrhscomp
     &           ,iregionlo0,iregionlo1
     &           ,iregionhi0,iregionhi1
     &           ,dx
     &           ,tmp
     &           ,itmplo0,itmplo1
     &           ,itmphi0,itmphi1
     &           ,ntmpcomp
     &           ,alpha
     &           ,beta
     &           ,redBlack
     &           )
      implicit none
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer nrhscomp
      integer irhslo0,irhslo1
      integer irhshi0,irhshi1
      REAL*8 rhs(
     &           irhslo0:irhshi0,
     &           irhslo1:irhshi1,
     &           0:nrhscomp-1)
      integer iregionlo0,iregionlo1
      integer iregionhi0,iregionhi1
      REAL*8 dx
      integer ntmpcomp
      integer itmplo0,itmplo1
      integer itmphi0,itmphi1
      REAL*8 tmp(
     &           itmplo0:itmphi0,
     &           itmplo1:itmphi1,
     &           0:ntmpcomp-1)
      REAL*8 alpha
      REAL*8 beta
      integer redBlack
      REAL*8 dx2t, lambda, lap, dxinv, helm
      integer i,j
      integer n,ncomp,indtot,imin,imax,red,black
      red = 0
      black = 1
      dx2t = (12.0d0)*dx*dx
      dxinv = (1.0d0)/(dx*dx)
      lambda = (1.0d0)/(alpha - beta*(30.0d0)*2*(1.000d0 / 12.000d0)*dxi
     &nv)
      lambda = lambda*(0.60)
      ncomp = nphicomp
      if(ncomp .ne. nrhscomp) then
         call MAYDAYERROR()
      endif
      do n = 0, ncomp - 1
         if (redBlack .eq. red) then
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
          lap = (
     &   (16.0d0)*phi(i-1,j  ,n) - phi(i-2,j  ,n)
     & + (16.0d0)*phi(i+1,j  ,n) - phi(i+2,j  ,n)
     & + (16.0d0)*phi(i  ,j-1,n) - phi(i  ,j-2,n)
     & + (16.0d0)*phi(i  ,j+1,n) - phi(i  ,j+2,n)
     &                     -((30.0d0)*2)*phi(i,j,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
          helm = alpha*phi(i,j,n) + beta*lap
          tmp(i,j,n) = phi(i,j,n) +
     &      lambda*( rhs(i,j,n) - helm )
            enddo
          enddo
        else if (redBlack .eq. black) then
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+black, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               lap = (
     &           (16.0d0)*tmp(i+1,j,n) - tmp(i+2,j,n)
     &         + (16.0d0)*tmp(i-1,j,n) - tmp(i-2,j,n)
     &         + (16.0d0)*tmp(i,j+1,n) - tmp(i,j+2,n)
     &         + (16.0d0)*tmp(i,j-1,n) - tmp(i,j-2,n)
     &                     -((30.0d0)*2)*tmp(i,j,n) )
     &       * (1.000d0 / 12.000d0) * dxinv
               helm = alpha*tmp(i,j,n) + beta*lap
               phi(i,j,n) = tmp(i,j,n) +
     &              lambda*( rhs(i,j,n) - helm )
            enddo
          enddo
          do j=iregionlo1, iregionhi1
            imin = iregionlo0
            indtot = imin + j
            imin = imin + abs(mod(indtot+red, 2))
            imax = iregionhi0
            do i = imin, imax, 2
               phi(i,j,n) = tmp(i,j,n)
            enddo
          enddo
        else
           call MAYDAYERROR()
        end if
      enddo
      return
      end
      subroutine NEWGETFLUX4(
     &           flux
     &           ,ifluxlo0,ifluxlo1
     &           ,ifluxhi0,ifluxhi1
     &           ,nfluxcomp
     &           ,phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,iboxlo0,iboxlo1
     &           ,iboxhi0,iboxhi1
     &           ,beta_dx
     &           ,a_idir
     &           )
      implicit none
      integer CHF_ID(0:5,0:5)
      data CHF_ID/ 1,0,0,0,0,0 ,0,1,0,0,0,0 ,0,0,1,0,0,0 ,0,0,0,1,0,0 ,0
     &,0,0,0,1,0 ,0,0,0,0,0,1 /
      integer nfluxcomp
      integer ifluxlo0,ifluxlo1
      integer ifluxhi0,ifluxhi1
      REAL*8 flux(
     &           ifluxlo0:ifluxhi0,
     &           ifluxlo1:ifluxhi1,
     &           0:nfluxcomp-1)
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer iboxlo0,iboxlo1
      integer iboxhi0,iboxhi1
      REAL*8 beta_dx
      integer a_idir
      INTEGER ncomp,n
      integer ii, jj
      integer i , j
      ncomp = nphicomp
      ii = CHF_ID(a_idir, 0)
      jj = CHF_ID(a_idir, 1)
      do n = 0, ncomp-1
      do j = iboxlo1,iboxhi1
      do i = iboxlo0,iboxhi0
          flux(i,j,n) = beta_dx * (1.000d0 / 12.000d0) *
     &        ( (15.0d0)*phi(i,j,n)
     &           + phi(i-2*ii,j-2*jj,n)
     &           - phi(i+ii,j+jj,n)
     &           - (15.0d0)*phi(i-ii,j-jj,n) )
      enddo
      enddo
      enddo
      return
      end
      subroutine PROLONGLINEAR(
     &           phi
     &           ,iphilo0,iphilo1
     &           ,iphihi0,iphihi1
     &           ,nphicomp
     &           ,coarse
     &           ,icoarselo0,icoarselo1
     &           ,icoarsehi0,icoarsehi1
     &           ,ncoarsecomp
     &           ,ifineBoxlo0,ifineBoxlo1
     &           ,ifineBoxhi0,ifineBoxhi1
     &           ,icrseBoxlo0,icrseBoxlo1
     &           ,icrseBoxhi0,icrseBoxhi1
     &           ,r
     &           )
      implicit none
      integer nphicomp
      integer iphilo0,iphilo1
      integer iphihi0,iphihi1
      REAL*8 phi(
     &           iphilo0:iphihi0,
     &           iphilo1:iphihi1,
     &           0:nphicomp-1)
      integer ncoarsecomp
      integer icoarselo0,icoarselo1
      integer icoarsehi0,icoarsehi1
      REAL*8 coarse(
     &           icoarselo0:icoarsehi0,
     &           icoarselo1:icoarsehi1,
     &           0:ncoarsecomp-1)
      integer ifineBoxlo0,ifineBoxlo1
      integer ifineBoxhi0,ifineBoxhi1
      integer icrseBoxlo0,icrseBoxlo1
      integer icrseBoxhi0,icrseBoxhi1
      integer r
      INTEGER ncomp, n
      integer i ,j
      integer ic,jc
      ncomp = nphicomp
      do n = 0, ncomp-1
      do j = ifineBoxlo1,ifineBoxhi1
      do i = ifineBoxlo0,ifineBoxhi0
           ic = i/r
           jc = j/r
           phi(i,j,n) =  phi(i,j,n) +
     &          coarse(ic,jc,n)
           if (ic.ne.icrseBoxhi0 .and.
     &         (ic*r.lt.i .or. ic.eq.icrseBoxlo0)) then
              phi(i,j,n) =  phi(i,j,n) +
     &             (coarse(ic+1,jc,n)
     &              - coarse(ic,jc,n))/r*(i+(0.500d0)-ic*r-(0.500d0)*r)
           else
              phi(i,j,n) =  phi(i,j,n) +
     &             (- coarse(ic-1,jc,n)
     &              + coarse(ic,jc,n))/r*(i+(0.500d0)-ic*r-(0.500d0)*r)
           endif
           if (jc.ne.icrseBoxhi1 .and.
     &         (jc*r.lt.j .or. jc.eq.icrseBoxlo1)) then
              phi(i,j,n) =  phi(i,j,n) +
     &             (coarse(ic,jc+1,n)
     &              - coarse(ic,jc,n))/r*(j+(0.500d0)-jc*r-(0.500d0)*r)
           else
              phi(i,j,n) =  phi(i,j,n) +
     &             (- coarse(ic,jc-1,n)
     &              + coarse(ic,jc,n))/r*(j+(0.500d0)-jc*r-(0.500d0)*r)
           endif
      enddo
      enddo
      enddo
      return
      end