C 	Copyright restrictions apply - see stsdas$copyright.stsdas
C
        SUBROUTINE GEOPOS(LATI,LONGI,MLAT,MLONG,ISTAT)
*
*  Module number:
*
*  Module name: GEOPOS
*
*  Keyphrase:
*  ----------
*       Calculate geomagnetic position from geographic position
*
*  Description:
*  ------------
*       This routine calculates the geomagnetic position given the
*	geographic latitude and longitude.
*	This routine is adapted from an algorith supplied by the FOS
*	IDT whose ultimate source is somewhere in the NSSDCA
*
*  FORTRAN name: GEOPOS.for
*
*  Keywords of accessed files and tables:
*  --------------------------------------
*  Name                         I/O     Description / Comments

*  Subroutines Called:
*  -------------------
*  CDBS:
*
*  History:
*  --------
*  Version      Date        Author          Description
*       1       Sep 91	S. Hulbert      Designed and coded
*       1.1     Sep 98  M.D. De La Pena Removed dead statement.
*-------------------------------------------------------------------------------
*
* INPUTS:
*	lati - geographic latitude
*	lngi - geographic longitude
*
* OUTPUT:
*	mlat - geomagnetic latitude
*	mlong - geomagnetic longitude
*       istat - error status
*
*----------------------------------------------------------------------------
        INTEGER ISTAT
        DOUBLE PRECISION LATI,LONGI,MLAT,MLONG
C------------------------------------------------------------------------------
C     UMSPUT DESTINATIONS -- CB, DAO, 4-SEP-87
C
      INTEGER STDOUT
      PARAMETER (STDOUT = 1)
      INTEGER STDERR
      PARAMETER (STDERR = 2)
C
      DOUBLE PRECISION PI
      PARAMETER (PI = 3.1415926535898D0)
      DOUBLE PRECISION RADDEG
      PARAMETER (RADDEG = 57.295779513082D0)
C
C LOCAL VARIABLES ------------------------------------------------------
	DOUBLE PRECISION DEGRAD,TWOPI,CBG,CI,SI,YLG,SBG,CLG,SLG
	DOUBLE PRECISION SBM,CBM,SLM,CLM
C
C-----------------------------------------------------------------------
C
	DEGRAD=1./RADDEG
	TWOPI=2.D0*PI
	CBG=11.4D0*DEGRAD
	CI=COS(CBG)
	SI=SIN(CBG)
C
C convert geographic latitude and longitude to geomagnetic
C
        YLG=LONGI+69.8D0
        CBG=COS(LATI*DEGRAD)
        SBG=SIN(LATI*DEGRAD)
        CLG=COS(YLG*DEGRAD)
        SLG=SIN(YLG*DEGRAD)
        SBM=SBG*CI+CBG*CLG*SI
        MLAT=ASIN(SBM)
        CBM=COS(MLAT)
        SLM=(CBG*SLG)/CBM
        CLM=(-SBG*SI+CBG*CLG*CI)/CBM
   	IF(CLM.GT.1.D0)CLM=1.D0
        MLONG=ACOS(CLM)
        IF(SLM.LT.0.D0)MLONG=TWOPI-ACOS(CLM)
        MLAT=MLAT*RADDEG
        MLONG=MLONG*RADDEG
C
        ISTAT=0
1000    RETURN
        END


      PROGRAM CANHFS

C===================================================================================
C 1. VARIABLE INITIALIZATION
C===================================================================================
      PARAMETER (NFIN=2,NFPR=3,NFOU=4,NDDD=12,NDCM=10,NDNR=36,NDNT=1030)

      REAL*4 P(NDDD),DP(NDDD),S(NDDD),DS(NDDD),
     1     RED(NDNR),TID(NDNR),G(NDCM,NDNT)

      COMPLEX SA,SA2,SB,SB2,EA,EB,EAB,RR,CA,CR,CI,C1,C2,C3,C4,C5

      CHARACTER*80 ALPN, FYLE

      PARAMETER (PI=3.14159265358979, CR=(1.0,0.0), CI=(0.0,1.0),
     1     EPS=1.0E-6, NOR=19, DDS=0.15, DRS=0.02)

C===================================================================================
C 2. FORMATING
C===================================================================================
10    FORMAT (A)
12    FORMAT (' INPUT FILE NAME  = ',$)
14    FORMAT (' OUTPUT FILE NAME = ',$)
16    FORMAT (1H ,A)
20    FORMAT (8F10.4)
44    FORMAT (5E16.7) ! 5 times E14.7   7 points after . each field have 14 places

C===================================================================================
C 3. OPEN FILES
C===================================================================================
      !PRINT 12
      !READ 10, FYLE
      !OPEN (NFIN, FILE="input.txt", STATUS='OLD', FORM='FORMATTED')
      OPEN (NFIN, FILE="input.txt")
      !PRINT 14
      !READ 10, FYLE
      OPEN (NFOU, FILE="output.txt")

C===================================================================================
C 4. READ FROM INPUT
C===================================================================================
      READ (NFIN,*) VP,VS,RHO,NT,DT,H,R,T

C===================================================================================
C 5. SET PARAMETERS AND VARIABLES
C===================================================================================
      NG = 5       	    ! NG
      VPI = 1.0/VP**2       ! (1/alpha)^2
      VSI = 1.0/VS**2       ! (1/beta)^2
      VI = SQRT(VSI - VPI)  ! (1/beta^2 - 1/alpha^2)^1/2, appears in p_2
      !R = RED(1)            ! R = x_1
      !T = TID(1)            ! t_i
      RS = SQRT(R*R + H*H)  ! r = (R^2 + H^2)^1/2
      STH = R/RS     	    ! sin_th = R/RS
      CTH = H/RS      	    ! cos_th = H/R
      SC = 1.0/(RS*PI*PI) * VSI/RHO  ! 1/(pi^2 * r * mu) coeficient in integrals

C===================================================================================
C 6. MAIN LOOP: OVER nt (Number of points in t)
C===================================================================================
      DO 600 I=1, NT
         DO 150 J=1, NG ! LOOP OVER NG=5
            P(J) = 0.0    ! Initialize P_1x12 with zeros for each nt
            S(J) = 0.0    ! Initialize S_1x12 with zeros for each nt
150      CONTINUE

      TP = RS/VP    ! r/alpha, p arrival time
      IF (T .GT. TP) THEN ! compute integrals for t > t_p, IMPORTANT !!!!
	!===================================================================================
	! Settings for alpha wave Integral
	!===================================================================================
         TR = T/RS            ! t/r
         TA2 = TR*TR - VPI    ! (t/r)^2 - (1/alpha)^2
         TA = SQRT(TA2)       ! ((t/r)^2 - (1/alpha)^2)^1/2 Upper limit in integral (26)
         TL = SQRT(TA)        ! ((t/r)^2 - (1/alpha)^2)^1/4 Upper limit in integral (134)

         DI = 0.0	      ! Set nu = 0 for each nt
	!===================================================================================
	! Integral for alpha wave for each nt
	!===================================================================================
	! LOOP 1: IF BLOCK AND GO TO, Integral in alpha!!!
200      DD = 0.15      ! Loop tag      Why this value, while nu < t_uplim
         D1 = TL - DI	! ((t/r)^2 - (1/alpha)^2)^1/4 - nu
         IF (D1 .LT. DD) THEN
            DD = D1
         END IF

         CALL ROMS (K, NG, DI, P, DP, DD, EPS, NOR)

	! LOOP 1.1: IF BLOCK AND GO TO, Integral in alpha!!!
210      CONTINUE      ! Loop tag
         DL = TA - DI*DI                   ! p = ((t/r)^2 - (1/alpha)^2)^1/2 - nu^2 substitution to remove singularity
	 DL2 = DL*DL                       ! p^2
	 CL = 2.0/SQRT(TA + DL)	   	   ! 2 * (2*((t/r)^2 - (1/alpha)^2)^1/2 - nu^2)^1/2 Coeficient in integral (134)
	 D1 = -TR*STH                      ! -t/r * sin_th
	 D2 = SQRT(ABS(TA2 - DL2))*CTH     ! ((t/r)^2 - (1/alpha)^2 - p^2)^1/2 * cos_th
	 SA = CMPLX(D1, D2)                ! q = -t/r * sin_th + i((t/r)^2 - (1/alpha)^2 - p^2)^1/2 * cos_th Equation (31)
	 SA2 = SA*SA 			   ! q^2
	 C4 = SA2 + DL2		   	   ! q^2 + p^2
	 C1 = SA2 - DL2		           ! q^2 - p^2
	 EA = CSQRT(VPI - C1)		   ! eta_alpha = ((1/alpha)^2 - q^2 + p^2)^1/2 Equation (21)
	 EB = CSQRT(VSI - C1)		   ! eta_beta = ((1/beta)^2 - q^2 + p^2)^1/2 Equation (23)
	 EAB = EA*EB 			   ! eta_alpha * eta_beta
	 C2 = EB*EB - C1 		   ! gamma = eta_beta^2 + p^2 - q^2 Equation (29)
	 C5 = C1 - 2.0*EA*EA 		   ! q^2 - p^2 - 2*eta_alpha^2
	 RR = 1.0/(C2*C2 + 4.0*EAB*C1)	   ! sigma^-1 = 1/(gamma^2 + 4*eta_alpha * eta_beta * (q^2 - p^2)) Equation (30)

         ! Assembly M_ij
         DP(1) = REAL(2.0*CL*EAB*SA2*RR)	! REAL(eta_alpha * sigma^-1 * CL * (2.0 * eta_beta * q^2))		M_??
         DP(2) = REAL(2.0*CL*EAB*DL2*RR)	! REAL(eta_alpha * sigma^-1 * CL * (2.0 * eta_beta * p^2))		M_??
         DP(3) = REAL(CL*SA*EA*C2*RR)		! REAL(eta_alpha * sigma^-1 * CL * (q * gamma))				M_??
         DP(4) = REAL(2.0*CL*SA*EA*EAB*RR)	! REAL(eta_alpha * sigma^-1 * CL * (2.0 * q * eta_alpha * eta_beta))	M_??
         DP(5) = REAL(CL*EA*EA*C2*RR)		! REAL(eta_alpha * sigma^-1 * CL * (eta_alpha * gamma))			M_33

         CALL ROMS1(K, NG, DI, P, DP, DD, EPS, NOR)

         ! The following block replaces GO TO (210,230,220), K
         IF (K .EQ. 1) THEN
            GO TO 210
            ! END LOOP 1.1------------------------------------------ Integral in alpha!!!
         ELSE IF (K .EQ. 2) THEN
            GO TO 220
         ELSE IF (K .EQ. 3) THEN
            GO TO 220
         END IF

220      CONTINUE
!230      CONTINUE

         IF (DI .LT. TL)  GO TO 200	! nu < t_uplim
         ! END LOOP 1------------------------------------------Integral in alpha!!!
!***

	!===================================================================================
	! Settings for beta wave Integral
	!===================================================================================
         TB2 = TR*TR - VSI   ! (t/r)^2 - (1/beta)^2
         DRD = -1000.0
         D2 = VS/VP

	!*************************************NASTY
         IF (STH .LE. D2)  GO TO 290  ! sin_th <= beta/alpha condition (27), (28)

         IF (TB2 .GT. 0.0)  GO TO 291 ! (t/r)^2 - (1/beta)^2 > 0
            D1 = R/VP + H*VI	! t_2 = R/alpha + xs_3*(1/beta^2 - 1/alpha^2)^1/2 ; sin_th > beta/alpha Eq. 28

         IF (T .LE. D1) THEN	! Do not compute integrals if t <= t_2, IMPORTANT !!!!
	    GO TO 480
         END IF

         TB = -SQRT(ABS(TB2))     ! -((t/r)^2 - (1/beta)^2)^1/2 Upper limit in integral (26) for beta
         TL = -SQRT(ABS(TB))	  ! -((t/r)^2 - (1/beta)^2)^1/4 Upper limit in integral (134) for beta
         GO TO 294

290      IF (TB2 .LE. 0.0) THEN	! (t/r)^2 - (1/beta)^2 > 0
	    GO TO 480
	 END IF

291      TB = SQRT(TB2)		! ((t/r)^2 - (1/beta)^2)^1/2 Upper limit in integral (26) for beta
         TL = SQRT(TB)		! ((t/r)^2 - (1/beta)^2)^1/4 Upper limit in integral (134) for beta

         DI = 0.0	! Set nu = 0
         C3 = CR
         KFL = 3
         !DRR = (DQR + VSI*CT2)/(1.0 - CT2)
         !
         !IF (DRR .GT. 0.0)  THEN
         !   DRD = SQRT(TB - SQRT(DRR)) - DRS
         !END IF

         IF (STH .LE. D2)  GO TO 300	! sin_th <= beta/alpha condition (27), (28)
294      D1 = SQRT(((TR - VI*CTH)/STH)**2 - VPI)
         DI = -SQRT(D1 - TB)	! p_2 = -SQRT(SQRT(((TR - VI*CTH)/STH)**2 - VPI) - TB) with variable change
         C3 = -CI
         KFL = 1
	!*************************************NASTY

	!===================================================================================
	! Integral for beta wave
	!===================================================================================
	! LOOP 2: IF BLOCK AND GO TO
	!        START LOOP 2------------------------------------
300      DD = DDS      ! Loop tag

       ! The following block replaces GO TO (302,304,306,301), KFL
         IF (KFL .EQ. 1) THEN
            D1 = DI + DD
            IF (D1 .GT. 0.0) THEN
               DD = -DI
               KFL = 2
            END IF
            GO TO 306
         ELSE IF (KFL .EQ. 2) THEN
            C3 = CR
            KFL = 3
            GO TO 306
         ELSE IF (KFL .EQ. 3) THEN
            GO TO 306
         ELSE IF (KFL .EQ. 4) THEN
            DD = DRS + DRS
            KFL = 3
            GO TO 306
         END IF

306      D1 = TL - DI
         IF (D1 .LT. DD) THEN
            DD = D1
         END IF

         IF (DI .LT. DRD)  THEN
            D1 = DI + DD
            IF (D1 .GE. DRD) THEN
               DD = DRD - DI
               KFL = 4
            END IF
         END IF

         CALL ROMS (K, NG, DI, S, DS, DD, EPS, NOR)

         ! START LOOP 2.1------------------------------------
