!***********************************************************
!*               Copyright (c) Wenbin Yu                   *
!*              All rights reserved                        *
!*          Mechanical and Aerospace Engineering           *
!*                                                         *
!*                Utah State University                    *
!***********************************************************

!***************************************************************
!*                                                             *
! This module contains information and calculation for an     *
!* element within a member
!* Outputs: ElemEqn,ElemJacobian                               *
!***************************************************************
!>This module contains information and calculation for an element within a member
MODULE Element

USE InternalData
USE PrescribedCondition
USE GlobalDataFun

IMPLICIT NONE

PRIVATE       ! So everything is private, except declared by PUBLIC

PUBLIC ElemEqn,ElemJacobian,ElemMass,ExtractElementProperties
PUBLIC load,exist_load,follower_load

! private data needed inside this module
!--------------------------------------------
REAL(DBL)::dL              !< length of the element
REAL(DBL)::Le              !< the ending arc length of the current element
REAL(DBL)::eCab(NDIM,NDIM) !< direction cosine matrix of the undeformed element
REAL(DBL)::eFlex(NSTRN,NSTRN) !<flexibility matrix of the elment
REAL(DBL)::eMass(NSTRN,NSTRN) !< inverse of the mass matrix of the element
TYPE(DistriLoad)::load        !< distributed load
LOGICAL::exist_load,follower_load !< flags to indicate whether distributed load exist and whether they are follower forces

REAL(DBL)::Ui(NDIM),theta(NDIM),Fi(NDIM),Mi(NDIM),e1GammaD(NDIM),kappa(NDIM),ePi(NDIM),Hi(NDIM),Vi(NDIM),OMEGAi(NDIM) ! e1GammaD=e1+GammaD
REAL(DBL)::ev_i(NDIM)    !< initial velocity of the mid point of the element
REAL(DBL)::eOmega_a(NDIM) !< initial angular velocity of the element

REAL(DBL)::eCT(NDIM,NDIM)         !< the transpose of the direction cosine matrix corresponding to elastic rotation
REAL(DBL)::eCTCAB(NDIM,NDIM)      !< eCT.Cab
REAL(DBL)::eCabhalfL(NDIM,NDIM)   !< eCab*dL/2
REAL(DBL)::eCTCabhalfL(NDIM,NDIM) !< eCTCab*dL/2

REAL(DBL)::UiDot(NDIM),ThetaDot(NDIM),CTCabPdot(NDIM),CTCabHdot(NDIM)

! data needed for aerodynamic forces
REAL(DBL)::Rho,Chord,X_cg,alpha,alphadot,hdot,aw,bw,U,hdotdot,alphadotdot,alpha_ac,beta_ac,beta
INTEGER  :: a_flag, g_flag
REAL(DBL)            :: lambda(NSTATES),lambdadot(NSTATES),P(NSTATES,2*NSTATES+2) ! induces-flow states
REAL(DBL)            :: Ident(NSTATES,NSTATES)
REAL(DBL)            :: lambda0 ! induces-flow states
REAL(DBL)            :: A(NSTATES,NSTATES),b(NSTATES),c(NSTATES)
REAL(DBL)            :: dir_moment(NDIM),dir_lift(NDIM),Jdir_moment(NDIM,NDIM),Jdir_lift(NDIM,NDIM)
REAL(DBL)            ::Jalpha_THETA(NDIM),Jalphadot_THETA(NDIM),Jhdot_THETA(NDIM),Jalphadot_PH(NDIM+NDIM),Jhdot_PH(NDIM+NDIM)
REAL(DBL)            ::Jalphadotdot_PHDOT(NDIM+NDIM),Jhdotdot_PHDOT(NDIM+NDIM)
REAL(DBL)            ::Jalphadotdot_PH(NDIM+NDIM),Jhdotdot_PH(NDIM+NDIM)
REAL(DBL)            ::Jlambda0_PHDOT(NDIM+NDIM),Jlambda0_LAMBDA(NSTATES)
REAL(DBL)            ::Jlift(3,18+NSTATES),Jmoment(3,18+NSTATES)
REAL(DBL)            ::eCaf(NDIM,NDIM),Wind(NDIM)

!=============================================

CONTAINS

!************************************************************
!*                                                          *
!* Evaluate nonlinear equations for each element, including *
!* the equation value due to x_0 and the term due to        *
!* distributed load. Note, it is put in the right hand side *
!*															*
!************************************************************
!> Compute the value of the Right Hand Side for a finite element
FUNCTION ElemEqn(ndof_el)

IMPLICIT NONE

INTEGER,INTENT(IN)  :: ndof_el !<#ioaero::ndof_el

REAL(DBL)::ElemEqn(ndof_el+NDOF_ND) ! the functional value for each element
REAL(DBL):: tmpR(NDIM),tmp,tmpN(NSTATES)

!initialisation
ElemEqn=0.D0

ElemEqn(1:3)=MATMUL(eCTCab,Fi)
ElemEqn(ndof_el+1:ndof_el+3)=-ElemEqn(1:3)

ElemEqn(4:6)= MATMUL(eCTCabhalfL,CrossProduct(e1GammaD,Fi))
tmpR=MATMUL(eCTCab,Mi)
ElemEqn(ndof_el+4:ndof_el+6)=-tmpR+ElemEqn(4:6)
ElemEqn(4:6)=tmpR+ElemEqn(4:6)

ElemEqn(7:9)=MATMUL(eCTCabhalfL,e1GammaD)-eCabhalfL(:,1)
ElemEqn(ndof_el+7:ndof_el+9)=Ui+ElemEqn(7:9)
ElemEqn(7:9)=-Ui+ElemEqn(7:9)

tmpR=MATMUL(eCabhalfL,kappa)
ElemEqn(10:12)=tmpR+CrossProduct(theta*0.5d0,tmpR)+MATMUL(OuterProduct(theta,theta),tmpR*0.25d0)
ElemEqn(ndof_el+10:ndof_el+12)=theta+ElemEqn(10:12)
ElemEqn(10:12)=-theta+ElemEqn(10:12)

IF(exist_load) THEN
	ElemEqn(1:6)=ElemEqn(1:6)+GetLoad(-1,dL,Le,eCT,load,follower_load)  ! add the contribution due to distributed load
	ElemEqn(ndof_el+1:ndof_el+6)=ElemEqn(ndof_el+1:ndof_el+6)+GetLoad(1,dL,Le,eCT,load,follower_load)  ! add the contribution due to distributed load
ENDIF

! for dynamic analysis
!------------------------------------------
IF(ndof_el>=18) THEN

	! modify the right hand side for dynamic analysis including
	! both steady state and initial conditions and time marching
	!--------------------------------------------------

	tmpR=CrossProduct(eOmega_a,MATMUL(eCTCabhalfL,ePi))
	IF(init_flag==2) tmpR=tmpR+two_divide_dt*MATMUL(eCTCabhalfL,ePi)
	ElemEqn(1:3)=ElemEqn(1:3)-tmpR
    ElemEqn(ndof_el+1:ndof_el+3)=ElemEqn(ndof_el+1:ndof_el+3)-tmpR

	tmpR=CrossProduct(eOmega_a,MATMUL(eCTCabhalfL,Hi))+MATMUL(eCTCabhalfL,CrossProduct(Vi,ePi))
    IF(init_flag==2) tmpR=tmpR+two_divide_dt*MATMUL(eCTCabhalfL,Hi)
	ElemEqn(4:6)=ElemEqn(4:6)-tmpR
    ElemEqn(ndof_el+4:ndof_el+6)=ElemEqn(ndof_el+4:ndof_el+6)-tmpR

	! the functions of fpi and fhi
	!------------------------------------------
	ElemEqn(NDOF_ND+1:NDOF_ND+3)=ev_i+CrossProduct(eOmega_a,Ui)-MATMUL(eCTCab,Vi)
    IF(init_flag==2) ElemEqn(NDOF_ND+1:NDOF_ND+3)=ElemEqn(NDOF_ND+1:NDOF_ND+3)+two_divide_dt*Ui
	ElemEqn(NDOF_ND+4:NDOF_ND+6)=MATMUL(TRANSPOSE(eCTCab),eOmega_a)-OMEGAi
ENDIF

IF(init_flag/=0) THEN !needed for the initial step and time marching
	ElemEqn(1:3)=ElemEqn(1:3)-dL*0.5D0*CTCabPdot
	ElemEqn(ndof_el+1:ndof_el+3)=ElemEqn(ndof_el+1:ndof_el+3)-dL*0.5D0*CTCabPdot

	ElemEqn(4:6)=ElemEqn(4:6)-dL*0.5D0*CTCabHdot
	ElemEqn(ndof_el+4:ndof_el+6)=ElemEqn(ndof_el+4:ndof_el+6)-dL*0.5D0*CTCabHdot

    ElemEqn(NDOF_ND+1:NDOF_ND+3)=ElemEqn(NDOF_ND+1:NDOF_ND+3)+UiDot
	ElemEqn(NDOF_ND+4:NDOF_ND+6)=ElemEqn(NDOF_ND+4:NDOF_ND+6)+ &
	&   MATMUL(TRANSPOSE(eCab),(ThetaDot-CrossProduct(theta*.5D0,ThetaDot))/(1+DOT_PRODUCT(theta,theta)*0.25D0) )
ENDIF

! quasi steady aerodynamics
IF (a_flag == 1) THEN
	!lift
	tmp = dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha)
	tmpR = tmp*dir_lift
	ElemEqn(1:3)=ElemEqn(1:3) + tmpR
	ElemEqn(ndof_el+1:ndof_el+3)=ElemEqn(ndof_el+1:ndof_el+3)+ tmpR
	!moment
	tmp = bw*(0.5+aw)*tmp -0.5*pi*Rho*U*bw**3*alphadot*0.5*dL
	tmpR = tmp*dir_moment
	ElemEqn(4:6)=ElemEqn(4:6)+ tmpR
	ElemEqn(ndof_el+4:ndof_el+6)=ElemEqn(ndof_el+4:ndof_el+6)+ tmpR

! quasi steady aerodynamics with added mass
ELSEIF (a_flag >= 2) THEN
	!case a_flag = 2 : added mass only
	IF (a_flag == 2) lambda0 = 0

	!lift
	tmp = 0.5*dL*PI*Rho*bw**2*(hdotdot-bw*aw*alphadotdot+U*alphadot) + &
	& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha-lambda0)
	tmpR = tmp*dir_lift
	ElemEqn(1:3)=ElemEqn(1:3)+ tmpR
	ElemEqn(ndof_el+1:ndof_el+3)=ElemEqn(ndof_el+1:ndof_el+3)+ tmpR
	!moment
	tmp = bw*(0.5+aw)*tmp-0.5*dL*PI*Rho*bw**3*(0.5*hdotdot+U*alphadot+bw*(0.125-0.5*aw)*alphadotdot)
	tmpR = tmp*dir_moment
	ElemEqn(4:6)=ElemEqn(4:6)+ tmpR
	ElemEqn(ndof_el+4:ndof_el+6)=ElemEqn(ndof_el+4:ndof_el+6)+ tmpR

	IF (a_flag ==3) THEN
		! induced-flow equation
		tmpN = -bw*MATMUL(A,lambdadot)-U*lambda+bw*(hdotdot+U*alphadot+bw*(0.5-aw)*alphadotdot)*c
		ElemEqn(ndof_el-NSTATES+1:ndof_el) = ElemEqn(ndof_el-NSTATES+1:ndof_el) + tmpN
	ENDIF

ENDIF

!with gravity
IF (g_flag /= 0) THEN
	tmpR = GRAV/eMass(3,3)*0.5*dL*(/0.D0,0.D0,1.D0/)
	ElemEqn(1:3)=ElemEqn(1:3)+ tmpR
	ElemEqn(ndof_el+1:ndof_el+3)=ElemEqn(ndof_el+1:ndof_el+3)+ tmpR

	tmpR = -GRAV/eMass(3,3)*X_cg*0.5*dL*(/0.D0,0.D0,1.D0/)
	ElemEqn(4:6)=ElemEqn(4:6)+ tmpR
	ElemEqn(ndof_el+4:ndof_el+6)=ElemEqn(ndof_el+4:ndof_el+6)+ tmpR
ENDIF

END FUNCTION ElemEqn
!***************************************************

!************************************************************
!*                                                          *
!>  Caculate the Jacobian matrix for each element
!*                                                          *
!*															*
!************************************************************
SUBROUTINE ElemJacobian(ndof_el,niter,elemJac)

IMPLICIT NONE

INTEGER,INTENT(IN)  :: ndof_el !< #ioaero::ndof_el
INTEGER,INTENT(IN)  :: niter !< #ioaero::niter

REAL(DBL),INTENT(OUT)::ElemJac(:,:) !< the coefficient matrix for each element

REAL(DBL)::ekttek(NDIM,NDIM,NDIM)   ! e_k.\theta^T+ \theta.e_k^T
REAL(DBL)::eCTtheta(NDIM,NDIM,NDIM),eCTtheta2(NDIM,NDIM,NDIM)  !derivatives of C^T w.r.t theta, eCTtheta(k,:,:)= d_C^T/d_thetak
REAL(DBL)::temp6(NDIM+NDIM),temp31(NDIM),temp32(NDIM),temp36(nDIM,NDIM+NDIM),temp333(NDIM,NDIM,NDIM) ! temporary arrays
REAL(DBL)::tempN6(NSTATES,6),tempNN(NSTATES,NSTATES),temp33(NDIM,NDIM),tempN(NSTATES)

REAL(DBL)::tilde_omega(NDIM,NDIM),tt4

INTEGER:: i

ElemJac=0.0D0

CALL CT_THETA(theta,eCT,ekttek,eCTtheta)  ! obtain derivatives of C^T w.r.t theta

! d_fu/d_theta
!--------------------
temp31=MATMUL(eCab,Fi)
ElemJac(1:3,4:6)=-MATMUL3(eCTtheta,temp31)   ! coefficient of theta of Fu
ElemJac(ndof_el+1:ndof_el+3,4:6)=-ElemJac(1:3,4:6)

! d_fu/d_F
!--------------------
ElemJac(1:3,7:9)=-eCTCab
ElemJac(ndof_el+1:ndof_el+3,7:9)=eCTCab

! d_fGammaD/d_theta
!--------------------
temp31=MATMUL(eCab,Mi)
temp32=-MATMUL(eCab,CrossProduct(e1GammaD,dL*0.5d0*Fi))
ElemJac(4:6,4:6)=MATMUL3(eCTtheta,temp32-temp31)  ! coefficient of theta of FGammaD1
ElemJac(ndof_el+4:ndof_el+6,4:6)=MATMUL3(eCTtheta,temp32+temp31)

! d_fGammaD/d_F
!--------------------
ElemJac(4:6,7:9)=-MATMUL(eCTCabhalfL,Tilde(e1GammaD)-MATMUL(Tilde(Fi),eFlex(1:3,1:3)))
ElemJac(ndof_el+4:ndof_el+6,7:9)=ElemJac(4:6,7:9)

