! H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
! H0 X
! H0 X   libAtoms+QUIP: atomistic simulation library
! H0 X
! H0 X   Portions of this code were written by
! H0 X     Albert Bartok-Partay, Silvia Cereda, Gabor Csanyi, James Kermode,
! H0 X     Ivan Solt, Wojciech Szlachta, Csilla Varnai, Steven Winfield.
! H0 X
! H0 X   Copyright 2006-2010.
! H0 X
! H0 X   These portions of the source code are released under the GNU General
! H0 X   Public License, version 2, http://www.gnu.org/copyleft/gpl.html
! H0 X
! H0 X   If you would like to license the source code under different terms,
! H0 X   please contact Gabor Csanyi, gabor@csanyi.net
! H0 X
! H0 X   Portions of this code were written by Noam Bernstein as part of
! H0 X   his employment for the U.S. Government, and are not subject
! H0 X   to copyright in the USA.
! H0 X
! H0 X
! H0 X   When using this software, please cite the following reference:
! H0 X
! H0 X   http://www.libatoms.org
! H0 X
! H0 X  Additional contributions by
! H0 X    Alessio Comisso, Chiara Gattinoni, and Gianpietro Moras
! H0 X
! H0 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

!% Types and routines for rigid body dynamics.
!%
!% \textbf{The implementation of the NO_SQUISH algorithm in AdvanceVerlet still requires
!% some work, so rigid bodies should be constructed from constraints for the time
!% being.}
!
module rigidbody_module

  use system_module
  use periodictable_module
  use linearalgebra_module
  use atoms_module
  use quaternions_module

  implicit none
  private

  public :: rigidbodymodel, rigidbody, initialise, finalise, print

  type RigidBodyModel
  !% Object used as a reference by the RigidBody type.
  !% Many RigidBodys can point to the same model

     type(Atoms)            :: at                   !% The actual atoms
     integer                :: d1a, d1b, d2a, d2b   !% Atoms used as reference directions, 'd1a$>$d1b' and 'd2a$>$d2b'
     real(dp), dimension(3) :: I                    !% Moments of inertia along x,y,z axes (after reorientation)
     real(dp)               :: Mtot                 !% The total mass of the body
     logical                :: initialised = .false.

  end type RigidBodyModel

  type RigidBody
  !% Holds one rigid body (e.g. one water molecule)

     integer                               :: N = 0   !% Number of atoms in the rigid body
     integer,  allocatable, dimension(:)   :: indices !% Atoms that make up the rigid body
     type(RigidBodyModel), pointer         :: model   !% A model of the rigid body in its correct conformation
     type(Quaternion)                      :: q       !% The orientation of the body
     type(Quaternion)                      :: p       !% The momentum of the orientation
     real(dp), dimension(3)                :: RCoM    !% The centre of mass position
     real(dp), dimension(3)                :: VCoM    !% The centre of mass velocity
     logical                               :: initialised = .false.

  end type RigidBody

  integer, parameter :: M_ROT = 10 !Increasing this number integrates the rigid rotations more accurately

  interface initialise
     module procedure rigidbody_initialise
  end interface initialise

  interface finalise
     module procedure rigidbody_finalise, rigidbodies_finalise
  end interface finalise

  interface assignment(=)
     module procedure rigidbody_assignment, rigidbodies_assignment
  end interface

  interface print
     module procedure rigidbodymodel_print, rigidbody_print, rigidbodies_print
  end interface print

  private P0, P1, P2, P3

