subroutine ring_calc (u)

use cesrv_utils_mod
use taylor2_mod

implicit none

type (universe_struct), target :: u
type (coord_struct), allocatable, save :: orb2(:)
type (ele_struct), pointer :: ele
type (ele_struct), save :: ele2
type (twiss_struct) twiss1, twiss2
type (lat_struct), pointer :: ring
type (ele_struct), pointer :: eles(:)
type (db_element_struct), pointer :: db_ele

integer i, j, k, ie, ix, stat, ix1, ix2, ix_end, ixc, ix_q, status, reflect

real(rp) cbar_mat(2,2), sin_t, cos_t, x, y, f
real(rp) u_mat(4,4), v(4,4), ubar(4,4), vbar(4,4), g(4,4)
real(rp) mat_conj(4,4), mat4(4,4), dkl, dmat4(4,4)
real(rp) t4(4,4), xx, yy, delta_pz, denom, ll, mag
real(rp) r_beta, x_y, x_y_model, y_x, y_x_model
real(rp) v16, v36,rel_amp_cos, rel_amp_sin
real(rp) step, dphi1_norm, dphi2_norm, dphi1_cross, dphi2_cross

logical mask(0:120), mask2(0:49), maximal_calc, orbit_calc, twiss_calc
logical error

! Init

u%ok_status = .false.
call reallocate_coord (orb2, size(u%orb))
u%orb%species = u%ring%param%particle
ring => u%ring
eles => ring%ele
delta_pz = 1e-8  

call lattice_bookkeeper (u%ring)

! What do we calculate? Try to do minimal calculation when the optimizer is
! running or recalculating the dmerit matrix.

maximal_calc = .not. (logic%opt_running .or. logic%dmerit_calc_on)
orbit_calc = maximal_calc .or. logic%opt_data == opt_orbit$ .or. &
             logic%opt_data == opt_all_data$
twiss_calc = maximal_calc .or. logic%opt_data == opt_twiss$ .or. &
             logic%opt_data == opt_all_data$ .or. any(u%mode_eta%y%d%useit_opt) .or. &
             any(u%eta%y%d%useit_opt) .or. any(u%ac_eta%y%d%useit_opt) 

!

if (maximal_calc .and. logic%lrbbi_inserted) then
  do i = 1, ring%n_ele_track
    if (ring%ele(i)%name(1:6) == 'LRBBI_') then
      ring%ele(i)%value(x_offset$) = -u%orb(i)%vec(1)
      ring%ele(i)%value(y_offset$) = -u%orb(i)%vec(3)
      ring%ele(i)%value(sig_x$) = sqrt(ring%ele(i)%a%beta * ring%a%emit)
      ring%ele(i)%value(sig_y$) = sqrt(ring%ele(i)%b%beta * ring%b%emit)
      call lat_make_mat6 (ring, i, u%orb)
    endif
  enddo
endif

! do this for closed orbits

if (maximal_calc)  call lat_make_mat6 (ring, -1, u%orb)

if (maximal_calc .and. .not. logic%calc_twiss_with_cut_ring) then
  call twiss_at_start (ring, status = status)
  if (status /= ok$) then
    call lat_make_mat6(ring)  ! remake around the zero orbit.
    call closed_orbit_calc (ring, u%orb, 4)
    !! return
  endif
endif

!

if (ring%param%geometry == open$) then
  ixc = logic%ix_cut_ring
  if (ring%param%particle == positron$) then
    ix_end = ixc - 1
    if (ixc == 0) ix_end = ring%n_ele_track  ! exception
  else
    ix_end = ixc + 1
    if (ixc == ring%n_ele_track) ix_end = 0
  endif

  call track_many (ring, u%orb, ixc, ix_end, ring%param%particle)

  if (logic%calc_twiss_with_cut_ring) call twiss_propagate_many (ring, &
                                          ixc, ix_end, ring%param%particle)
                    
elseif (orbit_calc) then

  if (logic%rf_on) then
    call closed_orbit_calc (ring, u%orb, 6)

  else
    if (.not. u%energy_var%v(1)%good_user) then  !  energy varies with orbit
      call closed_orbit_calc (ring, u%orb, 4)
      u%orb(0)%vec(6) = u%orb(0)%vec(6) + u%orb(ring%n_ele_track)%vec(5) / logic%l_times_alpha
    endif
    call closed_orbit_calc (ring, u%orb, 4, err_flag = error)
    if (error) return
  endif

