module orientation

  use typedef, only: &
       real64

  implicit none

contains

  subroutine regularize_orientation(y)

    use typedef, only: &
         real64, int64

    implicit none

  real(kind=real64), parameter:: &
       ROTLIM = 1.2d0, &
       PI = 4.d0 * atan(1.d0), &
       PI2N = -2.d0 * PI, &
       ROTLIMPI2 = (PI * ROTLIM)**2

    real(kind=real64), intent(inout), dimension(:) :: &
         y

    real(kind=real64), dimension(3) :: &
         w
    real(kind=real64) :: &
         wn
    integer(kind=int64) :: &
         i, j, n, nstar

    n = size(y)
    nstar = (n + 6) / 12
    if (nstar * 12 - 6 /= n) &
         ERROR STOP '[regularise_orentation] dimension error'

    do i=1, nstar
       j = 9 * nstar - 8 + 3 * i
       w(:) = y(j:j+2)
       wn = w(1)**2 + w(2)**2 + w(3)**2
       if (wn > ROTLIMPI2) then
          y(j:j+2) = w(:) * (1.d0 + PI2N /  sqrt(wn))
       end if
    end do

  end subroutine regularize_orientation


  subroutine regularize_quaternion(y)

    use typedef, only: &
         real64, int64

    implicit none

    real(kind=real64), parameter :: &
         eps = 1.d-14, &
         ope = 1.d0 + eps, &
         ome = 1.d0 - eps

    real(kind=real64), intent(inout), dimension(:) :: &
         y

    real(kind=real64), dimension(0:3) :: &
         f
    real(kind=real64) :: &
         fn
    integer(kind=int64) :: &
         i, j, n, nstar

    n = size(y)
    nstar = (n + 6) / 13
    if (nstar * 13 - 6 /= n) &
         ERROR STOP '[regularise_quaternion] dimension error'

    do i=1, nstar
       j = 9 * nstar - 9 + 4 * i
       f(:) = y(j:j+3)
       fn = f(0)**2 + f(1)**2 + f(2)**2 + f(3)**2
       if (fn > ope .or. fn < ome) then
          fn = 1.d0 / sqrt(fn)
          y(j:j+3) = f(:) * fn
       endif
    end do

  end subroutine regularize_quaternion


  pure function rotation_dqdt(q, w) result(r)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), intent(in), dimension(0:3) :: &
         q
    real(kind=real64), intent(in), dimension(3) :: &
         w
    real(kind=real64), dimension(0:3) :: &
         r

    r(0) = -0.5d0 * (w(1)*q(1) + w(2)*q(2) + w(3)*q(3))
    r(1) =  0.5d0 * (w(1)*q(0) + w(2)*q(3) - w(3)*q(2))
    r(2) =  0.5d0 * (w(2)*q(0) + w(3)*q(1) - w(1)*q(3))
    r(3) =  0.5d0 * (w(3)*q(0) + w(1)*q(2) - w(2)*q(1))

  end function rotation_dqdt


  pure function rotation_dodt(o, w) result(r)

    use typedef, only: &
         real64

    use vector, only: &
         cross_product

    implicit none

    real(kind=real64), parameter :: &
         eps = 1.d-99

    real(kind=real64), intent(in), dimension(3) :: &
         w, o
    real(kind=real64), dimension(3) :: &
         r

    real(kind=real64), dimension(3) :: &
         wp, wn
    real(kind=real64) :: &
         on2

    on2 = o(1)**2 + o(2)**2 + o(3)**2
    wp(:) = o(:) * ((o(1)*w(1) + o(2)*w(2) + o(3)*w(3)) / (on2 + eps))
    wn(:) = w(:) - wp(:)
    r(:) = cross_product(wn(:), 0.5d0 * o(:)) + &
         xcotx(0.5d0 * sqrt(on2)) * wn(:) + wp(:)

  end function rotation_dodt


  pure function rotation_w2A(w) result(M)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), intent(in), dimension(3) :: &
         w
    real(kind=real64), dimension(3,3) :: &
         M

    M(:,:) = rotation_f2A(rotation_w2f(w))

  end function rotation_w2A


  pure function rotation_w2f(w) result(f)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), parameter :: &
         lim_prec = 1.d-17

    real(kind=real64), intent(in), dimension(3) :: &
         w
    real(kind=real64), dimension(0:3) :: &
         f

    real(kind=real64) :: &
         t, s

    t = 0.5d0 * sqrt(w(1)**2 + w(2)**2 + w(3)**2)

    if (t < lim_prec) then
       s = 0.5d0
       f(0) = 1.d0
    else
       s = 0.5d0 * sin(t) / t
       f(0) = cos(t)
    endif

    f(1:3) = w(1:3) * s

  end function rotation_w2f


  pure function rotate_diag(d,A) result(M)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), intent(in), dimension(3) :: &
         d
    real(kind=real64), intent(in), dimension(3,3) :: &
         A
    real(kind=real64), dimension(3,3) :: &
         M

    real(kind=real64) :: &
         a11d1, a22d2, a33d3, a12d2, a23d3, a31d1

    ! A = transpose(AA)

    a11d1 = A(1,1)*d(1)
    a22d2 = A(2,2)*d(2)
    a33d3 = A(3,3)*d(3)
    a12d2 = A(1,2)*d(2)
    a23d3 = A(2,3)*d(3)
    a31d1 = A(3,1)*d(1)

    M(1,1) = a11d1*A(1,1) + a12d2*A(1,2) + A(1,3)**2*d(3)
    M(2,2) = A(2,1)**2*d(1) + a22d2*A(2,2) + a23d3*A(2,3)
    M(3,3) = a31d1*A(3,1) + A(3,2)**2*d(2) + a33d3*A(3,3)

    M(1,2) = a11d1*A(2,1) + A(1,2)*a22d2 + A(1,3)*a23d3
    M(1,3) = a11d1*A(3,1) + a12d2*A(3,2) + A(1,3)*a33d3
    M(2,3) = A(2,1)*a31d1 + a22d2*A(3,2) + A(2,3)*a33d3

    M(2,1) = M(1,2)
    M(3,1) = M(1,3)
    M(3,2) = M(2,3)

    ! M = transpose(M)

  end function rotate_diag


  pure function rotation_f2A(f) result(M)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), intent(in), dimension(0:3) :: &
         f
    real(kind=real64), dimension(3,3) :: &
         M

    real(kind=real64) :: &
         a, b, c, d, &
         aa, bb, cc, dd, &
         ab, ac, ad, bc, bd, cd

    a = f(0)
    b = f(1)
    c = f(2)
    d = f(3)

    aa = a * a
    bb = b * b
    cc = c * c
    dd = d * d

    bc = b * c
    ad = a * d
    ac = a * c
    ab = a * b
    bd = b * d
    cd = c * d

    M(:,:) = reshape( &
         (/ &
        aa + bb - cc - dd, 2.d0 * (bc + ad) , 2.d0 * (bd - ac), &
        2.d0 * (bc - ad) , aa + cc - bb - dd, 2.d0 * (cd + ab), &
        2.d0 * (bd + ac) , 2.d0 * (cd - ab) , aa + dd - bb - cc &
        /), &
        (/3, 3/))

  end function rotation_f2A


  pure elemental function xcotx(x) result(r)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), dimension(0:6), parameter :: &
         c = (/ &
         1.d0, &
         1.d0 / 3.d0, &
         1.d0 / 45.d0, &
         2.d0 / 925.d0, &
         1.d0 / 4725.d0, &
         2.d0 / 93555.d0, &
         1382.d0 / 638512875.d0 /)

    real(kind=real64), intent(in) :: &
         x
    real(kind=real64) :: &
         r
    real(kind=real64) :: &
         x2

    if (x < 1.d-7) then
       x2 = x**2
       r = c(0) - x2 * (c(1) + &
            x2 * (c(2) + &
            x2 * (c(3) + &
            x2 * (c(4) + &
            x2 * (c(5) + &
            x2 * c(6))))))
    else
       r = x / tan(x)
    endif

  end function xcotx


  pure function rotated_spin_tensor(spin, A, f) result(Q)

    use typedef, only: &
         real64

    implicit none

    real(kind=real64), parameter :: &
         MinusOneThird = -1.d0 / 3.d0

    real(kind=real64), intent(in), dimension(3) :: &
         spin
    real(kind=real64), intent(in) :: &
         f
    real(kind=real64), intent(in), dimension(3,3) :: &
         A
    real(kind=real64), dimension(3,3) :: &
         Q

    real(kind=real64), dimension(3) :: &
         w, wf
    real(kind=real64) :: &
         ff

    w(:) = matmul(A(:,:), spin(:))

    wf(:) = w(:) * f

    Q(1,1) = w(1) * wf(1)
    Q(2,2) = w(2) * wf(2)
    Q(3,3) = w(3) * wf(3)

    ff = (Q(1,1) + Q(2,2) + Q(3,3)) * MinusOneThird

    Q(1,1) = Q(1,1) + ff
    Q(2,2) = Q(2,3) + ff
    Q(3,3) = Q(2,3) + ff

    Q(1,2) = w(1) * wf(2)
    Q(1,3) = w(1) * wf(3)
    Q(2,3) = w(2) * wf(3)

    Q(2,1) = Q(1,2)
    Q(3,1) = Q(1,3)
    Q(3,2) = Q(2,3)

  end function rotated_spin_tensor

end module orientation