!--------------------------------------------------------------------------------
! Copyright (c) 2018 Peter Grünberg Institut, Forschungszentrum Jülich, Germany
! This file is part of FLEUR and available as free software under the conditions
! of the MIT license as expressed in the LICENSE file in more detail.
!--------------------------------------------------------------------------------

MODULE m_unfold_band_kpts

CONTAINS

  SUBROUTINE build_primitive_cell(banddos,p_cell,cell)
    USE m_types
    USE m_inv3
    USE m_constants, ONLY : tpi_const
    implicit none
    TYPE(t_banddos),INTENT(IN)  :: banddos
    TYPE(t_cell),INTENT(IN)     :: cell
    TYPE(t_cell),INTENT(INOUT)  :: p_cell

    INTEGER :: i
    DO i =1,3
	p_cell%amat(1,i)=cell%amat(1,i)/banddos%s_cell_x
	p_cell%amat(2,i)=cell%amat(2,i)/banddos%s_cell_y
	p_cell%amat(3,i)=cell%amat(3,i)/banddos%s_cell_z
!	p_cell%amat(i,1)=cell%amat(i,1)/banddos%s_cell_x
!	p_cell%amat(i,2)=cell%amat(i,2)/banddos%s_cell_y
!	p_cell%amat(i,3)=cell%amat(i,3)/banddos%s_cell_z
    END DO
    CALL inv3(p_cell%amat,p_cell%bmat,p_cell%omtil)
    p_cell%bmat=p_cell%bmat*tpi_const
  END SUBROUTINE  build_primitive_cell

  SUBROUTINE unfold_band_kpts(banddos,p_cell,cell,p_kpts,kpts)
    USE m_types
    USE m_inv3
    USE m_constants, ONLY : tpi_const

    implicit none

    TYPE(t_banddos),INTENT(IN)  :: banddos
    TYPE(t_cell),INTENT(IN)     :: cell
    TYPE(t_cell),INTENT(INOUT)  :: p_cell
    TYPE(t_kpts),INTENT(INOUT)  :: p_kpts
    TYPE(t_kpts),INTENT(INOUT)  :: kpts

    CALL build_primitive_cell(banddos,p_cell,cell)

    p_kpts=kpts
    !write(1088,*) 'banddos%unfoldband: ', banddos%unfoldband
    !write(1088,*) 'brav. matrix: '
    !write(1088,'(f15.8,f15.8,f15.8)') cell%amat(1,1), cell%amat(1,2), cell%amat(1,3)
    !write(1088,'(f15.8,f15.8,f15.8)') cell%amat(2,1), cell%amat(2,2), cell%amat(2,3)
    !write(1088,'(f15.8,f15.8,f15.8)') cell%amat(3,1), cell%amat(3,2), cell%amat(3,3)
    !write(1088,*) 'brav. rez. matrix: '
    !write(1088,'(f15.8,f15.8,f15.8)') cell%bmat(1,1), cell%bmat(1,2), cell%bmat(1,3)
    !write(1088,'(f15.8,f15.8,f15.8)') cell%bmat(2,1), cell%bmat(2,2), cell%bmat(2,3)
    !write(1088,'(f15.8,f15.8,f15.8)') cell%bmat(3,1), cell%bmat(3,2), cell%bmat(3,3)
    !write(1088,*) ' primitive brav. matrix: '
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%amat(1,1), p_cell%amat(1,2), p_cell%amat(1,3)
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%amat(2,1), p_cell%amat(2,2), p_cell%amat(2,3)
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%amat(3,1), p_cell%amat(3,2), p_cell%amat(3,3)
    !write(1088,*) 'primitive brav. rez. matrix: '
    !write(89,'(3f15.8)') p_cell%bmat
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%bmat(1,1), p_cell%bmat(1,2), p_cell%bmat(1,3)
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%bmat(2,1), p_cell%bmat(2,2), p_cell%bmat(2,3)
    !write(1088,'(f15.8,f15.8,f15.8)') p_cell%bmat(3,1), p_cell%bmat(3,2), p_cell%bmat(3,3)
    !write(1088,'(a,i7,a,i7)') 'kpts%nkpt',kpts%nkpt,'   p_kpts%nkpt',p_kpts%nkpt
    !write(1088,*) kpts%specialPoints
  END SUBROUTINE unfold_band_kpts

  SUBROUTINE find_supercell_kpts(banddos,p_cell,cell,p_kpts,kpts)
    USE m_types
    USE m_juDFT
    USE m_inv3
    implicit none

    TYPE(t_banddos),INTENT(IN)  :: banddos
    TYPE(t_cell),INTENT(IN)     :: cell
    TYPE(t_cell),INTENT(IN)     :: p_cell
    TYPE(t_kpts),INTENT(IN)     :: p_kpts
    TYPE(t_kpts),INTENT(INOUT)  :: kpts

    INTEGER :: i,m1,m2,m3
    REAL    :: rez_inv_to_internal(3,3)
    REAL    :: rez_inv_det
    REAL    :: list(13,p_kpts%nkpt)  !cartesion coordinates for k,K,m
    REAL    :: pc_kpoint_i(3)    !primitive cell kpoint internal
    REAL    :: sc_kpoint_i(3)    !super cell kpoint internal
    REAL    :: pc_kpoint_c(3)    !primitive cell kpoint cartesian
    REAL    :: sc_kpoint_c(3)    !super cell kpoint cartesian
    REAL    :: eps(3)
    REAL    :: eps_r, eps_kpt
    LOGICAL :: representation_found
    REAL    ::kpt_dist

    eps = 1.0e-10
    eps_r = 0.000000001

    CALL inv3(cell%bmat,rez_inv_to_internal,rez_inv_det)
    !write(1088,*) p_kpts%specialPoints
    !write(333,'(3f15.8)')p_kpts%bk
    kpt_dist=0
    DO i= 1,size(list,2)
	!        pc_kpoint_c(1)=p_kpts%bk(1,i)*p_cell%bmat(1,1)+p_kpts%bk(2,i)*p_cell%bmat(1,2)+p_kpts%bk(3,i)*p_cell%bmat(1,3)
	!        pc_kpoint_c(2)=p_kpts%bk(1,i)*p_cell%bmat(2,1)+p_kpts%bk(2,i)*p_cell%bmat(2,2)+p_kpts%bk(3,i)*p_cell%bmat(2,3)
	!        pc_kpoint_c(3)=p_kpts%bk(1,i)*p_cell%bmat(3,1)+p_kpts%bk(2,i)*p_cell%bmat(3,2)+p_kpts%bk(3,i)*p_cell%bmat(3,3)
		pc_kpoint_c(1)=p_kpts%bk(1,i)*p_cell%bmat(1,1)+p_kpts%bk(2,i)*p_cell%bmat(2,1)+p_kpts%bk(3,i)*p_cell%bmat(3,1)
		pc_kpoint_c(2)=p_kpts%bk(1,i)*p_cell%bmat(1,2)+p_kpts%bk(2,i)*p_cell%bmat(2,2)+p_kpts%bk(3,i)*p_cell%bmat(3,2)
		pc_kpoint_c(3)=p_kpts%bk(1,i)*p_cell%bmat(1,3)+p_kpts%bk(2,i)*p_cell%bmat(2,3)+p_kpts%bk(3,i)*p_cell%bmat(3,3)
		list(1,i)=pc_kpoint_c(1)
		list(2,i)=pc_kpoint_c(2)
		list(3,i)=pc_kpoint_c(3)
	!!!!------- finding kpts in primitive rez. unit cell -----
	!	representation_found=.false.
	!m_loop:	DO m1= -banddos%s_cell_x,banddos%s_cell_x
	!		DO m2= -banddos%s_cell_y,banddos%s_cell_y
	!			DO m3= -banddos%s_cell_z,banddos%s_cell_z
	!				pc_kpoint_c(1)=list(1,i)-m1*cell%bmat(1,1)-m2*cell%bmat(1,2)-m3*cell%bmat(1,3)
	!				pc_kpoint_c(2)=list(2,i)-m1*cell%bmat(2,1)-m2*cell%bmat(2,2)-m3*cell%bmat(2,3)
	!				pc_kpoint_c(3)=list(3,i)-m1*cell%bmat(3,1)-m2*cell%bmat(3,2)-m3*cell%bmat(3,3)
	!!				IF (         (dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,1)) >= 0).AND.((dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,1)) < dot_product(cell%bmat(:,1), cell%bmat(:,1)))) &
	!!				     & .AND. (dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,2)) >= 0).AND.((dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,2)) < dot_product(cell%bmat(:,2), cell%bmat(:,2)))) &
	!!				     & .AND. (dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,3)) >= 0).AND.((dot_product(pc_kpoint_c(:)+eps(:), cell%bmat(:,3)) < dot_product(cell%bmat(:,3), cell%bmat(:,3))))) THEN
	!				IF (all((matmul(rez_inv_to_internal,pc_kpoint_c)+eps(:))>=0).and.all((matmul(rez_inv_to_internal,pc_kpoint_c)+eps(:))<1)) THEN
	!					list(4,i)=pc_kpoint_c(1)
	!					list(5,i)=pc_kpoint_c(2)
	!					list(6,i)=pc_kpoint_c(3)
	!					list(7,i)=-m1
	!					list(8,i)=-m2
	!					list(9,i)=-m3
	!					representation_found=.true.
	!				END IF
	 !      			        IF (representation_found) EXIT m_loop
	!			END DO
	!		END DO
	!	END DO m_loop
	 !       IF (.not.representation_found) THEN
	  !      write(*,'(a,f15.8,f15.8,f15.8)') 'No representation found for the following kpoint:',list(1,i),list(2,i),list(3,i)
	   !     END IF
	   !----------------------- method internal coordintes --------------------
	    sc_kpoint_i(:)=matmul(pc_kpoint_c,rez_inv_to_internal)
	    pc_kpoint_i(:)=p_kpts%bk(1:3,i)
	    !sc_kpoint_i(:) = sc_kpoint_i(:) + 0.5
	    m1 = FLOOR(sc_kpoint_i(1))
	    m2 = FLOOR(sc_kpoint_i(2))
	    m3 = FLOOR(sc_kpoint_i(3))
	    m1=0
	    m2=0
	    m3=0
	    sc_kpoint_i(1) = sc_kpoint_i(1) - m1
	    sc_kpoint_i(2) = sc_kpoint_i(2) - m2
	    sc_kpoint_i(3) = sc_kpoint_i(3) - m3
	    !sc_kpoint_i(:) = sc_kpoint_i(:) - 0.5
	    list(4,i)=sc_kpoint_i(1)
	    list(5,i)=sc_kpoint_i(2)
	    list(6,i)=sc_kpoint_i(3)
	    list(7,i)=m1
	    list(8,i)=m2
	    list(9,i)=m3 !this whole block is to move kpoints into first BZ within -0.5 to 0.5

	!  	kpts%bk(:,i)=matmul(rez_inv_to_internal,pc_kpoint_c)
	    !-------------saving old kpts----------
	    list(11:13,i)=kpts%bk(:,i)
  	    !------finished---------
	    kpts%bk(:,i)=list(4:6,i)

	IF (i>1) THEN
	kpt_dist=kpt_dist+sqrt(dot_product(list(1:3,i)-list(1:3,i-1),list(1:3,i)-list(1:3,i-1)))
	END IF
	list(10,i)=kpt_dist
    END DO
    !write(91,'(3f15.8)') kpts%bk
    !write(92,*) kpts%wtkpt
    ALLOCATE (kpts%sc_list(13,p_kpts%nkpt))
    kpts%specialPointIndices(:) = p_kpts%specialPointIndices(:)
    kpts%sc_list=list
    !write(90,'(10f15.8)') kpts%sc_list
  END SUBROUTINE find_supercell_kpts

 SUBROUTINE calculate_plot_w_n(banddos,cell,kpts,smat_unfold,zMat,lapw,i_kpt,jsp,eig,results,input,atoms,unfoldingBuffer,fmpi)
	USE m_types
	USE m_juDFT
	USE m_inv3
	USE m_types_mpimat
        USE m_constants
	implicit none

        TYPE(t_input),INTENT(IN) :: input
        TYPE(t_atoms),INTENT(IN)     :: atoms
	TYPE(t_banddos),INTENT(IN)  :: banddos
	TYPE(t_results),INTENT(INOUT)  :: results
	TYPE(t_cell),INTENT(IN)     :: cell
	TYPE(t_kpts),INTENT(IN)     :: kpts
	CLASS(t_mat),INTENT(INOUT)  :: smat_unfold
	CLASS(t_mat),INTENT(IN)     :: zMat
	TYPE(t_lapw),INTENT(IN)     :: lapw
        TYPE(t_mpi),INTENT(IN)       :: fmpi
	TYPE(t_cell)      :: p_cell
	INTEGER, INTENT(IN)	    :: i_kpt,jsp
	REAL, INTENT(IN)	    :: eig(:)
        COMPLEX, INTENT(INOUT)         :: unfoldingBuffer(:,:,:)
	INTEGER :: i,j,k,l,n
	INTEGER :: na,n_i,nn,nk,nki,gi,lo
	REAL, ALLOCATABLE	::w_n(:)
	COMPLEX, ALLOCATABLE    ::w_n_c(:)
	REAL, ALLOCATABLE	::w_n_sum(:)
	COMPLEX, ALLOCATABLE    ::w_n_c_sum(:)
        LOGICAL :: method_rubel=.false.
        LOGICAL :: write_to_file=.false.
        CLASS(t_mat), ALLOCATABLE :: zMat_s