contains

  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  !X
  !X INITIALISATION / FINALISATION
  !X
  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

  !
  !% Initialise a RigidBodyModel:
  !% \begin{itemize}
  !% \item Calculates total mass.
  !% \item Shifts the centre of mass to the origin.
  !% \item Calculates moments of inertia and rotates
  !%       the atoms so that the principal axes.
  !%       lie along cartesian directions.
  !% \item Finds two reference directions which are
  !%       not (anti)parallel.
  !% \end{itemize}
  !
  subroutine rigidbodymodel_initialise(this,at)

    type(RigidBodyModel), intent(inout) :: this
    type(Atoms),          intent(in)    :: at
    !local variables
    real(dp), dimension(3,3)            :: I_matrix, I_vects
    real(dp), dimension(3)              :: XCoM, d1, d2
    real(dp)                            :: m
    integer                             :: i
    type(Quaternion)                    :: q

    !A rigid body must have at least 2 atoms
    if (at%N < 3) call system_abort('RigidBodyModel_Initialise: A rigid body must have at least 3 atoms')

    if (this%initialised) call rigidbodymodel_finalise(this)

    !Copy the atomic info
    call initialise(this%at,at%N,at%lattice)
    this%at%Z = at%Z
    this%at%pos = at%pos

    !Shift the model so that the centre of mass is at the origin (note: no periodic wrapping is assumed)
    XCoM = 0.0_dp
    this%Mtot = 0.0_dp

    do i = 1, this%at%N
       m = this%at%mass(i)
       XCoM = XCoM + m * this%at%pos(:,i)
       this%Mtot = this%Mtot + m
    end do

    XCoM = XCoM / this%Mtot

    do i = 1, this%at%N
       this%at%pos(:,i) = this%at%pos(:,i) - XCoM
    end do

    !Calculate the model's inertia tensor
    I_matrix = Inertia_Tensor(this%at)
    call diagonalise(I_matrix,this%I,I_vects) !Stores the moments of inertia in this%I

    !Rotate the model so that the principle axes point along the x,y,z directions.
    !The positions are then stored in body-fixed coordinates
    !Eigenvalues are already normalised and orthogonal.
    q = Orientation(I_vects(:,1),            I_vects(:,2), &                 !Go from where the eigenvalues point...
                    (/1.0_dp,0.0_dp,0.0_dp/),(/0.0_dp,1.0_dp,0.0_dp/),.true.)!... to the x and y axes

    do i = 1, this%at%N
       call rotate(this%at%pos(:,i),q)
    end do

    !Find two non-parallel reference directions within the structure
    this%d1a = 1; this%d1b = 2
    this%d2a = 1; this%d2b = 3
    do
       d1 = this%at%pos(:,this%d1b) - this%at%pos(:,this%d1a)
       d2 = this%at%pos(:,this%d2b) - this%at%pos(:,this%d2a)
       d1 = d1 / norm(d1)
       d2 = d2 / norm(d2)
       if (abs(d1 .dot. d2) .fne. 1.0_dp) exit
       !Directions are parallel... try a different set.
       this%d2b = this%d2b + 1
       if (this%d2b > this%at%N) then
          this%d1b = this%d1b + 1
          this%d2b = this%d1b + 1
          if (this%d2b == this%at%N) call System_Abort('RigidBodyModel_Initialise: Structure is Linear!')
       end if
    end do

    !Now the object is initialised
    this%initialised = .true.

  end subroutine rigidbodymodel_initialise

  subroutine rigidbodymodel_finalise(this)

    type(RigidBodyModel), intent(inout) :: this

    call finalise(this%at)
    this%d1a = 0; this%d1b = 0
    this%d2a = 0; this%d2b = 0
    this%I = 0.0_dp
    this%Mtot = 0.0_dp

    this%initialised = .false.

  end subroutine rigidbodymodel_finalise

  !
  !% Initialise: The atoms in 'indices' must be in the same order as in the RigidModel
  !
  subroutine rigidbody_initialise(this,at,indices,RigidModel)

    type(RigidBody),              intent(inout) :: this       !% The rigid body object we are initialising
    type(Atoms),                  intent(inout) :: at         !% The atoms object to which the indices refer
    integer, dimension(:),        intent(in)    :: indices    !% The indices of the atoms which make up the rigid body
    type(RigidBodyModel), target, intent(in)    :: RigidModel !% A model of the rigid body in its proper conformation
    !local variables
    integer                              :: i

    !Check the model has been initialised
    if (.not.RigidModel%initialised) call system_abort('RigidBody_Initialise: Model is not initialised')

    !Check for the correct number of atoms
    if (size(indices) /= RigidModel%at%N) then
       write(line,'(a,i0,a,i0,a)')'RigidBody_Initialise: The model contains ',RigidModel%at%N,' atoms but ', &
                                  size(indices),' indices have been provided'
       call system_abort(line)
    end if

    !Check the atomic indices and atomic numbers
    do i = 1, size(indices)
       if (indices(i) > at%N) then
          write(line,'(a,i0,a,i0,a)')'RigidBody_Initialise: Atom ',indices(i),' is out of range (',at%N,')'
          call system_abort(line)
       end if
       if (RigidModel%at%Z(i) /= at%Z(indices(i))) then
          write(line,'(4(a,i0),a)')'RigidBody_Initialise: Atom ',i, &
                                     ' of model ('//trim(ElementName(RigidModel%at%Z(i)))//', Z=', &
                                     RigidModel%at%Z(i),') does not match atom ',indices(i), &
                                     ' of atoms object ('//trim(ElementName(at%Z(indices(i))))//', Z=', &
                                     at%Z(indices(i)),')'
          call system_abort(line)
       end if
    end do

    call reallocate(this%indices,size(indices))
    this%indices = indices
    this%N = size(indices)

    this%model => RigidModel

    this%q = 0.0_dp
    this%p = 0.0_dp
    this%RCoM = 0.0_dp
    this%VCoM = 0.0_dp

    !Now we are initialised!
    this%initialised = .true.

  end subroutine rigidbody_initialise

  subroutine rigidbody_finalise(this)

    type(RigidBody), intent(inout) :: this

    this%N = 0
    if (allocated(this%indices)) deallocate(this%indices)
    this%model => null()

    this%q = 0.0_dp
    this%p = 0.0_dp
    this%RCoM = 0.0_dp
    this%VCoM = 0.0_dp

    this%initialised = .false.

  end subroutine rigidbody_finalise

  !
  !% Finalise an allocatable array of RigidBodys
  !
  subroutine rigidbodies_finalise(this)

    type(RigidBody), dimension(:), allocatable, intent(inout) :: this
    integer                                                   :: i

    if (allocated(this)) then
       do i = 1, size(this)
          call finalise(this(i))
       end do
       deallocate(this)
    end if

  end subroutine rigidbodies_finalise

  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  !X
  !X ASSIGNMENT
  !X
  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

  subroutine rigidbody_assignment(to,from)

    type(RigidBody), intent(inout) :: to
    type(RigidBody), intent(in)    :: from

    call finalise(to)

    if (from%initialised) then

       allocate(to%indices(size(from%indices)))

       to%N           =  from%N
       to%indices     =  from%indices
       to%model       => from%model
       to%q           =  from%q
       to%p           =  from%p
       to%RCoM        =  from%RCoM
       to%VCoM        =  from%VCoM
       to%initialised =  from%initialised

    end if

  end subroutine rigidbody_assignment

  subroutine rigidbodies_assignment(to,from)

    type(RigidBody), dimension(:), intent(inout) :: to
    type(RigidBody), dimension(:), intent(in)    :: from
    integer                                      :: i

    if (size(to) /= size(from)) call system_abort('RigidBodies_Assignment: Target array has different length to source array')

    do i = 1, size(from)
       to(i) = from(i)
    end do

  end subroutine rigidbodies_assignment

  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  !X
  !X I/O
  !X
  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

  subroutine rigidbodymodel_print(this, file)

    type(RigidBodyModel), intent(inout) :: this
    type(Inoutput), intent(inout), optional :: file

    call print('================', file=file)
    call print(' RigidBodyModel', file=file)
    call print('================', file=file)
    call print('', file=file)
    if (this%initialised) then
       call print('Atoms:', file=file)
       call print('------------', file=file)
       call print(this%at, file=file)
       call print('------------', file=file)
       call print('', file=file)
       write(line,'(2(a,i0))') 'Reference direction 1: atom ',this%d1a,' --> atom ',this%d1b
       call print(line, file=file)
       write(line,'(2(a,i0))') 'Reference direction 2: atom ',this%d2a,' --> atom ',this%d2b
       call print(line, file=file)
       call print('', file=file)
       call print('Inertia Tensor:', file=file)
       call print(inertia_tensor(this%at), file=file)
       call print('Stored moments of inertia:')
       call print(this%I, file=file)
       call print('', file=file)
       call print('Total Mass:', file=file)
       call print(this%Mtot, file=file)
    else
       call print('(uninitialised)', file=file)
    end if
    call print('', file=file)
    call print('================', file=file)

  end subroutine rigidbodymodel_print

  subroutine rigidbody_print(this, file)

    type(RigidBody), intent(inout) :: this
    type(Inoutput), intent(inout), optional :: file

    call print('XXXXXXXXXXXXXXXX', file=file)
    call print('   Rigid Body', file=file)
    call print('XXXXXXXXXXXXXXXX', file=file)
    call print('', file=file)
    if (this%initialised) then
       write(line,'(a,i0)')'Number of atoms = ',this%N
       call print(line, file=file)
       call print('', file=file)
       call print('Atomic indices:', file=file)
       call print(this%indices, file=file)
       call print('', file=file)
       call print('Reference Model =>', file=file)
       call print(this%model, file=file)
       call print('', file=file)
       call print('Orientation quaternion:', file=file)
       call print(this%q, file=file)
       call print('Conjugate momentum quaternion:', file=file)
       call print(this%p, file=file)
       call print('', file=file)
       call print('Centre of mass position:', file=file)
       call print(this%RCoM, file=file)
       call print('Centre of mass velocity:', file=file)
       call print(this%VCoM, file=file)
    else
       call print('(uninitialised)', file=file)
    end if
    call print('', file=file)
    call print('XXXXXXXXXXXXXXXX', file=file)

  end subroutine rigidbody_print

  subroutine rigidbodies_print(this,file)

    type(Rigidbody), dimension(:), intent(inout) :: this
    type(inoutput), optional,      intent(inout) :: file
    integer :: i

    do i = 1, size(this)
       call print(this(i),file)
    end do

  end subroutine rigidbodies_print

  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  !X
  !X FUNCTIONS FOR WORKING WITH RIGID BODIES
  !X
  !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

  function inertia_tensor(this) result(I)

    type(Atoms), intent(in)  :: this
    real(dp), dimension(3,3) :: I
    !local variables
    integer                  :: j
    real(dp)                 :: r(3), m, x2, xy, xz, y2, yz, z2

    I = 0.0_dp
    do j = 1, this%N
       r = this%pos(:,j)
       m = this%mass(j)
       x2 = r(1)*r(1); y2 = r(2)*r(2); z2 = r(3)*r(3)
       xy = r(1)*r(2); xz = r(1)*r(3); yz = r(2)*r(3)
       I(1,1) = I(1,1) + m * (y2 + z2)
       I(2,2) = I(2,2) + m * (x2 + z2)
       I(3,3) = I(3,3) + m * (x2 + y2)
       I(1,2) = I(1,2) - m * xy
       I(1,3) = I(1,3) - m * xz
       I(2,3) = I(2,3) - m * yz
    end do
    !Fill in symmetric bits
    I(2,1) = I(1,2)
    I(3,1) = I(1,3)
    I(3,2) = I(2,3)

  end function inertia_tensor

  !
  ! Functions and routines required by NO_SQUISH
  !
  ! From Miller et al.:
  ! P_0 q = {q0,q1,q2,q3}   P_1 q = {-q1,q0,q3,-q2}   P_2 q = {-q2,-q3,q0,q1}   P_3 q = {-q3,q2,-q1,q0}
  !
  !               ( (  |  ) (  |  ) (  |  ) (  |  ) )
  ! Matrix S(q) = ( (P_0 q) (P_1 q) (P_2 q) (P_3 q) )
  !               ( (  |  ) (  |  ) (  |  ) (  |  ) )
  !

  function P0(q)  !does nothing, but here for completeness
    type(Quaternion), intent(in) :: q
    type(Quaternion)             :: P0
    P0 = q
  end function P0

  function P1(q)
    type(Quaternion), intent(in) :: q
    type(Quaternion)             :: P1
    P1 = (/-q%b,q%a,q%d,-q%c/)
  end function P1

  function P2(q)
    type(Quaternion), intent(in) :: q
    type(Quaternion)             :: P2
    P2 = (/-q%c,-q%d,q%a,q%b/)
  end function P2

  function P3(q)
    type(Quaternion), intent(in) :: q
    type(Quaternion)             :: P3
    P3 = (/-q%d,q%c,-q%b,q%a/)
  end function P3

  function no_squish_S(q) result(S)
    type(Quaternion), intent(in) :: q
    real(dp), dimension(4,4)     :: S
    S(1,1) = +q%a; S(1,2) = -q%b; S(1,3) = -q%c; S(1,4) = -q%d
    S(2,1) = +q%b; S(2,2) = +q%a; S(2,3) = -q%d; S(2,4) = +q%c
    S(3,1) = +q%c; S(3,2) = +q%d; S(3,3) = +q%a; S(3,4) = -q%b
    S(4,1) = +q%d; S(4,2) = -q%c; S(4,3) = +q%b; S(4,4) = +q%a
  end function no_squish_S

  function no_squish_A_dot_transpose(q,q_dot) result(A_t)

    type(Quaternion), intent(in) :: q,q_dot
    real(dp), dimension(3,3)     :: A_t

    A_t(1,1) = q%a*q_dot%a + q%b*q_dot%b - q%c*q_dot%c - q%d*q_dot%d
    A_t(2,1) = q%b*q_dot%c + q%c*q_dot%b + q%a*q_dot%d + q%d*q_dot%a
    A_t(3,1) = q%b*q_dot%d + q%d*q_dot%b - q%a*q_dot%c - q%c*q_dot%a
    A_t(1,2) = q%b*q_dot%c + q%c*q_dot%b - q%a*q_dot%d - q%d*q_dot%a
    A_t(2,2) = q%a*q_dot%a - q%b*q_dot%b + q%c*q_dot%c - q%d*q_dot%d
    A_t(3,2) = q%c*q_dot%d + q%d*q_dot%c + q%a*q_dot%b + q%b*q_dot%a
    A_t(1,3) = q%b*q_dot%d + q%d*q_dot%b + q%a*q_dot%c + q%c*q_dot%a
    A_t(2,3) = q%c*q_dot%d + q%d*q_dot%c - q%a*q_dot%b - q%b*q_dot%a
    A_t(3,3) = q%a*q_dot%a - q%b*q_dot%b - q%c*q_dot%c + q%d*q_dot%d

    A_t = 2.0_dp * A_t

  end function no_squish_A_dot_transpose

  !
  ! Free_Rotor: part of the NO_SQUISH algorithm
  !
  subroutine no_squish_free_rotor(q,p,I,dt)

    type(Quaternion),       intent(inout) :: q,p
    real(dp), dimension(3), intent(in)    :: I
    real(dp)                              :: dt
    !local variables
    real(dp)                              :: zetadt_2, zetadt, delta_t
    integer                               :: j

    !Free rotation in M_ROT steps
    delta_t = dt / real(M_ROT,dp)
    do j = 1, M_ROT

       zetadt_2 = delta_t * (p .dot. P3(q)) / (8.0_dp * I(3))
       q = cos(zetadt_2)*q + sin(zetadt_2)*P3(q)
       p = cos(zetadt_2)*p + sin(zetadt_2)*P3(p)

       zetadt_2 = delta_t * (p .dot. P2(q)) / (8.0_dp * I(2))
       q = cos(zetadt_2)*q + sin(zetadt_2)*P2(q)
       p = cos(zetadt_2)*p + sin(zetadt_2)*P2(p)

       zetadt   = delta_t * (p .dot. P1(q)) / (4.0_dp * I(1))
       q = cos(zetadt_2)*q + sin(zetadt_2)*P1(q)
       p = cos(zetadt_2)*p + sin(zetadt_2)*P1(p)

       zetadt_2 = delta_t * (p .dot. P2(q)) / (8.0_dp * I(2))
       q = cos(zetadt_2)*q + sin(zetadt_2)*P2(q)
       p = cos(zetadt_2)*p + sin(zetadt_2)*P2(p)

       zetadt_2 = delta_t * (p .dot. P3(q)) / (8.0_dp * I(3))
       q = cos(zetadt_2)*q + sin(zetadt_2)*P3(q)
       p = cos(zetadt_2)*p + sin(zetadt_2)*P3(p)

    end do

  end subroutine no_squish_free_rotor

end module RigidBody_module

   !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
   !X
   !X RIGID BODY ROUTINES
   !X
   !XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

   !This is based on the NO_SQUISH algorithm
   !Miller et al., JChemPhys 116(20) 8649 (2002)

   !
   !% From Positions and Velocities of individual atoms, calculate the
   !% centre of mass, centre of mass velocity,  the orientation, and the
   !% orientation conjugate momentum.
   !