310      CONTINUE      ! Loop tag
         DL = TB - DI*ABS(DI)		! p = ((t/r)^2 - (1/alpha)^2)^1/2 - nu^2 substitution to remove singularity, abs!!!!!!!!
         DL2 = DL*DL			! p^2

         ! Implement this conditional !!!!!
         IF (TL .LT. 0.0) THEN	! if ((t/r)^2 - (1/beta)^2)^1/4 < 0
            CL = 2.0/SQRT(TB + DL + 2.0*(TB/DI)**2)
         ELSE
            CL = 2.0/SQRT(TB + DL)! if ((t/r)^2 - (1/beta)^2)^1/4 >= 0
         END IF

         D1 = -TR*STH				! -t/r * sin_th
         D2 = SQRT(ABS(TB2 - DL2))*CTH		! ((t/r)^2 - (1/beta)^2 - p^2)^1/2 * cos_th
         SB = D1*CR + D2*CI*C3 			! q = -t/r * sin_th + i((t/r)^2 - (1/beta)^2 - p^2)^1/2 * cos_th Equation (32)
         SB2 = SB*SB 				! q^2
         C4 = SB2 + DL2				! q^2 + p^2
         C1 = SB2 - DL2				! q^2 - p^2
         EA = CSQRT(VPI - C1)			! eta_alpha = ((1/alpha)^2 - q^2 + p^2)^1/2 Equation (21)
         EB = CSQRT(VSI - C1)			! eta_beta = ((1/beta)^2 - q^2 + p^2)^1/2 Equation (23)
         EAB = EA*EB 				! eta_alpha * eta_beta
         C2 = EB*EB - C1			! gamma = eta_beta^2 + p^2 - q^2 Equation (29)
         C5 = C2 - 4.0*EAB 			! gamma - 4*eta_alpha*eta_beta
         RR = 1.0/(C3*(C2*C2 + 4.0*EAB*C1))	! sigma^-1 = 1/(gamma^2 + 4*eta_alpha * eta_beta * (q^2 - p^2)) Equation (30)

         ! Assembly N_ij
         DS(1) = REAL(CL*(DL2*C5 + EB*EB*C2)*RR)! REAL(sigma^-1 * eta_beta * CL * (p^2*(gamma - 4*eta_alpha*eta_beta)/eta_beta + eta_beta*gamma))
         DS(2) = REAL(CL*(SB2*C5 - EB*EB*C2)*RR)! REAL(sigma^-1 * eta_beta * CL * (q^2*(gamma - 4*eta_alpha*eta_beta)/eta_beta - eta_beta*gamma))
         DS(3) = REAL(-2.0*CL*SB*EB*EAB*RR)	! REAL(sigma^-1 * eta_beta * CL * (2 * -q * gamma))
         DS(4) = REAL(-CL*SB*EB*C2*RR)		! REAL(sigma^-1 * eta_beta * CL * (-q * gamma))
         DS(5) = REAL(2.0*CL*EAB*C1*RR)		! REAL(sigma^-1 * eta_beta * CL * (2 * eta_alpha * (q^2 - p^2)))F

         CALL ROMS1(K, NG, DI, S, DS, DD, EPS, NOR)

         ! The following block replaces GO TO (310,330,320), K
         IF (K .EQ. 1) THEN
            GO TO 310
            ! END LOOP 2.1------------------------------------------
         ELSE IF (K .EQ. 2) THEN
            GO TO 330
         ELSE IF (K .EQ. 3) THEN
            GO TO 320
         END IF
320      CONTINUE
330      CONTINUE

         IF (DI .LT. TL)  GO TO 300
         ! END LOOP 2------------------------------------------

!***
480   END IF

      DO 490 J=1, NG
         D1 = SC*(P(J) + S(J))  ! Assemby of integral, NG = 5
         G(J,I) = D1	! I from main loop
490   CONTINUE

      T = T + DT
600   CONTINUE

      ! LOOP OVER NT (Number of points in t) to write resuls
      DO 800 I=1, NT
         WRITE (NFOU,44) (G(J,I),J=1,NG)  !Write results to output
800   CONTINUE

      CLOSE (UNIT=NFPR)
      CLOSE (UNIT=NFOU)
      STOP
      END
C###################################################################################

C===================================================================================
C SUBROUTINE ROMS, Romberg's scheme to compute integrals
c	K	: !!!
c	NY	: Number of entries in G, determines the number of integrals to compute
c	X	: Integration variable, nu
c	Y	: Output value of integral
c	YDOT	: Integrand
C	DX	: differential dnu
C	EPS 	: Error !!!
C	NOR	: Order 19 !!!
C===================================================================================
      SUBROUTINE ROMS (K, NY, X, Y, YDOT, DX, EPS, NOR)
      DIMENSION N(21),AA(20,20),BB(20,20),C(20),V(20),W(20),Y(20),
     1          YDOT(20)
      DATA MNOR,ISN,N(1),N(2),N(3)/20,1,1,2,3/	! Declare and initialize

      IF (ISN .NE. 0) THEN
         DO 90 I=4, MNOR, 2
            N(I) = N(I - 2) + N(I - 2)
            N(I+1) = N(I - 1) + N(I - 1)
90       CONTINUE
         ISN = 0
      END IF

      NN = NOR
      NM = NOR - 1
      XX = X + DX
      XMI = AMIN1(X, XX)
      XMA = AMAX1(X, XX)
      E = XMA - XMI
      IR = 0

      IF (EPS .LT. 0.0) THEN
         IR = 1
      END IF

      ERR = ABS(EPS)
      KI = 1
      K = 1

      RETURN
      !------------------------------------------------------------------

      ENTRY ROMS1 (K, NY, X, Y, YDOT, DX, EPS, NOR)
      ! The following block replaces GO TO (110,130,140,150), KI
      IF (KI .EQ. 1) THEN
         IF (E .EQ. 0.0) THEN
            K = 2
         ELSE
            DO 122 J=1, NY
               C(J) = YDOT(J)
 122        CONTINUE
            X = X + DX
            KI = 2
         END IF
         GO TO 400
      ELSE IF (KI .EQ. 2) THEN
         GO TO 130
      ELSE IF (KI .EQ. 3) THEN
	 GO TO 140
      ELSE IF (KI .EQ. 4) THEN
         GO TO 150
      END IF
!-130------------------------------------------------------------------
130      DO 134 J=1, NY
            DB = 0.5*(C(J) + YDOT(J))
            V(J) = E*DB
            DO 133 I=1, NN
               BB(I,J) = DB
133         CONTINUE
134      CONTINUE
         M = 0
138      M = M + 1
         M1 = M + 1
         M2 = M + 2
         NC = N(M1)
         NC1 = NC - 1
         FNC = FLOAT(NC)
         H = E/FNC
         X = XMI + H
         KI = 3
         GO TO 400
!-140------------------------------------------------------------------
140   DO 142 J=1, NY
         C(J) = YDOT(J)
142   CONTINUE
      KEO = 1
      IF (MOD(M1,2) .NE. 0) THEN
         KEO = 2
      END IF
      I = 1
      IF (M .EQ. 2) THEN
         I = 0
      END IF
      ! INIT LOOP 3------------------------------------
145   I = I + 2
      IF (I .GT. NC1)  GO TO 160
         ! The following block replaces GO TO (148,147), KEO
         IF (KEO .EQ. 1) THEN
            GO TO 148
         ELSE IF (KEO .EQ. 2) THEN
            GO TO 147
         END IF
147    IF (MOD(I,3) .EQ. 0)  GO TO 145
       ! END LOOP 3------------------------------------
148   X = XMI + FLOAT(I)*H
      KI = 4
      GO TO 400
!-150------------------------------------------------------------------
150   DO 152 J=1, NY
         C(J) = C(J) + YDOT(J)
152   CONTINUE
      GO TO 145   ! NASTY LOOP

160   DO 182 J=1, NY
         ! The following block replaces GO TO (161,170),  KEO
         IF (KEO .EQ. 1) THEN
            GO TO 161
         ELSE IF (KEO .EQ. 2) THEN
            GO TO 170
         END IF

161      DO 162 I=M2, NN
            BB(I,J) = BB(I,J) + C(J)
162      CONTINUE

         BB(M,J) = BB(M,J) + C(J)
         GO TO 180

170      DO 172 I=M, NN, 2
            BB(I,J) = BB(I,J) + C(J)
172      CONTINUE

180      AA(M,J) = 0.0
         C(J) = H*BB(M,J)
         W(J) = C(J)
182   CONTINUE

      DO 280 I=1, M
         MI = M - I
         D = FNC/FLOAT(N(MI + 1))
         D = D*D - 1.0
         ERS = 0.0
         DO 200 J=1, NY
            H = C(J)
            DD = (H - V(J))/D + H
            V(J) = AA(I,J)
            AA(I,J) = DD
            C(J) = DD
            DE = ABS(DD - H)

            IF (DD .NE. 0.0) THEN
               IF (IR .EQ. 1) THEN
		  DE = DE/ABS(DD)
	       END IF
               ERS = AMAX1(ERS, DE)
	    END IF
200	 CONTINUE

         IF (ERS .LE. ERR) THEN
	    GO TO 320
	 END IF
280   CONTINUE

      DO 302 J=1, NY
         V(J) = W(J)
302   CONTINUE

      IF (M .LT. NM)  GO TO 138  ! NASTY LOOP

      K = 3
      GO TO 340

320   K = 2
      M = I

340   DO 342 J=1, NY
         Y(J) = Y(J) + SIGN(1.0, DX)*AA(M,J)
342   CONTINUE

      X = XX
      GO TO 400
!------------------------------------------------------------------

400   RETURN
C---------------------------------------------------------------------
      END

notebooks/Computational Seismology/Wave Propagation & Analytical Solutions/Lambs_problem_3D/canhfs.for


!---------------------------------------------------!
! Copyright (c) 2017 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
subroutine GS_CG
  use global_variables
  use hpsi
  implicit none
  real(dp),allocatable :: xvec(:,:),pvec(:,:),rvec(:,:)
  real(dp),allocatable :: hxvec(:,:),gvec(:,:),hpvec(:,:)

  real(dp) :: xx,pp,xp,xhx,php,xhp,esp,esp_res,gg,gg0
  real(dp) :: ss,lambda,alpha,beta,aa,bb,cc
  integer :: iorb,iorb_t,ix,iy,iter_cg
  real(dp) :: esp_iter(Ncg),esp_res_iter(Ncg)

  allocate( xvec(0:Nx,0:Nx),pvec(0:Nx,0:Nx),rvec(0:Nx,0:Nx) )
  allocate( hxvec(0:Nx,0:Nx),gvec(0:Nx,0:Nx),hpvec(0:Nx,0:Nx) )

  write(*,'(A)')'===== Ground state calculation ===================================================='
  write(*,'(A)')
  write(*,'(A)')

  xvec(:,:)=wfn(:,:)

  call rhpsi(xvec,hxvec)

  xx=sum(xvec(:,:)**2)*dx**2
  xhx=sum(xvec(:,:)*hxvec(:,:))*dx**2
  lambda=xhx/xx
  do iter_cg=1,Ncg
     gvec(:,:)=2d0*(hxvec(:,:)-lambda*xvec(:,:))/xx

     gg0=sum(gvec(:,:)**2)*dx**2
     select case(iter_cg)
     case(1)
        pvec(:,:)=-gvec(:,:)
     case default
        beta=gg0/gg
        pvec(:,:)=-gvec(:,:)+beta*pvec(:,:)
     end select
     gg=gg0

     call rhpsi(pvec,hpvec)

     pp=sum(pvec(:,:)**2)*dx**2
     php=sum(pvec(:,:)*hpvec(:,:))*dx**2
     xp=sum(xvec(:,:)*pvec(:,:))*dx**2
     xhp=sum(hxvec(:,:)*pvec(:,:))*dx**2

     aa=php*xp-xhp*pp
     bb=php*xx-xhx*pp
     cc=xhp*xx-xhx*xp
     ss=bb**2-4d0*aa*cc
     if(ss > 0d0)then
        alpha=(-bb+sqrt(ss))/(2d0*aa)
     else
        exit
     end if

     xvec(:,:)=xvec(:,:)+alpha*pvec(:,:)

     call rhpsi(xvec,hxvec)
     xx=sum(xvec(:,:)**2)*dx**2
     xhx=sum(xvec(:,:)*hxvec(:,:))*dx**2
     lambda=xhx/xx
     esp_iter(iter_cg)=lambda
     esp_res_iter(iter_cg)=sum((hxvec(:,:)-lambda*xvec(:,:))**2)*dx**2

  end do

  xvec(:,:)=xvec(:,:)/sqrt(xx)
  call rhpsi(xvec,hxvec)
  esp=sum(xvec(:,:)*hxvec(:,:))*dx**2
  esp_res=sum((hxvec(:,:)-esp*xvec(:,:))**2)*dx**2
  wfn(:,:)=xvec(:,:)
  if(wfn(Nx/2,Nx/2) < 0d0) wfn(:,:) = -wfn(:,:)

  write(*,'(A)')'esp,     esp_res'
  write(*,'(e16.6e3,3x,e16.6e3)')esp,esp_res

  ss = 0d0
  do iy =0,Nx
  do ix =0,Nx
     ss = ss + abs(wfn(ix,iy)-wfn(iy,ix))
  end do
  end do
  ss = ss *dx**2
  write(*,'(A,2x,e26.16e3)')'Symmerty error =',ss

  open(90,file = 'GS_data.out')
  write(90,'(A)')'# iter,  esp(iter),  esp_res(iter)'
  do iter_cg =1,Ncg
     write(90,'(I6,2x,e26.16e3,2x,e26.16e3)')iter_cg,esp_iter(iter_cg),esp_res_iter(iter_cg)
  end do
  close(90)

  open(90,file = 'GS_wfn.out')
  write(90,'(A)')'# x, y, wfn(x,y)'
  do ix =1,Nx
  do iy =1,Nx
     write(90,'(100e26.16e3)')xn(ix),xn(iy),wfn(ix,iy)
  end do
  end do
  close(90)

  open(90,file = 'GS_wfn_matrix.out')
  do iy =0,Nx
     write(90,'(9999e26.16e3)')wfn(:,iy)
  end do
  close(90)

  call wfn_rho
  open(90,file = 'GS_rho.out')
  write(90,'(A)')'# x,  rho(x)'
  do ix =0,Nx
     write(90,'(100e26.16e3)')xn(ix),rho(ix)
  end do
  close(90)
  write(*,*)'rho',sum(rho(:))*dx

  write(*,'(A)')
  write(*,'(A)')
  write(*,'(A)')'===== End Ground state calculation ================================================'

  return
end subroutine GS_CG