!	method_rubel=.true.    !this switch is to switch between overlap matrix and rubel method (without overlap matrix)

	CALL build_primitive_cell(banddos,p_cell,cell)
	IF (.not. method_rubel) THEN
		DO j = 1, lapw%nv(jsp)
		  DO i = 1, j-1
	      		IF(smat_unfold%l_real) THEN
				smat_unfold%data_r(j,i) = smat_unfold%data_r(i,j)
	      		ELSE
				smat_unfold%data_c(j,i) = CONJG(smat_unfold%data_c(i,j))
	      		END IF
		   END DO
		END DO
	END IF
!   	write_to_file=.true.
	IF (write_to_file) THEN
		IF (i_kpt==1) THEN
			IF (jsp==1) OPEN (679,file='bands_sc_old.1',status='unknown') !This is kind of my birthday 6 july 1992 (S.R.)
			IF (jsp==2) OPEN (680,file='bands_sc_old.2',status='unknown')
		END IF
	END IF

!		write(*,*) 'real zmat size dim 1:', size(zMat%data_r,1), 'dim2:', size(zMat%data_r,2)
!		write(*,*) 'smat dim1', size(smat_unfold%data_r,1), 'dim2', size(smat_unfold%data_r,2),'data',smat_unfold%data_r(2,2)
!		write(222,'(234f15.8)') zMat%data_r
!		write(223,'(234f15.8)') smat_unfold%data_r

	IF (zmat%l_real) THEN
		ALLOCATE(w_n(zMat%matsize2))
	        w_n = 0
!	    IF (method_rubel) THEN
		ALLOCATE(w_n_sum(zMat%matsize2))
	        w_n_sum = 0
!	    END IF
	ELSE
		ALLOCATE(w_n_c(zMat%matsize2))
		w_n_c=0
!	    IF (method_rubel) THEN
		ALLOCATE(w_n_c_sum(zMat%matsize2))
		w_n_c_sum=0
!	    END IF
	END IF
!---------create zmat_s--- smat*zmat---------------------
	select type(zMat)
		type is (t_mat)
		allocate(t_mat::zMat_s)
		select type(zMat_s)
             		type is (t_mat)
	     		zMat_s=zMat
		end select
		type is (t_mpimat)
		allocate(t_mpimat::zMat_s)
		select type(zMat_s)
             		type is (t_mpimat)
	     		zMat_s=zMat
		end select
	end select
!---------------------------------------------------------
!		write(345,'(3I6)') lapw%gvec(:,:,jsp)
	write (*,*)results%ef
        write (*,*) i_kpt
	IF (.not. method_rubel) THEN
!          IF (fmpi%n_size==1) THEN
!             call smat_unfold%multiply(zMat,zMat_s)
!          ELSE
!             call smat_unfold%mpimat_multiply(zMat,zMat_s)
!          ENDIF
           call smat_unfold%multiply(zMat,zMat_s)
        END IF
       !$omp parallel private(j,n_i,nn,na,lo,nk,nki,gi)
       !$omp do
	DO i=1,zMat%matsize2
		IF (method_rubel) THEN
			DO j=1,lapw%nv(jsp)
				IF (zmat%l_real) THEN
					w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat%data_r(j,i)
!						write(*,*) 'zMat is real'
				ELSE
					w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(j,i)
!						write(*,*) 'zMat is complex'
				END IF
				IF ((modulo(lapw%gvec(1,j,jsp)+NINT(kpts%sc_list(7,i_kpt)),banddos%s_cell_x)==0).AND.&
				     &(modulo(lapw%gvec(2,j,jsp)+NINT(kpts%sc_list(8,i_kpt)),banddos%s_cell_y)==0).AND.&
				     &(modulo(lapw%gvec(3,j,jsp)+NINT(kpts%sc_list(9,i_kpt)),banddos%s_cell_z)==0)) THEN
					IF (zmat%l_real) THEN
						w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat%data_r(j,i)
!							write(*,*) 'zMat is real'
					ELSE
						w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(j,i)
!							write(*,*) 'zMat is complex'
					END IF
			   	END IF
			END DO
!------------------LO's------------------------
			na=0
			DO n_i=1,atoms%ntype
				DO nn=1,atoms%neq(n_i)
					na=na+1
					DO lo=1,atoms%nlo(n_i)
						nk=lapw%nkvec(lo,na)
						DO nki=1,nk
							gi=lapw%kvec(nki,lo,na)
							j=lapw%nv(jsp)+lapw%index_lo(lo,na)+nki
							IF (zmat%l_real) THEN
								w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat%data_r(j,i)
							ELSE
								w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(j,i)
							END IF
							IF ((modulo(lapw%gvec(1,gi,jsp)+NINT(kpts%sc_list(7,i_kpt)),banddos%s_cell_x)==0).AND.&
							   &(modulo(lapw%gvec(2,gi,jsp)+NINT(kpts%sc_list(8,i_kpt)),banddos%s_cell_y)==0).AND.&
							   &(modulo(lapw%gvec(3,gi,jsp)+NINT(kpts%sc_list(9,i_kpt)),banddos%s_cell_z)==0)) THEN
								IF (zmat%l_real) THEN
									w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat%data_r(j,i)
								ELSE
									w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(j,i)
								END IF
							END IF
						END DO
					END DO
				END DO
			END DO
!--------------------------LO's finished----------------
		ELSE
			DO j=1,lapw%nv(jsp)
!				DO k=1,zMat%matsize1
					IF (zmat%l_real) THEN
!						w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat%data_r(k,i)*smat_unfold%data_r(j,k)
						w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat_s%data_r(j,i)
					ELSE
!						w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(k,i)*smat_unfold%data_c(j,k)
						w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat_s%data_c(j,i)
					END IF
!				END DO
				IF ((modulo(lapw%gvec(1,j,jsp)+NINT(kpts%sc_list(7,i_kpt)),banddos%s_cell_x)==0).AND.&
				   &(modulo(lapw%gvec(2,j,jsp)+NINT(kpts%sc_list(8,i_kpt)),banddos%s_cell_y)==0).AND.&
				   &(modulo(lapw%gvec(3,j,jsp)+NINT(kpts%sc_list(9,i_kpt)),banddos%s_cell_z)==0)) THEN
!					DO k=1,zMat%matsize1
						IF (zmat%l_real) THEN
!							w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat%data_r(k,i)*smat_unfold%data_r(j,k)
							w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat_s%data_r(j,i)
						ELSE
!							w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(k,i)*smat_unfold%data_c(j,k)
							w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat_s%data_c(j,i)
						END IF
!					END DO
				END IF
			END DO
!			write(1250+fmpi%irank,'(4f15.8)') w_n_c(i),w_n_c_sum(i)
!------------------LO's------------------------
      			na=0
      			DO n_i=1,atoms%ntype
        			DO nn=1,atoms%neq(n_i)
          				na=na+1
          				DO lo=1,atoms%nlo(n_i)
						nk=lapw%nkvec(lo,na)
						DO nki=1,nk
							gi=lapw%kvec(nki,lo,na)
							j=lapw%nv(jsp)+lapw%index_lo(lo,na)+nki
				!			DO k=1,zMat%matsize1
								IF (zmat%l_real) THEN
				!						w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat%data_r(k,i)*smat_unfold%data_r(j,k)
									w_n_sum(i)=w_n_sum(i)+zMat%data_r(j,i)*zMat_s%data_r(j,i)
								ELSE
				!						w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(k,i)*smat_unfold%data_c(j,k)
									w_n_c_sum(i)=w_n_c_sum(i)+CONJG(zMat%data_c(j,i))*zMat_s%data_c(j,i)
								END IF
				!			END DO
							IF ((modulo(lapw%gvec(1,gi,jsp)+NINT(kpts%sc_list(7,i_kpt)),banddos%s_cell_x)==0).AND.&
							   &(modulo(lapw%gvec(2,gi,jsp)+NINT(kpts%sc_list(8,i_kpt)),banddos%s_cell_y)==0).AND.&
							   &(modulo(lapw%gvec(3,gi,jsp)+NINT(kpts%sc_list(9,i_kpt)),banddos%s_cell_z)==0)) THEN
			!					DO k=1,zMat%matsize1
									IF (zmat%l_real) THEN
			!							w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat%data_r(k,i)*smat_unfold%data_r(j,k)
										w_n(i)=w_n(i)+zMat%data_r(j,i)*zMat_s%data_r(j,i)
									ELSE
			!							w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat%data_c(k,i)*smat_unfold%data_c(j,k)
										w_n_c(i)=w_n_c(i)+CONJG(zMat%data_c(j,i))*zMat_s%data_c(j,i)
									END IF
		    	!					END DO
							END IF
						END DO
					END DO
				END DO
			END DO
!--------------------------LO's finished----------------
		END IF
!		IF (method_rubel) THEN
		IF (write_to_file) THEN
			IF (zmat%l_real) THEN
				IF (w_n(i)/w_n_sum(i)<0) w_n(i)=0   ! delete negative entries
				IF (jsp==1) write(679,'(3f15.8)') kpts%sc_list(10,i_kpt), ((eig(i)-results%ef)*hartree_to_ev_const),w_n(i)/w_n_sum(i)
				IF (jsp==2) write(680,'(3f15.8)') kpts%sc_list(10,i_kpt), ((eig(i)-results%ef)*hartree_to_ev_const),w_n(i)/w_n_sum(i)
				IF ((w_n(i)/w_n_sum(i)>1).or.(w_n(i)/w_n_sum(i)<0)) write(*,*) 'w_n/sum larger 1 or smaller 0', w_n(i)/w_n_sum(i), 'eigenvalue',eig(i)
			ELSE
				IF (real(w_n_c(i))<0) w_n_c(i)=0    ! delete negative entries
				IF (jsp==1) write(679,'(4f15.8)') kpts%sc_list(10,i_kpt), ((eig(i)-results%ef)*hartree_to_ev_const),w_n_c(i)/w_n_c_sum(i)
				IF (jsp==2) write(680,'(4f15.8)') kpts%sc_list(10,i_kpt), ((eig(i)-results%ef)*hartree_to_ev_const),w_n_c(i)/w_n_c_sum(i)
				IF ((abs(w_n_c(i)/w_n_c_sum(i))>1).or.(real(w_n_c(i))<0)) write(*,*) 'w_n_c/sum larger 1 or smaller 0', w_n_c(i)/w_n_c_sum(i), 'eigenvalue',eig(i)
		        END IF
                END IF
		IF (zmat%l_real) THEN
			IF (w_n(i)/w_n_sum(i)<0) w_n(i)=0   ! delete negative entries
			unfoldingBuffer(i,i_kpt,jsp)=w_n(i)/w_n_sum(i)
			IF ((w_n(i)/w_n_sum(i)>1).or.(w_n(i)/w_n_sum(i)<0)) write(*,*) 'w_n/sum larger 1 or smaller 0', w_n(i)/w_n_sum(i), 'eigenvalue',eig(i)
		ELSE
			IF (real(w_n_c(i))<0) w_n_c(i)=0    ! delete negative entries
			unfoldingBuffer(i,i_kpt,jsp)=w_n_c(i)/w_n_c_sum(i)
			IF ((abs(w_n_c(i)/w_n_c_sum(i))>1).or.(real(w_n_c(i))<0)) write(*,*) 'w_n_c/sum larger 1 or smaller 0', w_n_c(i)/w_n_c_sum(i), 'eigenvalue',eig(i)
	        END IF