!    subroutine rigidbody_calculate_variables(this,i)
!
!      type(DynamicalSystem), intent(inout) :: this
!      integer,               intent(in)    :: i
!      integer                              :: j,k, d1s_a, d1s_b, d2s_a, d2s_b
!      real(dp), dimension(3)               :: VCoM,L,r,v, d1s,d2s,d1b,d2b
!      real(dp), dimension(4,4)             :: S
!      real(dp), dimension(4)               :: L4
!      real(dp)                             :: m
!      type(Quaternion)                     :: q
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_CalculateVariables: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      !Work out which atoms the reference directions go bewteen in the full atomic system
!      d1s_a = this%rigidbody(i)%indices(this%rigidbody(i)%model%d1a)
!      d1s_b = this%rigidbody(i)%indices(this%rigidbody(i)%model%d1b)
!      d2s_a = this%rigidbody(i)%indices(this%rigidbody(i)%model%d2a)
!      d2s_b = this%rigidbody(i)%indices(this%rigidbody(i)%model%d2b)
!      !Find the directions in body and space coordinates
!      d1s = diff_min_image(this%atoms,d1s_a,d1s_b)
!      d2s = diff_min_image(this%atoms,d2s_a,d2s_b)
!      d1b = this%rigidbody(i)%model%at%pos(:,this%rigidbody(i)%model%d1b) - &
!           this%rigidbody(i)%model%at%pos(:,this%rigidbody(i)%model%d1a)
!      d2b = this%rigidbody(i)%model%at%pos(:,this%rigidbody(i)%model%d2b) - &
!           this%rigidbody(i)%model%at%pos(:,this%rigidbody(i)%model%d2a)
!      !Calculate the rotation quaternion to go from body to space coords
!      q = Orientation(d1b,d2b,d1s,d2s,correct_angle = .true.)
!      this%rigidbody(i)%q = q
!      !Calculate its angular momentum in the body frame of reference
!      VCoM = centre_of_mass_velo(this,this%rigidbody(i)%indices)
!      L = 0.0_dp
!      q = this%rigidbody(i)%q
!      do j = 1, this%rigidbody(i)%N
!
!         k = this%rigidbody(i)%indices(j) !index of the jth atom in rigid body i
!         m = this%atoms%Z(k) !the mass of the jth atom
!         r = this%rigidbody(i)%model%at%pos(:,j) !the body fixed coordinates of jth atom
!         v = Rotate((this%atoms%velo(:,k) - VCoM),.conj.q) !body fixed velocity of jth atom
!         L = L + m * (r .cross. v)
!
!      end do
!
!      !Calculate the momentum conjugate to the orientation quaternion
!      L4 = (/0.0_dp,L/) !this is D^-1 omega(4) in equation 2.15 of Miller et al.
!      S = no_squish_S(q)
!      this%rigidbody(i)%p = 2.0_dp * (S .mult. L4)
!
!      !Store the centre of mass position and velocity
!      this%rigidbody(i)%RCoM = centre_of_mass(this%atoms,this%rigidbody(i)%indices)
!      this%rigidbody(i)%VCoM = VCoM
!
!    end subroutine rigidbody_calculate_variables
!
!    subroutine rigidbody_write_positions(this,i)
!
!      type(DynamicalSystem), intent(inout) :: this
!      integer,               intent(in)    :: i
!      !local variables
!      integer                              :: j, k
!      real(dp), dimension(3)               :: r
!      type(Quaternion)                     :: q
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_WritePositions: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      q = this%rigidbody(i)%q
!
!      do j = 1, this%rigidbody(i)%model%at%N
!         k = this%rigidbody(i)%indices(j)        !jth atom of rigid body
!         r = this%rigidbody(i)%model%at%pos(:,j) !body coords of jth atom
!         this%atoms%pos(:,k) = this%rigidbody(i)%RCoM + Rotate(r,this%rigidbody(i)%q)
!      end do
!
!    end subroutine rigidbody_write_positions
!
!    subroutine rigidbody_write_velocities(this,i)
!
!      type(DynamicalSystem), intent(inout) :: this
!      integer,               intent(in)    :: i
!      !local variables
!      integer                              :: j, k
!      real(dp), dimension(3)               :: r, omega
!      type(Quaternion)                     :: q
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_WriteVelocities: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      q = this%rigidbody(i)%q
!      omega = RigidBody_angular_velocity(this,i)
!
!      do j = 1, this%rigidbody(i)%model%at%N
!         k = this%rigidbody(i)%indices(j)        !jth atom of rigid body
!         r = this%rigidbody(i)%model%at%pos(:,j) !body coords of jth atom
!         this%atoms%velo(:,k) = this%rigidbody(i)%VCoM + Rotate((omega .cross. r),q)
!      end do
!
!    end subroutine rigidbody_write_velocities
!
!    !
!    !% Reconstitute a rigid body that has been split over periodic boundaries
!    !
!    subroutine rigidbody_reconstitute(this,i)
!
!      type(DynamicalSystem), intent(inout) :: this
!      integer,               intent(in)    :: i
!      integer                              :: j, k, n, t(3)
!      real(dp), dimension(3)               :: r
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_Reconstitute: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      !Map everything back into the cell of the first atom in the body
!      j = this%rigidbody(i)%indices(1)
!      r = this%atoms%pos(:,j)
!      t = this%atoms%travel(:,j)
!      do n = 2, this%rigidbody(i)%N
!         k = this%rigidbody(i)%indices(n)
!         !Alter the position
!         this%atoms%pos(:,k) = r + diff_min_image(this%atoms,j,k)
!         !Make the travel values agree... RIGID BODIES CANNOT SPAN MORE THAN ONE CELL (why would they?)
!         this%atoms%travel(:,k) = t
!      end do
!
!    end subroutine rigidbody_reconstitute
!
!    !
!    !% Calculate the current angular velocity of rigid body 'i', in body frame
!    !
!    function rigidbody_angular_velocity(this,i) result(omega)
!
!      type(DynamicalSystem), intent(in) :: this
!      integer,               intent(in) :: i
!      real(dp), dimension(3)            :: omega
!      real(dp), dimension(4)            :: p4, sTp
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_AngularVelocity: Rigid Body ',i,' is out of range'
!         call System_Abort(line)
!      end if
!
!      p4 = this%rigidbody(i)%p
!      !sTp(1) should always be zero
!      sTp = transpose(no_squish_S(this%rigidbody(i)%q)) .mult. p4
!      omega = 0.5_dp * sTp(2:4) / this%rigidbody(i)%model%I
!
!    end function rigidbody_angular_velocity
!
!    !
!    !% Calculate the current angular velocity of rigid body 'i', in body frame
!    !
!    function rigidbody_angular_momentum(this,i) result(L)
!
!      type(DynamicalSystem), intent(in) :: this
!      integer,               intent(in) :: i
!      real(dp), dimension(3)            :: L
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_AngularMomentum: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      L = this%rigidbody(i)%model%I * RigidBody_Angular_Velocity(this,i) !elementwise multiplication since
!                                                                         !I is a vector, not a matrix
!
!    end function rigidbody_angular_momentum
!
!    !
!    !% Calculate the current torque on the rigid body 'i', in body frame
!    !
!    function rigidbody_torque(this,i) result(tau)
!
!      type(DynamicalSystem), intent(in) :: this
!      integer,               intent(in) :: i
!      real(dp), dimension(3)            :: tau, ACoM, r, a
!      real                              :: m
!      integer                           :: j, k
!      type(Quaternion)                  :: q
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_Torque: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      ACoM = centre_of_mass_acc(this,this%rigidbody(i)%indices)
!      q = this%rigidbody(i)%q
!      tau = 0.0_dp
!
!      do j = 1, this%rigidbody(i)%model%at%N
!         k = this%rigidbody(i)%indices(j)        !jth atom of rigid body i
!         m = this%rigidbody(i)%model%at%mass(j)
!         r = this%rigidbody(i)%model%at%pos(:,j) !body coords of jth atom
!         a = Rotate((this%atoms%acc(:,k) - ACoM), .conj.q) !Rotate the non-CoM acceleration into the body frame
!         tau = tau + m * (r .cross. a)
!      end do
!
!    end function rigidbody_torque
!
!    function rigidbody_rotational_energy(this,i) result(Erot)
!
!      type(DynamicalSystem), intent(in) :: this
!      integer,               intent(in) :: i
!      real(dp)                          :: Erot
!      real(dp), dimension(3)            :: Im, omega
!
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_RotationalEnergy: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      omega = RigidBody_Angular_Velocity(this,i)
!      Im = this%rigidbody(i)%model%I
!      Erot = 0.5_dp * (Im(1)*omega(1)*omega(1) + Im(2)*omega(2)*omega(2) + Im(3)*omega(3)*omega(3))
!
!    end function rigidbody_rotational_energy
!
!    function rigidbody_translational_energy(this,i) result(Etrans)
!
!      type(DynamicalSystem), intent(in) :: this
!      integer,               intent(in) :: i
!      real(dp)                          :: Etrans
!
!      if (i < 1 .or. i > this%Nrigid) then
!         write(line,'(a,i0,a)')'RigidBody_TranslationalEnergy: Rigid Body ',i,' is out of range'
!         call system_abort(line)
!      end if
!
!      Etrans = 0.5_dp*this%rigidbody(i)%model%Mtot * normsq(centre_of_mass_velo(this,this%rigidbody(i)%indices))
!
!    end function rigidbody_translational_energy
!
!   !
!   !% Add rigid bodies to the DynamicalSystem
!   !
!   subroutine ds_add_rigidbody(this,indices,model)
!
!     type(DynamicalSystem),        intent(inout) :: this
!     integer, dimension(:),        intent(in)    :: indices
!     type(RigidBodyModel), target, intent(in)    :: model
!     !local variables
!     integer                                     :: i, g1, g2
!
!     !Check if rigid bodies have been set up
!     if (.not.allocated(this%rigidbody)) call system_abort('DS_AddRigidBody: Rigid Bodies have not been allocated')
!
!     !Have we got space for another rigid body?
!     if (this%Nrigid == size(this%rigidbody)) then
!        write(line,'(a,i0,a)')'DS_AddRigidBody: Maximum number of rigid bodies reached (',this%Nrigid,')'
!        call system_abort(line)
!     end if
!
!     !Initialise the rigid body and update the group variables
!     this%Nrigid = this%Nrigid + 1
!     g1 = this%group_lookup(indices(1))
!     do i = 1, size(indices)
!        if (Atom_Type(this,indices(i)) /= TYPE_ATOM) then
!           write(line,'(a,i0,a)')'DS_AddRigidBody: Atom ',indices(i),&
!                ' is already being treated by another integration method'
!           call system_abort(line)
!        end if
!        g2 = this%group_lookup(indices(i))
!        call merge_groups(this%group(g1),this%group(g2))
!     end do
!
!     !Make atoms belong to this rigid body
!     call group_add_object(this%group(g1),this%Nrigid)
!     call initialise(this%rigidbody(this%Nrigid),this%atoms,indices,model)
!
!     !Calculate the dynamical variables
!     call rigidbody_calculate_variables(this,this%Nrigid)
!
!     !Reduce the number of degrees of freedom:
!     !Subtract 3 translational DoF for each atom, then add on the 3 trans + 3 rot DoF for the body
!     this%Ndof = this%Ndof - 3*size(indices) + 6
!
!   end subroutine ds_add_rigidbody
!
!   !
!   !% Correct the positions of all the atoms in rigid bodies
!   !
!   subroutine ds_correct_rigid_positions(this)
!
!     type(DynamicalSystem), intent(inout) :: this
!     integer                              :: i,j,n
!
!     do i = 1, this%Nrigid
!
!        call rigidbody_calculate_variables(this,i)
!
!        do j = 1, this%rigidbody(i)%N
!           n = this%rigidbody(i)%indices(j)
!           this%atoms%pos(:,n) = Rotate(this%rigidbody(i)%model%at%pos(:,j), this%rigidbody(i)%q) &
!                                 + this%rigidbody(i)%RCoM
!        end do
!
!     end do
!
!   end subroutine ds_correct_rigid_positions


      SUBROUTINE DECODE_OP_AREA
     &          (data_string,verify,
     &           identification,left_lon,right_lon,bottom_lat,top_lat)