program probit
use bklib, only : setseed,dp
use bkmath, only : pdfn,cdfn,inverse
! PROBITMC use monte, only : montecarlo
implicit none
real(kind=DP), dimension(:), allocatable :: y
real(kind=DP), dimension(:,:), allocatable :: x
! PROBITMC real(kind=DP), dimension(100) :: gamma
real(kind=DP) :: gammai,reprate
logical :: underreporting_correction
character(len=12) :: resultfile
! PROBITMC integer :: i

! PROBITMC do i=1,100
! PROBITMC   print *, i
   call setseed()
! PROBITMC   call montecarlo()
   call load_data()
!   gamma(i) = estimate(y,x)
   call estimate(y,x,reprate,gammai)
! PROBITMC gamma(i)=gammai
! PROBITMC deallocate(x,y)
! PROBITMC end do
! PROBITMC print *, "Avg gamma: ", sum(gamma)/100.0_dp
! PROBITMC print *, "Std. Dev.:", sqrt(sum((gamma-sum(gamma)/100.0_dp)**2)/100.0_dp)

contains

subroutine estimate(y,x,reprate,gamma)
  real(kind=DP), dimension(:), intent(in) :: y
  real(kind=DP), dimension(:,:), intent(in) :: x
  real(kind=DP), dimension(size(x,2)) :: b,db,g
  real(kind=DP), dimension(size(x,1)) :: pdf,cdf,d,xb,gfac
  real(kind=DP), dimension(:,:), allocatable :: h,xtmp
!  real(kind=DP), dimension(size(x,2),size(x,2)) :: h
!  real(kind=DP), dimension(size(x,1),size(x,2)) :: xtmp
  real(kind=DP), intent(in) :: reprate
  real(kind=DP), intent(inout) :: gamma
  integer :: i,j,n,ios
  logical :: file_found
  real(kind=DP) :: eps=100.0_dp,tol=0.000001_dp
  allocate (h(size(x,2),size(x,2)),xtmp(size(x,1),size(x,2)))
  n=size(y)
  h=matmul(transpose(x),x)
  call inverse(h)
  ! The function call below will return a segmentation fault if x is too big.
  ! It's the main constraint on the size of the data.
  b=matmul(h,matmul(transpose(x),y))
  do i=1,1000
     xb=matmul(x,b)
     pdf=pdfn(xb)*reprate
     cdf=cdfn(xb)*reprate
     gfac=y*(pdf/cdf)-(1.0_dp-y)*(pdf/(1.0_dp-cdf))
     do j=1,size(x,2)
        g(j)=sum(x(:,j)*gfac)
     end do
     d=pdf*((y*(pdf+(xb*cdf))/cdf**2) + ((1.0_dp-y)*(pdf-xb*(1.0_dp-cdf))/((1.0_dp-cdf)**2)))
     do j=1,size(x,2)
        xtmp(:,j)=x(:,j)*d
     end do
     h=matmul(transpose(xtmp),x)
     call inverse(h)
     db=matmul(h,g)
     b=b+db
     eps=sum(db**2)/real(size(db),kind=DP)
     if (eps < tol) then
        exit
     end if
  end do
  inquire (file=resultfile,exist=file_found)
  if (file_found) then
     open(unit=1,file=resultfile,iostat=ios,action="write",position="append",status="old")
  else
     open(unit=1,file=resultfile,iostat=ios,action="write",position="rewind",status="new")
  end if
  if (ios == 0) then
     write (unit=1,iostat=ios,fmt=*) sum(y*log(cdf)+(1.0_dp-y)*log(1.0_dp-cdf)), b
     close(unit=1,iostat=ios)
  else
     stop "Error: Unable to open result file"
  end if
  gamma=b(2)
  deallocate (h,xtmp)
end subroutine estimate

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! LOAD_DATA subroutine
!
! Format: load_data
!
! Loads data and user options from the appropriate files, and
! stores it in various global variables.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  subroutine load_data()
    character(len=12) :: datafile,parmfile="parm.dat  ",logfile
    character(len=80) :: toss,samptypelong
    character(len=1) :: sample_type
    logical :: file_found
    real(kind=DP), dimension(:,:), allocatable :: dat
    integer, dimension(:), allocatable :: groupid
    logical, dimension(:), allocatable :: msk
    integer :: i,j,nobs,nvar,ios
    inquire (file=parmfile,exist=file_found)
    if (.not.file_found) then
       stop "Error: parm.dat file not found"
    end if
    open(unit=1,file=parmfile,iostat=ios,action="read",position="rewind",status="old")
    if (ios /=0) then
       stop "Error: parm.dat file found but could not be opened"
    end if
    read (unit=1,fmt=*,iostat=ios) toss ! TOSS is used for comment lines; the program doesn't use it
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) nvar
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) nobs
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) samptypelong
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) underreporting_correction
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) datafile
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) logfile
    read (unit=1,fmt=*,iostat=ios) toss
    read (unit=1,fmt=*,iostat=ios) resultfile
    close (unit=1,iostat=ios)
    sample_type=uppercase(samptypelong(1:1))
    allocate(x(nobs,nvar+2),y(nobs))
    open(unit=1,file=logfile,iostat=ios,action="write",position="rewind",status="replace")
    write(unit=1,iostat=ios,fmt=*) "Probit estimation"
    write(unit=1,iostat=ios,fmt=*) "Number of variables:",nvar
    write(unit=1,iostat=ios,fmt=*) "Number of observations:",nobs
    write(unit=1,iostat=ios,fmt=*) "Sample type:",samptypelong
    write(unit=1,iostat=ios,fmt=*) "Underreporting corrrection?:",underreporting_correction
    write(unit=1,iostat=ios,fmt=*) "Data read from file:",datafile
    write(unit=1,iostat=ios,fmt=*) "Log file:",logfile
    write(unit=1,iostat=ios,fmt=*) "Results written to file:",resultfile
    close(unit=1,iostat=ios)
    if (sample_type == "I") then
       allocate(dat(nobs,nvar+3))
    elseif (sample_type=="G") then
       allocate(dat(nobs,nvar+2),groupid(nobs),msk(nobs))
    else
       stop "Illegal sample type"
    end if
    open (unit=1,file=datafile,iostat=ios,form="formatted",action="read",position="rewind",status="old")
    if (ios == 0) then
       do i=1,nobs
          read (unit=1,fmt=*,iostat=ios) dat(i,:)
          if (ios /= 0) then
             stop "Error: not enough observations in data file"
          end if
       end do
    else
       stop "Cannot open data file"
    end if
    close(unit=1,iostat=ios)
    x(:,1)=1.0_dp
    x(:,3:size(x,2))=dat(:,(3+size(dat,2)-size(x,2)):size(dat,2))
    if (sample_type == "I") then
       y=dat(:,1)
       x(:,2)=dat(:,2)
    else
       y=dat(:,2)
       groupid=nint(dat(:,1))
       do i=1,nobs
          j=groupid(i)
          msk=(groupid==j)
          msk(i)=.false.
          x(i,2)=sum(y,msk)/real(count(msk),kind=DP)
       end do
    end if
    deallocate(dat)
    if (sample_type == "G") then
       deallocate(msk,groupid)
    end if
    if (underreporting_correction) then
       reprate=sum(y)/sum(x(:,2))
       if (reprate > 1.0_dp) then
          reprate = 1.0_dp
       end if
    else
       reprate=1.0_dp
    end if
  end subroutine load_data

function uppercase(str) result (strup)
  character(len=1), intent(in) :: str
  character(len=1) :: strup
  strup=str
  if ((ichar(str) >= ichar("a")) .and. (ichar(str) <= ichar("z"))) then
     strup = char(ichar("A")+ichar(str)-ichar("a"))
  end if
end function uppercase

end program probit


!This program can be used to generate random numbers.
!Use this program, a file 'seed.in' must exist.
!'seed.in' contains a integer, for example 123456.
!#####################################################################

real(8) function ran()
!----------------------------------------------!
! 64-bit congruental generator                 !
! iran64=oran64*2862933555777941757+1013904243 !
!----------------------------------------------!
    implicit none

    real(8)    :: dmu64
    integer(8) :: ran64,mul64,add64
    common/bran64/dmu64,ran64,mul64,add64

    ran64=ran64*mul64+add64
    ran=0.5d0+dmu64*dble(ran64)

end function ran
!----------------!

!---------------------!
subroutine initran(w)
!---------------------!
    implicit none

    integer(8) :: irmax
    integer(4) :: w,nb,b

    real(8)    :: dmu64
    integer(8) :: ran64,mul64,add64
    common/bran64/dmu64,ran64,mul64,add64

    irmax=2_8**31
    irmax=2*(irmax**2-1)+1
    mul64=2862933555777941757_8
    add64=1013904243
    dmu64=0.5d0/dble(irmax)

    open(10,file='seed.in',status='old')

    read(10,*)ran64
    close(10)
    if (w.ne.0) then
        open(10,file='seed.in',status='unknown')
        write(10,*) abs((ran64*mul64)/5+5265361)
        close(10)
    end if

end subroutine initran
!----------------------!


	SUBROUTINE DP_PDIF  ( grid, igx, igy, nbits, idata, lendat,
     +			      p1, difmin, scale, iret )
C************************************************************************
C* DP_PDIF								*
C*									*
C* This subroutine packs a grid into the GEMPAK DIF format.  The	*
C* value of the difference between a point and the previous point	*
C* is packed using the equations:					*
C*									*
C*	IDIF  =  NINT ( ( GDIF - DIFMIN ) / SCALE )			*
C*	GDIF  =  DIFMIN  +  IDIF * SCALE				*
C*									*
C* DP_PDIF  ( GRID, IGX, IGY, NBITS, IDATA, LENDAT, P1, DIFMIN,		*
C*            SCALE, IRET )						*
C*									*
C* Input parameters:							*
C*	GRID (IGX,IGY)	REAL		Grid data			*
C*	IGX		INTEGER		Number of points in x dir	*
C*	IGY		INTEGER		Number of points in y dir	*
C*	NBITS		INTEGER		Number of bits			*
C*									*
C* Output parameters:							*
C*	IDATA (LENDAT)	INTEGER		Packed data			*
C*	LENDAT		INTEGER		Length of packed data array	*
C*	P1		REAL		Value of first grid point	*
C*	DIFMIN		REAL		Minimum value of differences	*
C*	SCALE		REAL		Scaling factor			*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					-10 = NBITS invalid		*
C*					-11 = invalid data range	*
C**									*
C* Log:									*
C* M. desJardins/GSFC	 3/89						*
C* K. Brill/GSC		 2/90	Fix to find negative max on grid	*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	REAL		grid  (IGX,IGY)
	INTEGER		idata (*)
C*
	LOGICAL		first, line
C*
	INCLUDE		'ERMISS.FNC'
C------------------------------------------------------------------------
	iret = 0
	kxky = igx * igy
C
C*	Check for valid input.
C
	IF  ( ( nbits .le. 1 ) .or. ( nbits .gt. 31 ) )  THEN
	    iret = -10
	    RETURN
	END IF
C
C*	Read through the grid finding the first point and the minimum
C*	and maximum difference values.
C
	dmin  = 1.0E+31
	dmax  = -1.0E+31
	first = .true.
	DO  j = 1, igy
	    line = .false.
	    DO  i = 1, igx
		IF  ( .not. ERMISS ( grid (i,j) ) )  THEN
		    IF  ( first )  THEN
			p1    = grid (i,j)
			dmin  = 0.
			dmax  = 0.
			psav  = p1
			plin  = p1
			line  = .true.
			first = .false.
		      ELSE
			IF  ( .not. line )  THEN
			    diff = grid (i,j) - plin
			    line = .true.
			    plin = grid (i,j)
			  ELSE
			    diff = grid (i,j) - psav
			END IF
			IF  ( diff .lt. dmin )  dmin = diff
			IF  ( diff .gt. dmax )  dmax = diff
			psav = grid (i,j)
		    END IF
		END IF
	    END DO
	END DO
C
C*	Find the data range and check that it is valid.
C
	pdif = dmax - dmin
	IF  ( pdif .lt. 0. )  THEN
	    iret = -11
	    RETURN
	END IF
C
C*	Scale the data to fit into NBITS.
C
	scale = FLOAT ( 2 ** nbits - 2 ) / pdif
C
C*	Compute the number of output words and initialize the output
C*	buffer.
C
	lendat = FLOAT ( nbits * kxky ) / 32.
	IF  ( lendat * 32 .ne. nbits * kxky ) lendat = lendat + 1
	DO  i = 1, lendat
	    idata (i) = 0
	END DO
C
C*	Set missing data value.
C
	imiss = 2 ** nbits - 1
C
C*	Add data points to output buffer.
C
	ibit  = 1
	iword = 1
C*
	first = .true.
	DO  j = 1, igy
	    line = .false.
	    DO  i = 1, igx
		IF  ( ERMISS ( grid (i,j) ) )  THEN
		    diff = RMISSD
		  ELSE
		    IF  ( first )  THEN
			diff  = 0.
			psav  = p1
			plin  = p1
			line  = .true.
			first = .false.
		      ELSE
			IF  ( .not. line )  THEN
			    diff = grid (i,j) - plin
			    line = .true.
			    plin = grid (i,j)
			  ELSE
			    diff = grid (i,j) - psav
			END IF
			psav = grid (i,j)
		    END IF
		END IF
C
C*		Scale the difference into an integer.
C
		IF  ( ERMISS ( diff ) )  THEN
		    idat = imiss
		  ELSE
		    gggg = diff - dmin
		    IF  ( gggg .lt. 0. ) gggg = 0.
		    idat = NINT ( gggg * scale )
		END IF
C
C*		Shift bits to correct location in word.
C
		jshft = 33 - nbits - ibit
		idat2 = ISHFT ( idat, jshft )
		idata (iword) = IOR ( idata (iword), idat2 )
C
C*		Check to see if packed integer overflows into next word.
C
		IF  ( jshft .lt. 0 )  THEN
		    jshft = 32 + jshft
		    idata (iword+1) = ISHFT ( idat, jshft )
		END IF
C
C*		Set location for next word.
C
		ibit = ibit + nbits
		IF  ( ibit .gt. 32 )  THEN
		    ibit  = ibit - 32
		    iword = iword + 1
		END IF
	    END DO
	END DO
C
C*	The scale factor to be saved is really the reciprocal of the
C*	scale which was computed.
C
	scale  = 1. / scale
	difmin = dmin
C*
	RETURN
	END


        SUBROUTINE UPQRQF(N,ETA,S,F0,F1,QT,R,W,T)