endif

if (maximal_calc .or. orbit_calc)  call lat_make_mat6 (ring, -1, u%orb)

if (twiss_calc .and. ring%param%geometry == closed$) then
  call twiss_at_start (ring, status = status)
  !!! if (status /= ok$) return
  call twiss_propagate_all (ring)
endif

!---------------------------------------------------------------
! now transfer data to %model arrays

! sextupole resonances

!!!! call taylor2_calc (u)

! beta

if (logic%opt_running) then
  mask = u%beta%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%beta%x%d(:)%good_opt
else
  mask = u%beta%x%d(:)%exists
endif

do i = lbound(u%beta%x%d, 1), ubound(u%beta%x%d, 1)
  if (.not. mask(i)) cycle
  ix = u%phase%x%d(i)%ix_ele
  if (ring%ele(ix)%mode_flip) then
    u%beta%x%d(i)%model = ring%ele(ix)%b%beta
    u%beta%y%d(i)%model = ring%ele(ix)%a%beta
  else
    u%beta%x%d(i)%model = ring%ele(ix)%a%beta
    u%beta%y%d(i)%model = ring%ele(ix)%b%beta
  endif
enddo

! phase

if (logic%opt_running) then
  mask = u%phase%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%phase%x%d(:)%good_opt
else                
  mask = u%phase%x%d(:)%exists
endif

do i = lbound(u%phase%x%d, 1), ubound(u%phase%x%d, 1)
  if (.not. mask(i)) cycle
  ix = u%phase%x%d(i)%ix_ele
  if (ring%ele(ix)%mode_flip) then
    u%phase%x%d(i)%model = ring%ele(ix)%b%phi
    u%phase%y%d(i)%model = ring%ele(ix)%a%phi
  else
    u%phase%x%d(i)%model = ring%ele(ix)%a%phi
    u%phase%y%d(i)%model = ring%ele(ix)%b%phi
  endif
enddo

! cbar

if (logic%opt_running) then
  mask = u%cbar%m12%d(:)%useit_opt .or. u%cmat_a%m12%d(:)%useit_opt .or. u%cmat_b%m12%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%cbar%m12%d(:)%good_opt .or. u%cmat_a%m12%d(:)%good_opt .or. u%cmat_b%m12%d(:)%good_opt
else
  mask = u%cbar%m12%d(:)%exists
endif

do i = lbound(u%cbar%m11%d, 1), ubound(u%cbar%m11%d, 1)
  if (.not. mask(i)) cycle
  call c_to_cbar (ring%ele(u%cbar%m11%d(i)%ix_ele), cbar_mat)
  ie = u%orbit%x%d(i)%ix_ele
  r_beta = sqrt(ring%ele(ie)%b%beta / ring%ele(ie)%a%beta)

  if (ring%ele(ie)%value(tilt$) == 0) then
    u%cbar%m11%d(i)%model = cbar_mat(1,1)
    u%cbar%m12%d(i)%model = cbar_mat(1,2)
    u%cbar%m21%d(i)%model = cbar_mat(2,1)
    u%cbar%m22%d(i)%model = cbar_mat(2,2)

  else
    if (logic%new_cbar_tilt_calc .eqv. .false.) then
       u%cbar%m12%d(i)%model = cbar_mat(1,2)
       u%cbar%m21%d(i)%model = cbar_mat(2,1)
   
       sin_t = sin(ring%ele(ie)%value(tilt$))
       cos_t = cos(ring%ele(ie)%value(tilt$))
   
       x_y = cbar_mat(1,1) / r_beta
       x_y_model = (x_y * cos_t + sin_t) / (-x_y * sin_t + cos_t)
       u%cbar%m11%d(i)%model = r_beta * x_y_model
   
       y_x = -r_beta * cbar_mat(2,2)
       y_x_model = (-sin_t + y_x * cos_t) / (cos_t + y_x * sin_t)
       u%cbar%m22%d(i)%model = -y_x_model / r_beta     
     else ! logic%new_cbar_tilt_calc .eqv. .true.

       ! use JSh new cbar rotation algorithm:
       call rotate_cbar (ring%ele(u%cbar%m11%d(i)%ix_ele), ring%ele(ie)%value(tilt$), cbar_mat)

       u%cbar%m11%d(i)%model = cbar_mat(1,1)
       u%cbar%m12%d(i)%model = cbar_mat(1,2)
       u%cbar%m21%d(i)%model = cbar_mat(2,1)
       u%cbar%m22%d(i)%model = cbar_mat(2,2)
     
     endif
 
  endif

  u%cmat_a%m22%d(i)%model = cbar_mat(1,1) * r_beta
  u%cmat_a%m12%d(i)%model = cbar_mat(1,2) * r_beta
  u%cmat_b%m12%d(i)%model = cbar_mat(1,2) / r_beta
  u%cmat_b%m11%d(i)%model = cbar_mat(2,2) / r_beta

