program betadots
  use sim_utils
  use precision_def

  implicit none

  integer i

  real(rp) omega, Period/149.2e-9/, omega_p, psi0/0.002/,  qv/0.4/, time_step
  real(rp) time, psi
  real(rp) beta_dot_s, delta_bds, s_mag
  real(rp) sign_s_perp/1./
 real(rp) delta_bds_0
  real(rp) B(1:3), beta(1:3), s(1:3), s_perp(1:3), beta_cross_B(1:3), s_x(0:2), curvature(2)
  omega = twopi/Period/30.
  omega_p = qv*twopi/Period 
  s_perp =0.
  s_perp(1)=1.
  B(1) = 0
  B(2) = 0
  B(3) = 1.
  beta_dot_s = 0.
  delta_bds=0.
  s(1:3)=0
  s(1) = 1.
  s_x = 0
  s_x(2)=1.
  time_step = Period/1000.
  write(11, '(6a16,3a16,a16,3a16,2a16)')'time', 'beta_y', 'beta_z', 'beta_X_B_x',  'delta_bds', 'beta_dot_s','s_perp_x','s_perp_y','s_perp_z','s_mag','s_x','s_y','s_z','curvature(1)','curvature(2)'
do i = 0, 1000000
  time = i*time_step
  psi = psi0 * cos(omega_p * time)
  beta(1)=0
  beta(2) = cos(psi)
  beta(3) = sin(psi)
  beta_cross_B(1:3) = reshape((/beta(2)*B(3) - beta(3)*B(2), beta(3)*B(1)-B(3)*beta(1), beta(1)*B(2) - beta(2)*B(1)/),shape(beta_cross_B)) 

!  s_perp(1:3) = s(1:3) - delta_bds * beta(1:3)
  
  call step(s_perp, beta_cross_B,omega,  time_step, delta_bds)
  beta_dot_s = beta_dot_s + delta_bds
  if(abs(beta_dot_s) > 1)then
     beta_dot_s = beta_dot_s - delta_bds 
     sign_s_perp = -sign_s_perp
     s_perp(1) = sign_s_perp*sqrt(1. -  (beta_dot_s)**2)
!    if((beta_dot_s - delta_bds_0)**2 > 1)print *,'beta_dot_s - delta_bds_0' ,beta_dot_s - delta_bds_0
     call step(s_perp, beta_cross_B,omega,  time_step, delta_bds)
     beta_dot_s = beta_dot_s + delta_bds
  else
   s_perp(1) = sign_s_perp*sqrt( 1.-(beta_dot_s)**2)
  endif
  s(1:3) = s_perp(1:3) + beta_dot_s*beta(1:3)
!  s(1:3) = s(1:3)/sqrt(dot_product(s,s))
  s_x(0) = s_x(1)
  s_x(1) = s_x(2)
  s_x(2) = s(1)
  curvature(1) = curvature(2)
  curvature(2) = (( s_x(2)+s_x(0) )/2. -s_x(1))/s_x(1)
  s_mag = sqrt(dot_product(s,s))
!  if(curvature(2) > 0 )s(1) = -s(1)
!  if(curvature(1)*curvature(2) <0)s(1) = -s(1)


     
  write(11, '(6es16.8,3es16.8,es16.8,3es16.8, 2es16.8)')time, beta(2), beta(3), beta_cross_B(1),  delta_bds, beta_dot_s, s_perp,s_mag, s, curvature
 end do


end

  subroutine step(s_perp, beta_cross_B, omega, time_step, delta_bds)
  use precision_def
  implicit none
  real(rp) omega, delta_bds, time_step
  real(rp) s_perp(1:3), beta_cross_B(1:3)
  
  delta_bds = -omega * dot_product(s_perp,beta_cross_B) * time_step
  return
  end subroutine