C
C SUBROUTINE  UPQRQF  PERFORMS A BROYDEN UPDATE ON THE  Q R
C FACTORIZATION OF A MATRIX  A, (AN APPROXIMATION TO J(X0)),
C RESULTING IN THE FACTORIZATION  Q+ R+ OF
C
C       A+  =  A  +  (Y - A*S) (ST)/(ST * S),
C
C (AN APPROXIMATION TO J(X1))
C WHERE S = X1 - X0, ST = S TRANSPOSE,  Y = F(X1) - F(X0).
C
C THE ENTRY POINT  R1UPQF  PERFORMS THE RANK ONE UPDATE ON THE QR
C FACTORIZATION OF
C
C       A+ =  A + Q*(T*ST).
C
C
C ON INPUT:
C
C N  IS THE DIMENSION OF X AND F(X).
C
C ETA  IS A NOISE PARAMETER.  IF (Y-A*S)(I) .LE. ETA*(|F1(I)|+|F0(I)|)
C    FOR 1 .LE. I .LE. N, THEN NO UPDATE IS PERFORMED.
C
C S(1:N) = X1 - X0   (OR S FOR THE ENTRY POINT R1UPQF).
C
C F0(1:N) = F(X0).
C
C F1(1:N) = F(X1).
C
C QT(1:N,1:N)  CONTAINS THE OLD Q TRANSPOSE, WHERE  A = Q*R .
C
C R(1:N*(N+1)/2)  CONTAINS THE OLD R, STORED BY ROWS.
C
C W(1:N), T(1:N)  ARE WORK ARRAYS ( T  CONTAINS THE VECTOR T FOR THE
C    ENTRY POINT  R1UPQF ).
C
C
C ON OUTPUT:
C
C N  AND  ETA  ARE UNCHANGED.
C
C QT  CONTAINS Q+ TRANSPOSE.
C
C R   CONTAINS R+, STORED BY ROWS.
C
C S, F0, F1, W, AND T  HAVE ALL BEEN CHANGED.
C
C
C CALLS  DAXPY, DDOT, AND DNRM2.
C
C ***** DECLARATIONS *****
C
C     FUNCTION DECLARATIONS
C
        DOUBLE PRECISION DDOT, DNRM2
C
C     LOCAL VARIABLES
C
        DOUBLE PRECISION C, DEN, ONE, SS, WW, YY
        INTEGER I, INDEXR, INDXR2, J, K
        LOGICAL SKIPUP
C
C     SCALAR ARGUMENTS
C
        DOUBLE PRECISION ETA
        INTEGER N
C
C     ARRAY DECLARATIONS
C
        DOUBLE PRECISION  S(N), F0(N), F1(N), QT(N,N), R(N*(N+1)/2),
     $    W(N), T(N), TT(2)
C
C ***** END OF DECLARATIONS *****
C
C ***** FIRST EXECUTABLE STATEMENT *****
C
        ONE = 1.0
        SKIPUP = .TRUE.
C
C ***** DEFINE T AND S SUCH THAT *****
C
C           A+ = Q*(R + T*ST).
C
C T = R*S.
C
        INDEXR = 1
        DO 10 I=1,N
          T(I) = DDOT(N-I+1,R(INDEXR),1,S(I),1)
          INDEXR = INDEXR + N - I + 1
  10    CONTINUE
C
C W = Y - Q*T  = Y - A*S.
C
        DO 20 I=1,N
          W(I) = F1(I) - F0(I) - DDOT(N,QT(1,I),1,T,1)
C
C         IF W(I) IS NOT SMALL, THEN UPDATE MUST BE PERFORMED,
C         OTHERWISE SET W(I) TO 0.
C
          IF (ABS(W(I)) .GT. ETA*(ABS(F1(I)) + ABS(F0(I)))) THEN
            SKIPUP = .FALSE.
          ELSE
            W(I) = 0.0
          END IF
  20    CONTINUE
C
C  IF NO UPDATE IS NECESSARY, THEN RETURN.
C
        IF (SKIPUP) RETURN
C
C T = QT*W = QT*Y - R*S.
C
        DO 30 I=1,N
          T(I) = DDOT(N,QT(I,1),N,W,1)
  30    CONTINUE
C
C S = S/(ST*S).
C
        DEN = 1.0/DDOT(N,S,1,S,1)
        CALL DSCAL(N,DEN,S,1)
C
C ***** END OF COMPUTATION OF  T & S      *****
C       AT THIS POINT,  A+ = Q*(R + T*ST).
C
        CALL r1upqf(n, s, t, qt, r, w)
        RETURN
        END


      SUBROUTINE GAULAT(GAUL,K)
!     LAST MODIFIED 7 April 1994 Mark Iredell
!
!     15 April 2014 Implicit none Robert Grumbine

      IMPLICIT none
      DOUBLE PRECISION A(3200)
      INTEGER K, KK, IS, ITER, N
      REAL GAUL(1)

      DOUBLE PRECISION ESP, C, FK, XZ, PKM1, PKM2, FN, PK, PKMRK
      DOUBLE PRECISION SP, AVSP, RADI
      INTEGER L

      ESP=1.D-14
      C=(1.D0-(2.D0/3.14159265358979D0)**2)*0.25D0
      FK=K
      KK=K/2

      CALL BSSLZ1(A,KK)

      DO IS=1,KK
      XZ=COS(A(IS)/SQRT((FK+0.5D0)**2+C))
      ITER=0
   10 PKM2=1.D0
      PKM1=XZ
      ITER=ITER+1
      IF(ITER.GT.10) RETURN

      DO N=2,K
        FN=N
        PK=((2.D0*FN-1.D0)*XZ*PKM1-(FN-1.D0)*PKM2)/FN
        PKM2=PKM1
        PKM1=PK
      ENDDO
      PKM1=PKM2
      PKMRK=(FK*(PKM1-XZ*PK))/(1.D0-XZ**2)
      SP=PK/PKMRK
      XZ=XZ-SP
      AVSP=ABS(SP)
      IF(AVSP.GT.ESP) GO TO 10
      A(IS)=XZ
      ENDDO

      IF(K.NE.KK*2) THEN
        A(KK+1)=0.D0
        PK=2.D0/FK**2
        DO N=2,K,2
          FN=N
          PK=PK*FN**2/(FN-1.D0)**2
        ENDDO
      ENDIF

      DO N=1,KK
        L=K+1-N
        A(L)=-A(N)
      ENDDO
C
      RADI=180./(4.*ATAN(1.))
      DO N=1,K
        !Drop to single precision A 14 December 2005 because of linux not
        !  finding proper library when compiling into C
        !GAUL(N)=ACOS(A(N))*RADI
        GAUL(N)=ACOS(SNGL(A(N)))*RADI
      ENDDO
C
      RETURN
      END


C$Procedure      CONVRT ( Convert Units )

      SUBROUTINE CONVRT ( X, IN, OUT, Y )

C$ Abstract
C
C      Take a measurement X, the units associated with
C      X, and units to which X should be converted; return Y ---
C      the value of the measurement in the output units.
C
C$ Disclaimer
C
C     THIS SOFTWARE AND ANY RELATED MATERIALS WERE CREATED BY THE
C     CALIFORNIA INSTITUTE OF TECHNOLOGY (CALTECH) UNDER A U.S.
C     GOVERNMENT CONTRACT WITH THE NATIONAL AERONAUTICS AND SPACE
C     ADMINISTRATION (NASA). THE SOFTWARE IS TECHNOLOGY AND SOFTWARE
C     PUBLICLY AVAILABLE UNDER U.S. EXPORT LAWS AND IS PROVIDED "AS-IS"
C     TO THE RECIPIENT WITHOUT WARRANTY OF ANY KIND, INCLUDING ANY
C     WARRANTIES OF PERFORMANCE OR MERCHANTABILITY OR FITNESS FOR A
C     PARTICULAR USE OR PURPOSE (AS SET FORTH IN UNITED STATES UCC
C     SECTIONS 2312-2313) OR FOR ANY PURPOSE WHATSOEVER, FOR THE
C     SOFTWARE AND RELATED MATERIALS, HOWEVER USED.
C
C     IN NO EVENT SHALL CALTECH, ITS JET PROPULSION LABORATORY, OR NASA
C     BE LIABLE FOR ANY DAMAGES AND/OR COSTS, INCLUDING, BUT NOT
C     LIMITED TO, INCIDENTAL OR CONSEQUENTIAL DAMAGES OF ANY KIND,
C     INCLUDING ECONOMIC DAMAGE OR INJURY TO PROPERTY AND LOST PROFITS,
C     REGARDLESS OF WHETHER CALTECH, JPL, OR NASA BE ADVISED, HAVE
C     REASON TO KNOW, OR, IN FACT, SHALL KNOW OF THE POSSIBILITY.
C
C     RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF
C     THE SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY
C     CALTECH AND NASA FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE
C     ACTIONS OF RECIPIENT IN THE USE OF THE SOFTWARE.
C
C$ Required_Reading
C
C     None.
C
C$ Keywords
C
C     CONVERSION, UNITS
C
C$ Declarations

      DOUBLE PRECISION X
      CHARACTER*(*)    IN
      CHARACTER*(*)    OUT
      DOUBLE PRECISION Y

C$ Brief_I/O
C
C     VARIABLE  I/O  DESCRIPTION
C     --------  ---  -------------------------------------------------
C     X          I   Number representing a measurement in some units.
C     IN         I   The units in which X is measured.
C     OUT        I   Desired units for the measurement.
C     Y          O   The measurement in the desired units.
C
C$ Detailed_Input
C
C     X          is a number representing a measurement in the units
C                specified by IN.
C
C     IN         represents the units associated with a measurement X.
C                Acceptable units are:
C
C                Angles:                 'RADIANS'
C                                        'DEGREES'
C                                        'ARCMINUTES'
C                                        'ARCSECONDS'
C                                        'HOURANGLE'
C                                        'MINUTEANGLE'
C                                        'SECONDANGLE'
C
C                Metric Distances:       'M'
C                                        'METERS'
C                                        'KM'
C                                        'KILOMETERS'
C                                        'CM'
C                                        'CENTIMETERS'
C                                        'MM'
C                                        'MILLIMETERS'
C
C                English Distances:      'FEET'
C                                        'INCHES'
C                                        'YARDS'
C                                        'STATUTE_MILES'
C                                        'NAUTICAL_MILES'
C
C                Astrometric Distances:  'AU'
C                                        'PARSECS'
C                                        'LIGHTSECS'
C                                        'LIGHTYEARS' julian lightyears
C
C                Time:                   'SECONDS'
C                                        'MINUTES'
C                                        'HOURS'
C                                        'DAYS'
C                                        'JULIAN_YEARS'
C                                        'TROPICAL_YEARS'
C                                        'YEARS' (same as julian years)
C
C     OUT        represents the units desired for the measurement X.
C                See the description of IN.
C
C$ Detailed_Output
C
C     Y          is the input measurement converted to the desired
C                units.
C
C
C$ Parameters
C
C     None.
C
C$ Exceptions
C
C     1) If the input units, output units, or both input and
C        output units are not recognized, the error
C        SPICE(UNITSNOTREC) is signaled.
C
C     2) If the units being converted between are incompatible, the
C        error SPICE(INCOMPATIBLEUNITS) is signaled.
C
C$ Files
C
C     None.
C
C$ Particulars
C
C     This routine converts a measurement X given in units specified by
C     IN to the equivalent value Y in units specified by OUT.
C
C     If a unit is not recognized, an error message is produced that
C     indicates which one was not recognized.
C
C     If input and output units are incompatible (for example ANGLE
C     and DISTANCE units) and error message will be produced stating
C     the requested units and associated types.
C
C$ Examples
C
C     To convert 1 meter to statute miles and feet you could
C
C        CALL CONVRT ( 1.0D0, 'METERS',        'STATUTE_MILES', MILES )
C        CALL CONVRT ( MILES, 'STATUTE_MILES', 'FEET',          FEET  )
C
C     or
C
C        CALL CONVRT ( 1.0D0, 'METERS', 'STATUTE_MILES', MILES )
C        CALL CONVRT ( 1.0D0, 'METERS', 'FEET',          FEET  )
C
C
C$ Restrictions
C
C     You should make sure that your units are appropriate for the
C     measurement. This routine does not do any checking for over-
C     flow. Something like
C
C        CALL ( 10.0D22, 'LIGHTYEARS', 'MM', Y )
C
C     will cause a floating point overflow.
C
C     Some of the units are not "defined" quantities.  In such a case
C     a best estimate is provided as of the date of the current version
C     of this routine. Those estimated quantities are:
C
C         1 AU    --- the astronomical unit. The value was taken from
C                     the JPL ephemeris DE125. This value is an
C                     approximation and should not be used for
C                     high-accuracy work. It agrees with the value used
C                     in the JPL planetary ephemeris DE430
C                     (149597870.700 km) at the 100m level.
C
C         The tropical year is the time from equinox to equinox.  This
C         varies slightly with time.
C
C         1 PARSEC --- is dependent upon the value of the astronomical
C                      unit.
C
C$ Literature_References
C
C     None.
C
C$ Author_and_Institution
C
C     N.J. Bachman    (JPL)
C     C.A. Curzon     (JPL)
C     H.A. Neilan     (JPL)
C     W.M. Owen       (JPL)
C     W.L. Taber      (JPL)
C     I.M. Underwood  (JPL)
C
C$ Version
C
C-    SPICELIB Version 2.0.0, 12-MAY-2015 (NJB)
C
C        Added support for full names of metric distance units. Added
C        support for the abbreviation 'M' indicating meters.
C
C-    SPICELIB Version 1.0.2, 01-JUL-2014 (NJB)
C
C        Updated the description of the AU in the Restrictions
C        section.
C
C-    SPICELIB Version 1.0.1, 10-MAR-1992 (WLT)
C
C        Comment section for permuted index source lines was added
C        following the header.
C
C-    SPICELIB Version 1.0.0, 31-JAN-1990 (CAC) (WMO) (WLT) (IMU)
C
C-&

C$ Index_Entries
C
C     convert units
C
C-&

C$ Revisions
C
C-    Beta Version 1.2.0, 05-JAN-1990 (WLT)
C
C        Data statements for double precision values were changed
C        to include a 'D' so that this routine would function properly
C        on the Univac.
C
C-   Beta Version 1.1.0, 02-MAR-1989 (HAN)
C
C        The variable LIGHTYEAR was changed to LTYEAR in order to
C        comply with the ANSI Fortran Standard six character
C        variable name length restriction.
C
C-&