! d_fGammaD/d_M
!--------------------
temp33=MATMUL(eCTCabhalfL,MATMUL(Tilde(Fi),eFlex(1:3,4:6)))
ElemJac(4:6,10:12)=temp33-eCTCab
ElemJac(ndof_el+4:ndof_el+6,10:12)=temp33+eCTCab

! d_fF/d_u
!--------------------
ElemJac(7:9,1:3)=I3
ElemJac(ndof_el+7:ndof_el+9,1:3)=-I3

! d_fF/d_theta
!--------------------
temp31=-MATMUL(eCabhalfL,e1GammaD)
ElemJac(7:9,4:6)=MATMUL3(eCTtheta,temp31)
ElemJac(ndof_el+7:ndof_el+9,4:6)=ElemJac(7:9,4:6)

! d_fF/d_F,d_fF/d_M
!--------------------
ElemJac(7:9, 7:12)=-MATMUL(eCTCabhalfL,eFlex(1:3,:))
ElemJac(ndof_el+7:ndof_el+9,7:12)=ElemJac(7:9, 7:12)

! d_fM/d_theta
!--------------------
DO i=1,NDIM
	temp333(i,:,:)=Tilde(I3(i,:))*0.5d0+ekttek(i,:,:)*0.25d0  ! \tilde{ek}/2+ekttek/4
ENDDO

temp33=MATMUL3(temp333,MATMUL(eCabhalfL,kappa))
ElemJac(10:12,4:6)=I3-temp33
ElemJac(ndof_el+10:ndof_el+12,4:6)=-I3-temp33

! d_fM/d_F,d_fM/d_M
!--------------------
ElemJac(10:12,7:12)=-MATMUL(MATMUL(I3+Tilde(theta)*.5d0+OuterProduct(theta,theta)*.25d0,eCabhalfL),&
                        & eFlex(4:6,:))
ElemJac(ndof_el+10:ndof_el+12,7:12)=ElemJac(10:12,7:12)

! add the contribution due to follower distributed load:\diff C^T/\theta.f
!----------------------------------------------------------
IF(follower_load)THEN
	ElemJac(1:6,4:6)=ElemJac(1:6,4:6)-GetLoadJ(-1,dL,Le,eCTtheta,load)
	ElemJac(ndof_el+1:ndof_el+6,4:6)=ElemJac(ndof_el+1:ndof_el+6,4:6)-GetLoadJ(1,dL,Le,eCTtheta,load)
ENDIF

! for dynamic analysis
!------------------------------------------
IF(ndof_el>=18) THEN
	tilde_omega=Tilde(eOmega_a)
	IF(init_flag==2) tilde_omega=tilde_omega+two_divide_dt*I3

    ! additional terms for fui due to P
	!--------------------------------------------
	! coefficient of theta of Fu due to ePi
    temp31=MATMUL(eCabhalfL,ePi)
	temp33=MATMUL(tilde_omega,MATMUL3(eCTtheta,temp31))
	ElemJac(1:3,4:6)=ElemJac(1:3,4:6)+temp33
	ElemJac(ndof_el+1:ndof_el+3,4:6)=ElemJac(ndof_el+1:ndof_el+3,4:6)+temp33

	! coefficient of p of Fu
	ElemJac(1:3,13:15)=MATMUL(tilde_omega,eCTCabhalfL)
	ElemJac(ndof_el+1:ndof_el+3,13:15)=ElemJac(1:3,13:15)

    ! additional terms for fGammaD due to P,H
	!--------------------------------------------
	! coefficient of theta of fGammaD due to P,H
    temp33=MATMUL3(eCTtheta,MATMUL(eCabHalfL,CrossProduct(Vi,ePi)))+ MATMUL(tilde_omega,MATMUL3(eCTtheta,MATMUL(eCabhalfL,Hi)))
	ElemJac(4:6,4:6)=ElemJac(4:6,4:6)+ temp33
    ElemJac(ndof_el+4:ndof_el+6,4:6)=ElemJac(ndof_el+4:ndof_el+6,4:6)+ temp33

	! coefficient of P,H of FGammaD
	ElemJac(4:6,13:15)=MATMUL(eCTCabhalfL,Tilde(Vi)-MATMUL(Tilde(ePi),eMass(1:3,1:3)))
	ElemJac(4:6,16:18)=MATMUL(tilde_omega,eCTCabhalfL)-MATMUL(eCTCabhalfL,MATMUL(Tilde(ePi),eMass(1:3,4:6)))
    ElemJac(ndof_el+4:ndof_el+6,13:18)=ElemJac(4:6,13:18)

	! the Jacobian of fPi
	!------------------------------------------
	ElemJac(NDOF_ND+1:NDOF_ND+3,1:3)=-tilde_omega
    ElemJac(NDOF_ND+1:NDOF_ND+3,4:6)=MATMUL3(eCTtheta,MATMUL(eCab,Vi))
    ElemJac(NDOF_ND+1:NDOF_ND+3,13:18)=MATMUL(eCTCab,eMass(1:3,:))

	! the Jacobian of fHi
	!------------------------------------------
	DO i=1,NDIM
		eCTtheta2(i,:,:)=TRANSPOSE(eCTtheta(i,:,:))
	ENDDO
	ElemJac(NDOF_ND+4:NDOF_ND+6,4:6)=-MATMUL(TRANSPOSE(eCab),MATMUL3(eCTtheta2,eOmega_a))
    ElemJac(NDOF_ND+4:NDOF_ND+6,13:18)=eMass(4:6,:)

	IF(init_flag==2) THEN
	   IF(niter/=1)THEN  ! nonlinear solution
			tt4=DOT_PRODUCT(theta,theta)*0.25D0

			DO i=1,NDIM
				temp333(i,:,:)=(Tilde(I3(i,:)*0.5d0*(1+tt4))+MATMUL(I3-Tilde(theta*0.5d0),ekttek(i,:,:)*0.25d0))/(1+tt4)**2
			ENDDO
			temp33=MATMUL3(temp333,ThetaDot)
			temp33=temp33-(I3-Tilde(theta*.5D0))/(1+tt4)*two_divide_dt
        ELSE  ! linear solution
		    temp33=-I3*two_divide_dt
		ENDIF

		ElemJac(NDOF_ND+4:NDOF_ND+6,4:6)=	ElemJac(NDOF_ND+4:NDOF_ND+6,4:6)+MATMUL(TRANSPOSE(eCab),temp33)
	ENDIF

ENDIF

! quasi-steady aerodynamics
IF(a_flag == 1) THEN
	!the Jacobian of the lift
	! coefficient theta of lift
	temp33 = OuterProduct(dir_lift,dL*PI*Rho*bw*U*(Jhdot_THETA+bw*(0.5-aw)*Jalphadot_THETA+U*Jalpha_THETA))+ &
	& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha)*Jdir_lift
	ElemJac(1:3,4:6)=ElemJac(1:3,4:6)-temp33
	ElemJac(ndof_el+1:ndof_el+3,4:6)=ElemJac(ndof_el+1:ndof_el+3,4:6)-temp33

	! coefficient PH of lift
	temp36 = OuterProduct(dir_lift,dL*PI*Rho*bw*U*(Jhdot_PH+bw*(0.5-aw)*Jalphadot_PH))
	ElemJac(1:3,13:18)=ElemJac(1:3,13:18)-temp36
	ElemJac(ndof_el+1:ndof_el+3,13:18)=ElemJac(ndof_el+1:ndof_el+3,13:18)-temp36

	!the Jacobian of the moment
	! coefficient theta of moment
	temp31 = PI*Rho*U*(aw + 0.5)*bw**2*dL*(U*Jalpha_THETA + Jhdot_THETA) + &
	& (PI*Rho*U*(aw + 0.5)*(-aw + 0.5)*bw**3*dL - 0.25*PI*Rho*U*bw**3*dL)*Jalphadot_THETA
	temp33 = OuterProduct(dir_moment,temp31)+(U*alpha + hdot)*PI*Rho*U*(aw + 0.5)*bw**2*dL + (PI*Rho*U*(aw + 0.5)*(-aw &
	& + 0.5)*bw**3*dL - 0.25*PI*Rho*U*bw**3*dL)*alphadot*Jdir_moment
	ElemJac(4:6,4:6)=ElemJac(4:6,4:6)-temp33
	ElemJac(ndof_el+4:ndof_el+6,4:6)=ElemJac(ndof_el+4:ndof_el+6,4:6)-temp33
	! coefficient PH of moment
	temp36 = OuterProduct(dir_moment,PI*Rho*U*(aw + 0.5)*bw**2*dL*Jhdot_PH + (PI*Rho*U*(aw + 0.5)*(-aw &
	& + 0.5)*bw**3*dL-0.25*PI*Rho*U*bw**3*dL)*Jalphadot_PH)
	ElemJac(4:6,13:18)=ElemJac(4:6,13:18)-temp36
	ElemJac(ndof_el+4:ndof_el+6,13:18)=ElemJac(ndof_el+4:ndof_el+6,13:18)-temp36
ENDIF

!~ ! quasi steady aerodynamic with added mass and Peters

IF(a_flag >= 2) THEN
	!the Jacobian of lift
	!-------------------------
!~ 		lift = 0.5*dL*PI*Rho*bw**2*(hdotdot-bw*aw*alphadotdot+U*alphadot) + &
!~ 		& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha-lambda0)
	Jlift = 0.D0
	! coefficient theta
	temp31= dL*PI*Rho*bw*U*(U*Jalpha_THETA)
	Jlift(:,4:6) = OuterProduct(dir_lift,temp31)
	!coefficient PH
	temp6 = 0.5*dL*PI*Rho*bw**2*(Jhdotdot_PH-bw*aw*Jalphadotdot_PH+U*Jalphadot_PH) + &
		& dL*PI*Rho*bw*U*(Jhdot_PH+bw*(0.5-aw)*Jalphadot_PH)
	Jlift(:,13:18) = OuterProduct(dir_lift,temp6)
	IF (a_flag ==3) THEN
		!coefficient Lambda
		tempN = dL*PI*Rho*bw*U*(-Jlambda0_LAMBDA)
		Jlift(:,19:ndof_el) = OuterProduct(dir_lift,tempN)
	ENDIF

	!the Jacobian of moment
	!---------------------------------
!~ 		moment = bw*(0.5+aw)*(0.5*dL*PI*Rho*bw**2*(hdotdot-bw*aw*alphadotdot+U*alphadot) + &
!~ 		& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha-lambda0)) - &
!~ 		& 0.5*dL*PI*Rho*bw**3*(0.5*hdotdot+U*alphadot+bw*(0.125-0.5*aw)*alphadotdot)
	Jmoment = 0.D0
	! coefficent theta
	temp31 = bw*(0.5+aw)*(dL*PI*Rho*bw*U*(U*Jalpha_THETA))
	Jmoment(:,4:6) = OuterProduct(dir_moment,temp31)
	!coefficient PH
	temp6 = bw*(0.5+aw)*(0.5*dL*PI*Rho*bw**2*(Jhdotdot_PH-bw*aw*Jalphadotdot_PH+U*Jalphadot_PH) + &
		& dL*PI*Rho*bw*U*(Jhdot_PH+bw*(0.5-aw)*Jalphadot_PH)) - &
		& 0.5*dL*PI*Rho*bw**3*(0.5*Jhdotdot_PH+U*Jalphadot_PH+bw*(0.125-0.5*aw)*Jalphadotdot_PH)
	Jmoment(:,13:18) = OuterProduct(dir_moment,temp6)
	IF (a_flag ==3) THEN
		!coefficient Lambda
		tempN = bw*(0.5+aw)*(dL*PI*Rho*bw*U*(-Jlambda0_LAMBDA))
		Jmoment(:,19:ndof_el) = OuterProduct(dir_moment,tempN)
	ENDIF

	! Contribution of lift and moment to the Jacobian
	! sign - come from the -fi and -mi in the expression of fui and fPhii
	ElemJac(1:3,:) = ElemJac(1:3,:) - Jlift(:,1:ndof_el)
	ElemJac(ndof_el+1:ndof_el+3,:) = ElemJac(ndof_el+1:ndof_el+3,:) - Jlift(:,1:ndof_el)

	ElemJac(4:6,:) = ElemJac(4:6,:) - Jmoment(:,1:ndof_el)
	ElemJac(ndof_el+4:ndof_el+6,:) = ElemJac(ndof_el+4:ndof_el+6,:) - Jmoment(:,1:ndof_el)

	IF (a_flag == 3) THEN
		! The Jacobian of flambdai
		! coefficient PH
		temp6 = -bw*(Jhdotdot_PH+U*Jalphadot_PH+bw*(0.5-aw)*Jalphadotdot_PH)
		tempN6 = OuterProduct(c,temp6)
		ElemJac(ndof_el-NSTATES+1:ndof_el,13:18)=ElemJac(ndof_el-NSTATES+1:ndof_el,13:18)+tempN6
		! coefficient lambda
!~ 		tempNN = U*Ident
        IF (U<TOLERANCE) THEN ! avoiding the zero speed singularity and force the decoupling between structural and induced flow modes at very low speed
		    tempNN = 1e12*Ident
        ELSE
            tempNN = max(1.D0/max(1e-12,U),U)*Ident
        ENDIF

		IF(init_flag==2) 	tempNN = tempNN + two_divide_dt*bw*A
		ElemJac(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el)=ElemJac(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el)+tempNN
	ENDIF

ENDIF

IF(init_flag==1) THEN !needed for the initial step calculating the initial conditions
	ElemJac(:,1:6)=0.D0
	ElemJac(1:3,1:3)=dL*.5d0*I3
    ElemJac(4:6,4:6)=ElemJac(1:3,1:3)
    ElemJac(ndof_el+1:ndof_el+3,1:3)=ElemJac(1:3,1:3)
    ElemJac(ndof_el+4:ndof_el+6,4:6)=ElemJac(1:3,1:3)
ENDIF

END SUBROUTINE ElemJacobian
!***************************************************

!************************************************************
!*                                                          *
!> Caculate the mass matrix for each element
!*                                                          *
!*															*
!************************************************************
SUBROUTINE ElemMass(ndof_el,elemM)

IMPLICIT NONE

INTEGER,INTENT(IN)  :: ndof_el !< #ioaero::ndof_el

REAL(DBL),INTENT(OUT)::ElemM(:,:) !< the mass matrix for each element
REAL(DBL) tmp31(NDIM),tmp36(NDIM,NDIM+NDIM),tmp6(NDIM+NDIM)
REAL(DBL) tmpN6(NSTATES,6),tmpN3(NSTATES,3),tmpNN(NSTATES,NSTATES)

ElemM=0.0D0

! d_fu/d_theta_t
!--------------------
ElemM(1:3,4:6)=CT_THETA_T(theta,eCT,MATMUL(eCabhalfL,ePi))
ElemM(ndof_el+1:ndof_el+3,4:6)=ElemM(1:3,4:6)

! d_fu/d_P_t
!--------------------
ElemM(1:3,13:15)=eCTCabhalfL
ElemM(ndof_el+1:ndof_el+3,13:15)=eCTCabhalfL