c***********************************************************************
c                   subroutine decode_op_area
c***********************************************************************

c  Program Source:  Naval Ocean Systems Center - Code 542

c  Date:
c     29 Jan 1992

c  Function:
c     Reads parameters of a geographic op area
c     from a data string which contains data in one of two forms:

c        identification lat1 lon1 lat2 lon2
c        identification op-area-specification-file

c     In the last form, OP-AREA-SPECIFICATION-FILE is the name of a
c     file which contains records containing the list of parameters for
c     the op area shown in the other forms; the file is searched for a
c     match with AREA-ID and the corresponding parameters are returned.

c  Parameters passed:
c     parameter-name [t,n] description {t is type, n is dimension}

c     data_string    [c,*] string containing the data to be decoded.
c     verify         [l  ] =.TRUE. checks input with LWPC_DAT:AREA.LIS

c  Parameters returned:
c     parameter-name [t,n] description {t is type, n is dimension}

c     identification [c,*] string which identifies the op area
c     left_lon       [r  ] longitude of left edge of the op area
c     right_lon      [r  ] longitude of right edge of the op area
c     bottom_lat     [r  ] latitude of the bottom of the op area
c     top_lat        [r  ] latitude of the top of the op area

c  Common blocks referenced:

c  Functions and subroutines referenced:
c     get_opa
c     str_count_list
c     str_get_item
c     str_lower

c  References:

c  Change history:
c    27 Oct 1992    Added common block LWPC_CFG.cmn to permit
c                   more flexibility in selecting location
c                   of data files.  The variable 'lwpcDAT_loc'
c                   must be set to contain the location of the
c                   file 'AREA.LIS' prior to calling this routine.

c    12 Jan 1994    Added tests to ensure valid definition of
c                   op area.

c*******************!***************************************************

      include      'lwpc_cfg.cmn'

      character*(*) data_string,identification
      character*120 input_file
      character*200 error_msg
      logical       verify
      integer       str_length
      real          list(4),
     &              left_lon

      if (STR_LENGTH(data_string) .eq. 0) THEN
         write(error_msg,
     &       '(''[DECODE_OP_AREA]: Data string missing'')')
         call LWPC_ERROR ('ERROR', error_msg)
      end if

      call STR_COUNT_LIST (data_string,0,0,nritem)

c     Get op area identification
      call STR_GET_ITEM (1,data_string,identification,n1,n2)
      call STR_LOWER (identification,0,0)

      if (nritem .le. 2) then
         if (nritem .eq. 1) then

c           Only one parameter;
c           must be IDENTIFICATION, use default master file
            input_file=lwpcDAT_loc(1:STR_LENGTH(lwpcDAT_loc))//
     &                'area.lis'
         else

c           Only two parameters in the data string;
c           must be IDENTIFICATION and a file name
            call STR_GET_ITEM (2,data_string,input_file,n1,n2)
            nritem=-1
         end if

c        Get parameters from the specified file
         call GET_OPA
     &       (input_file,
     &        identification,bottom_lat,left_lon,top_lat,right_lon)
      else

c        Extra parameters in the data string;
c        must be a full set of coordinates
         if (data_string(n2+1:n2+1) .eq. ',') n2=n2+1

         call DECODE_LIST_FLT (data_string(n2+1:),4,nrlist,list)

         if (nrlist .lt. 4) then
            write(error_msg,
     &          '(''[DECODE_OP_AREA]: '',
     &            ''Insufficient number of parameters to define '',
     &            ''op area'')')
            call LWPC_ERROR ('ERROR', error_msg)
         end if

         bottom_lat=list(1)
         left_lon  =list(2)
         top_lat   =list(3)
         right_lon =list(4)
      end if

      if (verify) then

         if (nritem .gt. 1 .and. identification .ne. 'dummy') then

c           Check values against the master list
            value1x=bottom_lat
            value2x=left_lon
            value3x=top_lat
            value4x=right_lon

            input_file=lwpcDAT_loc(1:STR_LENGTH(lwpcDAT_loc))//
     &                'area.lis'
            call GET_OPA (input_file,
     &                    identification,
     &                    value1x,value2x,value3x,value4x)

            if (value1x .ne. bottom_lat .or. value2x .ne. left_lon .or.
     &          value3x .ne. top_lat .or. value4x .ne. right_lon) then

               write(error_msg,
     &             '(''[DECODE_OP_AREA]: '',
     &               ''Input does not match master file'')')
               call LWPC_ERROR ('ERROR', error_msg)
            end if
         end if
      end if
      RETURN
      END      ! DECODE_OP_AREA


C    Copyright(C) 1988-2017 National Technology & Engineering Solutions
C    of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C    NTESS, the U.S. Government retains certain rights in this software.
C
C    Redistribution and use in source and binary forms, with or without
C    modification, are permitted provided that the following conditions are
C    met:
C
C    * Redistributions of source code must retain the above copyright
C       notice, this list of conditions and the following disclaimer.
C
C    * Redistributions in binary form must reproduce the above
C      copyright notice, this list of conditions and the following
C      disclaimer in the documentation and/or other materials provided
C      with the distribution.
C
C    * Neither the name of NTESS nor the names of its
C      contributors may be used to endorse or promote products derived
C      from this software without specific prior written permission.
C
C    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
C    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
C    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
C    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
C    OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
C    SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
C    LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
C    DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
C    THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
C    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
C    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

C     $Id: ovrmx2.f,v 1.3 1991/08/05 13:44:20 gdsjaar Exp $
C     $Log: ovrmx2.f,v $
C     Revision 1.3  1991/08/05 13:44:20  gdsjaar
C     Reordered penetration distance loops, fixed format statement
C
c     Revision 1.2  1991/02/21  16:38:01  gdsjaar
c     Moved ENGNOT function out of write statements
c
c     Revision 1.1.1.1  1991/02/21  15:44:42  gdsjaar
c     NUMBERS: Greg Sjaardema, initial Unix release
c
c     Revision 1.1  1991/02/21  15:44:41  gdsjaar
c     Initial revision
c
      SUBROUTINE OVRMX2 (LSTEL, CORD, IX, NSEG, MINMAX, NIQSLV,
     *     NIQS, TEMP, LTNESS, NUMIN, NUMFAC, NUMON,
     *     NUMEL, LFACE, NUMNP)
C
      INTEGER   LSTEL(*), IX(4,*), NIQSLV(*), LTNESS(2,*)
      INTEGER   LFACE(4,*)
      REAL      MINMAX(4,*), CORD(NUMNP,*), TEMP(*)

      CHARACTER*16 ENGNOT, ENG1
      DIMENSION MAP(2,4), V(4), FCORD(2,2), DIST(4), SCORD(2)
      LOGICAL   INSIDE, ONFACE, INIT
      PARAMETER (MAXFAC = 4)
      include 'nu_io.blk'
C
      DATA MAP /1, 2,  2, 3,  3, 4,  4, 1/

      INIT  = .FALSE.
      NUMIN = 0
      NUMON = 0
      NUMFAC = 0
C
      DO 10 I=1,NSEG
         IEL = LSTEL(I)
C
         MINMAX(1, I) = MIN( CORD(IX(1,IEL),1),  CORD(IX(2,IEL),1),
     *        CORD(IX(3,IEL),1),  CORD(IX(4,IEL),1))
         MINMAX(2, I) = MAX( CORD(IX(1,IEL),1),  CORD(IX(2,IEL),1),
     *        CORD(IX(3,IEL),1),  CORD(IX(4,IEL),1))

         MINMAX(3, I) = MIN( CORD(IX(1,IEL),2),  CORD(IX(2,IEL),2),
     *        CORD(IX(3,IEL),2),  CORD(IX(4,IEL),2))
         MINMAX(4, I) = MAX( CORD(IX(1,IEL),2),  CORD(IX(2,IEL),2),
     *        CORD(IX(3,IEL),2),  CORD(IX(4,IEL),2))
C
 10   CONTINUE
C
C     ... DETERMINE WHICH FACES HAVE SSET FLAG
C
      CALL INIINT (MAXFAC * NUMEL, 0, LFACE)

      DO 30 ISEG = 1, NSEG
         IEL = LSTEL(ISEG)
         IFAC1 = LTNESS(1,ISEG)
         IFAC2 = LTNESS(2,ISEG)

         DO 20 IFAC = 1, MAXFAC
            INOD1 = IX(MAP(1,IFAC),IEL)
            INOD2 = IX(MAP(2,IFAC),IEL)

            ITST1 = ISIGN(1,(INOD1-IFAC1)) + ISIGN(1,(IFAC1-INOD1)) +
     *           ISIGN(1,(INOD2-IFAC1)) + ISIGN(1,(IFAC1-INOD2))

            ITST2 = ISIGN(1,(INOD1-IFAC2)) + ISIGN(1,(IFAC2-INOD1)) +
     *           ISIGN(1,(INOD2-IFAC2)) + ISIGN(1,(IFAC2-INOD2))

            LFACE(IFAC,IEL) = LFACE(IFAC,IEL) + ITST1 * ITST2
 20      CONTINUE
 30   CONTINUE
C
C     ... DETERMINE IF NODE IS CLOSE TO ELEMENT
C     TEMP = 1.0 IF INSIDE MIN/MAX BOX
C
      DO 130 I=1, NSEG
         IEL = LSTEL(I)
