implicit none
      integer i,ii,kk,iii
      double precision Vector(3),Projected(3),theta,matrix(3,3)
      double precision tmpVec(3), phi,qparam(4),qinv(4),p(4),p1(4),p2(4)
      double precision u(3), Omega,qtmp(4)
      real PreviousX1, PreviousX2
      real PreviousY1, PreviousY2

      call pgbegin(0,'/xs',1,1)
c     call pgbegin(0,'?',1,1)
      call pgenv(-1.2,1.2,-1.2,1.2,1,-2)
      call pgsci(1)
c       call pgtext(-0.9,-0.33,'Equinox')
c      call pgtext(0.0,0.85,'North Pole')
c      call drawInitialConfig

CCCCCCCCCCCCCCCCCCCc
C     Equatorial to Galactic rotation with three Euler Angles
C     rotation around Z-axis
      call DrawAxis
      call pgsci(2)
      call pgtext(-0.47,-0.27,'GC')
      call pgtext(0.53,0.55,'NGP')
      call pgptext(0.3,0.3,-47.0,0.0,'Galactic Plane')

CCCCCCCCCCCCCCCCCCCc
      call sleep(4)
c     Quaternion rotation
c     Draw rotation axis
      call pgsci(5)
      call pgmove(0.0,0.0)
      Vector(1)= 0.5539d0
      Vector(2)= -0.22498
      Vector(3)=  -0.80158
      call Projection(Vector,Projected)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      call pgtext
     $     (-1.5,-1.2,'q=(0.4832,-0.1963,-0.6992,0.4889)')
      call pgtext
     $     (-1.5,-1.3,'Rotate 120.75 deg around (0.554,-0.225,-0.801)')
      qparam(1)= 0.48321075D0
      qparam(2)=-0.196253D0
      qparam(3)= -0.69923D0
      qparam(4)= 0.488947D0
      Omega =acos(qparam(4))
      u(1)= qparam(1)/sin(Omega)
      u(2)= qparam(2)/sin(Omega)
      u(3)= qparam(3)/sin(Omega)

      do kk=1,50
         call pgsci(7)
C     Take time
         do iii=1,70000000
         end do
         theta = Omega/50.0*(kk)
         qtmp(1)= u(1)*sin(theta)
         qtmp(2)= u(2)*sin(theta)
         qtmp(3)= u(3)*sin(theta)
         qtmp(4)= cos(theta)
         
         qinv(1)  =-qtmp(1)
         qinv(2)  =-qtmp(2)
         qinv(3)  =-qtmp(3)
         qinv(4)  = qtmp(4)
         
C     X-axis
         p(1)=1.0D0
         p(2)=0.0D0
         p(3)=0.0D0
         p(4)=0.0D0
         call q_multi(   p,qinv,p1)
         call q_multi(qtmp,  p1,p2)
c         write(*,*) p2(1),p2(2),p2(3),p2(4)
         Vector(1)= 0.0
         Vector(2)= 0.0
         Vector(3)= 0.0
         call Projection(Vector,Projected)
c         call pgmove(real(Projected(1)),real(Projected(2)))
         Vector(1)= p2(1)
         Vector(2)= p2(2)
         Vector(3)= p2(3)
         call Projection(Vector,Projected)
c         write(*,*) real(Projected(1)),real(Projected(2))
c     call pgdraw(real(Projected(1)),real(Projected(2)))
         if(Projected(3).gt.0.0) then
            call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
         else
            call pgpoint(1,real(Projected(1)),real(Projected(2)),1)
         endif
C     Y-axis
         p(1)=0.0D0
         p(2)=1.0D0
         p(3)=0.0D0
         p(4)=0.0D0
         call q_multi(     p,qinv,p1)
         call q_multi(qtmp,  p1,p2)
c         write(*,*) p2(1),p2(2),p2(3),p2(4)
         Vector(1)= 0.0
         Vector(2)= 0.0
         Vector(3)= 0.0
         call Projection(Vector,Projected)
c         call pgmove(real(Projected(1)),real(Projected(2)))
         Vector(1)= p2(1)
         Vector(2)= p2(2)
         Vector(3)= p2(3)
         call Projection(Vector,Projected)