! d_fGammaD/d_theta_t
!--------------------
ElemM(4:6,4:6)=CT_THETA_T(theta,eCT,MATMUL(eCabhalfL,Hi))
ElemM(ndof_el+4:ndof_el+6,4:6)=ElemM(4:6,4:6)

! d_fGammaD/d_H_t
!--------------------
ElemM(4:6,16:18)=eCTCabhalfL
ElemM(ndof_el+4:ndof_el+6,16:18)=eCTCabhalfL

! d_fPi/d_u_t
!------------------------------------------
ElemM(13:15,1:3)=-I3

! d_fHi/d_theta_t
!------------------------------------------
ElemM(16:18,4:6)=MATMUL(TRANSPOSE(eCab),(Tilde(theta*.5D0)-I3)/(1+DOT_PRODUCT(theta,theta)*0.25D0))

!~ IF(a_flag == 4) THEN
!~ 	! case a_flag = 2 : added mass only
!~ 	IF (a_flag >= 2) Jlambda0_PHDOT = 0.D0

!~ 	! d_fu/d_PHdot
!~ 	tmp6 = 0.5*dL*PI*Rho*bw**2*(Jhdotdot_PHDOT-bw*aw*Jalphadotdot_PHDOT)+&
!~ 	& dL*PI*Rho*U*bw*0*(Jhdotdot_PHDOT+bw*(0.5-aw)*Jalphadotdot_PHDOT-Jlambda0_PHDOT)
!~  	tmp36 = OuterProduct(dir_lift,tmp6)
!~ 	ElemM(1:3,13:18) = ElemM(1:3,13:18) - tmp36
!~ 	ElemM(ndof_el+1:ndof_el+3,13:18) = ElemM(1:3,13:18)

!~ 	! d_ftheta/d_PHdot
!~ 	tmp6 = bw*(0.5+aw)*tmp6 - 0.5*dL*PI*Rho*bw**3*(0.5*Jhdotdot_PHDOT+bw*(0.125-0.5*aw)*Jalphadotdot_PHDOT)
!~ 	tmp36 = OuterProduct(dir_moment,tmp6)
!~ 	ElemM(4:6,13:18) = ElemM(4:6,13:18) - tmp36
!~ 	ElemM(ndof_el+4:ndof_el+6,13:18) = ElemM(4:6,13:18)

!~ 	IF (a_flag == 3) THEN
!~ 		!d_flambda/d_THETAdot
!~ 		tmp31 = -bw*(U*dir_moment)
!~ 		tmpN3 = OuterProduct(c,tmp31)
!~ 		ElemM(ndof_el-NSTATES+1:ndof_el,4:6) = ElemM(ndof_el-NSTATES+1:ndof_el,4:6) + tmpN3

!~ 		!d_flambda/d_PHdot
!~ 		tmp6 = -bw*(Jhdotdot_PHDOT+bw*(0.5-aw)*Jalphadotdot_PHDOT)
!~ 		tmpN6 = OuterProduct(c,tmp6)
!~ 		ElemM(ndof_el-NSTATES+1:ndof_el,13:18) = ElemM(ndof_el-NSTATES+1:ndof_el,13:18) + tmpN6

!~ 		!d_flambda/d_lambdadot
!~ 		tmpNN = bw*A
!~ 		ElemM(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el) = ElemM(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el) + tmpNN
!~ 	ENDIF

!~ ENDIF

IF(a_flag >= 2) THEN
	!the Jacobian of lift
	!-------------------------
!~ 		lift = 0.5*dL*PI*Rho*bw**2*(hdotdot-bw*aw*alphadotdot+U*alphadot) + &
!~ 		& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha-lambda0)
	Jlift = 0.D0
	!coefficient PHDOT
	tmp6 = 0.5*dL*PI*Rho*bw**2*(Jhdotdot_PHDOT-bw*aw*Jalphadotdot_PHDOT)
	Jlift(:,13:18) = OuterProduct(dir_lift,tmp6)

	!the Jacobian of moment
	!---------------------------------
!~ 		moment = bw*(0.5+aw)*(0.5*dL*PI*Rho*bw**2*(hdotdot-bw*aw*alphadotdot+U*alphadot) + &
!~ 		& dL*PI*Rho*bw*U*(hdot+bw*(0.5-aw)*alphadot+U*alpha-lambda0)) - &
!~ 		& 0.5*dL*PI*Rho*bw**3*(0.5*hdotdot+U*alphadot+bw*(0.125-0.5*aw)*alphadotdot)
	Jmoment = 0.D0
	! coefficient PHDOT
	tmp6= bw*(0.5+aw)*(0.5*dL*PI*Rho*bw**2*(Jhdotdot_PHDOT-bw*aw*Jalphadotdot_PHDOT)) - &
	& 0.5*dL*PI*Rho*bw**3*(0.5*Jhdotdot_PHDOT+bw*(0.125-0.5*aw)*Jalphadotdot_PHDOT)
	Jmoment(:,13:18) = OuterProduct(dir_moment,tmp6)

	! Contribution of lift and moment to the Jacobian
	! sign - come from the -fi and -mi in the expression of fui and fPhii
	ElemM(1:3,:) = ElemM(1:3,:) - Jlift(:,1:ndof_el)
	ElemM(ndof_el+1:ndof_el+3,:) = ElemM(ndof_el+1:ndof_el+3,:) - Jlift(:,1:ndof_el)

	ElemM(4:6,:) = ElemM(4:6,:) - Jmoment(:,1:ndof_el)
	ElemM(ndof_el+4:ndof_el+6,:) = ElemM(ndof_el+4:ndof_el+6,:) - Jmoment(:,1:ndof_el)

	IF (a_flag ==3) THEN
		! The Jacobian of flambdai
!~ 			flambdai = bw*MATMUL(A,lambdadot)+U*lambda-bw*(hdotdot+U*alphadot+bw*(0.5-aw)*alphadotdot)*c
		! coefficient PHDOT
		tmp6 = -bw*(Jhdotdot_PHDOT+bw*(0.5-aw)*Jalphadotdot_PHDOT)
		tmpN6 = OuterProduct(c,tmp6)
		ElemM(ndof_el-NSTATES+1:ndof_el,13:18) = ElemM(ndof_el-NSTATES+1:ndof_el,13:18) + tmpN6
		! coefficient LAMBDADOT
		tmpNN = bw*A
		ElemM(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el) = ElemM(ndof_el-NSTATES+1:ndof_el,ndof_el-NSTATES+1:ndof_el) + tmpNN
	ENDIF

ENDIF

END SUBROUTINE ElemMass
!***************************************************

!************************************************************
!*                                                          *
!>  Extract element properties needed for element assembly
!************************************************************
SUBROUTINE  ExtractElementProperties(elem_no,memb_info_i,x_elem,v_root_a,omega_a,ndof_el,init_elem,aero_flag,grav_flag)

INTEGER,INTENT(IN)::elem_no !<Element index
TYPE (MemberInf),INTENT(IN)::memb_info_i !< the paramater of the members (see memberinf Type)

REAL(DBL),INTENT(IN)::x_elem(:) !< the 12 variables of the element u_i, \theta_i, F_i, M_i, for dynamic analysis, 6 more Pi, Hi. for initial step, x_elem contains, CTCabPdot, CTCabHdot, F_i, M_i, P_i, H_i. Finally for Peters aero the Ns induced flow states
REAL(DBL),INTENT(IN)::v_root_a(:) !< linear velocity of the root point in inertial frame
REAL(DBL),INTENT(IN)::omega_a(:) !< angular velocity of the root point in inertial frame

REAL(DBL),INTENT(INOUT)::init_elem(:,:) !< initial step: the 12 initial values of the element u_i,\f$ \theta_i, \dot{u}_i, \dot{theta}_i.\f$ Time marching: \f$2/dt ui+\dot{u}_i, 2/dt thetai+\dot{theta}_i, 2/dt CTCabP+\dot, 2/dt CTCabP+dot \f$
INTEGER,INTENT(IN)::ndof_el !<#ioaero::nedof_el
INTEGER,INTENT(IN)::aero_flag !< #ioaero::aero_flag
INTEGER,INTENT(IN)::grav_flag !< #ioaero::grav_flag

! coordinate system parameter
REAL(DBL)            :: xflow(NDIM),xB(NDIM),yB(NDIM),zB(NDIM)
! additionnal variables used for the aerodynamic model
REAL(DBL)            :: PHDotB(2*NDIM),ViDot(NDIM),OMEGAiDot(NDIM),init_PH(2*NDIM)

!----------------------------------------------------
! Extract sectional properties for this element
!----------------------------------------------------
eFlex=memb_info_i%mate(elem_no,1:NSTRN,:) ! element flexibility matrix
IF(ndof_el>=18) eMass=memb_info_i%mate(elem_no,NSTRN+1:,:)! element inverse mass matrix for all the analyses which are not static

!------------------------------------------------------------------

!----------------------------------------------------------------------------
! Extract b frame for this element
!----------------------------------------------------------------------------
eCab=memb_info_i%triad(elem_no,:,:)
dL=memb_info_i%dL
Le=memb_info_i%Le(elem_no) ! the ending x_1 of the element

!Angular velocity and linear velocity
!---------------------------------------
IF(ndof_el>=18)	THEN
    eOmega_a=omega_a  ! angular velocity

	!linear velocity at the middle of the element
	!-----------------------------------------------
	ev_i=v_root_a+CrossProduct(eOmega_a,memb_info_i%coordinate(elem_no,:)-xyz_pt1)
ENDIF

!----------------------------------------------------------------------------
! Extract internal variables for this element
!----------------------------------------------------------------------------
IF(init_flag==1) THEN  ! initial step
    Ui       = init_elem(elem_no,1:3)          ! given initial displacements
	theta    = init_elem(elem_no,4:6)          ! given initial rotations
	UiDot    = init_elem(elem_no,7:9)          ! given initial linear velocities
	ThetaDot = init_elem(elem_no,10:12)        ! given initial angular velocities
	CTCabPdot= x_elem(1:3)             ! calculated CTCabPdot from previous step
    CTCabHdot= x_elem(4:6)             ! calculated CTCabHdot from previous step
    IF (a_flag==3) THEN
		lambda = 0.D0
		lambdadot = 0.D0
	ENDIF
ELSE
	Ui   =x_elem(1:3)
	theta=x_elem(4:6)
	IF(init_flag==2) THEN  ! time marching
		UiDot=-init_elem(elem_no,1:3)
		ThetaDot=two_divide_dt*theta-init_elem(elem_no,4:6)
		CTCabPdot=-init_elem(elem_no,7:9)
		CTCabHdot=-init_elem(elem_no,10:12)
	ENDIF
ENDIF

Fi   =x_elem(7:9)
Mi   =x_elem(10:12)

IF (ndof_el>=18) THEN
	ePi   =x_elem(13:15)
	Hi    =x_elem(16:18)
ENDIF

e1GammaD=MATMUL(eFlex(1:3,:),x_elem(7:12))+e1
kappa=MATMUL(eFlex(4:6,:),x_elem(7:12))

IF(ndof_el>=18)THEN
	Vi=MATMUL(eMass(1:3,:),x_elem(13:18))
	OMEGAi=MATMUL(eMass(4:6,:),x_elem(13:18))
ENDIF

! Some numbers frequently needed for the element
!-----------------------------------------
eCabhalfL   =eCab*dL*0.5d0
eCT=DirCosineTRodrigues(theta)
eCTCab       =MATMUL(eCT,eCab)
eCTCabhalfL =eCTCab*dL*0.5d0

a_flag = aero_flag
g_flag = grav_flag

! Numbers related to aerodynamics (explanation in "modele aeroelastique couple")
IF (a_flag /= 0) THEN
	! for all model
	U =  memb_info_i%aerodyn_coef(elem_no,1)
	Rho = memb_info_i%aerodyn_coef(elem_no,2)
	Chord = memb_info_i%aerodyn_coef(elem_no,3)
	aw = memb_info_i%aerodyn_coef(elem_no,4)
	bw = 0.5*Chord

	! angles between flow and aircraft coordinate system

	alpha_ac = memb_info_i%aerodyn_coef(elem_no,5)
	beta_ac = memb_info_i%aerodyn_coef(elem_no,6)

!~ 	! transfert matrix from flow to aircraft coordinates system eCaf
!~ 	eCaf(:,1) = (/COS(alpha_ac)*COS(beta_ac),-COS(alpha_ac)*SIN(beta_ac),-SIN(alpha_ac)/)
!~ 	eCaf(:,2) = (/SIN(beta_ac),COS(beta_ac),0.D0/)
!~ 	eCaf(:,3) = (/SIN(alpha_ac)*COS(beta_ac),-SIN(alpha_ac)*SIN(beta_ac),COS(alpha_ac)/)
!~ 	eCaf = TRANSPOSE(eCaf)
    Wind = (/-COS(alpha_ac)*COS(beta_ac),-SIN(beta_ac),-SIN(alpha_ac)*COS(beta_ac)/)
    xflow = -Wind
	! extraction of aerodynamic section parameter from the coordinate system
	! all vector are defined in frame a
	!--------------------------------------------------------------------------------------------------------
    xB = eCTCAB(:,1)
    yB = eCTCAB(:,2)
    zB = eCTCAB(:,3)

    ! direction of the airfoil coordinate system (xflow,dir_moment,dir_lift) in the aircraft reference
    dir_lift = CrossProduct(xB,xflow)
    dir_lift = 1/Norm(dir_lift)*dir_lift
    dir_moment = CrossProduct(xflow,dir_lift)
    dir_moment = 1/Norm(dir_moment)*dir_moment

    ! airfoil parameter
    ! alpha ~ alpha_deformation + alpha_aircraft
    alpha = ASIN(DOT_PRODUCT(yB,dir_lift))

    alphadot = DOT_PRODUCT(MATMUL(eCTCAB,OMEGAi),dir_moment)
!~     alphadot = OMEGAi(1)
    hdot = -DOT_PRODUCT(MATMUL(eCTCAB,Vi),dir_lift)

!~     beta = ASIN(DOT_PRODUCT(xB,xflow))

	! flow velocity correction with beta :
!~ 	U = U*COS(beta)
    ! coefficient theta of Jacobian
    !---------------------------------------
    Jalpha_THETA = dir_moment
    Jhdot_THETA = 0.D0

	! coefficient PH of Jacobian
	Jalphadot_PH = MATMUL(dir_moment,MATMUL(eCTCab,eMass(4:6,:)))
