subroutine create_taylor_ele(ele, co)

  use bmad
  use parameters_bmad
  implicit none

  type (ele_struct) ele
  type (coord_struct) co

  real(rp) m(6,6)
  integer lun
  integer i
  character*27 ele_name 

  m = ele%mat6
  lun = lunget()
  ele_name = trim(ele%name)//'_taylor'
  open(unit = lun, file = trim(directory)//'/'//trim(ele_name)//'.dat')
  write(lun,'(a)')'parameter[custom_attribute3]   = "taylor::x_angle"'
  write(lun,'(a,a10,1x,a2,f8.3,a2,a8,es12.4,a3)')trim(ele_name),': taylor, ','l=',ele%value(l$),', ','x_angle=', co%vec(2),', &'
  do i=1,6
  if(i /= 6)then
  write(lun,'(6(a1,i1,a2,es11.4,a))') '{',i,': ' ,m(i,1), ', 1 0 0 0 0 0}, ', '{',i,': ',m(i,2), ', 0 1 0 0 0 0},' ,'{',i,': ',m(i,3), ', 0 0 1 0 0 0},' , &
                                      '{',i,': ' ,m(i,4), ', 0 0 0 1 0 0},' , '{',i,': ',m(i,5), ', 0 0 0 0 1 0},', '{',i,': ',m(i,6), ', 0 0 0 0 0 1}, &'
  else
  write(lun,'(6(a1,i1,a2,es11.4,a))') '{',i,': ' ,m(i,1), ', 1 0 0 0 0 0}, ', '{',i,': ',m(i,2), ', 0 1 0 0 0 0},' ,'{',i,': ',m(i,3), ', 0 0 1 0 0 0},' , &
                                      '{',i,': ' ,m(i,4), ', 0 0 0 1 0 0},' , '{',i,': ',m(i,5), ', 0 0 0 0 1 0},', '{',i,': ',m(i,6), ', 0 0 0 0 0 1}'
  endif

 end do
  close(unit = lun)
return
end

subroutine create_inv_taylor_ele(ele)

  use bmad
  use parameters_bmad
  implicit none

  type (ele_struct) ele
  
  real(rp) m(6,6), sig(6,6), S(6,6), T(6,6), M_inv(6,6)
  integer lun
  integer i
  character*31 ele_name
  
 m=ele%mat6
 call invert_matrix(m) 

print '(/,a)',' sigma_z'
do i=1,6
 print '(6es12.4)',sig(i,1:6)
end do
print '(/,a)',' S '
do i=1,6
 print '(6es12.4)',S(i,1:6)
end do
print '(/,a)',' m'
do i=1,6
 print '(6es12.4)',m(i,1:6)
end do


  lun = lunget()
  ele_name = trim(ele%name)//'_inv_taylor'
  open(unit = lun, file = trim(directory)//'/'//trim(ele_name)//'.dat')
  write(lun,'(a,a10,1x,a2,f8.3,a3)')trim(ele_name),': taylor, ','l=',ele%value(l$),', &'
  do i=1,6
  if(i /= 6)then
  write(lun,'(6(a1,i1,a2,es11.4,a))') '{',i,': ' ,m(i,1), ', 1 0 0 0 0 0}, ', '{',i,': ',m(i,2), ', 0 1 0 0 0 0},' ,'{',i,': ',m(i,3), ', 0 0 1 0 0 0},' , &
                                      '{',i,': ' ,m(i,4), ', 0 0 0 1 0 0},' , '{',i,': ',m(i,5), ', 0 0 0 0 1 0},', '{',i,': ',m(i,6), ', 0 0 0 0 0 1}, &'
  else
  write(lun,'(6(a1,i1,a2,es11.4,a))') '{',i,': ' ,m(i,1), ', 1 0 0 0 0 0}, ', '{',i,': ',m(i,2), ', 0 1 0 0 0 0},' ,'{',i,': ',m(i,3), ', 0 0 1 0 0 0},' , &
                                      '{',i,': ' ,m(i,4), ', 0 0 0 1 0 0},' , '{',i,': ',m(i,5), ', 0 0 0 0 1 0},', '{',i,': ',m(i,6), ', 0 0 0 0 0 1}'
  endif

 end do
  close(unit = lun)
return
end

subroutine invert_matrix(m)
  use bmad
  implicit none
  real(rp) m(6,6), sig(6,6), S(6,6), T(6,6), M_inv(6,6)
  integer i

  sig=0.
  S=0.
  do i=1,3 
    sig(2*i-1,2*i-1)=1.
    sig(2*i,2*i)=-1.
    S(2*i-1, 2*i) = -1.    
    S(2*i, 2*i-1) = 1.
  end do    

  T = matmul(transpose(m),S)
  m_inv = -matmul(S,T)
  T = matmul(m_inv,sig)
  m = matmul(sig,T)
  m = m_inv
return
end