!		ELSE
!			IF (zmat%l_real) THEN
!				IF (jsp==1) write(679,'(3f15.8)') kpt_dist, ((eig(i)-results%ef)*hartree_to_ev_const),w_n(i)
!				IF (jsp==2) write(680,'(3f15.8)') kpt_dist, ((eig(i)-results%ef)*hartree_to_ev_const),w_n(i)
!				IF ((w_n(i)>1).or.(w_n(i)<0)) write(*,*) 'w_n larger 1 or smaller 0', w_n(i), 'eigenvalue',eig(i)
!			ELSE
!				IF (jsp==1) write(679,'(4f15.8)') kpt_dist, ((eig(i)-results%ef)*hartree_to_ev_const),w_n_c(i)
!				IF (jsp==2) write(680,'(4f15.8)') kpt_dist, ((eig(i)-results%ef)*hartree_to_ev_const),w_n_c(i)
!				IF ((abs(w_n_c(i))>1).or.(real(w_n_c(i))<0)) write(*,*) 'w_n_c larger 1 or smaller 0', w_n_c(i), 'eigenvalue',eig(i)
!	        	END IF
!		END IF
	END DO
       !$omp end do
       !$omp end parallel
        write (*,*) 'finished',i_kpt
	IF (i_kpt==kpts%nkpt) THEN
		IF (write_to_file .AND. jsp==1) CLOSE (679)
		IF (jsp==input%jspins) THEN
			IF (write_to_file .AND. jsp==2) CLOSE (680)
			!kpts%bk(:,:)=kpts%sc_list(11:13,:)
			write(*,*) 'Unfolded Bandstructure calculated succesfully, calledby=calculate_plot_w_n'
			!CALL juDFT_error('Unfolded Bandstructure created succesfully - use band_sc.gnu to plot', calledby='calculate_plot_w_n')

		END IF
	END IF
 END SUBROUTINE

SUBROUTINE write_band_sc(kpts,results,eFermiPrev)
     USE m_types
     USE m_juDFT
     USE m_constants
     IMPLICIT NONE
	TYPE(t_results),INTENT(IN)  :: results
	TYPE(t_kpts),INTENT(IN)     :: kpts
        REAL, INTENT(IN) :: eFermiPrev
	INTEGER :: i,i_kpt,jsp

	OPEN (679,file='bands_sc.1',status='unknown') !This is kind of my birthday 6 july 1992 (S.R.)
	IF (SIZE(results%unfolding_weights,3)==2) OPEN (680,file='bands_sc.2',status='unknown')
        DO jsp=1,SIZE(results%unfolding_weights,3)
		DO i_kpt=1,SIZE(results%unfolding_weights,2)
			DO i=1,results%neig(i_kpt,jsp)
				IF (jsp==1) write(679,'(4f15.8)') kpts%sc_list(10,i_kpt), ((results%eig(i,i_kpt,1)-eFermiPrev)*hartree_to_ev_const),results%unfolding_weights(i,i_kpt,1)
				IF (jsp==2) write(680,'(4f15.8)') kpts%sc_list(10,i_kpt), ((results%eig(i,i_kpt,2)-eFermiPrev)*hartree_to_ev_const),results%unfolding_weights(i,i_kpt,2)
			END DO
		END DO
	END DO
	CLOSE (679)
	IF (SIZE(results%unfolding_weights,3)==2) CLOSE (680)
	write(*,*) 'Unfolded Bandstructure written succesfully - use band_sc.gnu to plot, calledby=write_band_sc',eFermiPrev
END SUBROUTINE

      SUBROUTINE write_gnu_sc(nosyp,d,ssy,input)
      	USE m_types
	USE m_juDFT
      IMPLICIT NONE

      TYPE(t_input),INTENT(IN) :: input
      INTEGER, INTENT (IN) :: nosyp
      REAL,    INTENT (IN) :: d(nosyp)
      CHARACTER(len=1), INTENT (IN) :: ssy(nosyp)

      INTEGER n,aoff,adel
      CHARACTER(LEN=200) tempTitle
      aoff = iachar('a')-1
      adel = iachar('a')-iachar('A')
      !write(*,*) aoff,adel

      OPEN (27,file='band_sc.gnu',status='unknown')
      WRITE (27,*) 'reset'
      WRITE (27,900)
      WRITE (27,901)
      WRITE (27,902)
      WRITE (27,903)
      WRITE(tempTitle,'(10a)') input%comment
      IF(TRIM(ADJUSTL(tempTitle)).EQ.'') THEN
         tempTitle = "Fleur Bandstructure"
      END IF
      WRITE (27,904) TRIM(ADJUSTL(tempTitle))
      DO n = 1, nosyp
        WRITE (27,905) d(n),d(n)
      ENDDO
      WRITE (27,906) d(1),d(nosyp)
!
! nomal labels
!
      IF (iachar(ssy(1)) < aoff ) THEN
        WRITE (27,907) ssy(1),d(1),achar(92)
      ELSE
        WRITE (27,907) " ",d(1),achar(92)
      ENDIF
      DO n = 2, nosyp-1
        IF (iachar(ssy(n)) < aoff ) THEN
          WRITE (27,908) ssy(n),d(n),achar(92)
        ELSE
          WRITE (27,908) " ",d(n),achar(92)
        ENDIF
      ENDDO
      IF (iachar(ssy(nosyp)) < aoff ) THEN
        WRITE (27,909) ssy(nosyp),d(nosyp)
      ELSE
        WRITE (27,909) " ",d(nosyp)
      ENDIF
!
! greek labels
!
      DO n = 1, nosyp
        IF (iachar(ssy(n)) > aoff ) THEN
          WRITE (27,914) achar(iachar(ssy(n))-adel),d(n)
        ENDIF
      ENDDO
!
! now write the rest
!
      WRITE (27,910)
      WRITE (27,*) 'set palette model RGB'
      WRITE (27,*) 'set palette defined (-2 "black", -1 "white" ,0 "white",',achar(92)
      WRITE (27,*) '0.67 "light-blue",1 "blue")'
      WRITE (27,*) 'set cbrange [-2:1]'
      WRITE (27,*) 'unset colorbox'
      WRITE (27,*) 'size1(x)=0.9*x**(0.4)'
      WRITE (27,*) 'color1(x)=0.3+x/2.4'
      WRITE (27,*) 'size2(x)=0.35*(1-x**(0.01))'
      WRITE (27,*) 'color2(x)=1.15*(x-1)'
      WRITE (27,*) 'e_f=0.000000 #fermi energy is already corrected when using hdf5'
      WRITE (27,911) d(nosyp)+0.00001,achar(92)
      IF (input%jspins == 2) THEN
	WRITE (27,912) achar(92)
	WRITE (27,916) achar(92)
      END IF
      WRITE (27,913) achar(92)
      WRITE (27,915)
      CLOSE (27)

 900  FORMAT ('set terminal postscript enhanced color "Times-Roman" 20')
 901  FORMAT ('set xlabel ""')
 902  FORMAT ('set ylabel "E - E_F (eV)"')
 903  FORMAT ('set nokey')
 904  FORMAT ('set title "',a,'"')
 905  FORMAT ('set arrow from',f9.5,', -9.0 to',f9.5,',  5.0 nohead')
 906  FORMAT ('set arrow from',f9.5,', 0.0 to',f9.5,', 0.0 nohead lt 3')
 907  FORMAT ('set xtics ("',a1,'"',f9.5,', ',a)
 908  FORMAT ('           "',a1,'"',f9.5,', ',a)
 909  FORMAT ('           "',a1,'"',f9.5,'  )')
 910  FORMAT ('set ytics -8,2,4')
 911  FORMAT ('plot [0:',f9.5,'] [-9:5] ',a)
 912  FORMAT ('"bands_sc.2" using 1:($2-e_f):(size1($3)):(color1($3))  w p pt 7 ps variable lc palette, ',a)
 916  FORMAT ('"bands_sc.2" using 1:($2-e_f):(size2($3)):(color2($3)) w p pt 7 ps variable lc palette,',a)
 913  FORMAT ('"bands_sc.1" using 1:($2-e_f):(size1($3)):(color1($3))  w p pt 7 ps variable lc palette, ',a)
 915  FORMAT ('"bands_sc.1" using 1:($2-e_f):(size2($3)):(color2($3)) w p pt 7 ps variable lc palette')
 914  FORMAT ('set label "',a1,'" at ',f9.5,', -9.65 center font "Symbol,20"')
      END SUBROUTINE write_gnu_sc
END MODULE m_unfold_band_kpts


module Mod_nsm_Viscosity
   use typre
   use Mod_Element