!~ 	Jalphadot_PH = eMass(4,:)
	Jalphadotdot_PHDOT = Jalphadot_PH
	Jhdot_PH = -MATMUL(dir_lift,MATMUL(eCTCab,eMass(1:3,:)))
	Jhdotdot_PHDOT = Jhdot_PH

	! determination of alphadotdot and hdotdot for dynamic simulation
	IF (init_flag==2) THEN

		! transfer PHDot at t=t0 from frame a to B
		init_PH(1:3) = MATMUL(TRANSPOSE(eCTCAB),init_elem(elem_no,7:9))
		init_PH(4:6) = MATMUL(TRANSPOSE(eCTCAB),init_elem(elem_no,10:12))
		! evaluate PHDot at t=t0+dt
		PHDotB = two_divide_dt*x_elem(13:18)-init_PH
		! evaluate linear and angular acceleration in frame a using PHDot
		ViDot = MATMUL(eMass(1:3,:),PHDotB)
		OMEGAiDot = MATMUL(eMass(4:6,:),PHDotB)
		! alphadotdot = angular acceleration about dir_moment; hdotdot = -linear acceleration about dir_lift
		alphadotdot = DOT_PRODUCT(OMEGAiDot,MATMUL(TRANSPOSE(eCTCAB),dir_moment))
		hdotdot = -DOT_PRODUCT(ViDot,MATMUL(TRANSPOSE(eCTCAB),dir_lift))
		Jalphadotdot_PH = two_divide_dt*MATMUL(eMass(:,4:6),MATMUL(TRANSPOSE(eCTCAB),dir_moment))
		Jhdotdot_PH = -two_divide_dt*MATMUL(eMass(:,1:3),MATMUL(TRANSPOSE(eCTCAB),dir_lift))

	ELSE ! for modal analysis alphadotdot and hdotdot are described in the mass matrix
		alphadotdot = 0.D0
		hdotdot = 0.D0
		Jalphadotdot_PH = 0.D0
		Jhdotdot_PH = 0.D0
	ENDIF

ENDIF

!Induced-flow states
IF (a_flag > 2) THEN

	P = Peters(NSTATES)
	A = P(:,1:NSTATES)
	b = P(:,NSTATES+1)
	c = P(:,NSTATES+2)
	Ident = P(:,NSTATES+3:)

	lambda = x_elem(ndof_el-NSTATES+1:ndof_el)
	IF (init_flag == 2) THEN
		lambdadot = two_divide_dt*lambda-init_elem(elem_no,12+1:12+NSTATES)
	ELSE
		lambdadot = 0.D0
	ENDIF
	lambda0 = 0.5*DOT_PRODUCT(b,lambda)
	Jlambda0_LAMBDA = 0.5*b

ENDIF

! Numbers related to gravity
IF (g_flag > 0) THEN
	X_cg = memb_info_i%aerodyn_coef(elem_no,8)
ENDIF

END SUBROUTINE ExtractElementProperties
!**********************************************************

END MODULE Element


FUNCTION rerf(y)
!
IMPLICIT NONE
!
! Function arguments
!
REAL :: y
REAL :: rerf
!
! Local variables
!
REAL :: a,a1,a2,alogy,b,b1,b2,base,c,c1,c2,d,d1,e,er1,er2,f,g,h,i,j,sqy,y2,y3,  &
      & y4,y5,z
REAL :: alog
!
!**********************************************************************
!     + + +  purpose + + +
!     for the equation y = erf(x), the function returns the value of x
!     given the value of y, i.e., the reverse of the error function.
!     written by l. hagen and coded by i. elmanyawi

!     + + +  argument declarations + + +

!     + + + local variables + + +
!     + + + end specifications + + +
!
a = 0.000009477
b = -2.76913995
c = 0.88589485
d = 2.30199509
e = -2.44996884
f = -0.14332263
g = 2.246990417
h = -0.54046067
i = -0.68292239
j = 0.15092933
z = y
y = abs(y)
!
a1 = 2857.5463
b1 = -0.00016423
c1 = -5717.0696
d1 = -2857.5783
a2 = 69161.649
b2 = -277330.28
c2 = 208168.95
!
sqy = sqrt(y)
alogy = alog(y)
y2 = y*y
y3 = y2*y
y4 = y3*y
y5 = y4*y
!
base = (a+c*y+e*y2+g*y3+i*y4)/(1.000000+b*y+d*y2+f*y3+h*y4+j*y5)
!
IF (y.LE.0.966105) THEN
  rerf = base
ELSE IF (y.GT.0.999311) THEN
!
  er2 = a2 + b2*y*sqy + c2*y2
  rerf = base - er2
ELSE
!
  er1 = a1 + b1/alogy + c1*alogy/y + d1/y2
  rerf = base - er1
END IF
rerf = rerf*z/y
END FUNCTION rerf

Project Documents/Source Code Original/Rerf.f90


      subroutine bfgsd(diag,n,nt,np,y,s,ys,condm,param,zero,index)
c     mise a jour de diag par la methode de bfgs diagonal
c     utiliser a la suite de la correction de powell
c     condm borne sup du conditionnement de diag
c     param borne inf rapport reduction diag(i)
c
      implicit double precision (a-h,o-z)
      dimension diag(n),y(nt,n),s(nt,n),ys(nt)
      integer index(nt)
c
      inp=index(np)
      ys1=1./ys(inp)
      sds=0.
      do 10 i=1,n
10    sds=sds + diag(i)*s(inp,i)**2
      sds1=1./sds
      dmin=1.e25
      dmax=0.
      do 20 i=1,n
      dd1=param*diag(i)
      dd1=dd1+1000.*zero
      dd=diag(i)+ys1*y(inp,i)**2-sds1*(diag(i)*s(inp,i))**2
      diag(i)=dd
c     sauvegarde positivite
      if(dd.le.dd1)diag(i)=dd1
c     calcul conditionnement
      if(diag(i).lt.dmin)dmin=diag(i)
      if(diag(i).gt.dmax)dmax=diag(i)
20    continue
c     limitation du conditionnement
      if((condm*dmin)/dmax.gt.1)return
      omeg=log(condm)/log(dmax/dmin)
      do 30 i=1,n
30    diag(i)=diag(i)**omeg
      return
      end


      SUBROUTINE SOLVER (LOWER,X,B,IN,OUT,EPS,IFL,SCR)
C
C    SOLVER PERFORMS THREE OPERATIONS--
C    1. SOLVES FOR B BY FORWARD-BACKWARD SUBSTITUTION
C    2. COMPUTES OUT = IN + B(T)*X
C    3. IF REQUESTED, COMPUTES EPSILON = NORM(OUT)/NORM(IN)
C
      INTEGER         X     ,OUT   ,FILEL ,FILEU ,FILEB ,FILEX ,SCR    ,
     1                PREC  ,SIGN  ,FILEE ,FILEF ,FILEG ,FILEH ,T      ,
     2                SIGNC ,SIGNAB,PRECX ,EOL   ,EOR   ,SYSBUF,SCRTCH ,
     3                B     ,SCR1  ,NAME(2)
      DOUBLE PRECISION       AD    ,NUM   ,DENOM
      DIMENSION       FILEL(7)     ,FILEU(7)     ,FILEB(7)     ,
     1                FILEX(7)     ,FILEE(7)     ,FILEF(7)     ,
     2                FILEG(7)     ,FILEH(7)
      CHARACTER       UFM*23,UWM*25
      COMMON /XMSSG / UFM   ,UWM
      COMMON /ZZZZZZ/ Z(1)
      COMMON /FBSX  / FILEL ,FILEU ,FILEB ,FILEX ,NZ    ,PREC  ,SIGN   ,
     1                SCR1
      COMMON /MPYADX/ FILEE ,FILEF ,FILEG ,FILEH ,NZZ   ,T     ,SIGNAB ,
     1                SIGNC ,PRECX ,SCRTCH
      COMMON /ZNTPKX/ AD(2) ,I     ,EOL   ,EOR
      COMMON /SYSTEM/ KSYSTM(65)
      EQUIVALENCE     (KSYSTM(1),SYSBUF)  ,(KSYSTM(55),IPREC)  ,
     1                (KSYSTM(2),IOUTPT)
C
C     INITIALIZE MATRIX CONTROL BLOCKS FOR FORWARD-BACKWARD SOLUTION
C
      NZ = KORSZ(Z)
      FILEL(1) = LOWER
      CALL RDTRL (FILEL)
      FILEB(1) = B
      CALL RDTRL (FILEB)
      CALL MAKMCB (FILEX,X,FILEB(3),FILEB(4),IPREC)
      PREC = IPREC
      SIGN = -1
C
C     SOLVE A*X = -B FOR X WHERE A HAS BEEN FACTORED
C
      SCR1 = SCR
      CALL FBS (Z,Z)
      CALL WRTTRL (FILEX)
C
C     INITIALIZE MATRIX CONTROL BLOCKS FOR MPYAD OPERATION
C
      DO 50 K = 1,7
      FILEE(K) = FILEB(K)
   50 FILEF(K) = FILEX(K)
      FILEG(1) = IN
      CALL RDTRL (FILEG)
      CALL MAKMCB (FILEH,OUT,FILEG(3),FILEG(4),IPREC)
      NZZ = NZ
      T   = 1
      SIGNAB = 1
      SIGNC  = 1
      PRECX  = IPREC
      SCRTCH = SCR
C
C     COMPUTE OUT = IN + B(T)*X
C
      CALL MPYAD  (Z,Z,Z)
      CALL WRTTRL (FILEH)
C
C     IF REQUESTED,COMPUTE EPS = NORM(OUT) / NORM(IN)
C
      IF (IFL .EQ. 0) RETURN
      N1 = NZ - SYSBUF
      N2 = N1 - SYSBUF
      CALL GOPEN (OUT,Z(N1+1),0)
      CALL GOPEN ( IN,Z(N2+1),0)
      NUM   = 0.0D0
      DENOM = 0.0D0
      NCOL  = FILEG(2)
      DO 130 K = 1,NCOL
      CALL INTPK (*110,OUT,0,2,0)
  100 CALL ZNTPKI
      NUM = NUM + DABS(AD(1))*DABS(AD(1))
      IF (EOL .EQ. 0) GO TO 100
  110 CALL INTPK (*130,IN,0,2,0)
  120 CALL ZNTPKI
      DENOM = DENOM + DABS(AD(1))*DABS(AD(1))
      IF (EOL .EQ. 0) GO TO 120
  130 CONTINUE
      IF (DENOM .EQ. 0.0D0) GO TO 160
      EPS = DSQRT(NUM/DENOM)
      GO TO 180
  160 CALL FNAME (IN,NAME)
      WRITE  (IOUTPT,170) UWM,NAME
  170 FORMAT (A25,' 2401, ',2A4,' MATRIX IS NULL.  AN ARBITRARY VALUE ',
     1       'OF 1.0 IS THEREFORE ASSIGNED TO', /5X,
     2       'THE RIGID BODY ERROR RATIO (EPSILON SUB E).')
      EPS = 1.0
  180 CALL CLOSE (IN, 1)
      CALL CLOSE (OUT,1)
      RETURN
      END


program pyramid_test
    use precision,      only : i4k, r8k
    use vtk_datasets,   only : polygonal_data
    use vtk_attributes, only : scalar, attributes
    use vtk_cells,      only : polygon
    use vtk,            only : vtk_legacy_write
    implicit none
    !! author: Ian Porter
    !! date: 12/31/2017
    !!
    !! this is a test of a pyramid geometry using polygonal data
    !!
    integer(i4k), parameter     :: n_params_to_write = 1
    type (polygonal_data)       :: pyramid
    type (attributes), dimension(n_params_to_write) :: point_vals_to_write, cell_vals_to_write
    integer(i4k)                :: i = 0_i4k
    integer(i4k),     parameter :: n_x = 3, n_y = 3, n_z = 3, n_faces = 20
    !character(len=*), parameter :: filename = 'legacy_polygonal_data' !! will use default out.vtk filename
    integer(i4k), dimension(3)  :: dims
    real(r8k), dimension(n_faces,1:n_params_to_write)     :: cell_vals
    real(r8k), dimension(n_x*n_y*n_z,1:n_params_to_write) :: point_vals
    real(r8k), dimension(3,n_x*n_y*n_z), parameter :: points = reshape ( &
        & [ 0.0_r8k,   0.0_r8k,   0.0_r8k,  &
        &   0.5_r8k,   0.0_r8k,   0.0_r8k,  &
        &   1.0_r8k,   0.0_r8k,   0.0_r8k,  &
        &   0.0_r8k,   0.5_r8k,   0.0_r8k,  &
        &   0.5_r8k,   0.5_r8k,   0.0_r8k,  &
        &   1.0_r8k,   0.5_r8k,   0.0_r8k,  &
        &   0.0_r8k,   1.0_r8k,   0.0_r8k,  &
        &   0.5_r8k,   1.0_r8k,   0.0_r8k,  &
        &   1.0_r8k,   1.0_r8k,   0.0_r8k,  &
        &   0.25_r8k,  0.25_r8k,  0.5_r8k,  &
        &   0.50_r8k,  0.25_r8k,  0.5_r8k,  &
        &   0.75_r8k,  0.25_r8k,  0.5_r8k,  &
        &   0.25_r8k,  0.50_r8k,  0.5_r8k,  &
        &   0.50_r8k,  0.50_r8k,  0.5_r8k,  &
        &   0.75_r8k,  0.50_r8k,  0.5_r8k,  &
        &   0.25_r8k,  0.75_r8k,  0.5_r8k,  &
        &   0.50_r8k,  0.75_r8k,  0.5_r8k,  &
        &   0.75_r8k,  0.75_r8k,  0.5_r8k,  &
        &   0.375_r8k, 0.375_r8k, 0.75_r8k, &
        &   0.50_r8k,  0.375_r8k, 0.75_r8k, &
        &   0.625_r8k, 0.375_r8k, 0.75_r8k, &
        &   0.375_r8k, 0.50_r8k,  0.75_r8k, &
        &   0.50_r8k,  0.50_r8k,  0.75_r8k, &
        &   0.625_r8k, 0.50_r8k,  0.75_r8k, &
        &   0.375_r8k, 0.625_r8k, 0.75_r8k, &
        &   0.50_r8k,  0.625_r8k, 0.75_r8k, &
        &   0.625_r8k, 0.625_r8k, 0.75_r8k ], [3,n_x*n_y*n_z] )
    real(r8k), dimension(n_x*n_y*n_z), parameter :: power = &
        & [ 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, &
        &   100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 200.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, &
        &   100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k, 100.0_r8k ]
    integer(i4k), dimension(n_faces), parameter :: cellid = &
        & [ 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3 ]
    type(polygon), dimension(n_faces) :: polygon_faces
    character(len=10), dimension(n_params_to_write), parameter :: cell_dataname = &
        & [ 'cellids   ' ]
    character(len=10), dimension(n_params_to_write), parameter :: point_dataname = &
        & [ 'power_(w) ' ]

    call polygon_faces(1)%setup ( [ 0, 1, 4, 3 ] )
    call polygon_faces(2)%setup ( [ 1, 2, 5, 4 ] )
    call polygon_faces(3)%setup ( [ 3, 4, 7, 6 ] )
    call polygon_faces(4)%setup ( [ 4, 5, 8, 7 ] )
    call polygon_faces(5)%setup ( [ 9, 10, 13, 12 ] )
    call polygon_faces(6)%setup ( [ 10, 11, 14, 13 ] )
    call polygon_faces(7)%setup ( [ 12, 13, 16, 15 ] )
    call polygon_faces(8)%setup ( [ 13, 14, 17, 16 ] )
    call polygon_faces(9)%setup ( [ 18, 19, 22, 21 ] )
    call polygon_faces(10)%setup ( [ 19, 20, 23, 22 ] )
    call polygon_faces(11)%setup ( [ 21, 22, 25, 24 ] )
    call polygon_faces(12)%setup ( [ 22, 23, 26, 25 ] )
    call polygon_faces(13)%setup ( [ 0, 1, 10, 19, 18, 9 ] )
    call polygon_faces(14)%setup ( [ 1, 2, 11, 20, 19, 10 ] )
    call polygon_faces(15)%setup ( [ 0, 3, 12, 21, 18, 9 ] )
    call polygon_faces(16)%setup ( [ 3, 6, 15, 24, 21, 12 ] )
    call polygon_faces(17)%setup ( [ 6, 7, 16, 25, 24, 15 ] )
    call polygon_faces(18)%setup ( [ 7, 8, 17, 26, 25, 16 ] )
    call polygon_faces(19)%setup ( [ 8, 5, 14, 23, 26, 17 ] )
    call polygon_faces(20)%setup ( [ 5, 2, 11, 20, 23, 14 ] )

    cell_vals(:,1)  = real(cellid(:))
    point_vals(:,1) = power(:)

    dims = [ n_x, n_y, n_z ]
    call pyramid%init (points=points, polygons=polygon_faces)
    do i = 1, n_params_to_write
        ! cell values
        if (.not. allocated(cell_vals_to_write(i)%attribute))then
            allocate(scalar::cell_vals_to_write(i)%attribute)
        end if
        call cell_vals_to_write(i)%attribute%init (trim(cell_dataname(i)), numcomp=1, real1d=cell_vals(:,i))
        ! point values
        if (.not. allocated(point_vals_to_write(i)%attribute))then
            allocate(scalar::point_vals_to_write(i)%attribute)
        end if
        call point_vals_to_write(i)%attribute%init (trim(point_dataname(i)), numcomp=1, real1d=point_vals(:,i))
    end do

    call vtk_legacy_write (pyramid, celldatasets=cell_vals_to_write, pointdatasets=point_vals_to_write)

    write(*,*) 'Finished'

