subroutine xbsm_calc (u, cmd_line)

use cesrv_struct
use cesrv_interface
use nr, only: zbrent
use twiss_and_track_mod

implicit none

type xbsm_line_struct
  character(16) name
  real(rp) r_detec(3), r_optics(3)  ! Positions of detector and optics
end type

type (xbsm_line_struct), target :: c_line, d_line  ! e- / e+ lines

type (universe_struct), target :: u
type (lat_struct), pointer :: lat
type (ele_struct), pointer :: bend, ele0
type (coord_struct) orb_at_s
type (ele_struct) ele_at_s
type (data_struct), pointer :: det1, det2
type (xbsm_line_struct), pointer :: line

character(*) cmd_line

! Check input

select case (cmd_line)
case ('ZERO', 'MODEL', 'DATA')
case ('')
  cmd_line = 'ZERO'
  print *, 'Default: Using ZERO orbit as particle orbit.'
case default
  print *, 'What is this? ', trim(cmd_line)
  return
end select

! Line setup

c_line%name = 'C-line (e-)'
c_line%r_detec  = [ 8.657984, -2.252472, 0.0]
c_line%r_optics = [18.988971,  0.212821, 0.0]

d_line%name = 'D-line (e+)'
d_line%r_detec  = [ -9.288702, -2.150914, 0.0]
d_line%r_optics = [-19.020430,  0.200139, 0.0]

lat => u%ring

call this_xbsm_calc (c_line)
call this_xbsm_calc (d_line)

!--------------------------------------------------------------------------------------------
contains

subroutine this_xbsm_calc (this_line)


type (xbsm_line_struct), target :: this_line
real(rp) d_near, d_this, ds, opening_angle
real(rp) d_source_to_optics, d_optics_to_detector

integer i, ix_det1, ix_det2

! Convert from Survey "lattice coordinants" to Bmad coordinates.

line => this_line

line%r_detec  = [-line%r_detec(2), 0.0_rp, line%r_detec(1)]
line%r_optics = [-line%r_optics(2), 0.0_rp, line%r_optics(1)]

! Find nearest bend by finding bend edge that is nearest

d_near = 1e30  ! Something large

do i = 1, lat%n_ele_track
  if (lat%ele(i)%key /= sbend$) cycle
  d_this = min(distance_to_line (lat%ele(i-1)%floor, line), distance_to_line (lat%ele(i)%floor, line))
  if (d_this > d_near) cycle
  d_near = d_this
  bend => lat%ele(i)
enddo

if (bend%slave_status == super_slave$) bend => pointer_to_lord(bend, 1)
ele0 => pointer_to_next_ele (bend, -1)

! Converge on nearest point

if (cmd_line == 'DATA') then
  do ix_det1 = 0, 98
    if (.not. u%orbit%x%d(ix_det1)%good_dat) cycle
    if (.not. u%orbit%x%d(ix_det1)%good_user) cycle
    if (u%orbit%x%d(ix_det1+1)%s > ele0%s) exit
  enddo
  do ix_det2 = 99, 1, -1
    if (.not. u%orbit%x%d(ix_det2)%good_dat) cycle
    if (.not. u%orbit%x%d(ix_det2)%good_user) cycle
    if (u%orbit%x%d(ix_det2-1)%s < ele0%s) exit
  enddo

  det1 => u%orbit%x%d(ix_det1)
  det2 => u%orbit%x%d(ix_det2)
endif

ds = zbrent (angle_to_line, 0.0_rp, bend%value(l$), 1d-10)
opening_angle = (3 * 12.4e-10 * bend%value(g$) / twopi)**(1.0/3)

d_source_to_optics = norm2(ele_at_s%floor%r-line%r_optics)
d_optics_to_detector = norm2(line%r_optics-line%r_detec)
 
print *
print *, 'Source point for:                     ', trim(line%name)
print *, 'Bend containing source pt:            ', trim(bend%name)
if (cmd_line == 'DATA') then
  print *, 'Position at detector:               ', det1%meas, '   ', trim(det1%ele_name)
  print *, 'Position at detector:               ', det2%meas, '   ', trim(det2%ele_name)
endif
print *, 'S-position of source:               ', ds + ele0%s
print *, 'Source-to_optics distance:          ', d_source_to_optics
print *, 'Optics-to-Detector distance:        ', d_optics_to_detector
print *, 'Magnification:                      ', d_optics_to_detector / d_source_to_optics
print *, 'Full Opening angle of 1keV photons: ', opening_angle
print *, 'Depth of field (+/-):               ', opening_angle * bend%value(rho$) / 2
print '(1x, a, 2f14.6)', 'Floor coords at Source (x, z):      ', ele_at_s%floor%r(1), ele_at_s%floor%r(3)
print *, 'Perpendicular distance from Source to XBSM line:', distance_to_line(ele_at_s%floor, line)

end subroutine this_xbsm_calc

!---------------------------------------------------------------------------
! contains

! Perpendicular distance from a given point to the xbsm line.

function distance_to_line (floor, xbsm) result (distance)

type (floor_position_struct) floor
type (xbsm_line_struct) xbsm
real(rp) alpha, distance, dr(3), r_alpha(3)

!

dr = xbsm%r_detec - xbsm%r_optics
alpha = dot_product (floor%r - xbsm%r_optics, dr) / norm2(dr)**2
r_alpha = xbsm%r_optics + alpha * dr
distance = norm2(r_alpha - floor%r)

end function distance_to_line

!---------------------------------------------------------------------------
! contains

! Difference in angle between particle trajectory and xbsm line.

function angle_to_line (ds) result (d_angle)

real(rp), intent(in) :: ds
real(rp) d_angle, particle_angle, line_angle

!

call twiss_and_track_at_s (lat, ds + ele0%s, ele_at_s, u%orb, orb_at_s)
select case (cmd_line)
case ('ZERO')
  particle_angle = ele_at_s%floor%theta
case ('MODEL')
  particle_angle = ele_at_s%floor%theta + orb_at_s%vec(2) / (1 + orb_at_s%vec(6))
case ('DATA')
  particle_angle = ele_at_s%floor%theta + (det2%meas - det1%meas) / (det2%s - det1%s)
end select

line_angle = atan2(line%r_optics(1)-line%r_detec(1), line%r_optics(3)-line%r_detec(3))
d_angle = modulo2(particle_angle - line_angle, pi/2)

end function angle_to_line

end subroutine