contains

   subroutine nsi_vislaw(e,grvel,lawvi,LawViParam,acvis)
      implicit none
      class(FiniteElement)     :: e
      integer(ip), intent(in)  :: lawvi
      real(rp),    intent(in)  :: LawViParam(*)
      real(rp),    intent(in)  :: grvel(e%ndime,e%ndime)
      real(rp),    intent(out) :: acvis
      integer(ip) :: idime,jdime,inode,vislaw
      real(rp)    :: vis_a,vispolaw,polawn,viszer,visinf,vislamb
      real(rp)    :: I2,strain(e%ndime,e%ndime),facvis1,facvis2,heavi

      !********************************************************************
      !vislawParam(1)=newtonian viscosity (mu) or consistency index (m)
      !visco(2)=infinity viscosity Carreau-Yasuda model
      !visco(3)=power law index in non newtonian models
      !visco(4)=lambda parameter in Carreau Yasuda model
      !visco(5)=a parameter in carreau yasuda model if a=2 carreau model
      !********************************************************************

      vispolaw=LawViParam(1)
      polawn=LawViParam(2)
      viszer=LawViParam(1)
      visinf=LawViParam(3)
      vislamb=LawViParam(4)
      vis_a=LawViParam(5)
      vislaw=lawvi

      !rate of strain tensor to use in non Newtonian models
      I2 = 0.0_rp
      strain = grvel + transpose(grvel)
      call tracedoubledot(e%ndime,strain,I2)
      !call trace(e%ndime,strain,tr_strain1)
      !call trace(e%ndime,matmul(strain,strain),tr_strain2)
      !I2 = tr_strain1**2 + tr_strain2

      if (vislaw == 1) then
         acvis = vispolaw*((0.5_rp*I2)**(polawn*0.5_rp-0.5_rp)) !Power Law viscosity
         if (polawn .gt. 1.0_rp) then
            facvis1 = LawViParam(1)/10000_rp
            facvis2 = LawViParam(1)*10000_rp
         end if
         if (polawn .le. 1.0_rp) then
            facvis1 = LawViParam(1)/10000_rp
            facvis2 = LawViParam(1)*10000_rp
         end if
         if (acvis <= facvis1) acvis = facvis1
         if (acvis >= facvis2) acvis = facvis2
      elseif (vislaw == 2) then
         acvis = visinf+(viszer-visinf)*(1.0_rp+(0.5_rp*vislamb*I2)**vis_a)**((polawn-1.0_rp)/vis_a)
      end if

   end subroutine nsi_vislaw

   subroutine nsm_smago(e,grvel,denac,turbu,vista)
      use typre
      use Mod_Element
      implicit none
      class(FiniteElement)  :: e
      real(rp), intent(in)  :: turbu
      real(rp), intent(in)  :: grvel(e%ndime,e%ndime)
      real(rp), intent(in)  :: denac
      real(rp), intent(out) :: vista
      real(rp)    :: seci4,divel
      integer(ip) :: igaus,inode,idime,jdime

      vista = 0.0_rp
      !Smagorinsky
      seci4=0.0_rp
      do idime=1,e%ndime
         do jdime=1,e%ndime
            seci4=seci4+grvel(idime,jdime)*(grvel(idime,jdime)+grvel(jdime,idime))
         end do
      end do
      divel=0.0_rp
      do idime=1,e%ndime
         divel=divel+grvel(idime,idime)
      end do
      vista=2.0_rp*denac*turbu*e%hleng(e%ndime)*e%hleng(e%ndime)*sqrt(seci4-2.0_rp*divel*divel/3.0_rp)

   end subroutine nsm_smago

   !> This is the WALE LES model based on  Subgrid-Scale Stress Modelling Based on the
   !>   Square of the Velocity Gradient Tensor. F. NICOUD and F. DUCROS Flow, Turbulence and Combustion 62: 183–200, 1999.
   subroutine nsm_wale(e,grvel,denac,turbu,vista)
      use typre
      use Mod_Element
      implicit none
      class(FiniteElement)  :: e
      real(rp), intent(in)  :: turbu
      real(rp), intent(in)  :: grvel(e%ndime,e%ndime)
      real(rp), intent(in)  :: denac
      real(rp), intent(out) :: vista
      real(rp) :: CaligraphicS(e%ndime,e%ndime)
      real(rp) :: S(e%ndime,e%ndime), g2(e%ndime,e%ndime)
      real(rp) :: MeanStressG2, OP1,OP2, TraceG2
      real(rp) :: CSDoubleContractionCS, SDoubleContractionS
      integer(ip) :: idime,jdime

      !Default value
      vista = 0.0_rp
      !Symmetric gradient
      S = 0.5_rp*(grvel + transpose(grvel))
      !Gradient times gradient
      g2 = matmul(grvel,grvel)

      TraceG2 = 0.0_rp
      do idime = 1,e%ndime
         TraceG2 = TraceG2 + g2(idime,idime)
      enddo
      MeanStressG2 = 1.0_rp/3.0_rp*TraceG2

      !CaligraphicS
      CaligraphicS = 0.5_rp*(g2 + transpose(g2))
      do idime = 1,e%ndime
         CaligraphicS(idime,idime) = CaligraphicS(idime,idime) - MeanStressG2
      enddo

      !Symmetric gradient against itself
      SDoubleContractionS = 0.0_rp
      do idime = 1,e%ndime
         do jdime = 1,e%ndime
            SDoubleContractionS = SDoubleContractionS + S(idime,jdime)*S(idime,jdime)
         enddo
      enddo

      !Caligraphic S against itself
      CSDoubleContractionCS = 0.0_rp
      do idime = 1,e%ndime
         do jdime = 1,e%ndime
            CSDoubleContractionCS = CSDoubleContractionCS + CaligraphicS(idime,jdime)*CaligraphicS(idime,jdime)
         enddo
      enddo

      !OP1 and OP2 as defined in the Nicoud paper
      OP1 = CSDoubleContractionCS**(3.0_rp/2.0_rp)
      OP2 = SDoubleContractionS**(5.0_rp/2.0_rp) + CSDoubleContractionCS**(5.0_rp/4.0_rp)

      if (OP2 /= 0.0_rp) vista = denac*((turbu*e%hleng(e%ndime))**2.0_rp)*OP1/OP2

   end subroutine nsm_wale

end module


module category_theory_concepts
    ! Note: Sensible implementations should satisfy
    ! associativity: a // (b // c) == (a // b) // c
    concept :: semigroup(T)
        operator(//)(lhs, rhs) result(combined)
            T, intent(in) :: lhs, rhs
            T :: combined
        end operator(//)
        function sconcat(xs, starting) result(combined)
            T, intent(in) :: xs(:), starting
            T :: combined
            default
                integer :: i
                combined = starting
                do i = 1, size(xs)
                    combined = combined // xs(i)
                end do
            end default
        end function
        function stimes(num, start) result(repeated)
            integer, intent(in) :: num
            T, intent(in) :: start
            T :: repeated
            default
                if (num < 1) then
                    if (monoid(T)) then
                        repeated = mempty(start)
                    else
                        error stop "attempted to repeat non-monoid 0 times"
                    end if
                else
                    repeated = sconcat([(start, i = 1, num-1)], start)
                end if
            end default
        end function
    end concept

    ! Note: Sensible implementations should satisfy
    ! identity: T // mempty(T) == T
    !           mempty(T) // T == T
    concept :: monoid(T)
        requirements :: semigroup(T)
        function mempty(mold)
            T, intent(in) :: mold
            T :: mempty
        end function
        function mappend(x, y) result(combined)
            T, intent(in) :: x, y
            T :: combined
            default
                combined = x // y
            end default
        end function
        function mconcat(xs) result(combined)
            T, intent(in) :: xs(:)
            T :: combined
            default
                combined = sconcat(xs, mempty(combined))
            end default
        end function
    end concept
end module

module maybe
    use category_theory_concepts, only: semigroup, monoid

    concept :: copyable(T)
        assignment: T = T
    end concept

    type :: maybe_t<T>
        private
        logical :: available
        T :: item
    end type
    if (semigroup(T)) then
        satisfies :: semigroup(maybe_t<T>)
        satisfies :: monoid(maybe_t<T>)
    end if

    interface maybe_t
        module procedure just
        module procedure nothing
    end interface

    if (semigroup(T)) then
        interface operator(//)
            module procedure concat
        end interface
    end if
contains
    function just(item) result(maybe)
        T, intent(in) :: item
        type(maybe_t<T>) :: maybe

        maybe%item = item
        maybe%available = .true.
    end function

    function nothing(mold) result(maybe)
        T, intent(in) :: mold
        type(maybe_t<T>) :: maybe

        maybe%available = .false.
    end function

    if (semigroup(T)) then
        function concat(lhs, rhs) result(combined)
            type(maybe_t<T>), intent(in) :: lhs, rhs
            type(maybe_t<T>) :: combined

            if (lhs%available) then
                if (rhs%available) then
                    combined%item = lhs%item // rhs%item
                    combined%available = .true.
                else
                    combined%item = lhs%item
                    combined%available = .true.
                end if
            else
                if (rhs%available) then
                    combined%item = rhs%item
                    combined%available = .true.
                else
                    combined%available = .false.
                end if
            end if
        end function

        function mempty(mold)
            type(maybe_t<T>), intent(in) :: mold
            type(maybe_t<T>) :: mempty

            mempty%available = .false.
        end function
    end if
end module


      SUBROUTINE iau_P06E ( DATE1, DATE2,
     :                      EPS0, PSIA, OMA, BPA, BQA, PIA, BPIA,
     :                      EPSA, CHIA, ZA, ZETAA, THETAA, PA,
     :                      GAM, PHI, PSI )
*+
*  - - - - - - - - -
*   i a u _ P 0 6 E
*  - - - - - - - - -
*
*  Precession angles, IAU 2006, equinox based.
*
*  This routine is part of the International Astronomical Union's
*  SOFA (Standards of Fundamental Astronomy) software collection.
*
*  Status:  canonical models.
*
*  Given:
*     DATE1,DATE2   d    TT as a 2-part Julian Date (Note 1)
*
*  Returned (see Note 2):
*     EPS0          d    epsilon_0
*     PSIA          d    psi_A
*     OMA           d    omega_A
*     BPA           d    P_A
*     BQA           d    Q_A
*     PIA           d    pi_A
*     BPIA          d    Pi_A
*     EPSA          d    obliquity epsilon_A
*     CHIA          d    chi_A
*     ZA            d    z_A
*     ZETAA         d    zeta_A
*     THETAA        d    theta_A
*     PA            d    p_A
*     GAM           d    F-W angle gamma_J2000
*     PHI           d    F-W angle phi_J2000
*     PSI           d    F-W angle psi_J2000
*
*  Notes:
*
*  1) The TT date DATE1+DATE2 is a Julian Date, apportioned in any
*     convenient way between the two arguments.  For example,
*     JD(TT)=2450123.7 could be expressed in any of these ways,
*     among others
*
*            DATE1          DATE2
*
*         2450123.7D0        0D0        (JD method)
*          2451545D0      -1421.3D0     (J2000 method)
*         2400000.5D0     50123.2D0     (MJD method)
*         2450123.5D0       0.2D0       (date & time method)
*
*     The JD method is the most natural and convenient to use in
*     cases where the loss of several decimal digits of resolution
*     is acceptable.  The J2000 method is best matched to the way
*     the argument is handled internally and will deliver the
*     optimum resolution.  The MJD method and the date & time methods
*     are both good compromises between resolution and convenience.
*
*  2) This routine returns the set of equinox based angles for the
*     Capitaine et al. "P03" precession theory, adopted by the IAU in
*     2006.  The angles are set out in Table 1 of Hilton et al. (2006):
*
*      EPS0   epsilon_0   obliquity at J2000.0
*      PSIA   psi_A       luni-solar precession
*      OMA    omega_A     inclination of equator wrt J2000.0 ecliptic
*      BPA    P_A         ecliptic pole x, J2000.0 ecliptic triad
*      BQA    Q_A         ecliptic pole -y, J2000.0 ecliptic triad
*      PIA    pi_A        angle between moving and J2000.0 ecliptics
*      BPIA   Pi_A        longitude of ascending node of the ecliptic
*      EPSA   epsilon_A   obliquity of the ecliptic
*      CHIA   chi_A       planetary precession
*      ZA     z_A         equatorial precession: -3rd 323 Euler angle
*      ZETAA  zeta_A      equatorial precession: -1st 323 Euler angle
*      THETAA theta_A     equatorial precession: 2nd 323 Euler angle
*      PA     p_A         general precession (n.b. see below)
*      GAM    gamma_J2000 J2000.0 RA difference of ecliptic poles
*      PHI    phi_J2000   J2000.0 codeclination of ecliptic pole
*      PSI    psi_J2000   longitude difference of equator poles, J2000.0
*
*     The returned values are all radians.
*
*     Note that the t^5 coefficient in the series for p_A from Capitaine
*     et al. (2003) is incorrectly signed in Hilton et al. (2006).
*
*  3) Hilton et al. (2006) Table 1 also contains angles that depend on
*     models distinct from the P03 precession theory itself, namely the
*     IAU 2000A frame bias and nutation.  The quoted polynomials are
*     used in other SOFA routines:
*
*     . iau_XY06 contains the polynomial parts of the X and Y series.
*
*     . iau_S06 contains the polynomial part of the s+XY/2 series.
*
*     . iau_PFW06 implements the series for the Fukushima-Williams
*       angles that are with respect to the GCRS pole (i.e. the variants
*       that include frame bias).
*
*  4) The IAU resolution stipulated that the choice of parameterization
*     was left to the user, and so an IAU compliant precession
*     implementation can be constructed using various combinations of
*     the angles returned by the present routine.
*
*  5) The parameterization used by SOFA is the version of the Fukushima-
*     Williams angles that refers directly to the GCRS pole.  These
*     angles may be calculated by calling the routine iau_PFW06.  SOFA
*     also supports the direct computation of the CIP GCRS X,Y by
*     series, available by calling iau_XY06.
*
*  6) The agreement between the different parameterizations is at the
*     1 microarcsecond level in the present era.
*
*  7) When constructing a precession formulation that refers to the GCRS
*     pole rather than the dynamical pole, it may (depending on the
*     choice of angles) be necessary to introduce the frame bias
*     explicitly.
*
*  References:
*
*     Capitaine, N., Wallace, P.T. & Chapront, J., 2003,
*     Astron.Astrophys., 412, 567
*
*     Hilton, J. et al., 2006, Celest.Mech.Dyn.Astron. 94, 351
*
*  Called:
*     iau_OBL06    mean obliquity, IAU 2006
*
*  This revision:  2020 June 2
*
*  SOFA release 2020-07-21
*
*  Copyright (C) 2020 IAU SOFA Board.  See notes at end.
*
*-----------------------------------------------------------------------

      IMPLICIT NONE

      DOUBLE PRECISION DATE1, DATE2,
     :                 EPS0, PSIA, OMA, BPA, BQA, PIA, BPIA,
     :                 EPSA, CHIA, ZA, ZETAA, THETAA, PA,
     :                 GAM, PHI, PSI