C
C     SPICELIB functions
C

      LOGICAL          RETURN
      INTEGER          ISRCHC
      DOUBLE PRECISION DPR

C
C     Local variables
C
      CHARACTER*16     INU
      CHARACTER*16     OUTU

      INTEGER          I
      INTEGER          J
      DOUBLE PRECISION TEMP

      DOUBLE PRECISION SECPJY
      PARAMETER      ( SECPJY =  86400.0D0 * 365.25D0      )

      DOUBLE PRECISION SECPTY
      PARAMETER      ( SECPTY =  86400.0D0 * 365.2421988D0 )

      DOUBLE PRECISION LTYEAR
      PARAMETER      ( LTYEAR = 86400.0D0 * 365.25D0 * 299792458.0D0)

      DOUBLE PRECISION DPM
      PARAMETER      ( DPM  = 1.0D0  /   60.0D0      )

      DOUBLE PRECISION DPS
      PARAMETER      ( DPS  = 1.0D0  / 3600.0D0      )

      DOUBLE PRECISION DPSA
      PARAMETER      ( DPSA = 15.0D0 / 3600.0D0      )

      DOUBLE PRECISION AU
      PARAMETER      ( AU   = 1.4959787061368887D11  )
C
C     1.0d0 divided by the sin of 1 arc second
C
      DOUBLE PRECISION SCALE
      PARAMETER      ( SCALE = 2.0626480624790438D+05 )

      DOUBLE PRECISION PARSEC
      PARAMETER      ( PARSEC = AU * SCALE )

      INTEGER          ANG
      PARAMETER      ( ANG  = 0         )

      INTEGER          DIST
      PARAMETER      ( DIST = ANG  + 7  )

      INTEGER          TME
      PARAMETER      ( TME  = DIST + 17 )

      INTEGER          TOTAL
      PARAMETER      ( TOTAL = TME + 7  )

      CHARACTER*16     UNITS  (TOTAL)
      SAVE             UNITS

      DOUBLE PRECISION CNVRTN (TOTAL)
      SAVE             CNVRTN

      CHARACTER*8      TYPE   (TOTAL)
      SAVE             TYPE

      LOGICAL          FIRST
      SAVE             FIRST

      DATA             FIRST / .TRUE. /

C
C     Angular Conversions:
C
C                 (1)  Degrees/Radians
C                 (2)  Degrees/Degrees
C                 (3)  Degrees/ARCMINUTES
C                 (4)  Degrees/ARCSECONDS
C
C                 ()   Degrees/HOURANGLE
C                 ()   Degrees/MINUTEANGLE
C                 ()   Degrees/SECONDANGLE
C
      DATA UNITS  (ANG + 1)  / 'RADIANS'     /
      DATA UNITS  (ANG + 2)  / 'DEGREES'     /
      DATA UNITS  (ANG + 3)  / 'ARCMINUTES'  /
      DATA UNITS  (ANG + 4)  / 'ARCSECONDS'  /
      DATA UNITS  (ANG + 5)  / 'HOURANGLE'   /
      DATA UNITS  (ANG + 6)  / 'MINUTEANGLE' /
      DATA UNITS  (ANG + 7)  / 'SECONDANGLE' /
C
C     DATA CNVRTN (ANG + 1)  /      DPR()   /
C
C     This value will be loaded using the SPICELIB function DPR()
C     on the first execution of this routine.
C
      DATA CNVRTN (ANG + 2)  /    1.0D0   /
      DATA CNVRTN (ANG + 3)  /      DPM   /
      DATA CNVRTN (ANG + 4)  /      DPS   /
      DATA CNVRTN (ANG + 5)  / 15.000D0   /
      DATA CNVRTN (ANG + 6)  /  0.250D0   /
      DATA CNVRTN (ANG + 7)  /  DPSA      /

      DATA TYPE   (ANG + 1)  / 'ANGLE'    /
      DATA TYPE   (ANG + 2)  / 'ANGLE'    /
      DATA TYPE   (ANG + 3)  / 'ANGLE'    /
      DATA TYPE   (ANG + 4)  / 'ANGLE'    /
      DATA TYPE   (ANG + 5)  / 'ANGLE'    /
      DATA TYPE   (ANG + 6)  / 'ANGLE'    /
      DATA TYPE   (ANG + 7)  / 'ANGLE'    /
C
C     Distance Conversions ( 8 through 17 )
C
C                 (+  1) Meters/Meter
C                 (+  2) Meters/Km
C                 (+  3) Meters/Cm
C                 (+  4) Meters/mm
C                 (+  5) Meters/Lightsecs
C                 (+  6) Meters/AU
C                 (+  7) Meters/Meter
C                 (+  8) Meters/Km
C                 (+  9) Meters/cm
C                 (+ 10) Meters/mm
C
      DATA UNITS (DIST + 1)  / 'METERS'               /
      DATA UNITS (DIST + 2)  / 'KM'                   /
      DATA UNITS (DIST + 3)  / 'CM'                   /
      DATA UNITS (DIST + 4)  / 'MM'                   /
      DATA UNITS (DIST + 5)  / 'LIGHTSECS'            /
      DATA UNITS (DIST + 6)  / 'AU'                   /
      DATA UNITS (DIST + 7)  / 'M'                    /
      DATA UNITS (DIST + 8)  / 'KILOMETERS'           /
      DATA UNITS (DIST + 9)  / 'CENTIMETERS'          /
      DATA UNITS (DIST + 10) / 'MILLIMETERS'          /

      DATA CNVRTN(DIST + 1)  / 1.00D0                 /
      DATA CNVRTN(DIST + 2)  / 1000.0D0               /
      DATA CNVRTN(DIST + 3)  / 0.01D0                 /
      DATA CNVRTN(DIST + 4)  / 0.001D0                /
      DATA CNVRTN(DIST + 5)  / 299 792 458.0D0        /
      DATA CNVRTN(DIST + 6)  / AU                     /
      DATA CNVRTN(DIST + 7)  / 1.00D0                 /
      DATA CNVRTN(DIST + 8)  / 1000.0D0               /
      DATA CNVRTN(DIST + 9)  / 0.01D0                 /
      DATA CNVRTN(DIST + 10) / 0.001D0                /

      DATA TYPE  (DIST + 1)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 2)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 3)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 4)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 5)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 6)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 7)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 8)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 9)  / 'DISTANCE'             /
      DATA TYPE  (DIST + 10) / 'DISTANCE'             /

C
C     Distance Conversions
C
C                 (+ 11) Meters/Foot
C                 (+ 12) Meters/inch
C                 (+ 13) Meters/Statute Mile
C                 (+ 14) Meters/Nautical Mile
C                 (+ 15) Meters/Yard
C
      DATA UNITS (DIST + 11) / 'FEET'            /
      DATA UNITS (DIST + 12) / 'INCHES'          /
      DATA UNITS (DIST + 13) / 'STATUTE_MILES'   /
      DATA UNITS (DIST + 14) / 'NAUTICAL_MILES'  /
      DATA UNITS (DIST + 15) / 'YARDS'           /

      DATA CNVRTN(DIST + 11) / 0.3048D0          /
      DATA CNVRTN(DIST + 12) / 0.0254D0          /
      DATA CNVRTN(DIST + 13) / 1 609.344D0       /
      DATA CNVRTN(DIST + 14) / 1 852.0D0         /
      DATA CNVRTN(DIST + 15) / 0.9144D0          /

      DATA TYPE  (DIST + 11) / 'DISTANCE'        /
      DATA TYPE  (DIST + 12) / 'DISTANCE'        /
      DATA TYPE  (DIST + 13) / 'DISTANCE'        /
      DATA TYPE  (DIST + 14) / 'DISTANCE'        /
      DATA TYPE  (DIST + 15) / 'DISTANCE'        /

C
C     Distance Conversions
C
C                 (+ 16) Meters/LightYear
C                 (+ 17) Meters/Parsec
C
      DATA UNITS (DIST + 16) / 'LIGHTYEARS'            /
      DATA UNITS (DIST + 17) / 'PARSECS'               /

      DATA CNVRTN(DIST + 16) / LTYEAR                  /
      DATA CNVRTN(DIST + 17) / PARSEC                  /

      DATA TYPE  (DIST + 16) / 'DISTANCE'              /
      DATA TYPE  (DIST + 17) / 'DISTANCE'              /

C
C     Time Conversions
C
C                 (+ 1 ) seconds / second
C                 (+ 2 ) seconds / minute
C                 (+ 3 ) seconds / hour
C                 (+ 4 ) seconds / day
C                 (+ 5 ) Seconds / Julian year
C                 (+ 6 ) Seconds / Tropical year
C                 (+ 7 ) Seconds / year          --- same as Julian year
C
      DATA UNITS ( TME + 1 ) / 'SECONDS'        /
      DATA UNITS ( TME + 2 ) / 'MINUTES'        /
      DATA UNITS ( TME + 3 ) / 'HOURS'          /
      DATA UNITS ( TME + 4 ) / 'DAYS'           /
      DATA UNITS ( TME + 5 ) / 'JULIAN_YEARS'   /
      DATA UNITS ( TME + 6 ) / 'TROPICAL_YEARS' /
      DATA UNITS ( TME + 7 ) / 'YEARS'          /

      DATA CNVRTN( TME + 1 ) /     1.0D0        /
      DATA CNVRTN( TME + 2 ) /    60.0D0        /
      DATA CNVRTN( TME + 3 ) /  3600.0D0        /
      DATA CNVRTN( TME + 4 ) / 86400.0D0        /
      DATA CNVRTN( TME + 5 ) / SECPJY           /
      DATA CNVRTN( TME + 6 ) / SECPTY           /
      DATA CNVRTN( TME + 7 ) / SECPJY           /

      DATA TYPE  ( TME + 1 ) / 'TIME'           /
      DATA TYPE  ( TME + 2 ) / 'TIME'           /
      DATA TYPE  ( TME + 3 ) / 'TIME'           /
      DATA TYPE  ( TME + 4 ) / 'TIME'           /
      DATA TYPE  ( TME + 5 ) / 'TIME'           /
      DATA TYPE  ( TME + 6 ) / 'TIME'           /
      DATA TYPE  ( TME + 7 ) / 'TIME'           /

C
C     Set up the error processing.
C
      IF ( RETURN() ) RETURN
      CALL CHKIN ( 'CONVRT' )

      IF ( FIRST ) THEN
         CNVRTN( ANG + 1 ) =  DPR()
         FIRST             = .FALSE.
      END IF

      CALL UCASE ( IN,  INU  )
      CALL UCASE ( OUT, OUTU )

      I    = ISRCHC( INU,  TOTAL, UNITS )
      J    = ISRCHC( OUTU, TOTAL, UNITS )

      IF (      ( I .EQ. 0 )
     .     .OR. ( J. EQ. 0 ) ) THEN

         IF (       ( I .EQ. 0 )
     .        .AND. ( J .EQ. 0 ) ) THEN

            CALL SETMSG ('CONVRT: Neither the input units ' // INU  //
     .                   'nor the output units '            // OUTU //
     .                   'were recognized.'
     .                  )

            CALL SIGERR ('SPICE(UNITSNOTREC)')

            CALL CHKOUT ( 'CONVRT' )
            RETURN

         ELSE IF ( I .EQ. 0 ) THEN

            CALL SETMSG ('CONVRT: Input units '// INU //
     .                   ' were not recognized'
     .                  )

            CALL SIGERR ('SPICE(UNITSNOTREC)')

            CALL CHKOUT ( 'CONVRT' )
            RETURN

         ELSE IF ( J .EQ. 0 ) THEN

            CALL SETMSG ('CONVRT: Output units '// OUTU //
     .                   ' were not recognized'
     .                  )

            CALL SIGERR ('SPICE(UNITSNOTREC)')

            CALL CHKOUT ( 'CONVRT' )
            RETURN

         END IF

      END IF

      IF ( TYPE(I) .NE. TYPE(J) ) THEN

         CALL SETMSG ('CONVRT: Incompatible units. '   //
     .                'You are attempting to convert ' //
     .                INU  // 'type: ' // TYPE(I)      //
     .                ' to '                           //
     .                OUTU // 'type: ' // TYPE(J)      //
     .                '.'
     .               )

         CALL SIGERR ('SPICE(INCOMPATIBLEUNITS)')

         CALL CHKOUT ( 'CONVRT' )
         RETURN

      END IF

      TEMP = X    * CNVRTN ( I )
      Y    = TEMP / CNVRTN ( J )

      CALL CHKOUT ( 'CONVRT' )
      RETURN

      END


C
C     JACOBI METHOD
C
C     LOOP DISTRIBUTION
C     INTERNAL SEQUENTIAL LOOP AND EXTERNAL PARALLEL LOOP
C
      SUBROUTINE JACOBI(N, A, B, X, Y)
      INTEGER N
      REAL A(N,N), B(N), X(N), Y(N)
C
      DO I = 1,N
         Y(I) = B(I)
         DO J = 1,N
            Y(I) = Y(I) - X(J)*A(I,J)
         ENDDO
      ENDDO
C
      DO I = 1,N
         X(I) = X(I) +Y(I)/A(I,I)
      ENDDO
C
      RETURN
      END


      PROGRAM xtridag
C     driver for routine tridag
      INTEGER NP
      PARAMETER (NP=20)
      INTEGER k,n
      REAL diag(NP),superd(NP),subd(NP),rhs(NP),u(NP)
      CHARACTER txt*3
      open(7,file='MATRX2.DAT',status='old')
10    read(7,'(a3)') txt
      if (txt.eq.'END') goto 99
      read(7,*)
      read(7,*) n
      read(7,*)
      read(7,*) (diag(k), k=1,n)
      read(7,*)
      read(7,*) (superd(k), k=1,n-1)
      read(7,*)
      read(7,*) (subd(k), k=2,n)
      read(7,*)
      read(7,*) (rhs(k), k=1,n)
C     carry out solution
      call tridag(subd,diag,superd,rhs,u,n)
      write(*,*) 'The solution vector is:'
      write(*,'(1x,6f12.6)') (u(k), k=1,n)
C     test solution
      write(*,*) '(matrix)*(sol''n vector) should be:'
      write(*,'(1x,6f12.6)') (rhs(k), k=1,n)
      write(*,*) 'Actual result is:'
      do 11 k=1,n
        if (k.eq.1) then
          rhs(k)=diag(1)*u(1) + superd(1)*u(2)
        else if (k.eq.n) then
          rhs(k)=subd(n)*u(n-1) + diag(n)*u(n)
        else
          rhs(k)=subd(k)*u(k-1) + diag(k)*u(k)
     *         + superd(k)*u(k+1)
        endif