end program pyramid_test


subroutine smoothz(p1,n,nc,m,al,be,dss,iflg)
  DIMENSION DSS(N),P1(nc,N),P2(nc,N),BE(M),AL(N,M),GA(nc,M),DE(nc,M)
      nm=n-1
!  recursive filter in the vertical direction
       if(iflg.eq.2)then
       do j=1,n
       do i=1,nc
       p2(i,j)=p1(i,j)*dss(j)
       enddo
       enddo
      CALL RF0V(GA,DE,M*nc)
      CALL RFHV(P2,p1,nc,Nm,M,AL,BE,GA,DE)
       endif
!  adjoint of filter in the vertical direction
        if(iflg.eq.1)then
      CALL RF0V(GA,DE,M*nc)
      CALL RFHV(P1,p2,nc,Nm,M,AL,BE,GA,DE)
       do i=1,n
       do j=1,nc
       p1(j,i)=p2(j,i)*dss(i)
       enddo
       enddo
       endif
      RETURN
      END
      SUBROUTINE RF0V(GAP,DEP,M)
      DIMENSION GAP(M),DEP(M)
      DO I=1,M
       GAP(I)=0.
       DEP(I)=0.
      ENDDO
      RETURN
      END
!------------------------------------------------------------------------------
!------------------------------------------------------------------------------
!   R.J.Purser, National Meteorological Center, Washington D.C.  1998
!                   SUBROUTINE RFHV
!   Apply pseudo-Gaussian "right-plus-left" smoother to a single "vector"
!   of data.
!
! --> P1:   Input data.
! <-- P2:   Output data.
! --> N:    Number of grid intervals (one less than number of points).
! --> M:    Degree of approximation of smoother to a Gaussian.
! --> AL:   Matrix of "Alpha" coefficients (computed in RFDPARV).
! --> BE:   Vector of "Beta" coefficients (computed in RFDPAR1).
! <-> GAP:  Gamma vector of exponential amplitudes of P decaying to the right.
! <-> DEP:  Delta vector of exponential amplitudes of P decaying to the left.
!------------------------------------------------------------------------------
 SUBROUTINE RFHV(P1,p2,nc,N,M,AL,BE,GA,DE)
  DIMENSION P1(nc,0:N),P2(nc,0:N),AL(0:N,M),BE(M),GA(nc,M),DE(nc,M)
      dimension dss(0:n)
      KMOD2=MOD(M,2)
       do j=0,n
       do i=1,nc
       p2(i,j)=0.
       enddo
       enddo
! Advancing filter:
      DO I=0,N
!      P1I=P1(I)
!      P2(I)=0.
       IF(KMOD2.EQ.1)THEN  ! Treat the real root:
        do j=1,nc
        GA(j,1)=AL(I,1)*GA(j,1)+BE(1)*P1(j,i)
        enddo
        do j=1,nc
        P2(j,I)=P2(j,I)+GA(j,1)
        enddo
       ENDIF
                           ! Treat remaining complex roots:
       DO KR=KMOD2+1,M,2   ! <-- Index of "real" components
        KI=KR+1            ! <-- Index of "imag" components
        do j=1,nc
        GAKR=GA(j,KR)
        GAKI=GA(j,KI)
        GA(j,KR)=AL(I,KR)*GAKR-AL(I,KI)*GAKI+BE(KR)*P1(j,i)
        GA(j,KI)=AL(I,KI)*GAKR+AL(I,KR)*GAKI+BE(KI)*P1(j,i)
        enddo
        do j=1,nc
        P2(j,I)=P2(j,I)+GA(j,KR)
        enddo
       ENDDO
      ENDDO

! Backing filter:
      DO I=N,0,-1
!      P1I=P1(I)
       IF(KMOD2.EQ.1)THEN  ! Treat the real root:
        do j=1,nc
        P2(j,I)=P2(j,I)+DE(j,1)
        enddo
        do j=1,nc
        DE(j,1)=AL(I,1)*(DE(j,1)+BE(1)*P1(j,i))
        enddo
       ENDIF
                           ! Treat remaining complex roots:
       DO KR=KMOD2+1,M,2   ! <-- Index of "real" components
        KI=KR+1            ! <-- Index of "imag" components
        do j=1,nc
        P2(j,I)=P2(j,I)+DE(j,KR)
        enddo
        do j=1,nc
        DEKR=DE(j,KR)+BE(KR)*P1(j,i)
        DEKI=DE(j,KI)+BE(KI)*P1(j,i)
        DE(j,KR)=AL(I,KR)*DEKR-AL(I,KI)*DEKI
        DE(j,KI)=AL(I,KI)*DEKR+AL(I,KR)*DEKI
        enddo
       ENDDO
      ENDDO
      RETURN
      END
!------------------------------------------------------------------------------
!------------------------------------------------------------------------------
!   R.J.Purser, National Meteorological Center, Washington D.C.  1998
!                   SUBROUTINE RFDPAR1
! Initialize the Beta and exponential Rate vectors characterizing the unit-
! width pseudo-Gaussian of degree M.
!
! <-- BE:   Beta vector of coupling coefficients between input and each
!           exponential component. For complex exponentials (which occur in
!           conjugate pairs) store REAL, then IMAGINARY, of one member of pair.
! <-- RATE: Vector of "decay rates" characteristic of the exponential
!           components. For paired complex rates, same convention as for BE.
! --> M:    Degree of approximation to the Gaussian [FT of this pseudo-
!           Gaussian is proportional to 1./(1+k**2+ ...+k**(2M)/M!)].
!------------------------------------------------------------------------------
      SUBROUTINE RFDPAR1(BE,RATE,M)
      IMPLICIT COMPLEX(C)
      PARAMETER(QCRIT=1.E-3)
      PARAMETER(NN=12)
      LOGICAL POLISH
      DIMENSION COF(0:NN),CROOT(NN),VAN(NN,NN),BE(m),RATE(m)
      DATA POLISH/.TRUE./
      polish=.true.
      KMOD2=MOD(M,2)
      COF(0)=1.
      DO I=1,M
       COF(I)=.5*COF(I-1)/I
      ENDDO
      CALL ZROOTS(COF,M,CROOT,POLISH)
      IF(KMOD2.EQ.1)THEN    ! Treat the single real root:
       R=-REAL(CROOT(1))
       Q=-AIMAG(CROOT(1))
       QA=ABS(Q)
       IF(QA.GT.QCRIT)STOP
       R=SQRT(R)
       RATE(1)=R
       VAN(1,1)=1.
       VAN(2,1)=r
       DO I=3,M
       VAN(I,1)=VAN(I-1,1)*R*R
       ENDDO
      ENDIF
      DO J2=2,M,2         ! Loop over remaining independent complex roots
       JREAL=KMOD2+J2-1
       JIMAG=KMOD2+J2
       CA=-CROOT(J2)
       CB=CSQRT(CA)
       R=REAL(CB)
       Q=AIMAG(CB)
       IF(R.LT.0.)THEN
        CB=-CB
        R=-R
        Q=-Q
       ENDIF
       RATE(JREAL)=R
       RATE(JIMAG)=Q
       VAN(1,JREAL)=1.
       VAN(1,JIMAG)=0.
       DO I=2,M
        IPOW=I*2-3
        CC=CB**IPOW
        VAN(I,JREAL)=REAL(CC)
        VAN(I,JIMAG)=-AIMAG(CC)
       ENDDO
      ENDDO
      BE(1)=1.
      DO I=2,M
       BE(I)=0.
      ENDDO
      CALL LINMM(VAN,BE,M,1,NN,M)
      RETURN
      END
!------------------------------------------------------------------------------
!------------------------------------------------------------------------------
!   R.J.Purser, National Meteorological Center, Washington D.C.  1998
!                   SUBROUTINE RFDPAR2
! Initialize the "turning" matrix and the amplitude rescaling factor
! of the pseudo-Gaussian smoother of degree M
!
! --> BE:   Beta vector (computed in previous call to RFDPAR1).
! --> RATE: Rate vector of the (in general complex) exponential contributions.
! <-- TURN: Turning matrix for 2nd sweep of filter along a line.
! <-- SAMP: Factor by which the amplitude at second sweep is normalized.
! --> M:    Degree of pseudo-Gaussian
!------------------------------------------------------------------------------
      SUBROUTINE RFDPAR2(BE,RATE,TURN,SAMP,M)
      PARAMETER(NN=12)
      IMPLICIT COMPLEX(C)
      DIMENSION BE(M),RATE(M),TURN(M,M)
      KMOD2=MOD(M,2)
      S=0.
      IF(KMOD2.EQ.1)THEN     ! The first root is real:
       R1=RATE(1)
       BE1=BE(1)
       TURN(1,1)=BE1/(2*R1)
       S=S+TURN(1,1)*BE1
       DO LR=KMOD2+1,M,2
        LI=LR+1
        CBE=CMPLX(BE(LR),BE(LI))
        CRL=CMPLX(RATE(LR),RATE(LI))
        CL1=CBE/(R1+CRL)
        TURN(LR,1)=REAL(CL1)
        TURN(LI,1)=AIMAG(CL1)
        C1L=BE1/(R1+CRL)
        TURN(1,LR)=REAL(C1L)
        TURN(1,LI)=-AIMAG(C1L)
        S=S+TURN(LR,1)*BE1+TURN(1,LR)*BE(LR)+TURN(1,LI)*BE(LI)
       ENDDO
      ENDIF
      DO KR=KMOD2+1,M,2
       KI=KR+1
       CRK=CMPLX(RATE(KR),RATE(KI))
       CRJ=CONJG(CRK)
       BEKR=BE(KR)
       BEKI=BE(KI)
       DO LR=KMOD2+1,M,2
        LI=LR+1
        CBEH=.5*CMPLX(BE(LR),BE(LI))
        CRL=CMPLX(RATE(LR),RATE(LI))
        CLK=CBEH/(CRL+CRK)
        CLJ=CBEH/(CRL+CRJ)
        CLKR=CLK+CLJ
        CLKI=CLK-CLJ
        TURN(LR,KR)=  REAL(CLKR)
        TURN(LI,KR)= AIMAG(CLKR)
        TURN(LR,KI)=-AIMAG(CLKI)
        TURN(LI,KI)=  REAL(CLKI)
        S=S+TURN(LR,KR)*BEKR+TURN(LR,KI)*BEKI
       ENDDO
      ENDDO
      SAMP=.5/S
      RETURN
      END

!------------------------------------------------------------------------------
!------------------------------------------------------------------------------
!   R.J.Purser, National Meteorological Center, Washington D.C.  1998
!                   SUBROUTINE RFDPARV
! Initialize Alpha coefficients for pseudo-Gaussian smoother of degree M
! along a line with varying effective scale at each point
!
! --> DSH:  Array of "effective" spacing (in stretched space S) between gridpts
! --> RATE: Exponential rates in this degree of approximation to the Gaussian.
! <-- AL:   Array of "transfer function" coefficients to propagate vectors
!           GAP and DEP of amplitudes of the exponential contributions to
!           a filtered field along this grid line.
! --> N:    Number of grid spaces along this line (ie, number of grdpts = N+1)
! --> M:    Degree of approximation to pseudo-Gaussian.
!------------------------------------------------------------------------------

      SUBROUTINE RFDPARV(DSH,RATE,AL,N,M)
      IMPLICIT COMPLEX(C)
      DIMENSION DSH(N),RATE(M),AL(n,m)

      KMOD2=MOD(M,2)
      IF(KMOD2.EQ.1)THEN
       R1=RATE(1)
       DO I=1,N
        AL(I,1)=EXP(-R1*DSH(I))
       ENDDO
      ENDIF
      DO KR=KMOD2+1,M,2
       KI=KR+1
       CRK=CMPLX(RATE(KR),RATE(KI))
       DO I=1,N
        CRDS=CRK*DSH(I)
        CAL=CEXP(-CRDS)
        AL(i,KR)=REAL(CAL)
        AL(i,KI)=AIMAG(CAL)
       ENDDO
      ENDDO
      RETURN
      END

      SUBROUTINE ZROOTS(A,M,ROOTS,POLISH)
      PARAMETER (EPS=1.E-6,MAXM=101)
      COMPLEX A(*),ROOTS(M),AD(MAXM),X,B,C
      LOGICAL POLISH
      DO 11 J=1,M+1
        AD(J)=A(J)
11    CONTINUE
      DO 13 J=M,1,-1
        X=CMPLX(0.,0.)
        CALL LAGUER(AD,J,X,EPS,.FALSE.)
        IF(ABS(AIMAG(X)).LE.2.*EPS**2*ABS(REAL(X))) X=CMPLX(REAL(X),0.)
        ROOTS(J)=X
        B=AD(J+1)
        DO 12 JJ=J,1,-1
          C=AD(JJ)
          AD(JJ)=B
          B=X*B+C