c         write(*,*) real(Projected(1)),real(Projected(2))
c     call pgdraw(real(Projected(1)),real(Projected(2)))
         if(Projected(3).gt.0.0) then
            call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
         else
            call pgpoint(1,real(Projected(1)),real(Projected(2)),1)
         endif
C     Z-axis
         p(1)=0.0D0
         p(2)=0.0D0
         p(3)=1.0D0
         p(4)=0.0D0
         call q_multi(     p,qinv,p1)
         call q_multi(qtmp,  p1,p2)
c         write(*,*) p2(1),p2(2),p2(3),p2(4)
         Vector(1)= 0.0
         Vector(2)= 0.0
         Vector(3)= 0.0
         call Projection(Vector,Projected)
c         call pgmove(real(Projected(1)),real(Projected(2)))
         Vector(1)= p2(1)
         Vector(2)= p2(2)
         Vector(3)= p2(3)
         call Projection(Vector,Projected)
c         write(*,*) real(Projected(1)),real(Projected(2))
c     call pgdraw(real(Projected(1)),real(Projected(2)))
         if(Projected(3).gt.0.0) then
            call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
         else
            call pgpoint(1,real(Projected(1)),real(Projected(2)),1)
         endif
      end do
CCCCCCCCCCCCCc

      call pgend
      end

      subroutine Projection(Vector,Projected)
      implicit none
      double precision Vector(3),Projected(3)
      double precision matrix1(3,3),matrix2(3,3),matrix3(3,3)
      double precision phi, theta,psi
      double precision tmpVec1(3),tmpVec2(3),tmpVec3(3)
      phi =    -230.0
      theta =  70.0
      psi   =   90.0
c      write(*,*) phi, theta, psi
CCCCCCCCCCCCCCCCCCCCc
      phi   = phi*1.74533e-2
      theta = theta*1.74533e-2
      psi   = psi*1.74533e-2
      matrix1(1,1)=cos(phi)
      matrix1(1,2)=-sin(phi)
      matrix1(1,3)=0.0
      matrix1(2,1)=sin(phi)
      matrix1(2,2)=cos(phi)
      matrix1(2,3)=0.0
      matrix1(3,1)=0.0
      matrix1(3,2)=0.0
      matrix1(3,3)=1.0
      matrix2(1,1)=cos(theta)
      matrix2(1,2)=0
      matrix2(1,3)=sin(theta)
      matrix2(2,1)=0.0
      matrix2(2,2)=1.0
      matrix2(2,3)=0.0
      matrix2(3,1)=-sin(theta)
      matrix2(3,2)=0.0
      matrix2(3,3)=cos(theta)

      matrix3(1,1)= cos(psi)
      matrix3(1,2)=-sin(psi)
      matrix3(1,3)=0.0
      matrix3(2,1)=sin(psi)
      matrix3(2,2)=cos(psi)
      matrix3(2,3)=0.0
      matrix3(3,1)=0.0
      matrix3(3,2)=0.0
      matrix3(3,3)=1.0
      call multi_matrix(Vector,  matrix1, tmpVec1)
      call multi_matrix(tmpVec1, matrix2, tmpVec2)
      call multi_matrix(tmpVec2, matrix3, tmpVec3)

      Projected(1)=tmpVec3(1)
      Projected(2)=tmpVec3(2)
      Projected(3)=tmpVec3(3)

      end

      subroutine multi_matrix(in, matrix, out)
      implicit none
      double precision in(3),out(3),matrix(3,3)
      integer i, j
      
      do i = 1, 3
         out(i)=0.0D0
         do j = 1, 3
            out(i) = out(i)+matrix(i,j)*in(j)
         end do
      end do
      end


      subroutine drawaxis
      implicit none
      double precision phi,theta,psi, angle
      double precision matrix1(3,3),matrix2(3,3),matrix3(3,3),
     $     matrix(3,3),tmpmatrix(3,3)
      integer axis
      integer i,ii
      double precision Vector1(3),Vector2(3),Vector3(3)
      double precision tmpVector1(3),tmpVector2(3),tmpVector3(3)


      phi=266.40500D0
      theta=28.936170D0
      psi=58.598660D0

