! -------------------------------------------------------------------------
! Bernese GNSS Software Version 5.3
! -------------------------------------------------------------------------

MODULE d_quaternion

! -------------------------------------------------------------------------
! Purpose:    Methods and operators for t_quaternion data type
!             (part related to interpolation adopted from old READATT.f90)
!
! Author:     M. Kalarus
!
! Created:    25-May-2021
!
! Changes:    22-Jun-2022 AV: Numerical issue for PG_F90 (SLERP)
!             10-Aug-2022 MK: Numerical issue for PG_F90 (SLERP) (modified version)
!             23-Jan-2023 MK: Remove archaic forms
!             21-Feb-2023 XM/DA: Check if quaternions change in neighbouring epochs
!
! Copyright:  Astronomical Institute
!             University of Bern
!             Switzerland
! -------------------------------------------------------------------------

  USE m_bern,     ONLY: r8b
  USE m_cpu,      ONLY: cpu_enter, cpu_exit
  USE d_const,    ONLY: CONST
  USE l_basfun,   ONLY: myDMOD

  IMPLICIT NONE

#include "COM_CPU.inc"
PRIVATE
  PUBLIC :: t_quaternion, qNorm, qConj, SLERP, OPERATOR(*)


  CHARACTER(LEN=*), PARAMETER :: modNam = 'D_QUATERNION'

  TYPE t_quaternion
    REAL(r8b), DIMENSION(4) :: Q = [0,0,0,1] ! x,y,z,w
  END TYPE t_quaternion

  INTERFACE OPERATOR(*)
    MODULE PROCEDURE qTimes
  END INTERFACE

CONTAINS


FUNCTION qTimes(A, B) RESULT(C)
! input/output
  TYPE(t_quaternion), INTENT(IN) :: A, B
  TYPE(t_quaternion)             :: C
! local parameter
  CHARACTER(LEN=*),   PARAMETER  :: srName = modNam // ':qTimes'

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

  C%Q(1) = A%Q(4)*B%Q(1) + A%Q(1)*B%Q(4) - A%Q(3)*B%Q(2) + A%Q(2)*B%Q(3)
  C%Q(2) = A%Q(4)*B%Q(2) + A%Q(2)*B%Q(4) - A%Q(1)*B%Q(3) + A%Q(3)*B%Q(1)
  C%Q(3) = A%Q(4)*B%Q(3) + A%Q(3)*B%Q(4) - A%Q(2)*B%Q(1) + A%Q(1)*B%Q(2)
  C%Q(4) = A%Q(4)*B%Q(4) - A%Q(1)*B%Q(1) - A%Q(2)*B%Q(2) - A%Q(3)*B%Q(3)

! ---------------------------------------------------
  IF (srfDebug) CALL cpu_exit(srName)  ! DEBUG: exit
  RETURN
END FUNCTION qTimes


FUNCTION qConj(A) RESULT(B)
! input/output
  TYPE(t_quaternion), INTENT(IN) :: A
  TYPE(t_quaternion)             :: B
! local parameter
  CHARACTER(LEN=*),   PARAMETER  :: srName = modNam // ':qConj'

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

  B%Q(1:3) = -A%Q(1:3)
  B%Q(4)   =  A%Q(4)

! ---------------------------------------------------
  IF (srfDebug) CALL cpu_exit(srName)  ! DEBUG: exit
  RETURN
END FUNCTION qConj


FUNCTION qNorm(A) RESULT(B)
! input/output
  TYPE(t_quaternion), INTENT(IN) :: A
  TYPE(t_quaternion)             :: B
! local parameter
  CHARACTER(LEN=*),   PARAMETER  :: srName = modNam // ':qNorm'

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

  B%Q = A%Q / sqrt(dot_product(A%Q, A%Q))

! ---------------------------------------------------
  IF (srfDebug) CALL cpu_exit(srName)  ! DEBUG: exit
  RETURN
END FUNCTION qNorm


FUNCTION SLERP(t, A, B) RESULT(C)  ! spherical linear interpolation
! input/output
  REAL(r8b),          INTENT(IN) :: t   ! <0,1>
  TYPE(t_quaternion), INTENT(IN) :: A, B
  TYPE(t_quaternion)             :: C
! local parameter
  CHARACTER(LEN=*),   PARAMETER  :: srName = modNam // ':slerp'
! local variables
  REAL(r8b)                      :: deltat, kappa
  TYPE(t_quaternion)             :: AB, dT


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

  AB = qConj(A) * B

! Resolve ambiguity in quaternions
  IF (AB%Q(4) < 0d0) AB%Q = -AB%Q

! Interpolation of rotation angle
  deltat = myDmod(2*acos(min(AB%Q(4), 1d0)), 2*CONST%pi) * t

! Quaternions might not change between neighbouring epochs
  IF (deltat < 1d-14) THEN
    C = A
  ELSE
! Corresponding quaternion
    dT%Q(4) = COS(deltat/2.d0)

    kappa = (1.d0 - dT%Q(4)*dT%Q(4)) / (1.d0 - AB%Q(4)*AB%Q(4))
    ! Check for numerical instability (PG_F90 issue)
    IF (kappa < 1d-14) kappa = 0d0

    dT%Q(1:3) = SQRT(kappa) * AB%Q(1:3)

    C = A * dT    ! Interpolated quaternion
  END IF

! ---------------------------------------------------
  IF (srfDebug) CALL cpu_exit(srName)  ! DEBUG: exit
  RETURN
END FUNCTION SLERP

END MODULE d_quaternion