12      CONTINUE
13    CONTINUE
      IF (POLISH) THEN
        DO 14 J=1,M
          CALL LAGUER(A,M,ROOTS(J),EPS,.TRUE.)
14      CONTINUE
      ENDIF
      DO 16 J=2,M
        X=ROOTS(J)
        DO 15 I=J-1,1,-1
          IF(REAL(ROOTS(I)).LE.REAL(X))GO TO 10
          ROOTS(I+1)=ROOTS(I)
15      CONTINUE
        I=0
10      ROOTS(I+1)=X
16    CONTINUE
      RETURN
      END

      SUBROUTINE LAGUER(A,M,X,EPS,POLISH)
      COMPLEX A(*),X,DX,X1,B,D,F,G,H,SQ,GP,GM,G2,ZERO
      LOGICAL POLISH
      PARAMETER (ZERO=(0.,0.),EPSS=6.E-8,MAXIT=100)
      DXOLD=CABS(X)
      DO 12 ITER=1,MAXIT
        B=A(M+1)
        ERR=CABS(B)
        D=ZERO
        F=ZERO
        ABX=CABS(X)
        DO 11 J=M,1,-1
          F=X*F+D
          D=X*D+B
          B=X*B+A(J)
          ERR=CABS(B)+ABX*ERR
11      CONTINUE
        ERR=EPSS*ERR
        IF(CABS(B).LE.ERR) THEN
          RETURN
        ELSE
          G=D/B
          G2=G*G
          H=G2-2.*F/B
          SQ=CSQRT((M-1)*(M*H-G2))
          GP=G+SQ
          GM=G-SQ
          IF(CABS(GP).LT.CABS(GM)) GP=GM
          DX=M/GP
        ENDIF
        X1=X-DX
        IF(X.EQ.X1)RETURN
        X=X1
        CDX=CABS(DX)
        DXOLD=CDX
        IF(.NOT.POLISH)THEN
          IF(CDX.LE.EPS*CABS(X))RETURN
        ENDIF
12    CONTINUE
      PAUSE 'too many iterations'
      RETURN
      END


      subroutine md
     *     (n, ia,ja, max, v,l, head,last,next, mark, flag)
clll. optimize
c***********************************************************************
c  md -- minimum degree algorithm (based on element model)
c***********************************************************************
c
c  description
c
c    md finds a minimum degree ordering of the rows and columns of a
c    general sparse matrix m stored in (ia,ja,a) format.
c    when the structure of m is nonsymmetric, the ordering is that
c    obtained for the symmetric matrix  m + m-transpose.
c
c
c  additional parameters
c
c    max  - declared dimension of the one-dimensional arrays v and l.
c           max must be at least  n+2k,  where k is the number of
c           nonzeroes in the strict upper triangle of m + m-transpose
c
c    v    - integer one-dimensional work array.  dimension = max
c
c    l    - integer one-dimensional work array.  dimension = max
c
c    head - integer one-dimensional work array.  dimension = n
c
c    last - integer one-dimensional array used to return the permutation
c           of the rows and columns of m corresponding to the minimum
c           degree ordering.  dimension = n
c
c    next - integer one-dimensional array used to return the inverse of
c           the permutation returned in last.  dimension = n
c
c    mark - integer one-dimensional work array (may be the same as v).
c           dimension = n
c
c    flag - integer error flag.  values and their meanings are -
c             0     no errors detected
c             9n+k  insufficient storage in md
c
c
c  definitions of internal parameters
c
c    ---------+---------------------------------------------------------
c    v(s)     - value field of list entry
c    ---------+---------------------------------------------------------
c    l(s)     - link field of list entry  (0 =) end of list)
c    ---------+---------------------------------------------------------
c    l(vi)    - pointer to element list of uneliminated vertex vi
c    ---------+---------------------------------------------------------
c    l(ej)    - pointer to boundary list of active element ej
c    ---------+---------------------------------------------------------
c    head(d)  - vj =) vj head of d-list d
c             -  0 =) no vertex in d-list d
c
c
c             -                  vi uneliminated vertex
c             -          vi in ek           -       vi not in ek
c    ---------+-----------------------------+---------------------------
c    next(vi) - undefined but nonnegative   - vj =) vj next in d-list
c             -                             -  0 =) vi tail of d-list
c    ---------+-----------------------------+---------------------------
c    last(vi) - (not set until mdp)         - -d =) vi head of d-list d
c             --vk =) compute degree        - vj =) vj last in d-list
c             - ej =) vi prototype of ej    -  0 =) vi not in any d-list
c             -  0 =) do not compute degree -
c    ---------+-----------------------------+---------------------------
c    mark(vi) - mark(vk)                    - nonneg. tag .lt. mark(vk)
c
c
c             -                   vi eliminated vertex
c             -      ei active element      -           otherwise
c    ---------+-----------------------------+---------------------------
c    next(vi) - -j =) vi was j-th vertex    - -j =) vi was j-th vertex
c             -       to be eliminated      -       to be eliminated
c    ---------+-----------------------------+---------------------------
c    last(vi) -  m =) size of ei = m        - undefined
c    ---------+-----------------------------+---------------------------
c    mark(vi) - -m =) overlap count of ei   - undefined
c             -       with ek = m           -
c             - otherwise nonnegative tag   -
c             -       .lt. mark(vk)         -
c
c-----------------------------------------------------------------------
c
      integer  ia(1), ja(1),  v(1), l(1),  head(1), last(1), next(1),
     *   mark(1),  flag,  tag, dmin, vk,ek, tail
      equivalence  (vk,ek)
c
c----initialization
      tag = 0
      call  mdi
     *   (n, ia,ja, max,v,l, head,last,next, mark,tag, flag)
      if (flag.ne.0)  return
c
      k = 0
      dmin = 1
c
c----while  k .lt. n  do
   1  if (k.ge.n)  go to 4
c
c------search for vertex of minimum degree
   2    if (head(dmin).gt.0)  go to 3
          dmin = dmin + 1
          go to 2
c
c------remove vertex vk of minimum degree from degree list
   3    vk = head(dmin)
        head(dmin) = next(vk)
        if (head(dmin).gt.0)  last(head(dmin)) = -dmin
c
c------number vertex vk, adjust tag, and tag vk
        k = k+1
        next(vk) = -k
        last(ek) = dmin - 1
        tag = tag + last(ek)
        mark(vk) = tag
c
c------form element ek from uneliminated neighbors of vk
        call  mdm
     *     (vk,tail, v,l, last,next, mark)
c
c------purge inactive elements and do mass elimination
        call  mdp
     *     (k,ek,tail, v,l, head,last,next, mark)
c
c------update degrees of uneliminated vertices in ek
        call  mdu
     *     (ek,dmin, v,l, head,last,next, mark)
c
        go to 1
c
c----generate inverse permutation from permutation
   4  do 5 k=1,n
        next(k) = -next(k)
   5    last(next(k)) = k
c
      return
      end


c
c**********************************************************************
      subroutine h3dpwdata(ier,kvec,rscale,center,
     &		radius,nterms,pot,dpotdn,w,lw,lused)
      implicit real *8 (a-h,o-z)
c**********************************************************************
c
c PURPOSE:
c
c     This subroutine creates the j-expansion at the center
c     center(3), due to a plane wave defined by kvec.
c     This is the memory management routine. Work is done in the
c     secondary call to h3dformta0 below.
c
c INPUT:
c
c     wavek   (complex *16)     : the Helmholtz coefficient
c     rscale   (real *8)        : scaling parameter
c                                   should be less than one in magnitude.
c                                   Needed for low frequency regime only
c                                  with rsclale abs(wavek) recommended.
c     center(3)   (real *8)     : coordinates of the expansion center
c     nterms   (integer *4)     : order of the j-expansion
c     w           (real *8)     : workspace
c     lw       (integer *4)     : workspace length
c
c
c OUTPUT:
c
c     ier      (integer *4)     : error return code
c		ier=0	returned successfully;
c		ier=8	not enough memory supplied to this subroutine
c	 	ier=16  d is out of range in hfuns3d
c     pot(0:nterms,0:nterms)    : potential in s.h. basis
c              (complex *16)    :
c     dpotdn(0:nterms,0:nterms) : normal deriv of potential in s.h. basis
c              (complex *16)    :
c     lused    (integer *4)     : amount of work space "w" used
c
c
      real *8 center(3),kvec(3)
      complex *16 wavek,pot(0:nterms,-nterms:nterms)
      complex *16 dpotdn(0:nterms,-nterms:nterms)
      real *8 w(1)
c
c ... assign workspaces:
c
      ier=0
c
      ipp=1
      lpp=(nterms+1)**2+7
c
      iephi=ipp+lpp
      lephi=2*(2*nterms+1)+7
c
      lused=iephi+lephi
c
      if (lused.gt.lw) then
         ier=8
         return
      endif
c
      do l = 0,nterms
         do m = -l,l
            pot(l,m) = 0.0d0
            dpotdn(l,m) = 0.0d0
         enddo
      enddo
c
      call prinf(' calling pwtasc0 *',ier,0)
      call h3dpwdata0(jer,kvec,rscale,center,radius,
     &		nterms,pot,dpotdn,w(ipp),w(iephi))
      if (jer.ne.0) ier=16
c
      return
      end
c
c
c
c**********************************************************************
      subroutine h3dpwdata0(ier,kvec,rscale,
     &		center,radius,nterms,pot,dpotdn,pp,ephi)
      implicit real *8 (a-h,o-z)
c
c     See h3dpwta for comments
c
c**********************************************************************
c
      integer *4 iw(0:2000)
      real *8 kvec(3),center(3),zdiff(3)
      complex *16 wavek,pot(0:nterms,-nterms:nterms)
      complex *16 dpotdn(0:nterms,-nterms:nterms)
c
      real *8 pp(0:nterms,0:nterms)
      complex *16 ephi(-nterms:nterms),ephi1,ephi1inv
      complex *16 ztmp,z,zconst
      complex *16 fjs(0:2000)
      complex *16 fjder(0:2000)
      complex *16 cscale,eye
c
      data eye/(0.0d0,1.0d0)/
      data thresh/1.0d-15/
c
c ... Initializing...
c
      ier=0
c
      zdiff(1) = kvec(1)
      zdiff(2) = kvec(2)
      zdiff(3) = kvec(3)
      wavek= dsqrt( kvec(1)**2 + kvec(2)**2 + kvec(3)**2 )
      cdot = kvec(1)*center(1) + kvec(2)*center(2) + kvec(3)*center(3)
      zconst = cdexp(eye*cdot)/(eye*wavek)
      call prin2(' zconst is *',zconst,2)
      call cart2polar(zdiff,r,theta,phi)
      ctheta = dcos(theta)
      cphi = dcos(phi)
      sphi = dsin(phi)
      ephi1=dcmplx(cphi,sphi)
c
c ... get all the e^{eye*m*phi}:
c
      ephi1inv=1.0d0/ephi1
c
      ephi(0)=1.0d0
      ephi(1)=ephi1
      ephi(-1)=ephi1inv
      do 1040 i=2,nterms
         ephi(i)=ephi(i-1)*ephi1
         ephi(-i)=ephi(-i+1)*ephi1inv
 1040 continue
c
c ... get the associated Legendre functions:
c
      call ylgndr(nterms,ctheta,pp)
c
c ... finally, calculate the coeff locexp:
c
      lwfjs = 2000
      ifder = 1
      z = wavek*radius
      call jfuns3d(jer,nterms,z,rscale,fjs,ifder,fjder,
     1             lwfjs,iw,ntop)
      cscale = rscale
      pot(0,0)=pot(0,0) + zconst*cscale*fjs(0)
      dpotdn(0,0)=dpotdn(0,0) + zconst*cscale*fjder(0)*wavek
      do 2040 n=1,nterms
         cscale = eye*cscale*rscale
         pot(n,0)=pot(n,0) + pp(n,0)*zconst*cscale*fjs(n)
         dpotdn(n,0)=dpotdn(n,0) + pp(n,0)*zconst*cscale*fjder(n)*wavek
         do 2020 m=1,n
            ztmp=pp(n,m)*zconst
	    pot(n,m)=pot(n,m) + ztmp*ephi(-m)*cscale*fjs(n)
	    pot(n,-m)=pot(n,-m) + ztmp*ephi(m)*cscale*fjs(n)
	    dpotdn(n,m)=dpotdn(n,m) +
     1 	    ztmp*ephi(-m)*cscale*fjder(n)*wavek
	    dpotdn(n,-m)=dpotdn(n,-m) +
     1	    ztmp*ephi(m)*cscale*fjder(n)*wavek
 2020    continue
 2040 continue
      return
      end
c
c


*DECK GAMR
      FUNCTION GAMR (X)
C***BEGIN PROLOGUE  GAMR
C***PURPOSE  Compute the reciprocal of the Gamma function.
C***LIBRARY   SLATEC (FNLIB)
C***CATEGORY  C7A
C***TYPE      SINGLE PRECISION (GAMR-S, DGAMR-D, CGAMR-C)
C***KEYWORDS  FNLIB, RECIPROCAL GAMMA FUNCTION, SPECIAL FUNCTIONS
C***AUTHOR  Fullerton, W., (LANL)
C***DESCRIPTION
C
C GAMR is a single precision function that evaluates the reciprocal
C of the gamma function for single precision argument X.
C
C***REFERENCES  (NONE)
C***ROUTINES CALLED  ALGAMS, GAMMA, XERCLR, XGETF, XSETF
C***REVISION HISTORY  (YYMMDD)
C   770701  DATE WRITTEN
C   861211  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900727  Added EXTERNAL statement.  (WRB)
C***END PROLOGUE  GAMR
      EXTERNAL GAMMA
C***FIRST EXECUTABLE STATEMENT  GAMR
      GAMR = 0.0
      IF (X.LE.0.0 .AND. AINT(X).EQ.X) RETURN
C
      CALL XGETF (IROLD)
      CALL XSETF (1)
      IF (ABS(X).GT.10.0) GO TO 10
      GAMR = 1.0/GAMMA(X)
      CALL XERCLR
      CALL XSETF (IROLD)
      RETURN
C
 10   CALL ALGAMS (X, ALNGX, SGNGX)
      CALL XERCLR
      CALL XSETF (IROLD)
      GAMR = SGNGX * EXP(-ALNGX)
      RETURN
C
      END


module material_water
  use EOS_JWL
  implicit none
  integer,parameter :: material_type = 0
  double precision :: a     = 15.85e11
  double precision :: b     =-0.0467e11
  double precision :: r1    = 8.94
  double precision :: r2    = 1.45
  double precision :: cv    = 3.498e6
  double precision :: gamma = 2.0d0
  double precision :: rho0  = 8900.d0
  NAMELIST/material_water_params/ a, b, gamma, cv, r1, &
                                  r2, rho0

  logical :: if_initialized = .false.
contains
subroutine write_material_water_inp(funit)
  implicit none
  integer,intent(in) :: funit
  write(funit,NML=material_water_params)
end subroutine write_material_water_inp

