C * * * * * * * * * * * * * * * * * * * * * * * * * * *
C DRIVER FOR DMV10CONJ.F FOR THE COMPUTATION OF THE MOTION
C OF A FREE RIGID BODY WITH SIMULTANEOUS COMPUTATION OF THE 
C TANGENT MAP FOR THE COMPUTATION OF FIRST CONJUGATE POINTS
C * * * * * * * * * * * * * * * * * * * * * * * * * * *
        include 'dmv10conj.f'
        include 'dmvlinalg.f'
        IMPLICIT DOUBLE PRECISION (A-H,O-Z)
        DIMENSION Q(4),AM(3),RPAR(20),IPAR(20)
        DIMENSION QVAR(3,3),AMVAR(3,3)
        LOGICAL ISCONJ
C --- INITIAL VALUES
C MOMENTS OF INERTIA
        AI1=0.6D0
        AI2=0.8D0
        AI3=1.0D0
        RPAR(11)=AI1
        RPAR(12)=AI2
        RPAR(13)=AI3
C ANGULAR MOMENTUM
        AM(1)=1.8D0
        AM(2)=0.4D0
        AM(3)=-0.9D0
C QUATERNION
        Q(1)=1.0D0
        Q(2)=0.0D0
        Q(3)=0.0D0
        Q(4)=0.0D0
C
        WRITE (6,*) '--- INITIAL CONDITIONS'
        WRITE (6,*) '  AM    ',AM(1),AM(2),AM(3)
        WRITE (6,*) '  QQ    ',Q(1),Q(2),Q(3),Q(4)
C INITIALISATION OF JACOBIAN MATRICES
        DO I=1,3
        DO J=1,3
          AMVAR(I,J)=0.0D0
          QVAR(I,J)=0.0D0
        END DO
          AMVAR(I,I)=1.0D0
        END DO
C ---
        DO I=1,10
          RPAR(I)=0.0D0
          IPAR(I)=0
        END DO
C
        H=0.01D0
        XEND=10.0D0
        NSTEP=XEND/H+0.5
        H=XEND/NSTEP
        
        CALL HAMIL (AM,HAM0,RPAR,IPAR)
        CALL INVARQY(Q,AM,AMM10,AMM20,AMM30)
               
               
        WRITE (6,*) 'STEPSIZE H=',H,' MAX NSTEP=',NSTEP
        CALL CPU_TIME(TIME0)
C ---
        CALL DMV10CONJ (AM,Q,AMVAR,QVAR,H,NSTEP,X,ISCONJ,RPAR,IPAR)
C ---
        CALL CPU_TIME(TIME1)
        WRITE (6,*) 'CPU TIME=',TIME1-TIME0
        
        IF (ISCONJ) THEN
          WRITE (6,*) '--- CONJUGATE POINT DETECTED AT'
        ELSE
          WRITE (6,*) '!!! NO CONJUGATE POINT DETECTED'
        END IF
        
        WRITE (6,*) '    TIME',X
        WRITE (6,*) '--- SOLUTION'
        WRITE (6,*) ' AM ',AM(1),AM(2),AM(3)
        WRITE (6,*) ' QQ ',Q(1),Q(2),Q(3),Q(4)
        write (6,*) '--- MATRIX AMVAR'
        CALL PRINTMAT(AMVAR)
        write (6,*) '--- MATRIX QVAR'
        CALL PRINTMAT(QVAR)
        write (6,*) '--- DETERMINANT OF MATRIX QVAR',DETMAT(QVAR)
        
        CALL HAMIL (AM,HAM1,RPAR,IPAR)
        CALL INVARQY(Q,AM,AMM1,AMM2,AMM3)
        WRITE (6,*) 'ERROR IN HAMILTONIAN',HAM1-HAM0
        WRITE (6,*) 'ERROR IN SPATIAL MOMENTUM',
     &               AMM1-AMM10,AMM2-AMM20,AMM3-AMM30
        
        STOP
        END



C SPATIAL MOMENTUM (FIRST INTEGRAL)
        SUBROUTINE INVARQY (Q,AM,AMM1,AMM2,AMM3)
        IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
        DIMENSION Q(4),AM(3)
        AM1=AM(1)
        AM2=AM(2)
        AM3=AM(3)
        Q0=Q(1)
        Q1=Q(2)
        Q2=Q(3)
        Q3=Q(4)
        AMM1=AM1-2.0D0*AM1*Q3**2-2.0D0*AM1*Q2**2-2.0D0*AM2*Q0*Q3
     &   +2.0D0*AM2*Q2*Q1+2.0D0*AM3*Q0*Q2+2.0D0*AM3*Q3*Q1
        AMM2=2.0D0*AM1*Q0*Q3+2.0D0*AM1*Q2*Q1+AM2-2.0D0*AM2*Q3**2
     &   -2.0D0*AM2*Q1**2-2.0D0*AM3*Q0*Q1+2.0D0*AM3*Q3*Q2
        AMM3=-2.0D0*AM1*Q0*Q2+2.0D0*AM1*Q3*Q1+2.0D0*AM2*Q0*Q1
     &   +2.0D0*AM2*Q3*Q2+AM3-2.0D0*AM3*Q2**2-2.0D0*AM3*Q1**2
        RETURN
        END 
C
c HAMILTONIAN (FIRST INTEGRAL)
        SUBROUTINE HAMIL (AM,HAM,RPAR,IPAR)
        IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
        DIMENSION  AM(3)
        DIMENSION IPAR(*),RPAR(*)
        HAM=AM(1)**2/RPAR(11)+AM(2)**2/RPAR(12)+AM(3)**2/RPAR(13)
        HAM=HAM/2.0D0
        RETURN
        END 