11    continue
      write(*,'(1x,6f12.6)') (rhs(k), k=1,n)
      write(*,*) '***********************************'
      write(*,*) 'Press RETURN for next problem:'
      read(*,*)
      goto 10
99    close(7)
      END

NR-Functions/Numerical Recipes- Example & Functions/Examples/xtridag.for


!**********************************************************************************************************************************
! LICENSING
! Copyright (C) 2015-2016  National Renewable Energy Laboratory
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
!     http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!**********************************************************************************************************************************
MODULE BeamDyn_Subs

   USE BeamDyn_Types
   !USE NWTC_LAPACK

   IMPLICIT NONE

   INTEGER, PARAMETER :: BDKi = R8Ki
   INTEGER, PARAMETER :: FLAG_R1R2   = 0 !<   BD_CrvCompose flag = 0: R(rr) = R    (pp) R    (qq)
   INTEGER, PARAMETER :: FLAG_R1TR2  = 1 !<   BD_CrvCompose flag = 1: R(rr) = R(T) (pp) R    (qq)
   INTEGER, PARAMETER :: FLAG_R1R2T  = 2 !<   BD_CrvCompose flag = 2: R(rr) = R    (pp) R(T) (qq)
   INTEGER, PARAMETER :: FLAG_R1TR2T = 3 !<   BD_CrvCompose flag = 3: R(rr) = R(T) (pp) R(T) (qq)

   INTEGER, PARAMETER :: GAUSS_QUADRATURE = 1 !<   p%quadrature method: gaussian quadrature
   INTEGER, PARAMETER :: TRAP_QUADRATURE  = 2 !<   p%quadrature method: trapeziodal quadrature

CONTAINS