enddo

! orbit

if (logic%opt_running) then
  mask = u%orbit%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%orbit%x%d(:)%good_opt
else
  mask = u%orbit%x%d(:)%exists
endif

do i = lbound(u%orbit%x%d, 1), ubound(u%orbit%x%d, 1)
  if (.not. mask(i)) cycle
  ie = u%orbit%x%d(i)%ix_ele
  if (ring%ele(ie)%value(tilt$) == 0) then
    u%orbit%x%d(i)%model = u%orb(ie)%vec(1)
    u%orbit%y%d(i)%model = u%orb(ie)%vec(3)
  else
    x = u%orb(ie)%vec(1)
    y = u%orb(ie)%vec(3)
    sin_t = sin(ring%ele(ie)%value(tilt$))
    cos_t = cos(ring%ele(ie)%value(tilt$))
    u%orbit%x%d(i)%model =  x * cos_t + y * sin_t 
    u%orbit%y%d(i)%model = -x * sin_t + y * cos_t
  endif
enddo

! xray

do i = lbound(u%e_xray%y%d, 1), ubound(u%e_xray%y%d, 1)
  if (.not. u%e_xray%y%d(i)%exists) cycle
  ie = u%e_xray%y%d(i)%ix_ele
  db_ele => u%db%e_chess_monitor_source(i)
  ll = db_ele%val(dist_to_detec$)
  mag = db_ele%val(magnification$)
  reflect = 1
  if (db_ele%logic(has_mirror$)) reflect = -1
  u%e_xray%y%d(i)%model = u%orb(ie)%vec(3) - reflect * u%orb(ie)%vec(4) * ll * mag
enddo

do i = lbound(u%p_xray%y%d, 1), ubound(u%p_xray%y%d, 1)
  if (.not. u%p_xray%y%d(i)%exists) cycle
  ie = u%p_xray%y%d(i)%ix_ele
  db_ele => u%db%p_chess_monitor_source(i)
  ll = db_ele%val(dist_to_detec$)
  mag = db_ele%val(magnification$)
  reflect = 1
  if (db_ele%logic(has_mirror$)) reflect = -1
  u%p_xray%y%d(i)%model = u%orb(ie)%vec(3) + reflect * u%orb(ie)%vec(4) * ll * mag
enddo

! energy

if (logic%opt_running) then
  mask = u%energy_data%d1%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%energy_data%d1%d(:)%good_opt
else
  mask = u%energy_data%d1%d(:)%exists
endif

where (mask) u%energy_data%d1%d(:)%model = u%orb(u%energy_data%d1%d(:)%ix_ele)%vec(6)

! mode_eta

if (logic%opt_running) then
  mask = u%mode_eta%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%mode_eta%x%d(:)%good_opt
else
  mask = u%mode_eta%x%d(:)%exists
endif

do i = lbound(u%mode_eta%x%d, 1), ubound(u%mode_eta%x%d, 1)
  if (.not. mask(i)) cycle
  ie = u%mode_eta%x%d(i)%ix_ele
  if (ring%ele(ie)%value(tilt$) == 0) then
    u%mode_eta%x%d(i)%model = ring%ele(ie)%a%eta
    u%mode_eta%y%d(i)%model = ring%ele(ie)%b%eta
  else
    x = ring%ele(ie)%a%eta
    y = ring%ele(ie)%b%eta
    sin_t = sin(ring%ele(ie)%value(tilt$))
    cos_t = cos(ring%ele(ie)%value(tilt$))
    u%mode_eta%x%d(i)%model =  x * cos_t + y * sin_t 
    u%mode_eta%y%d(i)%model = -x * sin_t + y * cos_t
  endif
enddo

! eta

if (logic%opt_running) then
  mask = u%eta%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%eta%x%d(:)%good_opt
else
  mask = u%eta%x%d(:)%exists
endif