*  Arcseconds to radians
      DOUBLE PRECISION DAS2R
      PARAMETER ( DAS2R = 4.848136811095359935899141D-6 )

*  Reference epoch (J2000.0), JD
      DOUBLE PRECISION DJ00
      PARAMETER ( DJ00 = 2451545D0 )

*  Days per Julian century
      DOUBLE PRECISION DJC
      PARAMETER ( DJC = 36525D0 )

      DOUBLE PRECISION T

      DOUBLE PRECISION iau_OBL06

* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

*  Interval between fundamental date J2000.0 and given date (JC).
      T = ( ( DATE1-DJ00 ) + DATE2 ) / DJC

*  Obliquity at J2000.0.

      EPS0 = 84381.406D0 * DAS2R

*  Luni-solar precession.

      PSIA = ( 5038.481507D0     +
     :       (   -1.0790069D0    +
     :       (   -0.00114045D0   +
     :       (    0.000132851D0  +
     :       (   -0.0000000951D0 )
     :       * T ) * T ) * T ) * T ) * T * DAS2R

*  Inclination of mean equator with respect to the J2000.0 ecliptic.

      OMA = EPS0 + ( -0.025754D0     +
     :             (  0.0512623D0    +
     :             ( -0.00772503D0   +
     :             ( -0.000000467D0  +
     :             (  0.0000003337D0 )
     :             * T ) * T ) * T ) * T ) * T * DAS2R

*  Ecliptic pole x, J2000.0 ecliptic triad.

      BPA = (  4.199094D0     +
     :      (  0.1939873D0    +
     :      ( -0.00022466D0   +
     :      ( -0.000000912D0  +
     :      (  0.0000000120D0 )
     :      * T ) * T ) * T ) * T ) * T * DAS2R

*  Ecliptic pole -y, J2000.0 ecliptic triad.

      BQA = ( -46.811015D0     +
     :      (   0.0510283D0    +
     :      (   0.00052413D0   +
     :      (  -0.000000646D0  +
     :      (  -0.0000000172D0 )
     :      * T ) * T ) * T ) * T ) * T * DAS2R

*  Angle between moving and J2000.0 ecliptics.

      PIA = ( 46.998973D0     +
     :      ( -0.0334926D0    +
     :      ( -0.00012559D0   +
     :      (  0.000000113D0  +
     :      ( -0.0000000022D0 )
     :      * T ) * T ) * T ) * T ) * T * DAS2R

*  Longitude of ascending node of the moving ecliptic.

      BPIA = ( 629546.7936D0      +
     :       (   -867.95758D0     +
     :       (      0.157992D0    +
     :       (     -0.0005371D0   +
     :       (     -0.00004797D0  +
     :       (      0.000000072D0 )
     :       * T ) * T ) * T ) * T ) * T ) * DAS2R

*  Mean obliquity of the ecliptic.

      EPSA = iau_OBL06 ( DATE1, DATE2 )

*  Planetary precession.

      CHIA = ( 10.556403D0     +
     :       ( -2.3814292D0    +
     :       ( -0.00121197D0   +
     :       (  0.000170663D0  +
     :       ( -0.0000000560D0 )
     :       * T ) * T ) * T ) * T ) * T * DAS2R

*  Equatorial precession: minus the third of the 323 Euler angles.

      ZA = (   -2.650545D0     +
     :     ( 2306.077181D0     +
     :     (    1.0927348D0    +
     :     (    0.01826837D0   +
     :     (   -0.000028596D0  +
     :     (   -0.0000002904D0 )
     :     * T ) * T ) * T ) * T ) * T ) * DAS2R

*  Equatorial precession: minus the first of the 323 Euler angles.

      ZETAA = (    2.650545D0     +
     :        ( 2306.083227D0     +
     :        (    0.2988499D0    +
     :        (    0.01801828D0   +
     :        (   -0.000005971D0  +
     :        (   -0.0000003173D0 )
     :        * T ) * T ) * T ) * T ) * T ) * DAS2R

*  Equatorial precession: second of the 323 Euler angles.

      THETAA = ( 2004.191903D0     +
     :         (   -0.4294934D0    +
     :         (   -0.04182264D0   +
     :         (   -0.000007089D0  +
     :         (   -0.0000001274D0 )
     :         * T ) * T ) * T ) * T ) * T * DAS2R

*  General precession.

      PA = ( 5028.796195D0     +
     :     (    1.1054348D0    +
     :     (    0.00007964D0   +
     :     (   -0.000023857D0  +
     :     (   -0.0000000383D0 )
     :     * T ) * T ) * T ) * T ) * T * DAS2R

*  Fukushima-Williams angles for precession.

      GAM = ( 10.556403D0     +
     :      (  0.4932044D0    +
     :      ( -0.00031238D0   +
     :      ( -0.000002788D0  +
     :      (  0.0000000260D0 )
     :      * T ) * T ) * T ) * T ) * T * DAS2R

      PHI = EPS0 + ( -46.811015D0     +
     :             (   0.0511269D0    +
     :             (   0.00053289D0   +
     :             (  -0.000000440D0  +
     :             (  -0.0000000176D0 )
     :             * T ) * T ) * T ) * T ) * T * DAS2R

      PSI = ( 5038.481507D0     +
     :      (    1.5584176D0    +
     :      (   -0.00018522D0   +
     :      (   -0.000026452D0  +
     :      (   -0.0000000148D0 )
     :      * T ) * T ) * T ) * T ) * T * DAS2R

*  Finished.

*+----------------------------------------------------------------------
*
*  Copyright (C) 2020
*  Standards Of Fundamental Astronomy Board
*  of the International Astronomical Union.
*
*  =====================
*  SOFA Software License
*  =====================
*
*  NOTICE TO USER:
*
*  BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING SIX TERMS AND
*  CONDITIONS WHICH APPLY TO ITS USE.
*
*  1. The Software is owned by the IAU SOFA Board ("SOFA").
*
*  2. Permission is granted to anyone to use the SOFA software for any
*     purpose, including commercial applications, free of charge and
*     without payment of royalties, subject to the conditions and
*     restrictions listed below.
*
*  3. You (the user) may copy and distribute SOFA source code to others,
*     and use and adapt its code and algorithms in your own software,
*     on a world-wide, royalty-free basis.  That portion of your
*     distribution that does not consist of intact and unchanged copies
*     of SOFA source code files is a "derived work" that must comply
*     with the following requirements:
*
*     a) Your work shall be marked or carry a statement that it
*        (i) uses routines and computations derived by you from
*        software provided by SOFA under license to you; and
*        (ii) does not itself constitute software provided by and/or
*        endorsed by SOFA.
*
*     b) The source code of your derived work must contain descriptions
*        of how the derived work is based upon, contains and/or differs
*        from the original SOFA software.
*
*     c) The names of all routines in your derived work shall not
*        include the prefix "iau" or "sofa" or trivial modifications
*        thereof such as changes of case.
*
*     d) The origin of the SOFA components of your derived work must
*        not be misrepresented;  you must not claim that you wrote the
*        original software, nor file a patent application for SOFA
*        software or algorithms embedded in the SOFA software.
*
*     e) These requirements must be reproduced intact in any source
*        distribution and shall apply to anyone to whom you have
*        granted a further right to modify the source code of your
*        derived work.
*
*     Note that, as originally distributed, the SOFA software is
*     intended to be a definitive implementation of the IAU standards,
*     and consequently third-party modifications are discouraged.  All
*     variations, no matter how minor, must be explicitly marked as
*     such, as explained above.
*
*  4. You shall not cause the SOFA software to be brought into
*     disrepute, either by misuse, or use for inappropriate tasks, or
*     by inappropriate modification.
*
*  5. The SOFA software is provided "as is" and SOFA makes no warranty
*     as to its use or performance.   SOFA does not and cannot warrant
*     the performance or results which the user may obtain by using the
*     SOFA software.  SOFA makes no warranties, express or implied, as
*     to non-infringement of third party rights, merchantability, or
*     fitness for any particular purpose.  In no event will SOFA be
*     liable to the user for any consequential, incidental, or special
*     damages, including any lost profits or lost savings, even if a
*     SOFA representative has been advised of such damages, or for any
*     claim by any third party.
*
*  6. The provision of any version of the SOFA software under the terms
*     and conditions specified herein does not imply that future
*     versions will also be made available under the same terms and
*     conditions.
*
*  In any published work or commercial product which uses the SOFA
*  software directly, acknowledgement (see www.iausofa.org) is
*  appreciated.
*
*  Correspondence concerning SOFA software should be addressed as
*  follows:
*
*      By email:  sofa@ukho.gov.uk
*      By post:   IAU SOFA Center
*                 HM Nautical Almanac Office
*                 UK Hydrographic Office
*                 Admiralty Way, Taunton
*                 Somerset, TA1 2DN
*                 United Kingdom
*
*-----------------------------------------------------------------------

      END


!Copyright (c) 2015 M. Zapukhlyak
!THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
!IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
!FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
!COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
!IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
!CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

!-----------------------------------------------------------------------
!Module crc_module  for CRC-32, IEEE 802 computation
! This implementation inspired by C version in "Reversing CRC – Theory and Practice."
! HU Berlin Public Report SAR-PR-2006-05, May 2006,
! Authors: Martin Stigge, Henryk Plötz, Wolf Müller, Jens-Peter Redlich
! https://sar.informatik.hu-berlin.de/research/publications/SAR-PR-2006-05/SAR-PR-2006-05_.pdf
!
! gfortran -cpp crc32.f90 -o crc32.x
! ifort -fpp crc32.f90 -o crc32.x
! pgfortran -Mpreprocess crc32.f90 -o crc32.x
! nagfor -fpp crc32.f90 -o crc32.x
!-----------------------------------------------------------------------
module crc_module
#if defined(__GFORTRAN__) || defined(NAGFOR)
   use,intrinsic :: iso_fortran_env, only: int64
#endif
   implicit none
   private ! all by default
   public :: crc_hash
#if defined(__INTEL_COMPILER) || defined(__PGI)
   integer, parameter :: int64 = selected_int_kind (8)
#endif
   type crc_hash
      integer(int64), dimension(256) :: table
      integer(int64), dimension(256) :: table_orig
      logical :: have_table = .false.
      integer(int64) :: crcpoly = int(z'edb88320',int64)
      integer(int64) :: crcinv = int(z'5b358fd3',int64)
      integer(int64) :: initxor = int(z'ffffffff',int64)
      integer(int64) :: finalxor = int(z'ffffffff',int64)
   contains
      procedure, pass :: init => init_crc32
      procedure, pass :: crc32 => calc_crc32
   end type

