!  Begin quaternion arithmetic.
!  then the algebra of quaternions
!  then a checkout program for them..
!
!     Ralph Jay Frisbie
!     rfrisbie@vcnet.com
!     13 December 1995
!   Modified and converted to F on and after 21 June 1996
!
!!  Constants  and Quaternion type Module
!
!	This module defines the arithmetic of Quaternions.
!  Quaternions are hyper complex numbers of the form:
!    q = a + b*i + c*j + d*k   where i^2=j^2+k^2 = -1
!  a,b,c,d are real numbers of kind=quat_kind, may be reset as desired.
!
!  See standard reference

module quat_utilities


public  :: assignment (=), &
           operator (+), operator (-), &
           operator (*), operator (/), &
           operator (**), &
           quat_out, quat_in, quat_norm,quat_conjugate

private  ::  quat_times_quat,quat_times_real,real_times_quat,&
             quat_plus_quat, quat_plus_real,real_plus_quat,&
             quat_minus_quat,quat_minus_real,real_minus_quat,&
             quat_div_quat,  quat_div_real,  real_div_quat,&
             quat_to_integer_power,reciprical,&
             quat_equals_real,quat_equals_quat, &
             quat_equals_array,array_equals_quat


   integer, parameter, public :: double_kind=selected_real_kind(12,300)
   integer, parameter, public :: real_kind=selected_real_kind(4,10)
   integer, parameter, public :: byte_kind=selected_int_kind(2)
   integer, parameter, public :: word_kind=selected_int_kind(4)
   integer, parameter, public :: quat_kind = double_kind

   type , public :: Quaternion
     private
     real  (kind=quat_kind),dimension(0:3) :: qp
   end type Quaternion
   real, parameter,private    :: one = 1.0_quat_kind
   real, parameter,private    :: zero = 0.0_quat_kind

   type (Quaternion), parameter, public :: Q_1 = Quaternion((/one,zero,zero,zero/))
   type (Quaternion), parameter, public :: Q_i = Quaternion((/zero,one,zero,zero/))
   type (Quaternion), parameter, public :: Q_j = Quaternion((/zero,zero,one,zero/))
   type (Quaternion), parameter, public :: Q_k = Quaternion((/zero,zero,zero,one/))
     interface assignment (=)
        module procedure quat_equals_real, &
                         quat_equals_quat, &
                         quat_equals_array,&
                         array_equals_quat
     end interface

     interface operator (*)
       module procedure quat_times_quat, &
                       quat_times_real, &
                       real_times_quat
      end interface

      interface   operator(+)
        module procedure quat_plus_quat,&
                         quat_plus_real, &
                         real_plus_quat
      end interface

       interface   operator(-)
         module procedure quat_minus_quat, &
                         quat_minus_real, &
                         real_minus_quat
       end interface

       interface   operator(/)
           module procedure quat_div_quat, &
                                 quat_div_real, &
                                 real_div_quat
       end interface

       interface operator (**)
          module procedure quat_to_integer_power
        end interface