subroutine initial_material_water(funit)
  implicit none
  integer,intent(in) :: funit
  if(.not.if_initialized) then
  read(funit,NML=material_water_params)
endif
end subroutine initial_material_water

subroutine scale_material_water(rho_rv,u_rv,temp_rv,time_rv, &
                          p_rv,e_rv,l_rv,cv_rv)
  implicit none
  double precision,intent(in) :: rho_rv,u_rv,temp_rv,time_rv, &
                                p_rv,e_rv,l_rv,cv_rv

    if(.not.if_initialized) then
      !-------Reactant
  a     = a/p_rv
  b     = b/p_rv
  cv    = cv/cv_rv
  rho0  = rho0/rho_rv
  if_initialized = .true.
endif
end subroutine scale_material_water

! compute the internal energy of reactant
subroutine compute_internal_energy_water(ei,p,r)
  implicit none
  double precision,intent(in) :: p,r
  double precision,intent(inout) :: ei
  double precision :: p_ref, ei_ref
  call compute_pref_eos_jwl(p_ref,r,a,b,r1,r2,rho0)
  call compute_eiref_eos_jwl(ei_ref,r,a,b,r1,r2,rho0)
  ei = (p - p_ref)/(r*gamma) + ei_ref
end subroutine compute_internal_energy_water

! compute the sound speed
subroutine compute_sound_speed2_water(c2,p,rho)
  implicit none
  double precision,intent(in) :: p,rho
  double precision,intent(inout) :: c2
  double precision :: dedp,dedrho
  call compute_dedp_eos_jwl(dedp,p,rho,gamma)
  call compute_dedr_eos_jwl(dedrho,p,rho,a,b,r1,r2,rho0,gamma)
  c2 = (p/rho**2-dedrho)/dedp
end subroutine compute_sound_speed2_water

subroutine compute_xi_water(xi,r)
  implicit none
  double precision :: r
  double precision,intent(inout) :: xi
  double precision :: gg
  xi = 1.d0/gamma
end subroutine compute_xi_water

subroutine compute_temperature_water(t,cv_o,p,rho)
  implicit none
  double precision,intent(in) :: p,rho
  double precision,intent(inout) :: t,cv_o
  double precision :: pref
  call compute_pref_eos_jwl(pref,rho,a,b,r1,r2,rho0)
  t = (p - pref)/(rho*gamma*cv)
  cv_o = cv
end subroutine compute_temperature_water

subroutine compute_eref_water(ei_ref,rho)
  implicit none
  double precision,intent(in) :: rho
  double precision,intent(inout) :: ei_ref
  call compute_eiref_eos_jwl(ei_ref,rho,a,b,r1,r2,rho0)
end subroutine compute_eref_water

subroutine compute_pref_water(p_ref,rho)
  implicit none
  double precision,intent(in) :: rho
  double precision,intent(inout) :: p_ref
  call compute_pref_eos_jwl(p_ref,rho,a,b,r1,r2,rho0)
end subroutine compute_pref_water

subroutine compute_rhogamma_water(rhogamma,rho)
  implicit none
  double precision,intent(in) :: rho
  double precision,intent(inout) :: rhogamma
  rhogamma = rho*gamma
end subroutine compute_rhogamma_water

subroutine compute_pressure_water(p,ei,rho)
  implicit none
  double precision,intent(in) :: rho,ei
  double precision,intent(inout) :: p
  double precision :: pref,eref
  call compute_pref_eos_jwl(pref,rho,a,b,r1,r2,rho0)
  p = pref + gamma*rho*(ei-eref)
end subroutine compute_pressure_water

! compute the reaction rate of IG model
subroutine compute_source_water(src0, src_num,                     &
  comm_prim, comm_prim_num,          &
  phs_prim, phs_prim_num,            &
  phs_cpnt_prim, phs_cpnt_prim_num)
  implicit none
  integer,intent(in) :: src_num, comm_prim_num, &
                        phs_prim_num, phs_cpnt_prim_num
  double precision,dimension(comm_prim_num),intent(in) :: comm_prim
  double precision,dimension(phs_prim_num),intent(in) :: phs_prim
  double precision,dimension(phs_cpnt_prim_num),intent(in) :: phs_cpnt_prim
  double precision,dimension(src_num),intent(inout) :: src0
end subroutine compute_source_water
end module


module subcellfv_mod

use constants_mod

!integer, parameter  :: RP = selected_real_kind(15)
!real(dp), parameter     :: PI = 4.0_rp * atan(1.0_rp)
!real(dp), parameter     :: TOL = 4.0e-16

contains

subroutine subcellfv_midpoints(N, sfvw, sfvx, sfvb)

    integer,intent(in) :: N             !! number of sub-cells
    real(dp),intent(out)   :: sfvw          !! width of sub-volumes in reference space
    real(dp),intent(out)   :: sfvx(N)       !! cell-centers of sub-cells in reference space
    real(dp),intent(out)   :: sfvb(N+1)     !! positions of boundaries of sub-cells in reference space

    integer :: i

    sfvw  = 2.0_dp/N

    sfvb(1) = -1.0_dp

    do i = 1,N
      sfvb(i+1) =         sfvb(i) + sfvw
      sfvx(i)   = 0.5_dp*(sfvb(i) + sfvb(i+1))
    end do

end subroutine

subroutine subcellfv_vanderMondeMatrix(N, M, nodes, weights, Vdm)

    use lagrange_mod, only: lagrange_VanderMondeMatrix

    integer, intent(in)     :: N, M
    real(dp), intent(in)        :: nodes(N)
    real(dp), intent(in)        :: weights(N)
    real(dp), intent(out)       :: Vdm(M,N)

    real(dp) :: sfvw
    real(dp) :: sfvx(M)
    real(dp) :: sfvb(M+1)

    real(dp) :: sfvVdm(N,N)
    real(dp) :: sfvnodes(N)

    integer :: i,j,k

    Vdm = 0.0_dp

    CALL subcellfv_midpoints(M, sfvw, sfvx, sfvb)

    do i = 1,M

        sfvnodes = sfvb(i) + 0.5_dp*(nodes+1.0_dp) * (sfvb(i+1) - sfvb(i))

        call lagrange_VanderMondeMatrix(N, N, nodes, sfvnodes, sfvVdm)

        do j = 1,N
            do k = 1,N
                Vdm(i,j) = Vdm(i,j) + 0.5_dp * weights(k) * sfvVdm(k,j)
            end do
        end do

    end do

end subroutine

# if 0
subroutine subcellfv_dgmidpoints(N, weights, dgb)

    integer,intent(in) :: N
    real(dp),intent(in)    :: weights
    real(dp),intent(out)   :: dgx(N)
    real(dp),intent(out)   :: dgb(N+1)

    integer :: i

    dgb(1) = -1.0_dp

    do i = 1,N
      dgb(i+1) = dgb(i) + weights(i)
      dgx(i)   = 0.5_dp*(dgb(i) + dgb(i+1))
    end do

end subroutine