!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine determines the (N+1) Gauss-Lobatto-Legendre points x and weights w
!!
!! For details, see
!! \@book{Deville-etal:2002,
!!  author =    {M. O. Deville and P. F. Fischer and E. H. Mund},
!!  title =     {High-Order Methods for Incompressible Fluid Flow},
!!  publisher = {Cambridge University Press},
!!  address = {Cambridge},
!!  year =      2002
!!}
SUBROUTINE BD_GenerateGLL(N1, GLL_nodes, ErrStat, ErrMsg)

   ! input variables

   INTEGER(IntKi),          INTENT(IN   ) :: N1             !< 1 + the order of spectral element, or equivalently p%node_elem
   REAL(BDKi), ALLOCATABLE, INTENT(  OUT) :: GLL_nodes(:)   !< location of GLL nodes
   INTEGER(IntKi),          INTENT(  OUT) :: ErrStat        !< Error status of the operation
   CHARACTER(*),            INTENT(  OUT) :: ErrMsg         !< Error message if ErrStat /= ErrID_None

   REAL(BDKi)      , PARAMETER            :: tol   = 10.0_BDKi*EPSILON(tol) / 2.0_BDKi   ! tolerance for newton-raphson solve (ignores 1 significant digit)
   INTEGER(IntKi)  , PARAMETER            :: maxit = 1000                                ! maximum allowable iterations in newton-raphson solve

   REAL(BDKi)                             :: x_it        ! current NR-iteration value
   REAL(BDKi)                             :: x_old       ! last NR-iteration value
   REAL(BDKi)                             :: dleg(N1)    ! legendre polynomial
   INTEGER(IntKi)                         :: N           ! Order of spectral element
   INTEGER(IntKi)                         :: i           ! do-loop counter
   INTEGER(IntKi)                         :: j           ! do-loop counter
   INTEGER(IntKi)                         :: k           ! do-loop counter

   INTEGER(IntKi)                         :: ErrStat2
   CHARACTER(ErrMsgLen)                   :: ErrMsg2
   character(*), parameter                :: RoutineName = 'BD_GenerateGLL'

   ErrStat = ErrID_None
   ErrMsg  = ""

   N = N1 - 1

   CALL AllocAry(GLL_nodes,N1,'GLL points array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
  !CALL AllocAry(GLL_weights,N1,'GLL weight array',ErrStat2,ErrMsg2); CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
      if (ErrStat >= AbortErrLev) return

   !bjj: these are getting overwritten later....
   ! enter known endpoints  [-1.0, 1.0]
   GLL_nodes(1) = -1.0_BDKi
   GLL_nodes(N1) = 1.0_BDKi

   DO i = 1, N1

      x_it = -COS(pi_D * REAL(i-1,BDKi) / N) ! initial guess - chebyshev points
      DO j = 1, maxit
         x_old = x_it
         dleg(1) = 1.0_BDKi
         dleg(2) = x_it
         DO k = 2,N
            dleg(k+1) = (  REAL(2*k - 1,BDKi) * dleg(k) * x_it &
                         - REAL(  k - 1,BDKi) * dleg(k-1) ) / REAL(k,BDKi)
         ENDDO
         x_it = x_it - ( x_it * dleg(N1) - dleg(N) ) / &
                       (REAL(N1,BDKi) * dleg(N1) )
         IF (ABS(x_it - x_old) .lt. tol) EXIT
      ENDDO

      GLL_nodes(i) = x_it
     !GLL_weights(i) = 2.0_BDKi / (REAL(N * N1, BDKi) * dleg(N1)**2 )

   ENDDO

END SUBROUTINE BD_GenerateGLL
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes the rotation tensor (RT=\f$ \phi \f$ )
!! given Wiener-Milenkovic rotation parameters, \f$ \vec{c} \f$,
!! where \f$ \vec{c}=4\tan\left(\frac{\phi}{4}\right)\bar{n} \f$.
SUBROUTINE BD_CrvMatrixR(cc,Rr)

   REAL(BdKi),    INTENT(IN   ):: cc(3)         !< \f$ \vec{c}\f$
   REAL(BdKi),    INTENT(  OUT):: Rr(3,3)       !< returned rotation tensor (transpose of DCM matrix)

   REAL(BDKi)                  :: c0
   REAL(BDKi)                  :: c1
   REAL(BDKi)                  :: c2
   REAL(BDKi)                  :: c3
   REAL(BDKi)                  :: tr0

   !bjj: these are 1/4 the values described in the BeamDyn manual.
   c1  = cc(1)/4.0_BDKi
   c2  = cc(2)/4.0_BDKi
   c3  = cc(3)/4.0_BDKi
   c0  = 0.5_BDKi * (1.0_BDKi-c1*c1-c2*c2-c3*c3)

   tr0 = 1.0_BDKi - c0
   tr0 = 2.0_BDKi/(tr0*tr0)

   !bjj: this is the equation from the BD manual, however it doesn't seem to equate to what is calculated here...
   !> \f{equation}{
   !!  \tens{R} (\vec{c}) = \frac{1}{(4-c_0)^2}
   !! \begin{bmatrix}
   !! c_0^2 + c_1^2 - c_2^2 - c_3^2 & 2(c_1 c_2 - c_0 c_3) & 2(c_1 c_3 + c_0 c_2) \\
   !! 2(c_1 c_2 + c_0 c_3) & c_0^2 - c_1^2 + c_2^2 - c_3^2 & 2(c_2 c_3 - c_0 c_1) \\
   !! 2(c_1 c_3 - c_0 c_2)  & 2(c_2 c_3 + c_0 c_1) & c_0^2 - c_1^2 - c_2^2 + c_3^2 \\
   !! \end{bmatrix}
   !! }\f

   Rr(1,1) = tr0*(c1*c1 + c0*c0) - 1.0_BDKi
   Rr(2,1) = tr0*(c1*c2 + c0*c3)
   Rr(3,1) = tr0*(c1*c3 - c0*c2)

   Rr(1,2) = tr0*(c1*c2 - c0*c3)
   Rr(2,2) = tr0*(c2*c2 + c0*c0) - 1.0_BDKi
   Rr(3,2) = tr0*(c2*c3 + c0*c1)

   Rr(1,3) = tr0*(c1*c3 + c0*c2)
   Rr(2,3) = tr0*(c2*c3 - c0*c1)
   Rr(3,3) = tr0*(c3*c3 + c0*c0) - 1.0_BDKi

END SUBROUTINE BD_CrvMatrixR
!-----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE BD_CrvMatrixH(cc,Hh)

   REAL(BDKi),INTENT(IN) ::cc(3)
   REAL(BDKi),INTENT(OUT)::Hh(3,3)

   REAL(BDKi):: cf1,cf2,cf3,cq,ocq,aa,cb0,cb1,cb2,cb3

   cf1 = cc(1)/4.0_BDKi
   cf2 = cc(2)/4.0_BDKi
   cf3 = cc(3)/4.0_BDKi
   cq  = cf1 * cf1 + cf2 * cf2 + cf3 * cf3
   ocq = 1.0_BDKi + cq
   aa  = 2.0_BDKi * ocq * ocq
   cb0 = 2.0_BDKi * (1.0_BDKi - cq) / aa
   cb1 = cc(1)/aa
   cb2 = cc(2)/aa
   cb3 = cc(3)/aa

   Hh(1,1) = cb1 * cf1 + cb0
   Hh(2,1) = cb2 * cf1 + cb3
   Hh(3,1) = cb3 * cf1 - cb2

   Hh(1,2) = cb1 * cf2 - cb3
   Hh(2,2) = cb2 * cf2 + cb0
   Hh(3,2) = cb3 * cf2 + cb1

   Hh(1,3) = cb1 * cf3 + cb2
   Hh(2,3) = cb2 * cf3 - cb1
   Hh(3,3) = cb3 * cf3 + cb0

END SUBROUTINE BD_CrvMatrixH
!-----------------------------------------------------------------------------------------------------------------------------------
!>   This subroutine composes two Wiener-Milenkovic parameters pp and qq to find the resulting parameter rr
!!   This method is detailed in the paper: Bauchau, O.A., 2008, "Interpolation of finite rotations in flexible
!!   multi-body dynamics simulations", IMechE, Equation (9).
!!   see http://www.dymoresolutions.com/publications/BauchauEppleHeo08.pdf \n
!!   FLAG_R1R2:   flag = 0: R(rr) = R    (pp) R    (qq) \n
!!   FLAG_R1TR2:  flag = 1: R(rr) = R(T) (pp) R    (qq) \n
!!   FLAG_R1R2T:  flag = 2: R(rr) = R    (pp) R(T) (qq) \n
!!   FLAG_R1TR2T: flag = 3: R(rr) = R(T) (pp) R(T) (qq)
SUBROUTINE BD_CrvCompose( rr, pp, qq, flag)

   REAL(BDKi),    INTENT(IN   ):: pp(3)     !< Input rotation 1
   REAL(BDKi),    INTENT(IN   ):: qq(3)     !< Input rotation 2
   INTEGER       ,INTENT(IN   ):: flag      !< Option flag
   REAL(BDKi),    INTENT(  OUT):: rr(3)     !< Composed rotation

   REAL(BDKi)                  :: pp0
   REAL(BDKi)                  :: pp1
   REAL(BDKi)                  :: pp2
   REAL(BDKi)                  :: pp3
   REAL(BDKi)                  :: qq0
   REAL(BDKi)                  :: qq1
   REAL(BDKi)                  :: qq2
   REAL(BDKi)                  :: qq3
   REAL(BDKi)                  :: tr1
   REAL(BDKi)                  :: tr2
   REAL(BDKi)                  :: dd1
   REAL(BDKi)                  :: dd2

   IF(flag==FLAG_R1TR2 .OR. flag==FLAG_R1TR2T) THEN ! "transpose" (negative) of first rotation parameter
       pp1 = -pp(1)
       pp2 = -pp(2)
       pp3 = -pp(3)
   ELSE
       pp1 = pp(1)
       pp2 = pp(2)
       pp3 = pp(3)
   ENDIF
   pp0 = 2.0_BDKi - (pp1 * pp1 + pp2 * pp2 + pp3 * pp3) / 8.0_BDKi

   IF(flag==FLAG_R1R2T .OR. flag==FLAG_R1TR2T) THEN ! "transpose" (negative) of second rotation parameter
       qq1 = -qq(1)
       qq2 = -qq(2)
       qq3 = -qq(3)
   ELSE
       qq1 = qq(1)
       qq2 = qq(2)
       qq3 = qq(3)
   ENDIF
   qq0 = 2.0_BDKi - (qq1 * qq1 + qq2 * qq2 + qq3 * qq3)/8.0_BDKi

   tr1 = (4.0_BDKi - pp0) * (4.0_BDKi - qq0)
   tr2 = pp0 * qq0 - pp1 * qq1 - pp2 * qq2 - pp3 * qq3
   dd1 = tr1 + tr2
   dd2 = tr1 - tr2

   IF(dd1>dd2) THEN
       tr1 = 4.0_BDKi / dd1
   ELSE
       tr1 = -4.0_BDKi / dd2
   ENDIF

   rr(1) = tr1 * (pp1 * qq0 + pp0 * qq1 - pp3 * qq2 + pp2 * qq3)
   rr(2) = tr1 * (pp2 * qq0 + pp3 * qq1 + pp0 * qq2 - pp1 * qq3)
   rr(3) = tr1 * (pp3 * qq0 - pp2 * qq1 + pp1 * qq2 + pp0 * qq3)

END SUBROUTINE BD_CrvCompose
!-----------------------------------------------------------------------------------------------------------------------------------
!> This subroutine computes the CRV parameters given
!! the rotation matrix
SUBROUTINE BD_CrvExtractCrv(Rr,cc)

   REAL(BDKi),    INTENT(IN   ):: Rr(3,3)       !< Rotation Matrix
   REAL(BDKi),    INTENT(  OUT):: cc(3)         !< Crv paramteres

   !Local variables
   REAL(BDKi)                  :: pivot
   REAL(BDKi)                  :: sm0
   REAL(BDKi)                  :: sm1
   REAL(BDKi)                  :: sm2
   REAL(BDKi)                  :: sm3
   REAL(BDKi)                  :: em
   REAL(BDKi)                  :: temp

   pivot = Rr(1,1) + Rr(2,2) + Rr(3,3)

   IF (Rr(3,3) .GT. pivot) THEN
      !pivot = Rr(3,3)
      !ipivot = 3
      sm0  = Rr(2,1) - Rr(1,2)
      sm1  = Rr(1,3) + Rr(3,1)
      sm2  = Rr(2,3) + Rr(3,2)
      sm3  = 1.0_BDKi - Rr(1,1) - Rr(2,2) + Rr(3,3)
      temp = SIGN( 2.0_BDKi*SQRT(ABS(sm3)), sm0 )
   ELSEIF (Rr(2,2) .GT. pivot) THEN
      !pivot = Rr(2,2)
      !ipivot = 2
      sm0  = Rr(1,3) - Rr(3,1)
      sm1  = Rr(1,2) + Rr(2,1)
      sm2  = 1.0_BDKi - Rr(1,1) + Rr(2,2) - Rr(3,3)
      sm3  = Rr(2,3) + Rr(3,2)
      temp = SIGN( 2.0_BDKi*SQRT(ABS(sm2)), sm0 )
   ELSEIF (Rr(1,1) .GT. pivot) THEN
      !pivot = Rr(1,1)
      !ipivot = 1
      sm0  = Rr(3,2) - Rr(2,3)
      sm1  = 1.0_BDKi + Rr(1,1) - Rr(2,2) - Rr(3,3)
      sm2  = Rr(1,2) + Rr(2,1)
      sm3  = Rr(1,3) + Rr(3,1)
      temp = SIGN( 2.0_BDKi*SQRT(ABS(sm1)), sm0 )
   ELSE
      !ipivot = 0
      sm0  = 1.0_BDKi + Rr(1,1) + Rr(2,2) + Rr(3,3)
      sm1  = Rr(3,2) - Rr(2,3)
      sm2  = Rr(1,3) - Rr(3,1)
      sm3  = Rr(2,1) - Rr(1,2)
      temp = SIGN( 2.0_BDKi*SQRT(ABS(sm0)), sm0 )
   ENDIF

   em = sm0 + temp
   em = 4.0_BDKi/em
   cc(1) = em*sm1
   cc(2) = em*sm2
   cc(3) = em*sm3

END SUBROUTINE BD_CrvExtractCrv
!------------------------------------------------------------------------------
!> This subroutine generates n-point gauss-legendre quadrature points and weights
!!
!! Subroutine is based on well-known formulas for generating Legendre polynomials, the
!! roots of which are the Gauss-Legendre quadrature points.  Also used are well-known
!! expressions for the quadrature weights associated with the GL quadrature points.
!! The basic idea of the logic is to use the roots of the Chebyshev polynomial as
!! an initial guess for the roots of the Legendre polynomial, and to then use Newton
!! iteration to find the "exact" roots.
SUBROUTINE BD_GaussPointWeight(n, x, w, ErrStat, ErrMsg)

   INTEGER(IntKi),INTENT(IN   ):: n       !< Number of Gauss point
   REAL(BDKi),    INTENT(  OUT):: x(n)    !< Gauss point location
   REAL(BDKi),    INTENT(  OUT):: w(n)    !< Gauss point weight
   INTEGER(IntKi),INTENT(  OUT):: ErrStat !< Error status of the operation
   CHARACTER(*),  INTENT(  OUT):: ErrMsg  !< Error message if ErrStat /=

   ! local variables

   REAL(BDKi),  PARAMETER:: eps = 1000.0_BDKi*EPSILON(eps) / 2.0_BDKi   ! tolerance for newton-raphson solve (ignores 3 significant digits)

   INTEGER(IntKi) :: n_half

   REAL(BDKi)     :: n_real  ! real version of loop index
   INTEGER(IntKi) :: i       ! loop index
   INTEGER(IntKi) :: j       ! loop index
   REAL(BDKi)     :: j_real  ! real version of loop index

   INTEGER(IntKi) :: newton     ! newton-iteration index
   INTEGER(IntKi), parameter :: newton_max = 10 ! maximum number of newton iterations; should not need many!

   REAL(BDKi)     :: p1  ! legendre polynomial j-2
   REAL(BDKi)     :: p2  ! legendre polynomial j-1
   REAL(BDKi)     :: p3  ! legendre polynomial j

   REAL(BDKi)     :: dp3  ! derivative of legendre polynomial j

   if (n .lt. 2) then
      ErrStat = ErrID_Fatal
      ErrMsg  = 'BD_GaussPointWeight: invalid value of n.'
   else
      ErrStat = ErrID_None
      ErrMsg  = ''
   endif

   n_real = REAL(n,BDKi)

   n_half = (n+1) / 2  ! integer for "half" the points: e.g., n=5 => n_half=3, n=4 => n_half = 2

   ! loop through each of the "half" n points
   do i = 1, n_half

      ! Intial guess for ith root; based on ith root of Chebyshev polynomial
      x(i) = - cos( REAL( 2*i - 1, BDKi)  * pi_D / (2.0_BDKi * n_real) )

      ! initialize newton iteration counter
      newton = 0

      p3 = 1. ! some intial big number;  logic will always take at least one newton iteration
      do while (abs(p3) .gt. eps .and. newton .le. newton_max)

         newton = newton + 1  ! keep track of number of newton iterations

         ! build the legendre polynomial and its derivative evaluated at current root guess
         p1 = 1.0_BDKi  ! zeroth order legendre polynomial
         p2 = x(i)      ! first-order legendre polynomial

         do j = 2, n

            j_real = REAL(j, BDKi)

            ! recurrence relation for legendre polynomial
            p3 = ( REAL(2*j - 1, BDKi) * x(i) * p2 - REAL(j - 1, BDKi) * p1 ) / j_real

            ! derivative of legendre polynomial; needed for newton iteration
            dp3 = j_real * ( - x(i) * p3 + p2 ) / ( 1.0_BDKi - x(i)*x(i) )

            ! save values for next iteration
            p1 = p2
            p2 = p3

         enddo

         ! newton iteration: xnew = xold - P_n(xold) / P_n'(xold)
         x(i) = x(i) - p3 / dp3

      enddo

      if (abs(x(i)) .lt. eps) x(i) = 0.0_BDKi

      ! fill in the other "side" of the array
      x(n-i+1) = - x(i)

      ! calculate weight for this Gauss point location
      w(i) = 2.0_BDKi / ( ( 1.0_BDKi - x(i)*x(i) ) * dp3 * dp3 )
      w(n-i+1) = w(i)

   enddo

END SUBROUTINE BD_GaussPointWeight
!-----------------------------------------------------------------------------------------------------------------------------------
! This subroutine computes trapezoidal quadrature points and weights, p%GL and p%GLw
SUBROUTINE BD_TrapezoidalPointWeight(p, InputFileData)

   TYPE(BD_ParameterType),INTENT(INOUT):: p              !< BeamDyn parameters
   TYPE(BD_InputFile),    INTENT(IN   ):: InputFileData  !< BeamDyn input-file data

   ! local variables
   REAL(BDKi)                 :: denom ! denominator for quadrature weight computations

   INTEGER(IntKi)             :: indx, temp_id0, temp_id1  ! temporary index into GL array
   INTEGER(IntKi), parameter  :: id0 = 1
   INTEGER(IntKi)             :: id1, j

      ! compute the trapezoidal quadrature points, p%GL and scale to range [-1,1]:
      !  If there is refinement, this will add new points between the specified ones. If p%refine == 1, can skip this.
   p%GL(1) = InputFileData%InpBl%station_eta(id0)
   DO j = 2,p%ngp
      indx =  id0+(j-2_IntKi)/p%refine       ! forced rounding to integer???  --> (J-2)/p%refine may not be integer.
      p%GL(j) =  InputFileData%InpBl%station_eta(indx) + &
               ((InputFileData%InpBl%station_eta(indx+1) - InputFileData%InpBl%station_eta(indx))/p%refine) * (MOD(j-2,p%refine) + 1)
   ENDDO
   p%GL = 2.0_BDKi*p%GL - 1.0_BDKi     ! rescale to range [-1,1]

      ! compute the trapezoidal quadrature weights, p%GLw:
   id1 = InputFileData%InpBl%station_total
   temp_id0 = (id0 - 1)*p%refine + 1            ! Starting index in GL --> always going to be 1
   temp_id1 = (id1 - 1)*p%refine + 1            ! ending index in GL --> will be  size(p%GL)
   denom = p%GL(temp_id1) - p%GL(temp_id0)      ! This is the range of GL --> for single member, is always == 2

   p%GLw(1)     =  (p%GL(temp_id0+1) - p%GL(temp_id0    ))/denom
   DO j=2,p%ngp-1
      p%GLw(j)  =  (p%GL(temp_id0+j) - p%GL(temp_id0+j-2))/denom
   ENDDO
   p%GLw(p%ngp) =  (p%GL(temp_id1  ) - p%GL(temp_id1-1  ))/denom

END SUBROUTINE BD_TrapezoidalPointWeight
!-----------------------------------------------------------------------------------------------------------------------------------
!adp:  This routine appears to be useless.
!SUBROUTINE BD_MotionTensor(RotTen,Pos,MotTen,flag)
!
!   REAL(BDKi),     INTENT(IN   ):: RotTen(3,3)
!   REAL(BDKi),     INTENT(IN   ):: Pos(3)
!   REAL(BDKi),     INTENT(  OUT):: MotTen(6,6)
!   INTEGER(IntKi), INTENT(IN   ):: flag            !< 0: Motion Tensor;
!                                                   !! 1: Inverse of Motion Tensor
!
!   MotTen = 0.0_BDKi
!   IF (flag .EQ. 0) THEN
!       MotTen(1:3,1:3) = RotTen(1:3,1:3)
!       MotTen(4:6,4:6) = RotTen(1:3,1:3)
!       MotTen(1:3,4:6) = MATMUL(SkewSymMat(Pos),RotTen)
!   ELSEIF(flag .EQ. 1) THEN
!       MotTen(1:3,1:3) = TRANSPOSE(RotTen(1:3,1:3))
!       MotTen(4:6,4:6) = TRANSPOSE(RotTen(1:3,1:3))
!       MotTen(1:3,4:6) = TRANSPOSE(MATMUL(SkewSymMat(Pos),RotTen))
!   ENDIF
!
!END SUBROUTINE BD_MotionTensor
!-----------------------------------------------------------------------------------------------------------------------------------
!> This routine calculates y%BldMotion%TranslationDisp, y%BldMotion%Orientation, y%BldMotion%TranslationVel, and
!! y%BldMotion%RotationVel, which depend only on states and parameters.
SUBROUTINE Set_BldMotion_NoAcc(p, x, y)

   TYPE(BD_ParameterType),       INTENT(IN   )  :: p           !< Parameters
   TYPE(BD_ContinuousStateType), INTENT(IN   )  :: x           !< Continuous states at t
   TYPE(BD_OutputType),          INTENT(INOUT)  :: y           !< Outputs computed at t (Input only so that mesh con-
                                                               !!   nectivity information does not have to be recalculated)
   INTEGER(IntKi)                               :: i
   INTEGER(IntKi)                               :: j
   INTEGER(IntKi)                               :: temp_id
   INTEGER(IntKi)                               :: temp_id2
   REAL(BDKi)                                   :: cc(3)
   REAL(BDKi)                                   :: cc0(3)
   REAL(BDKi)                                   :: temp_cc(3)
   REAL(BDKi)                                   :: temp_R(3,3)
   CHARACTER(*), PARAMETER                      :: RoutineName = 'Set_BldMotion_NoAcc'

   DO i=1,p%elem_total
      DO j=1,p%node_elem
         temp_id = (i-1)*(p%node_elem-1)+j       ! The last node of the first element is used as the first node in the second element.
         temp_id2= (i-1)*p%node_elem+j

         temp_cc = MATMUL(p%GlbRot,x%q(1:3,temp_id))
         y%BldMotion%TranslationDisp(1,temp_id2) = temp_cc(2)
         y%BldMotion%TranslationDisp(2,temp_id2) = temp_cc(3)
         y%BldMotion%TranslationDisp(3,temp_id2) = temp_cc(1)

         cc = MATMUL(p%GlbRot,x%q(4:6,temp_id))
         cc0 = p%uuN0(4:6,j,i)
         cc0 = MATMUL(p%GlbRot,cc0)
         CALL BD_CrvCompose(temp_cc,p%Glb_crv,cc0,FLAG_R1R2) ! temp_cc = p%Glb_crv composed with cc0
         CALL BD_CrvCompose(cc0,cc,temp_cc,FLAG_R1R2) ! cc0 = cc composed with temp_cc
         temp_cc(1) = cc0(2)
         temp_cc(2) = cc0(3)
         temp_cc(3) = cc0(1)
         CALL BD_CrvMatrixR(temp_cc,temp_R)  ! returns temp_R (the transpose of the DCM orientation matrix)
         y%BldMotion%Orientation(1:3,1:3,temp_id2) = TRANSPOSE(temp_R(1:3,1:3))

         temp_id = (i-1)*(p%node_elem-1)+j
         temp_cc = MATMUL(p%GlbRot,x%dqdt(1:3,temp_id))
         y%BldMotion%TranslationVel(1,temp_id2) = temp_cc(2)
         y%BldMotion%TranslationVel(2,temp_id2) = temp_cc(3)
         y%BldMotion%TranslationVel(3,temp_id2) = temp_cc(1)

         temp_cc(:) = MATMUL(p%GlbRot,x%dqdt(4:6,temp_id))
         y%BldMotion%RotationVel(1,temp_id2) = temp_cc(2)
         y%BldMotion%RotationVel(2,temp_id2) = temp_cc(3)
         y%BldMotion%RotationVel(3,temp_id2) = temp_cc(1)
      ENDDO
   ENDDO

END SUBROUTINE Set_BldMotion_NoAcc
!-----------------------------------------------------------------------------------------------------------------------------------
END MODULE BeamDyn_Subs


PROGRAM question2
    IMPLICIT NONE
    REAL :: a, b, c, d
    READ *, a, b, c, d
    PRINT *, a*c - b*d, " ", a*d + b*c
    STOP
END PROGRAM question2


      SUBROUTINE CAXPY(N,CA,CX,INCX,CY,INCY)
c Copyright (c) 1996 California Institute of Technology, Pasadena, CA.
c ALL RIGHTS RESERVED.
c Based on Government Sponsored Research NAS7-03001.
C>> 1994-11-11 CLL Typing all variables.
C>> 1987-10-19 CAXPY  Lawson  Initial code.
C
C     OVERWRITE COMPLEX CY WITH COMPLEX  CA*CX + CY.
C     FOR I = 0 TO N-1, REPLACE  CY(LY+I*INCY) WITH CA*CX(LX+I*INCX) +
C       CY(LY+I*INCY), WHERE LX = 1 IF INCX .GE. 0, ELSE LX = (-INCX)*N,
C       AND LY IS DEFINED IN A SIMILAR WAY USING INCY.
C
      integer i, n, incx, incy, kx, ky
      real canorm
      COMPLEX CX(*),CY(*),CA
C
      CANORM = ABS(REAL(CA)) + ABS(AIMAG(CA))
      IF(N.LE.0.OR.CANORM.EQ.0.E0) RETURN
      IF(INCX.EQ.INCY.AND.INCX.GT.0) GO TO 20
      KX = 1
      KY = 1
      IF(INCX.LT.0) KX = 1+(1-N)*INCX
      IF(INCY.LT.0) KY = 1+(1-N)*INCY
          DO 10 I = 1,N
          CY(KY) = CY(KY) + CA*CX(KX)
          KX = KX + INCX
          KY = KY + INCY
   10 CONTINUE
      RETURN
   20 CONTINUE
          DO 30 I=1,N*INCX,INCX
          CY(I) = CA*CX(I) + CY(I)
   30     CONTINUE
      RETURN
      END


  !! Merge all the local data
  CALL MergeMatrixLocalBlocks(this, LMAT)

  !! Compute The Local Contribution
  trace_value = 0
  CALL MatrixToTripletList(LMAT, TLIST)
  DO counter = 1, TLIST%CurrentSize
     IF (this%start_row + TLIST%data(counter)%index_row .EQ. &
          & this%start_column + TLIST%data(counter)%index_column) THEN
        trace_value = trace_value + &
             & REAL(TLIST%data(counter)%point_value, NTREAL)
     END IF
  END DO

  !! Sum Among Process Slice
  CALL MPI_Allreduce(MPI_IN_PLACE, trace_value, 1, MPINTREAL, &
       & MPI_SUM, this%process_grid%within_slice_comm, ierr)

  CALL DestructMatrix(LMAT)


!Print odd and even number from 1 to 10
program Odd_Even
    implicit none

    INTEGER :: i

    print *, "Even", " Odd"

    do i = 1, 99, 2
        print'(2I4)', i+1, i
    end do

end program Odd_Even


      subroutine msort(ar,n)
c        ________
c        Heapsort       ref: W.H.Press,'Numerical Recipes',p229,
c        ~~~~~~~~             Cambridge University Press 1989.
      integer ar(n),rra
      if(n.lt.2) return
      l=n/2+1
      ir=n
   10 continue
          if(l.gt.1) then
            l=l-1
            rra=ar(l)
          else
            rra=ar(ir)
            ar(ir)=ar(1)
            ir=ir-1
            if(ir.eq.1) then
              ar(1)=rra
              goto 1
            endif
          endif
          i=l
          j=l+l
   20     if(j.le.ir) then
            if(j.lt.ir) then
              if(ar(j).lt.ar(j+1)) j=j+1
            endif
            if(rra.lt.ar(j)) then
              ar(i)=ar(j)
              i=j
              j=j+j
            else
              j=ir+1
            endif
          goto 20
          endif
          ar(i)=rra
      goto 10
    1 continue
c     exclude equal elements
      ne=n
      i=2
   30 if(ar(i).eq.ar(i-1)) then
        do 32 j=i,ne
          ar(j-1)=ar(j)
   32   continue
        ne=ne-1
        goto 30
      else
        i=i+1
        if(i.gt.ne) then
          n=ne
          return
        else
          goto 30
        endif
      endif
      end
c
      real*8 function pselect(kk,arr,n)
      implicit none
      integer k,kk,n
      real*8 arr(n)
c     Returns the k-th smallest value in the array arr(1:n). The input
c     array willbe rearranged to have this value in location arr(k),
c     with all smaller elements moved to arr(1:k-1)(in arbutrary order)
c     and all larger elements in arr[k+1..n](also in arbitrary order).
      integer i,ir,j,l,mid
      real*8 a,temp
c
      k=min(max(1,kk),n)
      l=1
      ir=n
 1    if(ir-l.le.1)then
        if(ir-l.eq.1)then
          if(arr(ir).lt.arr(l))then
            temp=arr(l)
            arr(l)=arr(ir)
            arr(ir)=temp
          endif

        endif
        pselect=arr(k)
        return
      else
c       .not. (ir-l.le.1) => ir > l + 1
        mid=l+(ir-l)/2
        temp=arr(mid)
        arr(mid)=arr(l+1)
        arr(l+1)=temp
        if(arr(l+1).gt.arr(ir))then
          temp=arr(l+1)
          arr(l+1)=arr(ir)
          arr(ir)=temp
        endif
        if(arr(l).gt.arr(ir))then
          temp=arr(l)
          arr(l)=arr(ir)
          arr(ir)=temp
        endif
        if(arr(l+1).gt.arr(l))then
          temp=arr(l+1)
          arr(l+1)=arr(l)
          arr(l)=temp
        endif
        i=l+1
        j=ir
        a=arr(l)
 3      continue
        i=i+1
        if(arr(i).lt.a)go to 3
 4      continue
        j=j-1
        if(arr(j).gt.a)go to 4
        if(j.lt.i)go to 5
        temp=arr(i)
        arr(i)=arr(j)
        arr(j)=temp
        go to 3
 5      arr(l)=arr(j)
        arr(j)=a
        if(j.ge.k)ir=j-1
        if(j.le.k)l=i
      endif
      go to 1
      end


module test_utils
  use numeric_kinds, only: dp
  implicit none

contains

  subroutine write_vector(path_file, vector)
    !> Write vector to path_file
    character(len=*), intent(in) :: path_file
    real(dp), dimension(:) :: vector

    open(unit=314, file=path_file, status="REPLACE")
    write(314, *) vector
    close(314)

  end subroutine write_vector

  function cast_to_double(vector) result(rs)
    !> convert an array of int into double
    integer, dimension(:), intent(in) :: vector
    real(dp), dimension(size(vector))  :: rs
    integer :: i
    do i=1,size(vector)
       rs(i) = real(vector(i))
    end do

  end function cast_to_double

end module test_utils

program main
  use numeric_kinds, only: dp
  use davidson, only: generalized_eigensolver
  use lapack_wrapper, only: lapack_generalized_eigensolver
  use array_utils, only: norm, generate_diagonal_dominant
  use test_utils, only: write_vector, cast_to_double

  implicit none

  integer, parameter :: dim = 100
  real(dp), dimension(3) :: eigenvalues_DPR, eigenvalues_GJD
  real(dp), dimension(dim, 3) :: eigenvectors_DPR, eigenvectors_GJD
  real(dp), dimension(dim, dim) :: mtx
  real(dp), dimension(dim, dim) :: stx
  real(dp) :: test_norm_eigenvalues
  real(dp), dimension(dim) :: xs
  integer :: iter_i, j

  mtx = generate_diagonal_dominant(dim, 1d-3)
  stx = generate_diagonal_dominant(dim, 1d-3, 1d0)

  call generalized_eigensolver(mtx, eigenvalues_GJD, eigenvectors_GJD, 3, "GJD", 100, 1d-5, iter_i, 10, stx)
  print *, "GJD algorithm converged in: ", iter_i, " iterations!"
  call generalized_eigensolver(mtx, eigenvalues_DPR, eigenvectors_DPR, 3, "DPR", 100, 1d-5, iter_i, 10, stx)
  print *, "DPR algorithm converged in: ", iter_i, " iterations!"

  print *, "Test 1"
  test_norm_eigenvalues = norm(eigenvalues_GJD - eigenvalues_DPR)
  print *, "Check that eigenvalues norm computed by different methods are the same: ", test_norm_eigenvalues < 1e-6

  print *, "Test 2"
  print *, "Check that eigenvalue equation:  H V = l S V  holds!"
  print *, "DPR method:"
  do j=1,3
     xs = matmul(mtx, eigenvectors_DPR(:, j)) - (eigenvalues_DPR(j) * matmul(stx, eigenvectors_DPR(:, j)))
     print *, "eigenvalue ", j, ": ", eigenvalues_DPR(j), "||Error||: ", norm(xs)
  end do
  print *, "GJD method:"
  do j=1,3
     xs = matmul(mtx, eigenvectors_GJD(:, j)) - (eigenvalues_GJD(j) * matmul( stx, eigenvectors_GJD(:, j)))
     print *, "eigenvalue ", j, ": ",eigenvalues_GJD(j), "||Error||: ", norm(xs)
  end do

end program main


!*robodoc*u* tests/testACE
!  NAME
!    testACE
!  SYNOPSIS

!$Id: testACE.f90 526 2018-03-25 23:44:51Z mexas $

program testACE

!  PURPOSE
!    Test cgca_fwci/cgca_m2out - a debugging output routine.
!  DESCRIPTION
!    cgca_fwci/cgca_m2out collects coarray data from all images and
!    writes it out from image 1 in a formatted text file. A single
!    line contains the state of a single cells with its full local
!    global coordinates, i.e. its position in the coarray on this
!    image, and the image position in the coarray grid.
!    The data is dumped out as soon as the solidificaiton is finished.
!
!    Both grain and fracture layers are used and dumped.
!  NOTE
!    The program uses cgca_ins RND seed, to obtain a reproducible
!    model data. If this test is re-run on the same platform with
!    the same number of images, the results must be the same.
!  AUTHOR
!    Anton Shterenlikht
!  COPYRIGHT
!    See LICENSE
!  USES
!    cgca testaux
!  USED BY
!    Part of CGPACK test suite
!  SOURCE

use testaux

implicit none

logical( kind=ldef ), parameter :: yesdebug=.true., nodebug=.false.,   &
 periodicbc=.true.,  noperiodicbc=.false.

real, parameter :: gigabyte=real(2**30)

integer( kind=idef ) :: ir(3), img, nimgs,                             &
  ng,  & ! number of grains in the whole model
  c(3)   ! coarray dimensions

integer( kind=iarr ), allocatable :: space(:,:,:,:) [:,:,:]

integer( kind=ilrg ) :: icells, mcells

real( kind=rdef ) ::    &
 qual,                  & ! quality
 bsz0(3),               & ! the given "box" size
 bsz(3),                & ! updated "box" size
 dm,                    & ! mean grain size, linear dim, phys units
 lres,                  & ! linear resolution, cells per unit of length
 res                      ! resolutions, cells per grain
real( kind=rdef ), allocatable :: grt(:,:,:)[:,:,:]

logical( kind=ldef ) :: solid

!*********************************************************************72
! first executable statement

! physical dimensions of the box, assume mm
bsz0 = (/ 1.0, 1.0, 1.0 /)

! mean grain size, linear dimension, e.g. mean grain diameter, also mm
dm = 5.0e-1

! resolution
res = 1.0e5

! In this test set the number of images via the env var
! the code must be able to cope with any value >= 1.
   img = this_image()
 nimgs = num_images()

! do a check on image 1
if ( img .eq. 1 ) then

 ! print a banner
 call banner("ACE")

 ! print the parameter values
 call cgca_pdmp
 write (*,'(a,i0,a)') "running on ", nimgs, " images in a 3D grid"

end if

! want to sync here to make sure the banner is
! printed before the rest.
sync all

! each image calculates the coarray grid dimensions
call cgca_gdim( nimgs, ir, qual )

! calculate the resolution and the actual phys dimensions
! of the box
! subroutine cgca_cadim( bsz, res, dm, ir, c, lres, ng )
! c - coarray sizes
! ir - coarray grid sizes
bsz = bsz0
call cgca_cadim( bsz, res, dm, ir, c, lres, ng )

! total number of cells in a coarray
icells = int( c(1), kind=ilrg ) *    &
         int( c(2), kind=ilrg ) *    &
         int( c(3), kind=ilrg )

! total number of cells in the model
mcells = icells * int( nimgs, kind=ilrg )

if ( img .eq. 1 ) then
  write ( *, "(9(a,i0), 2(tr1, es9.2), 3(a, es9.2), a)" )              &
    "img: ", img  , " nimgs: ", nimgs, " (", c(1) ,                    &
    ","    , c(2) , ","       , c(3) , ")[", ir(1),                    &
    ","    , ir(2), ","       , ir(3), "] ", ng   ,                    &
    qual, lres,                                                        &
    " (", bsz(1), ",", bsz(2), ",", bsz(3), ")"
  write (*,*) "dataset sizes for ParaView", c*ir
  write (*,'(a,i0)') "Cells on each image: ", icells
  write (*,"(a, es10.2, a, i0)") "Total cells in the model (real): ",  &
    product( real(c) * real(ir) ), " (int): ", mcells
end if

! allocate space coarray with two layers, implicit SYNC ALL inside
call cgca_as(1, c(1), 1, c(2), 1, c(3), 1, ir(1), 1, ir(2), 1, 2, space)

! initialise the reproducible random number seed
call cgca_ins( yesdebug )

! Set initial values to all layers of space
space( :, :, :, cgca_state_type_grain ) = cgca_liquid_state
space( :, :, :, cgca_state_type_frac  ) = cgca_intact_state

! Allocate rotation tensors, implicit SYNC ALL inside
call cgca_art( 1, ng, 1, ir(1), 1, ir(2), 1, grt )

! Image 1 sets crack nuclei
if ( img .eq. 1 ) then

  ! set a single crack nucleus in the centre of the x3=max(x3) face
  space( c(1)/2, c(2)/2, c(3), cgca_state_type_frac )                  &
        [ ir(1)/2, ir(2)/2, ir(3) ] = cgca_clvg_state_100_edge
end if

! Set grain nuclei, SYNC ALL inside
call cgca_nr( space, ng, yesdebug )

! assign rotation tensors, SYNC ALL inside
call cgca_rt( grt )

! solidify, implicit SYNC ALL inside
!subroutine cgca_sld( coarray, periodicbc, iter, heartbeat, solid )
call cgca_sld( space, noperiodicbc, 0, 10, solid )

! Initiate grain boundaries. cgca_igb has no remote comms. Halo
! exchange is needed to update other images.
call cgca_igb( space )
sync all
call cgca_hxi( space )
sync all

! Smoothen the GB, several iterations.
! cgca_gbs has no remote comms.
call cgca_gbs( space )
sync all
call cgca_hxi( space )
sync all
call cgca_gbs( space )
sync all
call cgca_hxi( space )
sync all

if ( img .eq. 1 ) write (*,*) "dumping model to files"

  ! dump space arrays to files, only image 1 does it, all others
  ! wait at the barrier, hence sync needed
  call cgca_fwci( space, cgca_state_type_grain, "zg0.raw" )
  call cgca_fwci( space, cgca_state_type_grain, "zf0.raw" )

if ( img .eq. 1) write (*,*) "finished dumping model to files"

sync all

! deallocate all coaarrays, implicit sync all.
call cgca_ds( space )
call cgca_drt( grt )

if ( img .eq. 1 ) write (*,*) "Test ACE completed sucessfully"

end program testACE

!*roboend*