C     Rotate around Z
      Vector1(1)=1.0
      Vector1(2)=0.0
      Vector1(3)=0.0
      Vector2(1)=0.0
      Vector2(2)=1.0
      Vector2(3)=0.0
      Vector3(1)=0.0
      Vector3(2)=0.0
      Vector3(3)=1.0
      do i =1,101
         angle = phi*1.74533e-2/100*(i-1)         
         matrix1(1,1)= cos(angle)
         matrix1(1,2)=-sin(angle)
         matrix1(1,3)= 0.0
         matrix1(2,1)= sin(angle)
         matrix1(2,2)= cos(angle)
         matrix1(2,3)= 0.0
         matrix1(3,1)= 0.0
         matrix1(3,2)= 0.0
         matrix1(3,3)= 1.0
         call draweachaxis(matrix1,vector1,vector2,vector3,
     $        tmpVector1, tmpVector2,tmpVector3, i)
         call pgsci(1)
         call pgtext(0.5,1.2,'Rotate 266.405deg around Z')
      end do

C     Rotate around Y
      do i =1,101
         angle = theta*1.74533e-2/100*(i-1)         
         matrix2(1,1)= cos(angle)
         matrix2(1,2)= 0.0
         matrix2(1,3)= sin(angle)
         matrix2(2,1)= 0.0
         matrix2(2,2)= 1.0
         matrix2(2,3)= 0.0
         matrix2(3,1)= -sin(angle)
         matrix2(3,2)= 0.0
         matrix2(3,3)= cos(angle)
         call multiply_matrix(matrix1,matrix2,matrix)
         call draweachaxis(matrix,vector1,vector2,vector3,
     $        tmpVector1,tmpVector2, tmpVector3,i)
         call pgsci(1)
         call pgtext(0.5,1.2,'Rotate 266.405deg around Z')
         call pgtext(0.5,1.1,'Rotate 28.936 deg around Y')
      end do

C     Rotate around X
      do i =1,101
         angle = psi*1.74533e-2/100*(i-1)         
         matrix3(1,1)= 1.0
         matrix3(1,2)= 0.0
         matrix3(1,3)= 0.0
         matrix3(2,1)= 0.0
         matrix3(2,2)= cos(angle)
         matrix3(2,3)=-sin(angle)
         matrix3(3,1)= 0.0
         matrix3(3,2)= sin(angle)
         matrix3(3,3)= cos(angle)
         call multiply_matrix(matrix1,matrix2,tmpmatrix)
         call multiply_matrix(tmpmatrix,matrix3,matrix)
         call draweachaxis(matrix,vector1,vector2,vector3,
     $        tmpVector1,tmpVector2, tmpVector3,i)
         call pgsci(1)
         call pgtext(0.5,1.2,'Rotate 266.405deg around Z')
         call pgtext(0.5,1.1,'Rotate 28.936 deg around Y')
         call pgtext(0.5,1.0,'Rotate 58.599 deg around X')
      end do

      end

      subroutine draweachaxis(matrix,vector1,vector2,vector3,
     $     tmpVector1,tmpVector2,tmpVector3,i)
      implicit none
      double precision vector1(3),vector2(3),vector3(3)
      double precision tmpVector1(3),tmpVector2(3),tmpVector3(3)
      double precision Projected(3),matrix(3,3)
      real theta
      real PreviousX1, PreviousX2
      real PreviousY1, PreviousY2
      real PreviousZ1, PreviousZ2
      integer i, ii,k, l,m
      double precision Vector(3)

c      call pgeras
      call pgpage
      call pgsci(1)
      call pgtext(-0.9,-0.33,'Equinox')
      call pgtext(0.0,0.85,'North Pole')
C     Original Axis
      call pgsci(1)
      call pgslw(1)
      Vector(1)= 0.0
      Vector(2)= 0.0
      Vector(3)= 0.0
      call Projection(Vector,Projected)
      call pgmove(real(Projected(1)),real(Projected(2)))
      Vector(1)= 1.0
      Vector(2)= 0.0
      Vector(3)= 0.0
      call Projection(Vector,Projected)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      Vector(1)= 0.0
      Vector(2)= 1.0
      Vector(3)= 0.0
      call Projection(Vector,Projected)
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      call pgmove(real(Projected(1)),real(Projected(2)))
      Vector(1)= 0.0
      Vector(2)= 0.0
      Vector(3)= 0.0
      call Projection(Vector,Projected)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      Vector(1)= 0.0
      Vector(2)= 0.0
      Vector(3)= 0.0
      call Projection(Vector,Projected)
      call pgmove(real(Projected(1)),real(Projected(2)))
      Vector(1)= 0.0
      Vector(2)= 0.0
      Vector(3)= 1.0
      call Projection(Vector,Projected)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)

