! From MPE's mc_ibs_mod.f90
! See Bmad manual on macroparticle tracking.


SUBROUTINE make_sigma_mat(canonical_dist,sigma_mat,sigma_beta_mat)
  USE bmad
  
  IMPLICIT none
  
  TYPE(coord_struct) canonical_dist(:)
  REAL(rp) sigma_mat(:,:), sigma_beta_mat(:,:)
  
  INTEGER j, k
  INTEGER Nparts
  
  
  Nparts = SIZE(canonical_dist)
  
  DO j=1,6
     DO k=j,6
      !Fortran * on two arrays is element-wise multiplication
        sigma_mat(j,k) = SUM(canonical_dist(:)%vec(j)*canonical_dist(:)%vec(k)) / Nparts
        IF( j .ne. k ) THEN
           sigma_mat(k,j) = sigma_mat(j,k)
        ENDIF
     ENDDO
  ENDDO

  DO j=1,6
     DO k=j,6        
        sigma_beta_mat(j,k) = sigma_mat(j,k) - sigma_mat(j,6)*sigma_mat(k,6)/sigma_mat(6,6)
        IF( j .ne. k ) THEN
           sigma_beta_mat(k,j) = sigma_beta_mat(j,k)
        ENDIF
     ENDDO
  ENDDO
  
END SUBROUTINE make_sigma_mat

!############################################
SUBROUTINE normal_sigma_mat(sigma_mat,normal)
  USE bmad
  
  IMPLICIT none

  REAL(rp) sigma_mat(1:6,1:6)
  REAL(rp) normal(1:3)
  REAL(rp) sigmaS(1:6,1:6)
  REAL(rp) S(1:6,1:6)
  REAL(rp), PARAMETER :: S2(1:2,1:2) = reshape([0,-1,1,0], [2,2])

  complex(rp) eigen_val(6), eigen_vec(6,6)

  LOGICAL error

  S = 0.0d0
  S(1:2,1:2) = S2
  S(3:4,3:4) = S2
  S(5:6,5:6) = S2

  sigmaS = MATMUL(sigma_mat,S)

  CALL mat_eigen(sigmaS, eigen_val, eigen_vec, error)

  normal(1) = ABS(aimag(eigen_val(1)))
  normal(2) = ABS(aimag(eigen_val(3)))
  normal(3) = ABS(aimag(eigen_val(5)))
END SUBROUTINE normal_sigma_mat