contains

   !-----------------------------------------------------------------------
   !subroutine
   !-----------------------------------------------------------------------
   subroutine  init_crc32(this) ! bind(c, name='init_crc32')
      implicit none
      class(crc_hash), intent(inout) :: this
      integer(int64) :: rem
      integer :: i,j

      if (.not. this % have_table) then
         !Calculate CRC table
         do i = 1, 256
            rem = int(i-1,int64);  !/* remainder from polynomial division */
            do j=1,8
               if( iand(rem, 1_int64) /= 0_int64) then
                  rem = ieor(ishft(rem, -abs(1_int64)), this % crcpoly)
               else
                  rem = ishft(rem, -abs(1_int64))
               endif
            enddo
            this % table(i) = rem
         enddo
         ! for check only
         this % table_orig = [&
         int(z'00000000',int64), int(z'77073096',int64), int(z'EE0E612C',int64), int(z'990951BA',int64), &
         int(z'076DC419',int64), int(z'706AF48F',int64), int(z'E963A535',int64), int(z'9E6495A3',int64), &
         int(z'0EDB8832',int64), int(z'79DCB8A4',int64), int(z'E0D5E91E',int64), int(z'97D2D988',int64), &
         int(z'09B64C2B',int64), int(z'7EB17CBD',int64), int(z'E7B82D07',int64), int(z'90BF1D91',int64), &
         int(z'1DB71064',int64), int(z'6AB020F2',int64), int(z'F3B97148',int64), int(z'84BE41DE',int64), &
         int(z'1ADAD47D',int64), int(z'6DDDE4EB',int64), int(z'F4D4B551',int64), int(z'83D385C7',int64), &
         int(z'136C9856',int64), int(z'646BA8C0',int64), int(z'FD62F97A',int64), int(z'8A65C9EC',int64), &
         int(z'14015C4F',int64), int(z'63066CD9',int64), int(z'FA0F3D63',int64), int(z'8D080DF5',int64), &
         int(z'3B6E20C8',int64), int(z'4C69105E',int64), int(z'D56041E4',int64), int(z'A2677172',int64), &
         int(z'3C03E4D1',int64), int(z'4B04D447',int64), int(z'D20D85FD',int64), int(z'A50AB56B',int64), &
         int(z'35B5A8FA',int64), int(z'42B2986C',int64), int(z'DBBBC9D6',int64), int(z'ACBCF940',int64), &
         int(z'32D86CE3',int64), int(z'45DF5C75',int64), int(z'DCD60DCF',int64), int(z'ABD13D59',int64), &
         int(z'26D930AC',int64), int(z'51DE003A',int64), int(z'C8D75180',int64), int(z'BFD06116',int64), &
         int(z'21B4F4B5',int64), int(z'56B3C423',int64), int(z'CFBA9599',int64), int(z'B8BDA50F',int64), &
         int(z'2802B89E',int64), int(z'5F058808',int64), int(z'C60CD9B2',int64), int(z'B10BE924',int64), &
         int(z'2F6F7C87',int64), int(z'58684C11',int64), int(z'C1611DAB',int64), int(z'B6662D3D',int64), &
         int(z'76DC4190',int64), int(z'01DB7106',int64), int(z'98D220BC',int64), int(z'EFD5102A',int64), &
         int(z'71B18589',int64), int(z'06B6B51F',int64), int(z'9FBFE4A5',int64), int(z'E8B8D433',int64), &
         int(z'7807C9A2',int64), int(z'0F00F934',int64), int(z'9609A88E',int64), int(z'E10E9818',int64), &
         int(z'7F6A0DBB',int64), int(z'086D3D2D',int64), int(z'91646C97',int64), int(z'E6635C01',int64), &
         int(z'6B6B51F4',int64), int(z'1C6C6162',int64), int(z'856530D8',int64), int(z'F262004E',int64), &
         int(z'6C0695ED',int64), int(z'1B01A57B',int64), int(z'8208F4C1',int64), int(z'F50FC457',int64), &
         int(z'65B0D9C6',int64), int(z'12B7E950',int64), int(z'8BBEB8EA',int64), int(z'FCB9887C',int64), &
         int(z'62DD1DDF',int64), int(z'15DA2D49',int64), int(z'8CD37CF3',int64), int(z'FBD44C65',int64), &
         int(z'4DB26158',int64), int(z'3AB551CE',int64), int(z'A3BC0074',int64), int(z'D4BB30E2',int64), &
         int(z'4ADFA541',int64), int(z'3DD895D7',int64), int(z'A4D1C46D',int64), int(z'D3D6F4FB',int64), &
         int(z'4369E96A',int64), int(z'346ED9FC',int64), int(z'AD678846',int64), int(z'DA60B8D0',int64), &
         int(z'44042D73',int64), int(z'33031DE5',int64), int(z'AA0A4C5F',int64), int(z'DD0D7CC9',int64), &
         int(z'5005713C',int64), int(z'270241AA',int64), int(z'BE0B1010',int64), int(z'C90C2086',int64), &
         int(z'5768B525',int64), int(z'206F85B3',int64), int(z'B966D409',int64), int(z'CE61E49F',int64), &
         int(z'5EDEF90E',int64), int(z'29D9C998',int64), int(z'B0D09822',int64), int(z'C7D7A8B4',int64), &
         int(z'59B33D17',int64), int(z'2EB40D81',int64), int(z'B7BD5C3B',int64), int(z'C0BA6CAD',int64), &
         int(z'EDB88320',int64), int(z'9ABFB3B6',int64), int(z'03B6E20C',int64), int(z'74B1D29A',int64), &
         int(z'EAD54739',int64), int(z'9DD277AF',int64), int(z'04DB2615',int64), int(z'73DC1683',int64), &
         int(z'E3630B12',int64), int(z'94643B84',int64), int(z'0D6D6A3E',int64), int(z'7A6A5AA8',int64), &
         int(z'E40ECF0B',int64), int(z'9309FF9D',int64), int(z'0A00AE27',int64), int(z'7D079EB1',int64), &
         int(z'F00F9344',int64), int(z'8708A3D2',int64), int(z'1E01F268',int64), int(z'6906C2FE',int64), &
         int(z'F762575D',int64), int(z'806567CB',int64), int(z'196C3671',int64), int(z'6E6B06E7',int64), &
         int(z'FED41B76',int64), int(z'89D32BE0',int64), int(z'10DA7A5A',int64), int(z'67DD4ACC',int64), &
         int(z'F9B9DF6F',int64), int(z'8EBEEFF9',int64), int(z'17B7BE43',int64), int(z'60B08ED5',int64), &
         int(z'D6D6A3E8',int64), int(z'A1D1937E',int64), int(z'38D8C2C4',int64), int(z'4FDFF252',int64), &
         int(z'D1BB67F1',int64), int(z'A6BC5767',int64), int(z'3FB506DD',int64), int(z'48B2364B',int64), &
         int(z'D80D2BDA',int64), int(z'AF0A1B4C',int64), int(z'36034AF6',int64), int(z'41047A60',int64), &
         int(z'DF60EFC3',int64), int(z'A867DF55',int64), int(z'316E8EEF',int64), int(z'4669BE79',int64), &
         int(z'CB61B38C',int64), int(z'BC66831A',int64), int(z'256FD2A0',int64), int(z'5268E236',int64), &
         int(z'CC0C7795',int64), int(z'BB0B4703',int64), int(z'220216B9',int64), int(z'5505262F',int64), &
         int(z'C5BA3BBE',int64), int(z'B2BD0B28',int64), int(z'2BB45A92',int64), int(z'5CB36A04',int64), &
         int(z'C2D7FFA7',int64), int(z'B5D0CF31',int64), int(z'2CD99E8B',int64), int(z'5BDEAE1D',int64), &
         int(z'9B64C2B0',int64), int(z'EC63F226',int64), int(z'756AA39C',int64), int(z'026D930A',int64), &
         int(z'9C0906A9',int64), int(z'EB0E363F',int64), int(z'72076785',int64), int(z'05005713',int64), &
         int(z'95BF4A82',int64), int(z'E2B87A14',int64), int(z'7BB12BAE',int64), int(z'0CB61B38',int64), &
         int(z'92D28E9B',int64), int(z'E5D5BE0D',int64), int(z'7CDCEFB7',int64), int(z'0BDBDF21',int64), &
         int(z'86D3D2D4',int64), int(z'F1D4E242',int64), int(z'68DDB3F8',int64), int(z'1FDA836E',int64), &
         int(z'81BE16CD',int64), int(z'F6B9265B',int64), int(z'6FB077E1',int64), int(z'18B74777',int64), &
         int(z'88085AE6',int64), int(z'FF0F6A70',int64), int(z'66063BCA',int64), int(z'11010B5C',int64), &
         int(z'8F659EFF',int64), int(z'F862AE69',int64), int(z'616BFFD3',int64), int(z'166CCF45',int64), &
         int(z'A00AE278',int64), int(z'D70DD2EE',int64), int(z'4E048354',int64), int(z'3903B3C2',int64), &
         int(z'A7672661',int64), int(z'D06016F7',int64), int(z'4969474D',int64), int(z'3E6E77DB',int64), &
         int(z'AED16A4A',int64), int(z'D9D65ADC',int64), int(z'40DF0B66',int64), int(z'37D83BF0',int64), &
         int(z'A9BCAE53',int64), int(z'DEBB9EC5',int64), int(z'47B2CF7F',int64), int(z'30B5FFE9',int64), &
         int(z'BDBDF21C',int64), int(z'CABAC28A',int64), int(z'53B39330',int64), int(z'24B4A3A6',int64), &
         int(z'BAD03605',int64), int(z'CDD70693',int64), int(z'54DE5729',int64), int(z'23D967BF',int64), &
         int(z'B3667A2E',int64), int(z'C4614AB8',int64), int(z'5D681B02',int64), int(z'2A6F2B94',int64), &
         int(z'B40BBE37',int64), int(z'C30C8EA1',int64), int(z'5A05DF1B',int64), int(z'2D02EF8D',int64)]

         this % have_table = .true.
      endif

      do i = 1, 256
         !          write(*,'(z16,2X,z16)') this % table(i), this % table_orig(i)
         if(this % table(i) /= this % table_orig(i)) stop "error in table"
      enddo

   end subroutine init_crc32

   !-----------------------------------------------------------------------
   !Function generates a CRC-32 checksum for the ASCII encoded string
   !-----------------------------------------------------------------------
   function  calc_crc32(this, buf)
      implicit none
      class(crc_hash), intent(in) :: this
      character(len=*) :: buf
      integer(int64) :: calc_crc32
      integer :: l
      integer :: i,j
      integer(int64) :: crc
      integer(int64) :: ff = int(z'ff',int64)

      crc = this % initxor
      l = len(buf(:))
      do i = 1, l
         j = int(iand(ieor(crc,int(iachar(buf(i:i)),int64)),ff))
         crc = ieor(ishft(crc, -abs(8_int64)), this % table(j+1))
      enddo
      calc_crc32 = ieor(crc, this % finalxor)
   end function calc_crc32
end module crc_module

!-----------------------------------------------------------------------
!Main program crc32_test
!-----------------------------------------------------------------------
program    crc32_test
#if defined(__GFORTRAN__) || defined(NAGFOR)
   use,intrinsic :: iso_fortran_env, only: int64
#endif
   use crc_module
   implicit none
#if defined(__INTEL_COMPILER) || defined(__PGI)
   integer, parameter :: int64 = selected_int_kind (8)
#endif
   type(crc_hash) :: hash
   character(len=*), parameter :: s = "The quick brown fox jumps over the lazy dog"
   integer :: i
   integer(int64) :: crc
   ! init hash
   call hash % init()
   ! calculate hash from string s
   crc = hash % crc32(s)
   ! print hex of string s
   write(*,'(43z2)') (int(iachar(s(i:i)),int64), i=1,len(s(:)))
   ! print crc32
   write(*,'(z8)') crc
   ! print known crc32 for s
   write(*,'(z8)') z'414fa339'

end program crc32_test


SUBROUTINE yaw_nom (e_BX, v_sat, beta, satblk, cosE, ANOON, ANIGHT, Mangle, Yangle)