subroutine subcellfv_reconstruction(N, M, nodes, weights)

    use lagrange_mod, only: lagrange_VanderMondeMatrix

    integer, intent(in)     :: N, M
    real(dp), intent(in)        :: nodes(N)
    real(dp), intent(in)        :: weights(N)
    real(dp), intent(out)       :: Vdm(N,M)

    real(dp) :: sfvw
    real(dp) :: sfvx(M)
    real(dp) :: sfvb(M+1)

    real(dp) :: sfvVdm(N,N)
    real(dp) :: sfvnodes(N)

    real(dp) :: dgb(N)
    real(dp) :: dgb(N+1)

    integer :: i,j,k

    Vdm = 0.0_dp

    call subcellfv_midpoints(M, sfvw, sfvx, sfvb)
    call subcellfv_dgmidpoints(N, dgx, dgb)

    write (*,*) dgx
    write (*,*) dgb

    !do j = 1,M
    !    do i = 1,N
    !        Vdm(i,j) = merge(sfvw, 0.0,
    !    end do
    !end do

end subroutine
# endif

end module


MODULE bz_module

  use io_tools_module
  use symmetry_module, only: isymmetry, nsym, rgb

  implicit none

  PRIVATE
  PUBLIC :: generate_bz
  PUBLIC :: read_bz
  PUBLIC :: bzinfo
  PUBLIC :: construct_bzinfo_bz
  PUBLIC :: write_info_bz
  PUBLIC :: read_from_file_bz

  integer,PUBLIC :: Nbzsm
  integer,PUBLIC :: MMBZ
  real(8),allocatable,PUBLIC :: kbb(:,:)
  real(8),allocatable,PUBLIC :: weight_bz(:)

  integer :: nk
  integer :: mmm(3,2)
  integer :: npbz
  integer :: ndata_read_k=0
  real(8) :: kbb0(3)
  data kbb0/0.d0,0.d0,0.d0/
  integer :: use_inversion=1

  TYPE bzinfo
     integer :: nk
     integer :: n_irwedge, n_whole
     integer :: indx_range(2,3)
     integer,allocatable :: kgrid(:,:,:)
     integer,allocatable :: kpt(:,:)
     real(8),allocatable :: weight(:)
  END type bzinfo

  type(bzinfo) :: bz_master

  integer :: unit_out=20, unit_in=21
  character(16) :: file_out="bz_info"
  character(16) :: file_in=""

CONTAINS

  SUBROUTINE read_bz
    implicit none
    nk=0
    mmm(1:3,1)=(/ 0,0,0 /)
    mmm(1:3,2)=(/ 0,0,0 /)
    ndata_read_k=0
    kbb0(1:3)=0.0d0
    npbz=0
    use_inversion=1
    call IOTools_readStringKeyword( "FILEBZ", file_in )
    call IOTools_readIntegerKeyword( "NK", nk )
    call IOTools_readIntegerKeyword( "MMM1", mmm(:,1) )
    call IOTools_readIntegerKeyword( "MMM2", mmm(:,2) )
    call IOTools_readIntegerKeyword( "NPBZ", npbz )
    call IOTools_readIntegerKeyword( "INVBZ", use_inversion )
  END SUBROUTINE read_bz

  SUBROUTINE generate_bz
    implicit none
    logical :: disp_switch
    integer :: i,k,k1,iw,iw0,iw1,m1,m2,m3,mm1,mm2,mm3,i1,i2,i3,p1(3),p2(3)
    integer,allocatable :: mm(:,:),m(:,:),w(:),map(:)

    call write_border( 0," generate_bz(start)" )

    if ( file_in /= "" ) then

       call read_from_file_bz( file_in, kbb, weight_bz, nk, Nbzsm, MMBZ )

       allocate( m(3,Nbzsm) ) ; m=0
       allocate( mm(3,MMBZ) ) ; mm=0
       allocate( map(MMBZ)  ) ; map=0

    else

       if ( nk <= 0 ) then

          if ( all(mmm(:,1)==0) ) then
             call write_string( "gamma point sampling is assumed" )
             nk=2
             mmm(:,1)=(/ 2,2,2 /)
             mmm(:,2)=(/ 2,2,2 /)
          else
             call Monkhorst_Pack( nk, mmm )
          end if

       end if

       if ( isymmetry > 0 ) then

          call generate_bz_sym( m, mm, map )

       else

          m1 =mmm(1,1) ; m2 =mmm(2,1) ; m3 =mmm(3,1)
          mm1=mmm(1,2) ; mm2=mmm(2,2) ; mm3=mmm(3,2)

          k=(2*m1+1)*(2*m2+1)*(2*m3+1)*2
          allocate( mm(3,k),m(3,k),w(k),map(k) )
          mm=0 ; m=0 ; w=0 ; map=0
          k=0 ; k1=0

          iw1= 1
          iw0=-1 ; if ( use_inversion < 1 ) iw0=1

          do i1=-m1,m1,mm1
          do i2=-m2,m2,mm2
          loop_A : do i3=-m3,m3,mm3

             do iw=iw1,iw0,-2

                p1(1)=i1*iw ; p1(2)=i2*iw ; p1(3)=i3*iw ; p2(1:3)=p1(1:3)
                do i=1,3
                   p1(i)=mod(p2(i),nk)
                   if ( p1(i)>  nk/2 ) p1(i)=p1(i)-nk
                   if ( p1(i)<=-nk/2 ) p1(i)=p1(i)+nk
                end do
                if ( k1 > 0 ) then
                   do i=1,k1
                      if ( mm(1,i)==p1(1) .and. &
                           mm(2,i)==p1(2) .and. &
                           mm(3,i)==p1(3)         ) cycle loop_A
                   end do
                end if

                if ( iw == 1 ) then
                   k=k+1
                   m(1:3,k)=p1(1:3)
                   w(k)=1
                else
                   w(k)=2
                end if

                k1=k1+1
                mm(1:3,k1)=p1(1:3)
                map(k1)=k

             end do ! iw

          end do loop_A
          end do
          end do

          Nbzsm = k
          if ( npbz > k ) Nbzsm=npbz

          MMBZ  = k1

          allocate( weight_bz(Nbzsm) ) ; weight_bz=0.d0
          allocate( kbb(3,Nbzsm)     ) ; kbb=0.d0

          kbb(1:3,1:k)=real(m(1:3,1:k),8)/nk
          weight_bz(1:k)=real(w(1:k),8)/MMBZ

          if ( Nbzsm == ndata_read_k ) then
             kbb(1:3,1) = kbb0(1:3)
             ndata_read_k = 0
          end if

          deallocate( w )

       end if

    end if

    call construct_bzinfo_master &
         ( nk, Nbzsm, MMBZ, m, mm, map, weight_bz, bz_master )

    deallocate( map,m,mm )

    call check_disp_switch( disp_switch, 0 )
    if ( disp_switch ) then
       write(*,*) "Nbzsm, MMBZ =",Nbzsm,MMBZ
       write(*,'(1x,a4,a30,a12)') "","kbb","weight_bz"
       do k=1,Nbzsm
          write(*,'(1x,i4,3f10.5,f12.5)') k,kbb(:,k),weight_bz(k)
       end do
    end if

    call write_border( 0," generate_bz(end)" )

  END SUBROUTINE generate_bz

  SUBROUTINE generate_bz_sym( m, mm, map )
    implicit none
    integer,allocatable,intent(INOUT) :: m(:,:), mm(:,:), map(:)
    logical :: disp_switch
    integer,allocatable :: w(:),w1(:),w2(:)
    integer :: m1,m2,m3,mm1,mm2,mm3,i1,i2,i3,p1(3),p2(3)
    integer :: i,k,k1,iw,iw0,iw1,nkmax,p3(3),ns,ni,is,nni,ig
    real(8) :: c,tmp(3)

    call write_border( 0," generate_bz_sym(start)" )

    m1 =mmm(1,1) ; m2 =mmm(2,1) ; m3 =mmm(3,1)
    mm1=mmm(1,2) ; mm2=mmm(2,2) ; mm3=mmm(3,2)

    nkmax=(2*m1+1)*(2*m2+1)*(2*m3+1)*2
    allocate( mm(3,nkmax),m(3,nkmax),w(nkmax) ) ; mm=0 ; m=0 ; w=0
    allocate( map(nkmax) ) ; map=0
    allocate( w1(nkmax),w2(nkmax) ) ; w1=0 ; w2=0

    iw1= 1
    iw0=-1 ; if ( use_inversion < 1 ) iw0=1

    ni=1
    is=1

    do i1=-m1,m1,mm1
    do i2=-m2,m2,mm2
       loop_3 : do i3=-m3,m3,mm3

          p1(1)=i1 ; p1(2)=i2 ; p1(3)=i3 ; p2(1:3)=p1(1:3)

          do i=1,3
             p1(i)=mod(p2(i),nk)
             if ( p1(i) >   nk/2 ) p1(i)=p1(i)-nk
             if ( p1(i) <= -nk/2 ) p1(i)=p1(i)+nk
          end do

          do i=1,ni-1
             if ( all(p1(1:3)==mm(1:3,i)) ) cycle loop_3
          end do

          ns =0
          nni=ni

          do iw=iw1,iw0,-2
             loop_sym : do ig=1,nsym

                if ( ni>nkmax ) stop "generate_bz_sym(1)"

                tmp(:) = matmul( rgb(:,:,ig),p1(:) )*iw
                p3(:) = nint( tmp(:) )

                do i=1,3
                   p2(i)=mod(p3(i),nk)
                   if ( p2(i) >  nk/2 ) p2(i)=p2(i)-nk
                   if ( p2(i) <=-nk/2 ) p2(i)=p2(i)+nk
                end do

                do i=nni,ni-1
                   if ( all(p2(:)==mm(:,i)) ) cycle loop_sym
                end do
                ns=ns+1
                mm(:,ni)=p2(:)
                map(ni)=is
                ni=ni+1

             end do loop_sym
          end do ! iw

          w(is)=ns

          m(1:3,is)=mm(1:3,nni)

          is=is+1

       end do loop_3
    end do ! i2
    end do ! i1

    is=is-1
    ni=ni-1

    do k=1,ni
       w1(k)=1
       do i=1,3
          i1=mod(i,3)+1
          i2=mod(i+1,3)+1
          if ( abs(mm(i,k)) == nk/2 ) then
             do k1=1,ni
                if ( k == k1 ) cycle
                if ( mm(i ,k1) ==-mm(i ,k) .and. &
                     mm(i1,k1) == mm(i1,k) .and. &
                     mm(i2,k1) == mm(i2,k) ) w1(k)=w1(k)*2
             end do
          end if
       end do ! i
    end do ! k

    do k=1,is
       do k1=1,ni
          if ( all(m(1:3,k)==mm(1:3,k1)) ) then
             w2(k)=w1(k1)
             exit
          end if
       end do
    end do

    Nbzsm = is
    if ( npbz > is ) Nbzsm = npbz

    MMBZ = ni

    allocate( weight_bz(Nbzsm) ) ; weight_bz=0.0d0
    allocate( kbb(3,Nbzsm)     ) ; kbb=0.0d0

    do k=1,Nbzsm
       kbb(1:3,k)   = real( m(1:3,k), 8 )/real( nk, 8 )
       weight_bz(k) = real( w(k), 8 )/real( w2(k), 8 )
    end do

    c=sum( weight_bz(:) )
    weight_bz(:)=weight_bz(:)/c

    call check_disp_switch( disp_switch, 0 )
    if ( disp_switch ) then
       write(*,*) "sum(w)  =",sum(w),sum(w2),sum(w1)
       write(*,*) "sum(wbz)=",sum(weight_bz)
    end if

    deallocate( w2,w1,w )

    call write_border( 0," generate_bz_sym(end)" )

    return

  END SUBROUTINE generate_bz_sym

  SUBROUTINE construct_bzinfo_master( nk, n_ir, n_wh, m, mm, map, w, bz )
    implicit none
    integer,intent(IN) :: nk, n_ir, n_wh
    integer,intent(IN) :: m(3,n_ir), mm(3,n_wh), map(n_wh)
    real(8),intent(IN) :: w(n_ir)
    type(bzinfo),intent(INOUT) :: bz
    integer :: i,j
    bz%nk        = nk
    bz%n_irwedge = n_ir
    bz%n_whole   = n_wh
    do i=1,3
       bz%indx_range(1,i) = minval( mm(i,1:n_wh) )
       bz%indx_range(2,i) = maxval( mm(i,1:n_wh) )
    end do
    allocate( bz%kgrid(bz%indx_range(1,1):bz%indx_range(2,1) &
                      ,bz%indx_range(1,2):bz%indx_range(2,2) &
                      ,bz%indx_range(1,3):bz%indx_range(2,3)) ) ; bz%kgrid=0
    do i=1,n_wh
       bz%kgrid( mm(1,i),mm(2,i),mm(3,i) ) = map(i)
    end do
    allocate( bz%kpt(3,n_ir)  ) ; bz%kpt(:,:) = m(:,:)
    allocate( bz%weight(n_ir) ) ; bz%weight(:) = w(:)
  END SUBROUTINE construct_bzinfo_master

  SUBROUTINE construct_bzinfo_bz( bz )
    implicit none
    type(bzinfo),intent(INOUT) :: bz
    bz=bz_master
    return
!
! Allocatable arrays in derived-type variables are automatically allocated
! in Fortran2003 or later. The followings may work with older Fortran.
!
!    bz%nk        = bz_master%nk
!    bz%n_irwedge = bz_master%n_irwedge
!    bz%n_whole   = bz_master%n_whole
!    bz%indx_range(:,:) = bz_master%indx_range(:,:)
!    allocate( bz%kgrid(bz%indx_range(1,1):bz%indx_range(2,1) &
!                      ,bz%indx_range(1,2):bz%indx_range(2,2) &
!                      ,bz%indx_range(1,3):bz%indx_range(2,3)) ) ; bz%kgrid=0
!    bz%kgrid(:,:,:) = bz_master%kgrid(:,:,:)
!    allocate( bz%kpt(3,bz%n_irwedge)  ) ; bz%kpt(:,:) = bz_master%kpt(:,:)
!    allocate( bz%weight(bz%n_irwedge) ) ; bz%weight(:) = bz_master%weight(:)
  END SUBROUTINE construct_bzinfo_bz

  SUBROUTINE write_info_bz( b_vec, bz_in )

    implicit none
    real(8),intent(IN) :: b_vec(3,3)
    type(bzinfo),optional,intent(IN) :: bz_in
    integer :: k,k1,k2,k3,ir
    type(bzinfo) :: bz

    if ( file_in /= "" ) return

    if ( present(bz_in) ) then
       bz=bz_in
    else
       bz=bz_master
    end if

    open( unit_out, file=file_out )
    write(unit_out,'("Reciprocal lattice vectors")')
    write(unit_out,'(1x,3f22.15)') b_vec(1:3,1)
    write(unit_out,'(1x,3f22.15)') b_vec(1:3,2)
    write(unit_out,'(1x,3f22.15)') b_vec(1:3,3)
    write(unit_out,'("Number of division along a side            :",i4)') bz%nk
    write(unit_out,'("Number of k points in the irreducible wedge:",i4)') bz%n_irwedge
    write(unit_out,'("Number of k points in the whole BZ         :",i4)') bz%n_whole
    write(unit_out,'("Sampled k points")')
    write(unit_out,'(9x,3a4,a20)') "k1","k2","k3","weight"
    do k=1,bz%n_irwedge
       write(unit_out,'(1x,i6,2x,3i4,2x,3f20.15)') k,bz%kpt(1:3,k),bz%weight(k)
    end do
    write(unit_out,'("grid index range (k1):",2i4)') bz%indx_range(1:2,1)
    write(unit_out,'("grid index range (k2):",2i4)') bz%indx_range(1:2,2)
    write(unit_out,'("grid index range (k3):",2i4)') bz%indx_range(1:2,3)
    write(unit_out,'("k points in the whole BZ")')
    k=0
    do k3=bz%indx_range(1,3),bz%indx_range(2,3)
    do k2=bz%indx_range(1,2),bz%indx_range(2,2)
    do k1=bz%indx_range(1,1),bz%indx_range(2,1)
       ir=bz%kgrid(k1,k2,k3)
       if ( ir == 0 ) cycle
       k=k+1
       write(unit_out,'(1x,i6,2x,3i4,i6)') k,k1,k2,k3,ir
    end do
    end do
    end do
    close(unit_out)

  END SUBROUTINE write_info_bz

  SUBROUTINE read_from_file_bz( file, kbb, weight, nk, NBZsmp, NBZtot )

    implicit none
    character(*),intent(IN) :: file
    real(8),allocatable,intent(INOUT) :: kbb(:,:), weight(:)
    integer,intent(OUT) :: nk, NBZsmp, NBZtot
    integer :: k,mrnk
    character(32) :: cbuf
    include 'mpif.h'

    if ( file == "" ) return

    call write_border( 0, "read_form_file_bz(start)" )

    if ( allocated(kbb) ) deallocate(kbb)
    if ( allocated(weight_bz) ) deallocate(weight_bz)

    call MPI_COMM_RANK( MPI_COMM_WORLD, mrnk, k )

    if ( mrnk == 0 ) then

       open(unit_in,file=file,status="old")

       read(unit_in,'(a)') cbuf
       backspace(unit_in)

       if ( cbuf == "Reciprocal lattice vectors" ) then
          call read_info_bz( kbb, weight, nk, NBZsmp, NBZtot )
       else
          read(unit_in,*) NBZsmp
          allocate( kbb(3,NBZsmp) ) ; kbb=0.0d0
          do k=1,NBZsmp
             read(unit_in,*) kbb(:,k)
          end do
          allocate( weight(NBZsmp) ) ; weight=0.0d0
          nk=1
          NBZtot=NBZsmp
       end if

       close(unit_in)

    end if

    call MPI_BCAST( NBZsmp, 1, MPI_INTEGER, 0, MPI_COMM_WORLD, k )
    call MPI_BCAST( nk    , 1, MPI_INTEGER, 0, MPI_COMM_WORLD, k )
    call MPI_BCAST( NBZtot, 1, MPI_INTEGER, 0, MPI_COMM_WORLD, k )

    if ( .not.allocated(kbb) ) then
       allocate( kbb(3,NBZsmp) ) ; kbb=0.0d0
    end if
    if ( .not.allocated(weight) ) then
       allocate( weight(NBZsmp) ) ; weight=0.0d0
    end if

    call MPI_BCAST( kbb, size(kbb), MPI_REAL8, 0, MPI_COMM_WORLD, k )
    call MPI_BCAST( weight, size(weight), MPI_REAL8, 0, MPI_COMM_WORLD, k )

    kbb(:,:) = kbb(:,:)/dble(nk)

    call write_border( 0, "read_form_file_bz(end)" )

  END SUBROUTINE read_from_file_bz

  SUBROUTINE read_info_bz( kbb, weight, n_div, n_irwedge, n_whole )
    implicit none
    real(8),allocatable,intent(INOUT) :: kbb(:,:), weight(:)
    integer,intent(OUT) :: n_div, n_irwedge, n_whole
    character(80) :: cbuf
    integer :: i,k
    read(unit_in,*)
    read(unit_in,*)
    read(unit_in,*)
    read(unit_in,*)
    read(unit_in,'(a)') cbuf
    call get_num_from_chr( cbuf, n_div )
    read(unit_in,'(a)') cbuf
    call get_num_from_chr( cbuf, n_irwedge )
    read(unit_in,'(a)') cbuf
    call get_num_from_chr( cbuf, n_whole )
    read(unit_in,*)
    read(unit_in,*)
    if ( allocated(kbb) ) deallocate(kbb)
    allocate( kbb(3,n_irwedge) ) ; kbb=0.0d0
    if ( allocated(weight) ) deallocate(weight)
    allocate( weight(n_irwedge) ) ; weight=0.0d0
    do i=1,n_irwedge
       read(unit_in,*) k, kbb(1:3,i), weight(i)
    end do
  END SUBROUTINE read_info_bz

  SUBROUTINE get_num_from_chr( cbuf, n )
    implicit none
    character(*),intent(IN) :: cbuf
    integer,intent(OUT) :: n
    integer :: i
    do i=1,len_trim(cbuf)
       if ( cbuf(i:i) == ":" ) exit
    end do
    read(cbuf(i+1:),*) n
  END SUBROUTINE get_num_from_chr

  SUBROUTINE Monkhorst_Pack( n, m )
    implicit none
    integer,intent(INOUT) :: n, m(3,2)
    logical :: disp
    integer :: lcm_12, lcm, i
    call check_disp_switch( disp, 0 )
    if ( disp ) then
       write(*,'(1x,"nk=",i5,"   ( MP mesh is used )")') n
       write(*,'(1x,"mmm1=",3i4)') m(:,1)
    end if
! Least Common Multiplier
    lcm_12 = m(1,1)*m(2,1)/gcd( m(1,1),m(2,1) )
    n = lcm_12*m(3,1)/gcd( lcm_12,m(3,1) )
    n = n*2
    m(:,2) = n/m(:,1)
    do i=1,3
       if ( mod(m(i,1),2) == 1 ) then
          m(i,1) = m(i,2)*m(i,1)
       else
          m(i,1) = (n/2)/m(i,1) + m(i,2)*m(i,1)
       end if
    end do
    if ( disp ) then
       write(*,'(1x,"nk=",i5)') n
       write(*,'(1x,"mmm1=",3i4)') m(:,1)
       write(*,'(1x,"mmm2=",3i4)') m(:,2)
    end if
  END SUBROUTINE Monkhorst_Pack

  FUNCTION gcd(m0,n0)
    implicit none
    integer :: gcd,m0,n0
    integer :: m,n,mtmp,loop

    if ( m0 >= n0 ) then
       m=m0
       n=n0
    else
       m=n0
       n=m0
    end if

    do loop=1,10000
       if ( n == 0 ) exit
       mtmp = n
       n = mod(m,n)
       m = mtmp
    end do

    gcd = m

  END FUNCTION gcd

END MODULE bz_module


!---------------------------------------------------!
! Copyright (c) 2016 Shunsuke A. Sato               !
! Released under the MIT license                    !
! https://opensource.org/licenses/mit-license.php   !
!---------------------------------------------------!
!-------10--------20--------30--------40--------50--------60--------70--------80--------90
subroutine preparation_RT
  use global_variables
  implicit none
  integer :: ix,iy

  allocate(zwfn(0:Nx,0:NR))
  allocate(dipole_t(0:Nt_iter),norm_t(0:Nt_iter),quadrupole_t(0:Nt_iter))
!Initial condition
!  zwfn = wfn ! GS wavefunction

!!momentum kick
!  do iy=0,NR
!    do ix=0,Nx
!      zwfn(ix,iy) = exp(zI*kick_mom*xn(ix))*wfn(ix,iy)
!    end do
!  end do

!!quadrupole kick for nuclei
  do iy=0,NR
    do ix=0,Nx
      zwfn(ix,iy) = exp(zI*kick_mom*Rn(iy)**2)*wfn(ix,iy)
    end do
  end do

  return
end subroutine preparation_RT