contains

     subroutine quat_equals_array(q1,r1)
        type (Quaternion),intent(out) :: q1
        real (kind=quat_kind),intent(in),dimension(0:) :: r1
           q1%qp = r1
        end subroutine quat_equals_array
     subroutine array_equals_quat(r1,q1)
        real (kind=quat_kind),intent(out),dimension (0:) :: r1
        type (Quaternion), intent(in)   :: q1
          r1 = q1%qp
      end subroutine array_equals_quat

     function quat_norm(q1) result(r1)
       type (Quaternion) ,intent(in)  :: q1
        real (kind=quat_kind)  :: r1
        r1 = sqrt(sum(q1%qp**2))
      end function quat_norm

     function quat_conjugate(q1) result(q2)
       type (Quaternion), intent(in) :: q1
       type (Quaternion)             :: q2
        q2%qp(0) = q1%qp(0)
        q2%qp(1:3) = -q1%qp(1:3)
      end function quat_conjugate

     subroutine quat_equals_quat(q1,q2)
         type (Quaternion), intent(out) :: q1
         type (Quaternion), intent(in)  :: q2

         q1%qp = q2%qp
     end subroutine quat_equals_quat

     subroutine quat_equals_real (q2,r1)
         type (Quaternion) ,intent(out)     :: q2
         real (kind=quat_kind),intent(in)  :: r1

           q2%qp(0) = r1
           q2%qp(1:3) = 0.0
     end subroutine quat_equals_real

     subroutine quat_in(q)
        type (Quaternion),intent(out) :: q
        real (kind=quat_kind),dimension(0:3)        :: inp
        read *,inp
        q%qp = inp
     end subroutine quat_in

     subroutine quat_out(q,fmtspec)
         character (len=*),optional,intent(in) :: fmtspec
         type (Quaternion), intent (in)  :: q
         integer     :: i
         character (len=30) :: fmt
         if(present(fmtspec)) then
              fmt = "(a,4"//fmtspec//")"
          else
              fmt = "(a,4(g10.2,1x))"
          end if
         print fmt,"Quaternion = ",(/ (q%qp(i),i=0,3) /)
      end subroutine quat_out

      function quat_to_integer_power(q,n) result (pow)
         type (Quaternion),intent(in)  :: q
         integer, intent(in)           :: n
         type (Quaternion)             :: pow
         integer                       :: absn,i

         if(n == 0) then
            pow%qp(0:3) = (/1.0_quat_kind,0.0_quat_kind,&
                            0.0_quat_kind,0.0_quat_kind/)
            return
         end if
         absn = abs(n)
         pow = q
         do i = 2,absn
            pow = pow*q
         end do
         if (n < 0) then
            pow = reciprical(pow)
         end if
      end function quat_to_integer_power




     function quat_times_quat(q1,q2) result(quat_mult)

          type (Quaternion) ,intent(in) :: q1,q2
          type (Quaternion)  :: quat_mult
          quat_mult%qp(0) = q1%qp(0)*q2%qp(0) - q1%qp(1)*q2%qp(1)  &
                     - q1%qp(2)*q2%qp(2) - q1%qp(3)*q2%qp(3)
          quat_mult%qp(1) = q1%qp(0)*q2%qp(1) + q1%qp(1)*q2%qp(0)  &
                     + q1%qp(2)*q2%qp(3) - q1%qp(3)*q2%qp(2)
          quat_mult%qp(2) = q1%qp(0)*q2%qp(2) + q1%qp(2)*q2%qp(0)  &
                     + q1%qp(3)*q2%qp(1) - q1%qp(1)*q2%qp(3)
          quat_mult%qp(3) = q1%qp(0)*q2%qp(3) + q1%qp(3)*q2%qp(0)  &
                     + q1%qp(1)*q2%qp(2) - q1%qp(2)*q2%qp(1)

    end function quat_times_quat

   function quat_times_real(q1,r1) result(quat_mult)
      type (Quaternion),intent(in) :: q1
      real (kind=quat_kind) ,intent(in)       :: r1
      type (Quaternion)            :: quat_mult


            quat_mult%qp(0:3) = q1%qp(0:3)*r1

    end function quat_times_real

    function real_times_quat(r1,q1) result(quat_mult)
        type (Quaternion), intent(in) :: q1
        real (kind=quat_kind),intent(in) :: r1
        type (Quaternion)                :: quat_mult
           quat_mult%qp(0:3) = q1%qp(0:3)*r1
    end function real_times_quat

       function quat_plus_quat(q1,q2)  result(quat_add)
           type (Quaternion), intent(in) :: q1,q2
           type (Quaternion)             :: quat_add

              quat_add%qp(0:3) = q1%qp(0:3) + q2%qp(0:3)

           return
       end function quat_plus_quat

       function quat_plus_real(q1,r1) result (quat_add)
           type (Quaternion), intent(in) :: q1
           real (kind=quat_kind),intent(in) :: r1
           type (Quaternion)                 :: quat_add
              quat_add%qp(0) = q1%qp(0) + r1
              quat_add%qp(1:3) = q1%qp(1:3)
       end function quat_plus_real

       function real_plus_quat(r1,q1) result (quat_add)
           type (Quaternion), intent(in) :: q1
           real (kind=quat_kind),intent(in) :: r1
           type (Quaternion)                 :: quat_add
              quat_add%qp(0) = q1%qp(0) + r1
              quat_add%qp(1:3) = q1%qp(1:3)
       end function real_plus_quat

       function quat_minus_quat(q1,q2)result(quat_sub)

           type (Quaternion), intent(in) :: q1,q2
           type (Quaternion)             :: quat_sub

              quat_sub%qp(0:3) = q1%qp(0:3) - q2%qp(0:3)

       end function quat_minus_quat

       function quat_minus_real (q1,r1) result(quat_sub)
           type (Quaternion), intent(in) :: q1
           real (kind=quat_kind),intent(in) :: r1
           type (Quaternion)              :: quat_sub
               quat_sub%qp(0) = q1%qp(0) - r1
               quat_sub%qp(1:3) = q1%qp(1:3)
       end function quat_minus_real

       function real_minus_quat(r1,q1) result(quat_sub)

           type (Quaternion), intent(in) :: q1
           real (kind=quat_kind),intent(in) :: r1
           type (Quaternion)               :: quat_sub

               quat_sub%qp(0) = r1 - q1%qp(0)
               quat_sub%qp(1:3) = q1%qp(1:3)
       end function real_minus_quat

       function reciprical(q1) result(recip)
           type(Quaternion),intent(in) :: q1
           type(Quaternion)            :: recip

           real (kind=quat_kind)         :: norm2

           norm2 = sum(q1%qp(:)**2)
           recip%qp(1:3) = -q1%qp(1:3)/norm2
           recip%qp(0)   =  q1%qp(0)/norm2
       end function reciprical

       function quat_div_quat(q1,q2)result(quat_div)

           type (Quaternion), intent(in) :: q1,q2
           type (Quaternion)             :: quat_div
           integer                       :: i
          type (Quaternion)              :: recip

          quat_div = q1*reciprical(q2)
         end function quat_div_quat

    function quat_div_real(q1,r1) result (quat_div)
         type (Quaternion),intent(in)  :: q1
         real (kind=quat_kind), intent(in) :: r1
         type (Quaternion)                  :: quat_div
         quat_div%qp = q1%qp/r1
    end function quat_div_real

    function real_div_quat(r1,q1) result (quat_div)
         type (Quaternion),intent(in)  :: q1
         real (kind=quat_kind), intent(in) :: r1
         type (Quaternion)                  :: quat_div
         quat_div = r1*reciprical(q1)
    end function real_div_quat