! ----------------------------------------------------------------------
! SUBROUTINE: yaw_nom.f90
! ----------------------------------------------------------------------
! Purpose:
!  Nominal yaw-attitude
!
!  The sign of the nominal yaw angle follows the convections of the eclips.f
! ----------------------------------------------------------------------
! Input arguments:
! - e_BX: 			Satellite Body-X unit vector (m)
! - v_sat: 			Satellite velocity vector (m/sec)
! - r_sun:			Sun position vector (m)
! - beta:			Sun angle with the orbital plane in degrees (see Note 1)
! - satblk:			Satellite Block series number 1=I, 2=II, 3=IIA, IIR=(4, 5) IIF=6
! - cosE:			Cosine of the angle (E) between the sv radius vector and
!					the sun radius vector (see Note 2)
! - ANOON			Beta angle limit (deg) for noon  (see Note 3)
! - ANIGHT			Beta angle limit (deg) for night (see Note 3)
!
! Output arguments:
! - Yangle :		Yaw angle (in degrees)
! - Mangle : 		Orbit angle (M) between satellite position vector and orbit midnight (in degrees)
! ----------------------------------------------------------------------
! Note 1:
!  Beta angle is computed by the beta_angle.f90 subroutine
!
! Note 2:
!  cosE (SVBCOS) is computed as the dot product of the two unit vectors
!  within the attitude_yaw.f90 subroutine
!
! Note 3:
!  ANOON and ANIGHT arguments are no longer necessary and should be removed
!  from the yaw_nom.f90 subroutine
! ----------------------------------------------------------------------
! Dr. Thomas D. Papanikolaou, Geoscience Australia             June 2016
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      REAL (KIND = prec_d), INTENT(IN) :: e_BX(3), v_sat(3), beta, cosE
      REAL (KIND = prec_d), INTENT(IN) :: ANOON, ANIGHT
      INTEGER (KIND = 4), INTENT(IN) :: satblk

! OUT
      REAL (KIND = prec_d), INTENT(OUT) :: Yangle, Mangle
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
      INTEGER :: yaw_opt
      REAL (KIND = prec_d) :: beta_rad, Mangle_rad, Mangle_deg, cosM, yaw, Yangle_rad
      REAL (KIND = prec_d) :: phi_sun, lamda_sun, radius_sun, phi_sat, lamda_sat, radius_sat
      REAL (KIND = prec_d) :: PHI, BETADG, SVBCOS, PI, DTR, CNOON, CNIGHT, DET
      REAL (KIND = prec_d) :: YawBias
      LOGICAL NOON, NIGHT
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Yaw angle computation (in degrees)
! ----------------------------------------------------------------------
      svbcos = cosE
      BETADG = beta
      PI = 3.1415926536D0
      DTR = PI/180.D0

! ----------------------------------------------------------------------
! Yaw angle (deg) as dot product of Body-X unit vector and intertial velocity unit vector
      yaw = acos( (e_BX(1) * v_sat(1) + e_BX(2) * v_sat(2) + e_BX(3) * v_sat(3)) &
	                  / sqrt(v_sat(1)**2 + v_sat(2)**2 + v_sat(3)**2) ) &
					 * (180D0 / PI_global)

! IIR YANGLE has the same sign as beta, II/IIA has the opposite sign
      if (beta .LT. 0.d0 .AND. satblk .GE. 4 .AND. satblk .LE. 5) then
          yaw = -yaw
      end if

      if (beta .GT. 0.d0 .AND. satblk .NE. 4 .AND. satblk .NE. 5) then
          yaw = -yaw
      end if
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Towards Night or Noon semicycle
! ----------------------------------------------------------------------
      CNOON  = DCOS(ANOON*DTR)
      CNIGHT = DCOS(ANIGHT*DTR)

! 18 August 2016: IIR night analogous to noon turn has been deactivated
! GPS Block IIR:
!      IF(satblk.EQ.4 .OR. satblk.EQ.5) THEN
!         CNIGHT = DCOS( (ANOON + 180.d0)*DTR )
!      END IF

      NOON=.FALSE.
      NIGHT=.FALSE.

      If (SVBCOS < 0.0D0) Then
        NIGHT=.TRUE.
      Else If (SVBCOS > 0.0D0) then
        NOON=.TRUE.

! ----------------------------------------------------------------------
      Else If (SVBCOS == 0.0D0) then
	  ! Special case: E = 90deg
        NOON=.TRUE.
! ----------------------------------------------------------------------

      END IF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Yaw angle (Phi) and Orbital angle (Mangle)
! ----------------------------------------------------------------------
! init PHI
PHI = PI/2.d0
IF ( (NIGHT .OR. NOON) ) THEN
      DET = SQRT( (180.d0-acos(svbcos)/DTR)**2 - BETADG**2)
      PHI = PI/2.d0
      Mangle = DET

! Towards night
       IF (NIGHT) THEN
! GPS IIR
         IF (satblk.EQ.4 .OR. satblk.EQ.5) THEN
          IF (DABS(yaw).GT.90.d0) DET=-DET
          If (DET.NE.0.d0) Then
			PHI = ATAN2( TAN(BETADG*DTR),-SIN(-DET*DTR))/DTR
			Mangle = -DET
		  End If
         ELSE
! GPS IIA/IIF & GLONASS
          IF (DABS(yaw).LT.90.d0) DET=-DET
          If (DET.NE.0.d0) Then
			PHI = ATAN2(-TAN(BETADG*DTR), SIN(-DET*DTR))/DTR
			Mangle = -DET
		  End If
         END IF
       END IF

! Towards noon
       IF (NOON) THEN
         DET = SQRT( (acos(svbcos) * 180.0D0 /pi)**2 - BETADG**2)
! GPS IIR
         IF (satblk.EQ.4 .OR. satblk.EQ.5) THEN
          IF (DABS(yaw).LT.90.d0) DET=-DET
          IF (DET.NE.0.d0) Then
			PHI = ATAN2(TAN(BETADG*DTR),-SIN(PI-DET*DTR))/DTR
			Mangle = 180.D0 - DET
		  End If
         ELSE
! GPS IIA/IIF & GLONASS
          IF (DABS(yaw).GT.90.d0) DET=-DET
          If (DET.NE.0.d0) Then
			PHI = ATAN2(-TAN(BETADG*DTR),SIN(PI-DET*DTR))/DTR
			Mangle = 180.D0 - DET
		  End If
         END IF
       END IF
End If
! ----------------------------------------------------------------------
      !PRINT *,"DET", DET
      !PRINT *,"PHI", PHI

      Yangle = PHI

! ----------------------------------------------------------------------
! Yaw Bias: Applied for IIA only
if (satblk == 3) then
    YawBias = 0.5D0
    !YawBias = -0.5D0 ! GPS23 (PRN23 t<=13/02/2004, PRN32 t>=26/02/2008)
	Yangle = Yangle + YawBias
end if
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Set Orbital angle in the range {-180,+180}
Mangle = MOD (Mangle, 360.D0)

If (ABS(Mangle) > 180.D0) Then
	Mangle = Mangle - 360.D0 * (Mangle / ABS(Mangle) )
End If
! ----------------------------------------------------------------------

END


      SUBROUTINE SQDM21
C
C     PHASE-I STRESS-DATA-RECOVERY ROUTINE FOR THE -QDMEM2- ELEMENT.
C
C     THIS ROUTINE WILL PREPARE FOR USE BY -SQDM22-, THE PHASE-II
C     ROUTINE, A TABLE CONTAINING THE FOLLOWING.
C
C     TABLE WORDS        DISCRIPTION
C     ------------------------------------------------------
C       1 THRU   1       ELEMENT-ID
C       2 THRU   5       4 SILS
C       6 THRU   6       ELEMENT-THICKNESS
C       7 THRU   7       REFERENCE TEMP -TSUB0-
C       8 THRU 151       16 (3X3) KIJ-G MATRICES
C     152 THRU 187       4 (3X3) STRESS MATRICES
C     188 THRU 199       4 (3X1) TEMP VECTORS
C     200 THRU 202       ST (3X1) STRESS-TEMPERATURE VECTOR
C     203 THRU 206       4 SIDE LENGTHS
C
C     ELEMENT EST ENTRY CONTENTS
C     + + + + + + + + + + + + + + + + + + + + + + + + + +
C     +   1 = ID                                        +
C     +   2 = SIL-PT-A            (ELEMENT CONNECTS     +
C     +   3 = SIL-PT-B             GRID POINTS A,B,     +
C     +   4 = SIL-PT-C             C,D IN THAT ORDER)   +
C     +   5 = SIL-PT-D                                  +
C     +   6 = MATERIAL-ANGLE                            +
C     +   7 = MATERIAL-ID                               +
C     +   8 = THICKNESS OF ELEMENT                      +
C     +   9 = NON-STRUCTURAL-MASS                       +
C     +  10 = COORD-SYS-ID PT-A OR 0                    +
C     +  11 = XA                                        +
C     +  12 = YA                                        +
C     +  13 = ZA                                        +
C     +  14 = COORD-SYS-ID PT-B OR 0                    +
C     +  15 = XB                                        +
C     +  16 = YB                                        +
C     +  17 = ZB                                        +
C     +  18 = COORD-SYS-ID PT-C OR 0                    +
C     +  19 = XC                                        +
C     +  20 = YC                                        +
C     +  21 = ZC                                        +
C     +  22 = COORD-SYS-ID PT-D OR 0                    +
C     +  23 = XD                                        +
C     +  24 = YD                                        +
C     +  25 = ZD                                        +
C     +  26 = AVERAGE OF CONNECTED GRID TEMPERATURES    +
C     + + + + + + + + + + + + + + + + + + + + + + + + + +
C
      LOGICAL         PLANAR
      INTEGER         NEST(7), MAP(4,3)
      REAL            K1SUM, K5SUM, ISINTH, ICOSTH, KMAT(63), SMAT(27),
     1                PMAT(9), JTEMP9, K5MOD, KTEMP9(9), ZMAT(9),
     2                ITEMP9(9), Q(3,3,4), IMAT12, RMAT(3,5), ETI(36),
     3                DVEC(3,4), KVEC(3)
      CHARACTER       UFM*23, UWM*25
      COMMON /XMSSG / UFM, UWM
C
C     FOLLOWING COMMON BLOCK MUST BE DIMENSIONED AT LEAST 350 IN SDR2B
C
      COMMON /SDR2X5/ EST(100), ID, ISILS(4), ELTHIK, REFTMP,
     1                K1SUM(9,16), SG(36),PT(3,4), ST(3),RG(4)
C
C     WORKING STORAGE BLOCK (KEEP .LE. 300 WORDS)
C
      COMMON /SDR2X6/ K5SUM(9,5), SISUM(9,5), PISUM(3,5), R(3,4,5),
     1                K5MOD(9,5), G(36),  T(9), E(9), IMAT12(12),
     2                JTEMP9(9), GSUBE(9)
      COMMON /MATIN / MATID, INFLAG, ELTEMP, STRESS, SINTH, COSTH
      COMMON /MATOUT/ G11, G12, G13, G22, G23, G33, RHO, ALPS(3), TSUB0
      COMMON /SYSTEM/ KSYSTM(65)
      COMMON /CONDAS/ PI, TWOPI, RADEG, DEGRA, S4PISQ
      EQUIVALENCE     (KSYSTM(2),IOUTPT), (NEST(1),EST(1))
      DATA    MAP   / 1, 2, 3, 4,
     1                2, 3, 4, 1,
     2                5, 5, 5, 5  /
C
C     COMPUTE BASIC SIN AND COSINE OF ELEMENT MATERIAL ANGLE.
C
      ANGL   = EST(6)*DEGRA
      ISINTH = SIN(ANGL)
      ICOSTH = COS(ANGL)
C
C     COMPUTE GSUBE MATRIX
C
      INFLAG = 2
      MATID  = NEST(7)
      ELTEMP = EST(26)
      SINTH  = 0.0
      COSTH  = 1.0
      CALL MAT (NEST(1))
      GSUBE(1) = G11
      GSUBE(2) = G12
      GSUBE(3) = G13
      GSUBE(4) = G12
      GSUBE(5) = G22
      GSUBE(6) = G23
      GSUBE(7) = G13
      GSUBE(8) = G23
      GSUBE(9) = G33