do i = lbound(u%eta%x%d, 1), ubound(u%eta%x%d, 1)
  if (.not. mask(i)) cycle
  ie = u%eta%x%d(i)%ix_ele

  x = ring%ele(ie)%x%eta
  y = ring%ele(ie)%y%eta

  if (ring%ele(ie)%value(tilt$) == 0) then
    u%eta%x%d(i)%model = x
    u%eta%y%d(i)%model = y
  else
    sin_t = sin(ring%ele(ie)%value(tilt$))
    cos_t = cos(ring%ele(ie)%value(tilt$))
    u%eta%x%d(i)%model =  x * cos_t + y * sin_t 
    u%eta%y%d(i)%model = -x * sin_t + y * cos_t
  endif
enddo

! ac_eta

if (logic%opt_running) then
  mask = u%ac_eta%x%d(:)%useit_opt
elseif (logic%dmerit_calc_on) then
  mask = u%ac_eta%x%d(:)%good_opt
else
  mask = u%ac_eta%x%d(:)%exists
endif

if (any (mask)) then
  call mode3_calc (u)
  ring%z%tune = ring%ele(ring%n_ele_track)%mode3%c%phi
  u%tune%z%d(1)%model = -ring%z%tune
endif

do i = lbound(u%ac_eta%x%d, 1), ubound(u%ac_eta%x%d, 1)
  if (.not. mask(i)) cycle
  ie = u%ac_eta%x%d(i)%ix_ele

  if (.false.) then
    x = ring%ele(ie)%mode3%a%eta
    y = ring%ele(ie)%mode3%b%eta
    v16 = ring%ele(ie)%mode3%V(1,6)
    v36 = ring%ele(ie)%mode3%V(3,6)
  else
    x = ring%ele(ie)%mode3%x%eta
    y = ring%ele(ie)%mode3%y%eta
    v16 = ring%ele(ie)%mode3%V(1,6)
    v36 = ring%ele(ie)%mode3%V(3,6)
  endif

  if (ring%ele(ie)%value(tilt$) == 0) then
    u%ac_eta%x%d(i)%model = x
    u%ac_eta%y%d(i)%model = y
    u%ac_eta_c12%a%d(i)%model = v16    !v16 model
    u%ac_eta_c12%b%d(i)%model = v36    !v36 model
    call ac_eta_relative_xy_calc(ring%ele(ie), rel_amp_cos, rel_amp_sin)  !compute yamp/xamp * cos (phi_y-phi_x), and sin 
    u%ac_eta_yx%yxcos%d(i)%model = rel_amp_cos   !yamp/xamp * cos (phi_y-phi_x)
    u%ac_eta_yx%yxsin%d(i)%model = rel_amp_sin   !yamp/xamp * sin (phi_y-phi_x)
  else
    sin_t = sin(ring%ele(ie)%value(tilt$))
    cos_t = cos(ring%ele(ie)%value(tilt$))
    u%ac_eta%x%d(i)%model =  x * cos_t + y * sin_t 
    u%ac_eta%y%d(i)%model = -x * sin_t + y * cos_t
  endif
enddo

! Renomalize measured ac_eta

mask = u%ac_eta%x%d(:)%useit_opt
if (any(mask)) then
  denom = sum(u%ac_eta%x%d(:)%meas, mask)
  if (denom == 0) then
    f = 0
  else
    f = sum (u%ac_eta%x%d(:)%model, mask) / denom
  endif
  u%ac_eta%x%d(:)%meas = f * u%ac_eta%x%d(:)%meas
  u%ac_eta%y%d(:)%meas = f * u%ac_eta%y%d(:)%meas
  if (u%ac_eta%ref_measured) then
    u%ac_eta%x%d(:)%ref = f * u%ac_eta%x%d(:)%ref
    u%ac_eta%y%d(:)%ref = f * u%ac_eta%y%d(:)%ref
  endif
endif

! tune

ie = u%tune%x%d(1)%ix_ele
u%tune%x%d(1)%model = ring%ele(ie)%a%phi
u%tune%y%d(1)%model = ring%ele(ie)%b%phi

! chrom

if (maximal_calc .or. u%chrom%x%d(1)%useit_opt) then
  call chrom_calc (ring, 5.0d-4, xx, yy)
  u%chrom%x%d(1)%model = xx; u%chrom%y%d(1)%model = yy
endif

u%ok_status = .true.

end subroutine