C
         DO 40 ISLV = 1, NIQS
            ISN = NIQSLV(ISLV)
            TEMP(ISLV) =
     *           (0.5 + SIGN( 0.5,  CORD (ISN,1) - MINMAX(1,I) )) *
     *           (0.5 + SIGN( 0.5, -CORD (ISN,1) + MINMAX(2,I) )) *
     *           (0.5 + SIGN( 0.5,  CORD (ISN,2) - MINMAX(3,I) )) *
     *           (0.5 + SIGN( 0.5, -CORD (ISN,2) + MINMAX(4,I) ))
 40      CONTINUE
C
C     ... DETERMINE IF ANY INSIDE BOX ( TEMP = 1.0 )
C
C     ... FOR EACH NODE INSIDE BOX, DETERMINE IF ACTUALLY INSIDE ELEMENT
C
         DO 120 ISLV = 1, NIQS
            IF (TEMP(ISLV) .EQ. 1.0) THEN
               INOD = NIQSLV(ISLV)

               X3 = CORD(INOD,1)
               Y3 = CORD(INOD,2)

               INSIDE = .TRUE.
               ONFACE = .FALSE.
               DO 50 IPYR = 1, 4

                  X1 = CORD(IX(MAP(1,IPYR),IEL),1)
                  Y1 = CORD(IX(MAP(1,IPYR),IEL),2)

                  X2 = CORD(IX(MAP(2,IPYR),IEL),1)
                  Y2 = CORD(IX(MAP(2,IPYR),IEL),2)

C
C     ... CALCULATE TRIANGLE AREAS (SHOULD BE DIVIDED BY 2 FOR AREA)
C
                  V(IPYR) = X1 * (Y2 - Y3) + X2 *  (Y3 - Y1)
     *                 + X3 * (Y1 - Y2)

                  IF (V(IPYR) .LT. 0.0) INSIDE = .FALSE.
                  IF (V(IPYR) .EQ. 0.0) ONFACE = .TRUE.
 50            CONTINUE
C
C     ... FLAG NODE AND ELEMENT IF INSIDE
C
               IF (ONFACE .AND. INSIDE) THEN
                  INSIDE = .TRUE.
                  ONFACE = .FALSE.
                  DO 60 IFAC = 1, MAXFAC
                     IF (V(IFAC) .EQ. 0.0 .AND.
     *                    LFACE(IFAC,IEL) .NE. 0 ) THEN
                        INSIDE = .FALSE.
                        ONFACE = .TRUE.
                     END IF
 60               CONTINUE
               END IF
C
C     ... CHECK FOR NODE ON BOTH SURFACES
C
               IF (INSIDE) THEN
                  DO 70 INOD = 1, 4
                     IF (IX(INOD,IEL) .EQ. NIQSLV(ISLV)) THEN
                        INSIDE = .FALSE.
                        NUMON = NUMON + 1
                     END IF
 70               CONTINUE
               END IF

               IF (INSIDE) THEN
                  IF (.NOT. INIT) THEN
                     INIT = .TRUE.
                     DO 80 IO=IOMIN, IOMAX
                        WRITE (IO, 150)
 80                  CONTINUE
                  END IF
                  DO 90 IFAC = 1, MAXFAC
                     IF (LFACE(IFAC,IEL) .NE. 0 ) THEN
                        FCORD(1,1) = CORD(IX(MAP(1,IFAC),IEL),1)
                        FCORD(2,1) = CORD(IX(MAP(1,IFAC),IEL),2)
                        FCORD(1,2) = CORD(IX(MAP(2,IFAC),IEL),1)
                        FCORD(2,2) = CORD(IX(MAP(2,IFAC),IEL),2)
                        SCORD(1)   = CORD(NIQSLV(ISLV),1)
                        SCORD(2)   = CORD(NIQSLV(ISLV),2)
                        CALL PENDIS (SCORD, FCORD, DIST(IFAC), 2, 2)
                     END IF
 90               CONTINUE
                  DO 100 IFAC = 1, MAXFAC
                     IF (LFACE(IFAC,IEL) .NE. 0 .AND.
     *                    DIST(IFAC) .NE. 0.0) THEN
                        NUMIN = NUMIN + 1
                        ENG1 = ENGNOT(DIST(IFAC),2)
                        DO 110 IO=IOMIN, IOMAX
                           WRITE (IO,140) NIQSLV(ISLV), IEL,
     *                          ENG1,
     *                          IFAC, CORD(NIQSLV(ISLV),1),
     $                          CORD(NIQSLV(ISLV),2)
 110                    CONTINUE
                     END IF
 100              CONTINUE
               ELSE IF (ONFACE) THEN
                  NUMFAC = NUMFAC + 1
               END IF
            END IF
 120     CONTINUE
 130  CONTINUE
 140  FORMAT (T3,I6,T11,I6,T18,A16,T37,I1,T43,2(F15.8,2X))
 150  FORMAT ('  Slave    Master  Penetration  Element'/
     *     '   Node   Element    Distance     Face      Location')
      RETURN
      END


module type_equation_set
#include <messenger.h>
    use mod_kinds,                      only: rk,ik
    use mod_constants,                  only: INTERIOR, CHIMERA, DIAG, &
                                              BOUNDARY_ADVECTIVE_FLUX, &
                                              BOUNDARY_DIFFUSIVE_FLUX, &
                                              VOLUME_ADVECTIVE_FLUX, VOLUME_DIFFUSIVE_FLUX, &
                                              BC_FLUX, XI_MIN, XI_MAX, ETA_MIN, ETA_MAX,    &
                                              ZETA_MIN, ZETA_MAX, BOUNDARY,                 &
                                              dD_DIFF, dQ_DIFF, dX_DIFF, dBC_DIFF, NO_DIFF
    use mod_operators,                  only: operator_factory
    use mod_models,                     only: model_factory
    use mod_DNAD_tools,                 only: element_compute_seed, face_compute_seed

    use type_operator,                  only: operator_t
    use type_operator_wrapper,          only: operator_wrapper_t
    use type_model,                     only: model_t
    use type_model_wrapper,             only: model_wrapper_t
    use type_properties,                only: properties_t
    use type_equationset_function_data, only: equationset_function_data_t
    use type_chidg_worker,              only: chidg_worker_t
    use type_mesh,                      only: mesh_t
    use type_bc_state_group,            only: bc_state_group_t
    use type_solverdata,                only: solverdata_t
    use type_element_info,              only: element_info_t
    use type_face_info,                 only: face_info_t
    use type_function_info,             only: function_info_t
    use type_pseudo_timestep,           only: pseudo_timestep_t, default_pseudo_timestep_t
    implicit none

    !>  Abstract equation-set type. Can be extended to implement a concrete equation set.
    !!      - Contains name and number of equations.
    !!      - Contains properties type with equations and definitions
    !!      - Contains arrays of flux components
    !!
    !!  When a new equation set is defined. It should extend from this abstract type.
    !!  It must then implement the 'init' function where equations/variables can be added,
    !!  and fluxes can be added to the definition of the equation set.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   2/8/2016
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/16/2016
    !!  @note   Reorganized to contain operators instead of fluxes
    !!
    !----------------------------------------------------------------------------------------
    type, public :: equation_set_t

        ! Name
        character(:),               allocatable :: name
        integer(ik)                             :: eqn_ID
        type(properties_t)                      :: prop

        ! Operators
        type(operator_wrapper_t),   allocatable :: boundary_advective_operator(:)
        type(operator_wrapper_t),   allocatable :: boundary_diffusive_operator(:)
        type(operator_wrapper_t),   allocatable :: volume_advective_operator(:)
        type(operator_wrapper_t),   allocatable :: volume_diffusive_operator(:)
        type(operator_wrapper_t),   allocatable :: bc_operator(:)

        ! Models
        type(model_wrapper_t),      allocatable :: models(:)
!        type(model_wrapper_t),      allocatable :: io_models(:)

        ! Pseudo time-step calculator
        class(pseudo_timestep_t),   allocatable :: pseudo_timestep

        ! Data for the flux and source functions. Ex how many. This gets passed to a container
        ! in sdata that keeps track of whether these have been executed or not.
        type(equationset_function_data_t)       :: function_data

    contains

        procedure   :: set_name
        procedure   :: get_name

        procedure   :: add_operator
        procedure   :: add_model
        procedure   :: add_pseudo_timestep

        procedure   :: clear_io_fields
        procedure   :: add_io_field

        procedure   :: compute_boundary_advective_operators
        procedure   :: compute_boundary_diffusive_operators
        procedure   :: compute_volume_advective_operators
        procedure   :: compute_volume_diffusive_operators
        procedure   :: compute_bc_operators

        procedure   :: compute_pseudo_timestep

        procedure   :: get_face_ncompute
        procedure   :: get_element_ncompute

        procedure   :: nmodels

    end type equation_set_t
    !**************************************************************************************