end module quat_utilities

program test_quaternions
  use quat_utilities
   type (Quaternion) ::  a,b,c,d,e,f,g,q,i,j,k
   real (kind=quat_kind),dimension(0:3)  :: z
   real (kind=quat_kind), parameter  :: one = 1.0_quat_kind
   real (kind=quat_kind), parameter :: zero = 0.0_quat_kind
   integer           :: indx
   real (kind=quat_kind),dimension(0:3)  :: realq,x,y

   print *,"1,i,j,k"
   call quat_out(Q_1)
   call quat_out(Q_i)
   call quat_out(Q_j)
   call quat_out(Q_k)
   q = (/1.0_quat_kind,-2.3_quat_kind,4.7_quat_kind,-10.2_quat_kind/)
   call quat_out(q)
   print *,"normq = ",quat_norm(q)
   q = quat_conjugate(q)
   call quat_out(q)
   read *
   a = Q_i + Q_j
   print *,"4(i+j) + 3 Q_k"
   d = 4.0_quat_kind*a + Q_k*3.0_quat_kind
   call quat_out(d)
   b = a + Q_k
   c = b + a

   call quat_out(a)
   call quat_out(b)
   call quat_out(c)
   call quat_out(a*b)
   call quat_out(Q_i*Q_j,"f4.1")
   call quat_out(Q_j*Q_k,"f5.1")
   call quat_out(Q_k*Q_i,"f6.3")
   call quat_out(Q_j*Q_i)
   call quat_out(Q_k*Q_j)
   call quat_out(Q_i*Q_k)
   call quat_out(Q_i*Q_j*Q_k)
   call quat_out((Q_i*Q_j)/Q_j)
   read *
   d = (/one,one,one,one/)
   f = quat_conjugate(d)
   call quat_out(d)
   call quat_out(f)
   realq = f
   print "(4f12.5)",realq
   read *
  do indx = 1,10

     print *,"Iteration ",indx
     f = f*d
     print *,quat_norm(f)
     call quat_out(f*quat_conjugate(f))
     call quat_out(f)
  end do

   stop
end program test_quaternions