C
C     BASIC WHOLE-ELEMENT CALCULATIONS
C
      CALL Q2BCS (EST,PLANAR,RMAT,E,IERROR)
      IF (IERROR) 10,10,400
C
C     ZERO SUMMATION ARRAYS
C
   10 DO 40 I = 1,9
      DO 20 J = 1,16
      K1SUM(I,J) = 0.0
   20 CONTINUE
      DO 30 J = 1,5
      K5SUM(I,J) = 0.0
      SISUM(I,J) = 0.0
   30 CONTINUE
   40 CONTINUE
C
      DO 60 I = 1,5
      PISUM(1,I) = 0.0
      PISUM(2,I) = 0.0
      PISUM(3,I) = 0.0
   60 CONTINUE
C
C     SUB-TRIANGLES ARE COMPUTED AND RESULTS SUMMED.
C
      DO 100 I = 1,4
C
C     CALL TRIANGLE CALCULATION ROUTINE TO GET (3X3) SUB-PARTITIONS
C
      IA = MAP(I,1)
      IB = MAP(I,2)
      IC = MAP(I,3)
C
      CALL Q2TRMS (RMAT(1,IA),RMAT(1,IB),RMAT(1,IC),ALPS ,ISINTH,ICOSTH,
     1             GSUBE,EST(8),IERROR,3,KMAT,PMAT,SMAT,ZMAT)
      IF (IERROR) 70,70,400
C
C     SUM IN KCA,KCB,KCC 3-(3X3)-S STORED FIRST IN KMAT
C
C     ALSO SUM IN KAA,KAB,KBA,KBB = LAST 4-(3X3)-S STORED IN KMAT.
C     THESE GO INTO 4 OF THE 16 POSSIBLE (3X3) SUM MATRICES = ,
C
C     K11,K12,K13,K14,K21,K22,K23,K24,K31,K32,K33,K34,K41,K42,K43,K44
C
C     J1,J2,J3,J4 WILL EACH POINT TO 1 OF THE 16 (3X3)-S.
C
   70 J1 = 5*IA - 4
      J2 = 4*IA - 4 + IB
      J3 = 4*IB - 4 + IA
      J4 = 5*IB - 4
C
      DO 80 K = 1,9
      K5SUM(K,IA) = K5SUM(K,IA) + KMAT(K   )
      K5SUM(K,IB) = K5SUM(K,IB) + KMAT(K+ 9)
      K5SUM(K,IC) = K5SUM(K,IC) + KMAT(K+18)
      K1SUM(K,J1) = K1SUM(K,J1) + KMAT(K+27)
      K1SUM(K,J2) = K1SUM(K,J2) + KMAT(K+36)
      K1SUM(K,J3) = K1SUM(K,J3) + KMAT(K+45)
      K1SUM(K,J4) = K1SUM(K,J4) + KMAT(K+54)
      SISUM(K,IA) = SISUM(K,IA) + SMAT(K   )
      SISUM(K,IB) = SISUM(K,IB) + SMAT(K+ 9)
      SISUM(K,IC) = SISUM(K,IC) + SMAT(K+18)
   80 CONTINUE
C
      DO 90 K = 1,3
      PISUM(K,IA) = PISUM(K,IA) + PMAT(K  )
      PISUM(K,IB) = PISUM(K,IB) + PMAT(K+3)
      PISUM(K,IC) = PISUM(K,IC) + PMAT(K+6)
   90 CONTINUE
C
  100 CONTINUE
C
C     FORMATION OF THE FOUR (3X3) G MATRICES.
C                     -1
C     (G ) = -(K5SUM  ) (K  )   NOTE.  IF -PLANAR- THEN MODIFIED
C       I           55    5I           K5SUM MATRICES ARE USED.
C
      IF (PLANAR) GO TO 120
      DO 110 I = 1,5
      DO 110 J = 1,9
      K5MOD(J,I) = K5SUM(J,I)
  110 CONTINUE
      GO TO 140
C
  120 DO 130 I = 1,5
      K5MOD(1,I) = K5SUM(1,I)
      K5MOD(2,I) = K5SUM(2,I)
      K5MOD(3,I) = K5SUM(3,I)
      K5MOD(4,I) = K5SUM(4,I)
      K5MOD(5,I) = K5SUM(5,I)
      K5MOD(6,I) = K5SUM(6,I)
      K5MOD(7,I) = 0.0
      K5MOD(8,I) = 0.0
      K5MOD(9,I) =-0.25
  130 CONTINUE
      K5MOD(9,5) = 1.0
C
C     INVERT K5MOD   AND NEGATE RESULT.
C                 55
C
  140 CONTINUE
C
C     NO NEED TO COMPUTE DETERMINANT SINCE IT IS NOT USED SUBSEQUENTLY.
C
      ISING = -1
      CALL INVERS (3,K5MOD(1,5),3,DUMMY,0,DETERM,ISING,ITEMP9)
      IF (ISING .EQ. 2) GO TO 400
C
      DO 150 I = 1,9
      K5MOD(I,5) = -K5MOD(I,5)
  150 CONTINUE
C
C     FORM G MATRICES
C
      DO 160 I = 1,4
      CALL GMMATS (K5MOD(1,5),3,3,0, K5MOD(1,I),3,3,0, G(9*I-8))
  160 CONTINUE
C
C     FORM STIFFNESS MATRIX BY ROW-PARTIONS.
C
      DO 210 I = 1,4
C                          T
C     IF -PLANAR- FORM (G ) (K  ) FOR USE IN COLUMN-PARTITIONS LOOP.
C                        I    55
C
      IF (.NOT.PLANAR) GO TO 170
      CALL GMMATS (G(9*I-8),3,3,1, K5SUM(1,5),3,3,0, ITEMP9)
C
C     COLUMN-PARTITIONS-LOOP
C
  170 DO 200 J = 1,4
C                                   T
C     FORM (K  ) = (K1SUM  ) + (K  ) (G )
C            IJ          IJ      5I    J
C
      CALL GMMATS (K5SUM(1,I),3,3,1, G(9*J-8),3,3,0, JTEMP9)
      LPART = 4*I - 4 + J
      DO 180 K = 1,9
      K1SUM(K,LPART) = K1SUM(K,LPART) + JTEMP9(K)
  180 CONTINUE
C
C     BALANCE OF TERMS IF -PLANAR-
C
C                T            T
C     ADD IN (G ) (K  ) + (G ) (K  )(G )
C              I    5J      I    55   J
C
      IF (.NOT.PLANAR) GO TO 200
      CALL GMMATS (ITEMP9,3,3,0, G(9*J-8),3,3,0, JTEMP9)
      CALL GMMATS (G(9*I-8),3,3,1, K5SUM(1,J),3,3,0, KTEMP9)
      DO 190 K = 1,9
      K1SUM(K,LPART) = K1SUM(K,LPART) + KTEMP9(K) + JTEMP9(K)
  190 CONTINUE
  200 CONTINUE
  210 CONTINUE
C
C     CALCULATION OF 4 (Q ) MATRICES, EACH 3X3.
C                        I
C
      DO 260 I = 1,4
      IA = MAP(I,1)
      IB = MAP(I,2)
      DO 230 J = 1,3
      DVEC(J,I) = RMAT(J,IB) - RMAT(J,IA)
  230 CONTINUE
      FMAG  = SQRT(SADOTB(DVEC(1,I),DVEC(1,I)))
      RG(I) = FMAG
      IF (FMAG) 400,400,240
  240 DO 250 J = 1,3
      DVEC(J,I) = DVEC(J,I)/FMAG
  250 CONTINUE
  260 CONTINUE
C
      DO 280 I = 1,4
      J = I - 1
      IF (J .EQ. 0) J = 4
      I1 = MAP(J,1)
      I2 = MAP(J,2)
      CALL SAXB (DVEC(1,I2),DVEC(1,I1),KVEC)
C
C     NORMALIZE, NEGATE, AND STORE AS DELTA-VEC IN (Q )
C                                                    I
      FMAG = SQRT(SADOTB(KVEC,KVEC))
      IF (FMAG) 400,400,270
  270 Q(1,3,I) = -KVEC(1)/FMAG
      Q(2,3,I) = -KVEC(2)/FMAG
      Q(3,3,I) = -KVEC(3)/FMAG
C
C     STORE D VECTORS AS ALPHA- VECTORS IN (Q )
C                                            I
      Q(1,1,I) = -DVEC(1,I)
      Q(2,1,I) = -DVEC(2,I)
      Q(3,1,I) = -DVEC(3,I)
C
      Q(1,2,I) = DVEC(1,J)
      Q(2,2,I) = DVEC(2,J)
      Q(3,2,I) = DVEC(3,J)
C
C     INVERT 3X3
C
C     AGAIN NO NEED TO COMPUTE DETERMINANT SINCE IT IS NOT USED .
C     SUBSEQUENTLY.
C
      ISING = -1
      CALL INVERS (3,Q(1,1,I),3,DUMMY,0,DETERM,ISING,JTEMP9)
      IF (ISING .EQ. 2) GO TO 400
  280 CONTINUE
C
C     FORM FINAL OUTPUTS
C
      DO 370 I = 1,4
      II = 9*I - 8
C
C     TRANSFORMATION ETI = (E)(T )
C                               I
C
      KK = 4*I
      IF (NEST(KK+6)) 290,300,290
  290 CALL TRANSS (NEST(KK+6),T)
      CALL GMMATS (E,3,3,0, T,3,3,0, ETI(II))
      GO TO 320
C
  300 KK = II
      DO 310 J = 1,9
      ETI(KK) = E(J)
      KK = KK + 1
  310 CONTINUE
C
C       G            E      E
C     (S ) = 0.25( (S ) + (S )(G ) )(E)(T )
C       I            I      5   I        I
C
  320 CALL GMMATS (SISUM(1,5),3,3,0, G(II),3,3,0, JTEMP9)
      DO 330 J = 1,9
      JTEMP9(J) = 0.25*(JTEMP9(J)+SISUM(J,I))
  330 CONTINUE
      CALL GMMATS (JTEMP9,3,3,0, ETI(II),3,3,0, SG(II))
C
C       T     -         T -
C     (P ) = (P ) + (G ) (P )
C       I      I      I    5
C
      CALL GMMATS (G(II),3,3,1, PISUM(1,5),3,1,0, PT(1,I))
      DO 360 J = 1,3
      PISUM(J,I) = PT(J,I) + PISUM(J,I)
  360 CONTINUE
      CALL GMMATS (Q(1,1,I),3,3,1, PISUM(1,I),3,1,0, PT(1,I))
  370 CONTINUE
C
C     TRANSFORM STIFFNESS MATRIX TO GLOBAL
C
C        G           E
C     (K  ) = (Q )(K  )(E)(T )
C       IJ      I   IJ      J
C
      JPART = 0
      DO 390 I = 1,4
      DO 380 J = 1,4
      JPART = JPART + 1
      CALL GMMATS (Q(1,1,I),3,3,1, K1SUM(1,JPART),3,3,0, JTEMP9)
      CALL GMMATS (JTEMP9,3,3,0, ETI(9*J-8),3,3,0, K1SUM(1,JPART))
  380 CONTINUE
  390 CONTINUE
C
C     (S ) = (GSUBE)(ALPHAS)
C       T
C
      CALL GMMATS (GSUBE,3,3,0, ALPS,3,1,0, ST)
C
C     MISC. DATA FOR PHASE-II
C
      ID       = NEST(1)
      ISILS(1) = NEST(2)
      ISILS(2) = NEST(3)
      ISILS(3) = NEST(4)
      ISILS(4) = NEST(5)
      ELTHIK   = EST(8)
      REFTMP   = TSUB0
      RETURN
C
C     ERROR CONDITION
C
  400 WRITE  (IOUTPT,410) UWM,NEST(1)
  410 FORMAT (A25,' 3101, SINGULARITY OR BAD GEOMETRY FOR QDMEM2 ELEM.',
     1       ' ID =',I9, /5X,'STRESS OR FORCES WILL BE INCORRECT.')
      RETURN
      END