CCCCCCCCCCCCCCCCCCCc
C     Sphere
      do k =1, 101
         theta=3.141592654/50*(k-1)
         Projected(1)= cos(theta)
         Projected(2)= sin(theta)
         if(k.eq.1) then
            call pgmove(real(Projected(1)),real(Projected(2)))
         else
            call pgsci(1)
            call pgdraw(real(Projected(1)),real(Projected(2)))
         endif
      end do
c     Original XY plane
      do m=1,101
         theta=3.141592654/50*(m-1)
         Vector(1)= cos(theta)
         Vector(2)= sin(theta)
         Vector(3)=0.0
         call Projection(Vector,Projected)
         call pgsci(2)
         if(Projected(3).lt.0.0) call pgsls(4)
         call pgsci(1)
         if(m.eq.1) then
            call pgmove(real(Projected(1)),real(Projected(2)))
         else
            call pgdraw(real(Projected(1)),real(Projected(2)))
         endif
      end do
      call pgsls(1)


C     X-axis
      call multi_matrix(Vector1, matrix, tmpVector1)      
      call Projection(tmpVector1,Projected)
      call pgsci(2)
      call pgmove(0.0,0.0)
      call pgslw(3)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      PreviousX1=real(Projected(1))
      PreviousX2=real(Projected(2))

C     XY plane
      Vector(3)=0.0
      do l=1,101
         theta=3.141592654/50*(l-1)
         Vector(1)= cos(theta)
         Vector(2)= sin(theta)
         call multi_matrix(Vector, matrix, tmpVector1)      
         call Projection(tmpVector1,Projected)
         call pgsci(2)
         if(l.eq.1) then
            call pgmove(real(Projected(1)),real(Projected(2)))
         else
            if(Projected(3).lt.0.0) call pgsls(4)
            call pgdraw(real(Projected(1)),real(Projected(2)))
         endif
         call pgsls(1)
      end do


C     Y-axis
      call multi_matrix(Vector2,  matrix, tmpVector2)      
      call Projection(tmpVector2,Projected)
      call pgsci(3)
      call pgmove(0.0,0.0)
      call pgslw(3)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      PreviousY1=real(Projected(1))
      PreviousY2=real(Projected(2))
      
C     Z-axis
      call multi_matrix(Vector3,  matrix, tmpVector3)      
      call Projection(tmpVector3,Projected)
      call pgsci(4)
      call pgmove(0.0,0.0)
      call pgslw(3)
      call pgdraw(real(Projected(1)),real(Projected(2)))
      call pgpoint(1,real(Projected(1)),real(Projected(2)),17)
      PreviousZ1=real(Projected(1))
      PreviousZ2=real(Projected(2))


C     Take time
c      do ii=1,1500000
c      end do
      end
      

      subroutine multiply_matrix(mat1,mat2,mat3)
      implicit none
      double precision mat1(3,3),mat2(3,3),mat3(3,3)
      integer i,j,k
      do i=1, 3
         do j=1,3
            mat3(i,j)=0.0
         end do
      end do
      do i = 1, 3
         do j = 1, 3
            do k=1,3
               mat3(i,j)=mat1(i,k)*mat2(k,j)+mat3(i,j)
            end do
         end do
      end do
      end

      subroutine q_multi(q1,q2,q3)
      implicit none
      double precision q1(4), q2(4), q3(4)
c     q3 = q1q2
      q3(1)= q1(2)*q2(3)-q1(3)*q2(2)+q1(4)*q2(1)+q2(4)*q1(1)
      q3(2)= q1(3)*q2(1)-q1(1)*q2(3)+q1(4)*q2(2)+q2(4)*q1(2)
      q3(3)= q1(1)*q2(2)-q1(2)*q2(1)+q1(4)*q2(3)+q2(4)*q1(3)
      q3(4)= q1(4)*q2(4)-q1(1)*q2(1)-q1(2)*q2(2)-q1(3)*q2(3)

      end