contains

    !> Set name of the equation set.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   1/28/2016
    !!
    !!  @param[in]  name_string     Character string indicating the name of the equation set
    !!
    !--------------------------------------------------------------------------------------
    subroutine set_name(self,ename)
        class(equation_set_t),  intent(inout)   :: self
        character(*),           intent(in)      :: ename

        self%name = trim(ename)

    end subroutine set_name
    !**************************************************************************************

    !>  Return the name of the equation set.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   2/8/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    function get_name(self) result(ename)
        class(equation_set_t),   intent(in)   :: self

        character(:),   allocatable :: ename

        ename = trim(self%name)

    end function get_name
    !**************************************************************************************

    !>  Add a spatial operator to the equation set.
    !!
    !!  This accepts a string that is the name of an operator to add. This function then
    !!  searched the operator register for an operator that matches the incoming string.
    !!  If one is found, it is created and added to the equation set.
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/29/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine add_operator(self,string)
        class(equation_set_t),  intent(inout)   :: self
        character(*),           intent(in)      :: string

        class(operator_t),          allocatable :: new_operator
        class(operator_wrapper_t),  allocatable :: temp(:)
        integer(ik)                             :: ierr, iflux, operator_type, ifield, imodel

        !
        ! Create new operator
        !
        allocate(new_operator, source=operator_factory%produce(string), stat=ierr)
        if (ierr /= 0) call AllocationError

        !
        ! Get operator type
        !
        operator_type = new_operator%get_operator_type()

        !
        ! Add to correct operator array
        !
        if (operator_type == BOUNDARY_ADVECTIVE_FLUX) then

            ! Allocate temporary flux array with one additional slot
            if (allocated(self%boundary_advective_operator)) then

                ! Allocate
                allocate(temp(size(self%boundary_advective_operator) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do iflux = 1,size(self%boundary_advective_operator)
                    allocate(temp(iflux)%op,source=self%boundary_advective_operator(iflux)%op, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else
                ! Allocate new slot
                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

        else if (operator_type == BOUNDARY_DIFFUSIVE_FLUX) then

            ! Allocate temporary flux array with one additional slot
            if (allocated(self%boundary_diffusive_operator)) then

                ! Allocate
                allocate(temp(size(self%boundary_diffusive_operator) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do iflux = 1,size(self%boundary_diffusive_operator)
                    allocate(temp(iflux)%op,source=self%boundary_diffusive_operator(iflux)%op, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else
                ! Allocate new slot
                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

        else if (operator_type == VOLUME_ADVECTIVE_FLUX) then

            ! Allocate temporary flux array with one additional slot
            if (allocated(self%volume_advective_operator)) then

                ! Allocate
                allocate(temp(size(self%volume_advective_operator) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do iflux = 1,size(self%volume_advective_operator)
                    allocate(temp(iflux)%op,source=self%volume_advective_operator(iflux)%op, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else

                ! Allocate new slot
                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

        else if (operator_type == VOLUME_DIFFUSIVE_FLUX) then

            ! Allocate temporary flux array with one additional slot
            if (allocated(self%volume_diffusive_operator)) then

                ! Allocate
                allocate(temp(size(self%volume_diffusive_operator) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do iflux = 1,size(self%volume_diffusive_operator)
                    allocate(temp(iflux)%op,source=self%volume_diffusive_operator(iflux)%op, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else

                ! Allocate new slot
                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

        else if (operator_type == BC_FLUX) then

            ! Allocate temporary flux array with one additional slot
            if (allocated(self%bc_operator)) then

                ! Allocate
                allocate(temp(size(self%bc_operator) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do iflux = 1,size(self%bc_operator)
                    allocate(temp(iflux)%op,source=self%bc_operator(iflux)%op, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else

                ! Allocate new slot
                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

        else
            call chidg_signal_one(FATAL,"equation_set%add_operator: 'Operator type was not valid'", operator_type)
        end if

        !
        ! Assign new operator
        !
        allocate(temp(size(temp))%op, source=new_operator, stat=ierr)
        if (ierr /= 0) call AllocationError

        !
        ! Copy extended temp array to equation set storage
        !
        if (operator_type == BOUNDARY_ADVECTIVE_FLUX) then
            self%boundary_advective_operator = temp
            self%function_data%nboundary_advective_flux = size(self%boundary_advective_operator)

        else if (operator_type == BOUNDARY_DIFFUSIVE_FLUX) then
            self%boundary_diffusive_operator = temp
            self%function_data%nboundary_diffusive_flux = size(self%boundary_diffusive_operator)

        else if (operator_type == VOLUME_ADVECTIVE_FLUX) then
            self%volume_advective_operator = temp
            self%function_data%nvolume_advective_flux = size(self%volume_advective_operator)

        else if (operator_type == VOLUME_DIFFUSIVE_FLUX) then
            self%volume_diffusive_operator = temp
            self%function_data%nvolume_diffusive_flux = size(self%volume_diffusive_operator)

        else if (operator_type == BC_FLUX) then
            self%bc_operator = temp

        else
            call chidg_signal_one(FATAL,"equation_set%add_operator: 'Operator type was not valid'", operator_type)
        end if

        ! Turn on primary fields from the new operator
        do ifield = 1,new_operator%nprimary_fields()
            call self%prop%add_primary_field(new_operator%get_primary_field(ifield))
            call self%prop%add_io_field(new_operator%get_primary_field(ifield))
        end do

        ! Turn on auxiliary fields from the new operator
        do ifield = 1,new_operator%nauxiliary_fields()
            call self%prop%add_auxiliary_field(new_operator%get_auxiliary_field(ifield))
        end do

        ! Turn on models from the new operator
        do imodel = 1,new_operator%nmodels()
            call self%add_model(new_operator%get_model(imodel))
        end do

    end subroutine add_operator
    !****************************************************************************************

    !>  Add a model to the equation set.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   11/29/2016
    !!
    !---------------------------------------------------------------------------------------
    subroutine add_model(self,string)
        class(equation_set_t),  intent(inout)   :: self
        character(*),           intent(in)      :: string

        integer(ik)                         :: imodel, ierr, ifield
        logical                             :: already_added
        class(model_t),         allocatable :: new_model
        type(model_wrapper_t),  allocatable :: temp(:)

        !
        ! Check that the model wasn't already added.
        !
        already_added = .false.
        do imodel = 1,self%nmodels()
            already_added = (trim(string) == self%models(imodel)%model%get_name())
            if (already_added) exit
        end do

        !
        ! Add the model if it wasn't added already by another operator.
        !
        if (.not. already_added) then

            allocate(new_model, source=model_factory%produce(string), stat=ierr)
            if (ierr /= 0) call AllocationError

            !
            ! Extend storage
            !
            if (allocated(self%models)) then

                allocate(temp(size(self%models) + 1), stat=ierr)
                if (ierr /= 0) call AllocationError

                ! Copy current flux components to temp array
                do imodel = 1,size(self%models)
                    allocate(temp(imodel)%model,source=self%models(imodel)%model, stat=ierr)
                    if (ierr /= 0) call AllocationError
                end do

            else

                allocate(temp(1), stat=ierr)
                if (ierr /= 0) call AllocationError

            end if

            !
            ! Allocate new model to end of extended array
            !
            allocate(temp(size(temp))%model, source=new_model, stat=ierr)
            if (ierr /= 0) call AllocationError

            ! Turn on model fields from the new model
            do ifield = 1,new_model%nmodel_fields()
                call self%prop%add_model_field(new_model%get_model_field(ifield))
            end do

            !
            ! Move extended temp allocation to data type
            !
            call move_alloc(from=temp, to=self%models)

        end if

    end subroutine add_model
    !***************************************************************************************

    !>  Clear/remove all IO fields.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   8/24/2017
    !!
    !-------------------------------------------------------------------------------
    subroutine clear_io_fields(self)
        class(equation_set_t),  intent(inout)   :: self

        call self%prop%clear_io_fields()

    end subroutine clear_io_fields
    !*******************************************************************************

    !>  Add an IO field.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   8/24/2017
    !!
    !-------------------------------------------------------------------------------
    subroutine add_io_field(self, field)
        class(equation_set_t),  intent(inout)   :: self
        character(*),           intent(in)      :: field

        call self%prop%add_io_field(field)

    end subroutine add_io_field
    !*******************************************************************************

    !>  Set a pseudo time-step calculator.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   11/17/2016
    !!
    !!
    !----------------------------------------------------------------------------------------
    subroutine add_pseudo_timestep(self,calculator)
        class(equation_set_t),      intent(inout)   :: self
        class(pseudo_timestep_t),   intent(in)      :: calculator

        integer(ik) :: ierr

        if (allocated(self%pseudo_timestep)) deallocate(self%pseudo_timestep)

        allocate(self%pseudo_timestep, source=calculator, stat=ierr)
        if (ierr /= 0) call AllocationError

    end subroutine add_pseudo_timestep
    !****************************************************************************************

    !>  Loops through the attached boundary advective operator functions and computes them
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/15/2016
    !!
    !!
    !---------------------------------------------------------------------------------------
    subroutine compute_boundary_advective_operators(self,worker,differentiate,diff_pattern)
        class(equation_set_t),   intent(inout)          :: self
        type(chidg_worker_t),    intent(inout)          :: worker
        integer(ik),             intent(in)             :: differentiate
        integer(ik),             intent(in), optional   :: diff_pattern(:)

        integer(ik),    allocatable :: compute_pattern(:)
        integer(ik)                 :: nfcn, ifcn, icompute, ncompute, ipattern, idiff
        logical                     :: interior_face, chimera_face, compute, &
                                       compute_function, differentiate_function

        associate( mesh  => worker%mesh,                    &
                   idom  => worker%element_info%idomain_l,  &
                   ielem => worker%element_info%ielement_l, &
                   iface => worker%iface,                   &
                   prop  => self%prop )

        ! Only call 'Boundary Advective' operators for face type:
        !   - INTERIOR
        !   - CHIMERA
        interior_face = ( mesh%domain(idom)%faces(ielem,iface)%ftype == INTERIOR )
        chimera_face  = ( mesh%domain(idom)%faces(ielem,iface)%ftype == CHIMERA )
        compute       = (interior_face .or. chimera_face)

        if (compute) then

            ! Determine pattern to compute functions. Depends on if we are differentiating
            ! or not. These will be used to set idiff, indicating the differentiation
            ! direction.
            !if (differentiate) then
            if (differentiate == dQ_DIFF .or. &
                differentiate == dX_DIFF .or. &
                differentiate == dBC_DIFF .or. &
                differentiate == dD_DIFF) then
                ! compute function, wrt internal, external(in direction iface) states
                compute_pattern = [DIAG,iface]
                if (present(diff_pattern)) compute_pattern = diff_pattern
            else
                ! compute function, but do not differentiate
                compute_pattern = [0]
            end if

            ! Execute compute pattern
            do ipattern = 1,size(compute_pattern)
                ! Set differentiation indicator
                idiff = compute_pattern(ipattern)

                ! Determine the number of times to execute the functions.
                ! Depends on if we are differentiating or not.
                ncompute = self%get_face_ncompute(mesh,worker%face_info(),idiff)

                if (allocated(self%boundary_advective_operator)) then
                    nfcn = size(self%boundary_advective_operator)
                    do ifcn = 1,nfcn

                        worker%function_info%type   = BOUNDARY_ADVECTIVE_FLUX
                        worker%function_info%ifcn   = ifcn
                        worker%function_info%idiff  = idiff
                        worker%function_info%dtype  = differentiate

                        compute_function       = worker%solverdata%function_status%compute_function(   worker%element_info, worker%function_info, worker%iface)
                        differentiate_function = worker%solverdata%function_status%linearize_function( worker%element_info, worker%function_info, worker%iface)

                        if ( compute_function .or. differentiate_function ) then
                            ! If we are differentiating, compute boundary flux once for each
                            ! contributing element:
                            !   - For conforming faces ncompute == 1.
                            !   - For Chimera faces ncompute is potentially > 1
                            do icompute = 1,ncompute
                                worker%function_info%seed    = face_compute_seed(mesh,idom,ielem,iface,icompute,idiff,worker%itime)
                                worker%function_info%idepend = icompute

                                call self%boundary_advective_operator(ifcn)%op%compute(worker,prop)

                            end do !icompute
                        end if

                    end do ! ifcn
                end if ! allocated boundary_advective_flux
            end do !ipattern
        end if !compute

        end associate

    end subroutine compute_boundary_advective_operators
    !***************************************************************************************

    !>  Loops through the attached boundary diffusive operator functions and computes them
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/15/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute_boundary_diffusive_operators(self,worker,differentiate,diff_pattern)
        class(equation_set_t),  intent(inout)           :: self
        type(chidg_worker_t),   intent(inout)           :: worker
        integer(ik),            intent(in)              :: differentiate
        integer(ik),            intent(in), optional    :: diff_pattern(:)

        integer(ik),    allocatable :: compute_pattern(:)
        integer(ik)                 :: nfcn, ifcn, ipattern, icompute, ncompute, idiff, &
                                       ChiID, eqn_m, eqn_p
        logical                     :: compute_function, linearize_function,            &
                                       interior_face, chimera_face, compute, skip

        associate( mesh  => worker%mesh,                    &
                   idom  => worker%element_info%idomain_l,  &
                   ielem => worker%element_info%ielement_l, &
                   iface => worker%iface,                   &
                   prop  => self%prop)

        ! Only call 'Boundary Advective' operators for face type:
        !   - INTERIOR
        !   - CHIMERA
        interior_face = ( mesh%domain(idom)%faces(ielem,iface)%ftype == INTERIOR )
        chimera_face  = ( mesh%domain(idom)%faces(ielem,iface)%ftype == CHIMERA  )
        compute       = (interior_face .or. chimera_face)

        ! Determine if multi-fidelity interface. Check that equation sets match:
        !   - interior faces: should have the same equation set. No need to skip
        !   - chimera faces: could be different. If they are, we want to skip
        skip = .false.
        if (interior_face) then
            skip = .false.
        else if (chimera_face) then
            ChiID = mesh%domain(idom)%faces(ielem,iface)%ChiID
            eqn_m = self%eqn_ID
            eqn_p = mesh%domain(idom)%chimera%recv(ChiID)%donor(1)%elem_info%eqn_ID
            skip = (eqn_m /= eqn_p)
        end if

        if (compute .and. (.not. skip)) then

            ! Determine pattern to compute functions. Depends on if we are differentiating
            ! or not. These will be used to set idiff, indicating the differentiation
            ! direction.
            !if (differentiate) then
            if (differentiate == dQ_DIFF .or. &
                differentiate == dX_DIFF .or. &
                differentiate == dBC_DIFF .or. &
                differentiate == dD_DIFF) then
                ! compute function, wrt internal, external(in direction iface) states
                compute_pattern = [DIAG,iface]
                if (present(diff_pattern)) compute_pattern = diff_pattern
            else
                ! compute function, but do not differentiate
                compute_pattern = [0]
            end if

            ! Execute compute pattern
            do ipattern = 1,size(compute_pattern)
                ! Set differentiation indicator
                idiff = compute_pattern(ipattern)

                ! Get number of elements we are linearizing with respect to
                ncompute = self%get_face_ncompute(mesh,worker%face_info(),idiff)

                if (allocated(self%boundary_diffusive_operator)) then
                    nfcn = size(self%boundary_diffusive_operator)
                    do ifcn = 1,nfcn

                        worker%function_info%type   = BOUNDARY_DIFFUSIVE_FLUX
                        worker%function_info%ifcn   = ifcn
                        worker%function_info%idiff  = idiff
                        worker%function_info%dtype  = differentiate

                        compute_function     = worker%solverdata%function_status%compute_function(   worker%element_info, worker%function_info, worker%iface )
                        linearize_function   = worker%solverdata%function_status%linearize_function( worker%element_info, worker%function_info, worker%iface )

                        if ( compute_function .or. linearize_function ) then
                            ! If we are differentiating, compute boundary flux once for each
                            ! contributing element:
                            !   - For conforming faces ncompute == 1.
                            !   - For Chimera faces ncompute is potentially > 1
                            do icompute = 1,ncompute
                                worker%function_info%seed    = face_compute_seed(mesh,idom,ielem,iface,icompute,idiff,worker%itime)
                                worker%function_info%idepend = icompute

                                call self%boundary_diffusive_operator(ifcn)%op%compute(worker,prop)

                            end do
                        end if

                    end do ! ifcn
                end if ! boundary_diffusive_flux loop
            end do !ipattern
        end if ! compute

        end associate

    end subroutine compute_boundary_diffusive_operators
    !***************************************************************************************

    !>  Loops through the attached volume advective operator functions and computes them
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/15/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute_volume_advective_operators(self,worker,differentiate,diff_pattern)
        class(equation_set_t),      intent(inout)           :: self
        type(chidg_worker_t),       intent(inout)           :: worker
        integer(ik),                intent(in)              :: differentiate
        integer(ik),                intent(in), optional    :: diff_pattern(:)

        integer(ik),    allocatable :: compute_pattern(:)
        integer(ik)                 :: nfcn, ifcn, icompute, ncompute, idiff, ipattern
        type(function_info_t)       :: function_info
        logical                     :: compute

        associate( mesh      => worker%mesh,                    &
                   elem_info => worker%element_info,            &
                   idom      => worker%element_info%idomain_l,  &
                   ielem     => worker%element_info%ielement_l, &
                   prop      => self%prop)

        ! Determine pattern to compute functions. Depends on if we are differentiating
        ! or not. These will be used to set idiff, indicating the differentiation
        ! direction.
        !if (differentiate) then
        if (differentiate == dQ_DIFF .or. &
            differentiate == dX_DIFF .or. &
            differentiate == dBC_DIFF .or. &
            differentiate == dD_DIFF) then
            ! compute function, wrt internal state
            compute_pattern = [DIAG]
        else
            ! compute function, but do not differentiate
            compute_pattern = [0]
        end if

        ! Execute compute pattern
        do ipattern = 1,size(compute_pattern)
            ! Set differentiation indicator
            idiff = compute_pattern(ipattern)

            ! Volume advective flux only depends on the interior element
            icompute = 1

            if (allocated(self%volume_advective_operator)) then
                nfcn = size(self%volume_advective_operator)
                do ifcn = 1,nfcn

                    worker%function_info%type    = VOLUME_ADVECTIVE_FLUX
                    worker%function_info%ifcn    = ifcn
                    worker%function_info%idiff   = idiff
                    worker%function_info%idepend = icompute
                    worker%function_info%seed    = element_compute_seed(mesh,idom,ielem,icompute,idiff,worker%itime)
                    worker%function_info%dtype   = differentiate

                    call self%volume_advective_operator(ifcn)%op%compute(worker,prop)

                end do ! ifcn
            end if

        end do ! ipattern

        end associate

    end subroutine compute_volume_advective_operators
    !***************************************************************************************

    !>  Loops through the attached volume diffusive operator functions and computes them
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/16/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute_volume_diffusive_operators(self,worker,differentiate,diff_pattern)
        class(equation_set_t),      intent(inout)           :: self
        type(chidg_worker_t),       intent(inout)           :: worker
        integer(ik),                intent(in)              :: differentiate
        integer(ik),                intent(in), optional    :: diff_pattern(:)

        integer(ik),    allocatable :: compute_pattern(:)
        integer(ik)                 :: nfcn, ifcn, icompute, ncompute, ipattern, idiff
        type(function_info_t)       :: function_info
        logical                     :: diff_none, diff_interior, diff_exterior
        logical                     :: linearize_me, compute_function

        associate( mesh      => worker%mesh,                    &
                   elem_info => worker%element_info,            &
                   idom      => worker%element_info%idomain_l,  &
                   ielem     => worker%element_info%ielement_l, &
                   prop      => self%prop)

        ! Determine pattern to compute functions. Depends on if we are differentiating
        ! or not. These will be used to set idiff, indicating the differentiation
        ! direction.
        !if (differentiate) then
        if (differentiate == dQ_DIFF .or. &
            differentiate == dX_DIFF .or. &
            differentiate == dBC_DIFF .or. &
            differentiate == dD_DIFF) then
            ! compute function, wrt (all exterior)/interior states
            compute_pattern = [1,2,3,4,5,6,DIAG]
            if (present(diff_pattern)) compute_pattern = diff_pattern
        else
            ! compute function, but do not differentiate
            compute_pattern = [0]
        end if

        ! Execute compute pattern
        do ipattern = 1,size(compute_pattern)
            ! Set differentiation indicator
            idiff = compute_pattern(ipattern)

            diff_none = (idiff == 0)
            diff_interior = (idiff == DIAG)
            diff_exterior = ( (idiff == 1) .or. (idiff == 2) .or. &
                              (idiff == 3) .or. (idiff == 4) .or. &
                              (idiff == 5) .or. (idiff == 6) )

            ! Get number of elements we are linearizing with respect to
            ncompute = self%get_element_ncompute(mesh,elem_info,idiff)

            if (diff_none) then
                compute_function = .true.
            else if (diff_interior) then
                compute_function = .true.
            else if (diff_exterior) then
                compute_function = ( (mesh%domain(elem_info%idomain_l)%faces(elem_info%ielement_l,idiff)%ftype == INTERIOR) .or. &
                                     (mesh%domain(elem_info%idomain_l)%faces(elem_info%ielement_l,idiff)%ftype == CHIMERA) )
            end if

            if (compute_function) then

                if (allocated(self%volume_diffusive_operator)) then
                    nfcn = size(self%volume_diffusive_operator)
                    do ifcn = 1,nfcn
                        ! Compute boundary flux once for each donor.
                        !   - For interior faces ndepend == 1.
                        !   - For Chimera faces ndepend is potentially > 1.
                        do icompute = 1,ncompute

                            worker%function_info%type    = VOLUME_DIFFUSIVE_FLUX
                            worker%function_info%ifcn    = ifcn
                            worker%function_info%idiff   = idiff
                            worker%function_info%idepend = icompute
                            worker%function_info%seed    = element_compute_seed(mesh,idom,ielem,icompute,idiff,worker%itime)
                            worker%function_info%dtype   = differentiate

                            call self%volume_diffusive_operator(ifcn)%op%compute(worker,prop)

                        end do

                    end do ! ifcn
                end if
            end if
        end do !ipattern

        end associate

    end subroutine compute_volume_diffusive_operators
    !***************************************************************************************

    !>  Loops through the attached boundary advective operator functions and computes them
    !!
    !!  @author Nathan A. Wukie (AFRL)
    !!  @date   8/15/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute_bc_operators(self,worker,bc_state_group,differentiate)
        class(equation_set_t),      intent(inout)   :: self
        type(chidg_worker_t),       intent(inout)   :: worker
        type(bc_state_group_t),     intent(inout)   :: bc_state_group(:)
        integer(ik),                intent(in)      :: differentiate

        integer(ik),    allocatable :: compute_pattern(:)
        integer(ik)                 :: nfcn, ifcn, icompute, ncompute, group_ID, patch_ID, &
                                       face_ID, idiff, ipattern, itime_start, itime_end, itime_couple
        logical                     :: boundary_face, compute

        associate( mesh  => worker%mesh,                    &
                   idom  => worker%element_info%idomain_l,  &
                   ielem => worker%element_info%ielement_l, &
                   iface => worker%iface,                   &
                   prop  => self%prop )

        ! Only call the following routines for interior faces -- ftype == 0
        ! Furthermore, only call the routines if we are computing derivatives for the
        ! neighbor of iface or for the current element(DIAG). This saves a lot of
        ! unnecessary compute_boundary calls.
        boundary_face = (mesh%domain(idom)%faces(ielem,iface)%ftype == BOUNDARY)
        compute       = (boundary_face)

        if (compute) then

            ! Determine pattern to compute functions. Depends on if we are differentiating
            ! or not. These will be used to set idiff, indicating the differentiation
            ! direction.
            !if (differentiate) then
            if (differentiate == dQ_DIFF .or. &
                differentiate == dX_DIFF .or. &
                differentiate == dBC_DIFF .or. &
                differentiate == dD_DIFF) then
                ! compute function, wrt (all exterior)/interior states
                !compute_pattern = [DIAG]
                compute_pattern = [iface]
            else
                ! compute function, but do not differentiate
                compute_pattern = [0]
            end if

            do ipattern = 1,size(compute_pattern)
                ! Set differentiation indicator
                idiff = compute_pattern(ipattern)

                ! Get number of elements we are linearizing with respect to
                ncompute = self%get_face_ncompute(mesh,worker%face_info(),idiff)

                ! Get index of boundary condition, patch, patch face.
                group_ID = mesh%domain(idom)%faces(ielem,iface)%group_ID
                patch_ID = mesh%domain(idom)%faces(ielem,iface)%patch_ID
                face_ID  = mesh%domain(idom)%faces(ielem,iface)%face_ID

                ! Get range of time instances we need to compute coupling for
                !if (differentiate) then
                if (differentiate == dQ_DIFF .or. &
                    differentiate == dX_DIFF .or. &
                    differentiate == dBC_DIFF .or. &
                    differentiate == dD_DIFF) then
                    if (mesh%bc_patch_group(group_ID)%patch(patch_ID)%temporal_coupling == 'Global') then
                        itime_start = 1
                        itime_end = mesh%domain(idom)%ntime
                    else
                        itime_start = worker%itime
                        itime_end = worker%itime
                    end if
                else
                    itime_start = worker%itime
                    itime_end = worker%itime
                end if

                do itime_couple = itime_start,itime_end
                    if (allocated(self%bc_operator)) then
                        nfcn = size(self%bc_operator)
                        do ifcn = 1,nfcn

                            worker%function_info%type   = BC_FLUX
                            worker%function_info%ifcn   = ifcn
                            worker%function_info%idiff  = idiff
                            worker%function_info%dtype  = differentiate

                            ! Compute boundary flux once for each donor.
                            !   - For interior faces ndepend == 1.
                            !   - For Chimera faces ndepend is potentially > 1.
                            !   - For BC faces ndepend is potentially > 1.
                            do icompute = 1,ncompute
                                ! Get coupled element to linearize against.
                                worker%function_info%seed    = face_compute_seed(mesh,idom,ielem,iface,icompute,idiff,itime_couple)
                                worker%function_info%idepend = icompute

                                call self%bc_operator(ifcn)%op%compute(worker,prop)
                            end do

                        end do ! ifcn

                    end if ! boundary_advective_flux loop
                end do !itime_couple

            end do ! ipattern
        end if !compute

        end associate

    end subroutine compute_bc_operators
    !**************************************************************************************

    !>  Return the number of times to compute a function on a face. Depends on if
    !!  we are differentiating or not, indicated by the value of 'idiff'.
    !!
    !!  Often, this may be just 1. Because a flux differentiated wrt its owner element
    !!  just has 1 dependent element; itself. A flux differentiated wrt its neighbor
    !!  element has just 1 dependent element; it's neighbor. If however, a face is a
    !!  Chimera face, then when differentiated with respect to its donors, ncompute will
    !!  be equal to the number of donor elements that are providing information to
    !!  that face.
    !!
    !!  Additionally, if we are not differentiating(idiff == 0) we at least want to
    !!  compute the function once so ncompute = 1.
    !!
    !!  @author Nathan A. Wukie(AFRL)
    !!  @date   8/15/2016
    !!
    !--------------------------------------------------------------------------------------
    function get_face_ncompute(self,mesh,face_info,idiff) result(ncompute)
        class(equation_set_t),  intent(in)   :: self
        type(mesh_t),           intent(in)   :: mesh
        type(face_info_t),      intent(in)   :: face_info
        integer(ik),            intent(in)   :: idiff

        integer(ik) :: ChiID, group_ID, patch_ID, face_ID, ncompute
        logical     :: compute_wrt_none, compute_wrt_interior, compute_wrt_exterior, &
                       chimera_face, bc_face

        associate( idom  => face_info%idomain_l,  &
                   ielem => face_info%ielement_l, &
                   iface => face_info%iface )

        compute_wrt_none     = (idiff == 0)
        compute_wrt_interior = (idiff == DIAG)
        compute_wrt_exterior = (idiff == XI_MIN   .or. idiff == XI_MAX  .or. &
                                idiff == ETA_MIN  .or. idiff == ETA_MAX .or. &
                                idiff == ZETA_MIN .or. idiff == ZETA_MAX)

        if (compute_wrt_none) then
            ncompute = 1

        else if (compute_wrt_interior) then
            ncompute = 1

        else if (compute_wrt_exterior) then

            chimera_face  = ( mesh%domain(idom)%faces(ielem,iface)%ftype == CHIMERA )
            bc_face       = ( mesh%domain(idom)%faces(ielem,iface)%ftype == BOUNDARY)

            if ( chimera_face ) then
                ChiID    = mesh%domain(idom)%faces(ielem,iface)%ChiID
                ncompute = mesh%domain(idom)%chimera%recv(ChiID)%ndonors()

            else if ( bc_face ) then
                group_ID = mesh%domain(idom)%faces(ielem,iface)%group_ID
                patch_ID = mesh%domain(idom)%faces(ielem,iface)%patch_ID
                face_ID  = mesh%domain(idom)%faces(ielem,iface)%face_ID
                ncompute = mesh%bc_patch_group(group_ID)%patch(patch_ID)%ncoupled_elements(face_ID)

            else
                ! Standard conforming neighbor, only one dependent element.
                ncompute = 1
            end if

        end if

        end associate

    end function get_face_ncompute
    !**************************************************************************************

    !>  Return the number of elements that a volume function depends on in the linearization
    !!  direction 'idiff'.
    !!
    !!  Often, this may be just 1. Because a flux linearized wrt its owner element just has
    !!  1 dependent element; itself. A flux linearized wrt its neighbor element has just
    !!  1 dependent element; it's neighbor. If however, a face is a Chimera face, then
    !!  when linearized with respect to its donors, ndepend will be equal to the number of
    !!  donor elements that are providing information to that face.
    !!
    !!  @author Nathan A. Wukie(AFRL)
    !!  @date   8/16/2016
    !!
    !--------------------------------------------------------------------------------------
    function get_element_ncompute(self,mesh,elem_info,idiff) result(ncompute)
        class(equation_set_t),  intent(in)   :: self
        type(mesh_t),       intent(in)   :: mesh
        type(element_info_t),   intent(in)   :: elem_info
        integer(ik),            intent(in)   :: idiff

        integer(ik) :: ChiID, iface, ncompute
        logical     :: compute_wrt_none, compute_wrt_interior, compute_wrt_exterior, &
                       chimera_face

        associate( idom => elem_info%idomain_l, ielem => elem_info%ielement_l )

        compute_wrt_none     = (idiff == 0)
        compute_wrt_interior = (idiff == DIAG)
        compute_wrt_exterior = (idiff == XI_MIN   .or. idiff == XI_MAX  .or. &
                                idiff == ETA_MIN  .or. idiff == ETA_MAX .or. &
                                idiff == ZETA_MIN .or. idiff == ZETA_MAX)

        if (compute_wrt_none) then
            ncompute = 1

        else if (compute_wrt_interior) then
            ncompute = 1

        else if (compute_wrt_exterior) then
            ! Search iface in the direction of linearization
            iface = idiff

            chimera_face  = ( mesh%domain(idom)%faces(ielem,iface)%ftype == CHIMERA )

            if ( chimera_face ) then
                ! only need to compute multiple times when we need the linearization of
                ! the chimera neighbors
                ChiID  = mesh%domain(idom)%faces(ielem,iface)%ChiID
                ncompute = mesh%domain(idom)%chimera%recv(ChiID)%ndonors()
            else
                ! Standard conforming neighbor, only one dependent element.
                ncompute = 1
            end if

        end if

        end associate

    end function get_element_ncompute
    !**************************************************************************************

    !>  Compute a pseudo-timestep.
    !!
    !!  If none was allocated, use the default pseudo time-step based on element volume.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   11/17/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    subroutine compute_pseudo_timestep(self,idomain,mesh,sdata,cfln,itime)
        class(equation_set_t),  intent(in)      :: self
        integer(ik),            intent(in)      :: idomain
        type(mesh_t),       intent(inout)   :: mesh
        type(solverdata_t),     intent(inout)   :: sdata
        real(rk),               intent(in)      :: cfln(:)
        integer(ik),            intent(in)      :: itime

        type(default_pseudo_timestep_t) :: default_timestep

        if (allocated(self%pseudo_timestep)) then
            call self%pseudo_timestep%compute(idomain,mesh,self%prop,sdata,cfln,itime)
        else
            call default_timestep%compute(idomain,mesh,self%prop,sdata,cfln,itime)
        end if

    end subroutine compute_pseudo_timestep
    !***************************************************************************************

    !>  Return the number of models allocated in the equation set.
    !!
    !!  @author Nathan A. Wukie
    !!  @date   12/1/2016
    !!
    !!
    !--------------------------------------------------------------------------------------
    function nmodels(self) result(nmodels_)
        class(equation_set_t),  intent(in)  :: self

        integer(ik) :: nmodels_

        if (allocated(self%models)) then
            nmodels_ = size(self%models)
        else
            nmodels_ = 0
        end if

    end function nmodels
    !**************************************************************************************

end module type_equation_set