! find r and vl at outer mid-plane conserving 
! energy and canonical momentum
subroutine orbrz_find_rmid(zr0, zvl0, cnt, iflag, cnt_max, err_max)

  use ezspline, only : ezspline_interp2,ezspline_gradient2
  use nbi_com,only : orbrzv_zzerr_con,zc,zepslon=>epslon
  use orbrzv
  use xplasma_obj_instance
  use logmod
  
  implicit none
  real(fp), intent(out) :: zr0, zvl0
  integer, intent(out) :: cnt, iflag
  integer, intent(in) :: cnt_max
  real(fp), intent(in) :: err_max
  integer :: ier
  real(fp) :: b, db(1:2), psi, dpsi(1:2), g, dg(1:2), phi, dphi(1:2), &
            & tcmtp, zs, zerr, zz0, zdumj(2,2), zrho, zdcmdr, zdvdr, &
            & zsgn, zerr_max, zdr, zth, zrtmp, zztmp

  zr0  = orbrzv_zz(1)
  zvl0 = orbrzv_zz(3)
  iflag = 0
  zdr = 0.5_fp*(orbrzv_prmax + orbrzv_rmax) - orbrzv_prmax

  ! current position is close enough to outter mid-plane already
  if(dabs(orbrzv_zz(2)-orbrzv_zc) <= &
   & orbrzv_zzscal_min(2)*orbrzv_zzerr_con) return

  ! find rho and theta
  zrtmp = orbrzv_zz(1)*1.0e-2_fp
  zztmp = orbrzv_zz(2)*1.0e-2_fp
  call xplasma_ctrans(s,ier,r_in=zrtmp,z_in=zztmp,rho_out=zrho,theta_out=zth)

  ! if particle is too close to center, also use the current position
  if(zrho <= 0.05_fp) return

  zsgn = dsign(1.0_fp, zvl0)
  zrho = dmin1(1.0_fp, zrho)
  zerr_max = dmax1(1.0e-2_fp, err_max)

  ! guess of outter mid-plane crossing point
  call xplasma_ctrans(s, ier, rho_in = zrho, theta_in = 0.0_fp, & 
                    & r_out=zrtmp, z_out=zztmp)
  zr0 = zrtmp*1.0e2_fp
  zz0 = zztmp*1.0e2_fp

  do cnt = 1, cnt_max

    call ezspline_interp2(orbrzv_b_spo, zr0, zz0, b, ier)
    call ezspline_gradient2(orbrzv_b_spo, zr0, zz0, db, ier)

    call ezspline_interp2(orbrzv_psi_spo, zr0, zz0, psi, ier)
    call ezspline_gradient2(orbrzv_psi_spo, zr0, zz0, dpsi, ier)

    call ezspline_interp2(orbrzv_g_spo, zr0, zz0, g, ier)
    call ezspline_gradient2(orbrzv_g_spo, zr0, zz0, dg, ier)

    call ezspline_interp2(orbrzv_phi_spo, zr0, zz0, phi, ier)
    call ezspline_gradient2(orbrzv_phi_spo, zr0, zz0, dphi, ier)

    zvl0 = orbrzv_teng - orbrzv_zz(4)*b - orbrzv_qs*phi
    !if(b <= orbrzv_b) return
    if(zvl0 < 0.0_fp) then
       call errorlog(' orbrz_find_rmid: negative sqrt for zvl0, ',zvl0/orbrzv_teng)
       return
    endif
    zvl0 = zvl0*2.0/orbrzv_ms
    zvl0 = zsgn*dsqrt(zvl0)

    tcmtp = orbrzv_ms*zvl0*g/b - orbrzv_qs*psi/zc

    if(zvl0 .ne. 0.0_fp) then
      zdvdr = orbrzv_zz(4)*db(1) + orbrzv_qs*dphi(1)
      zdvdr = -zdvdr/(orbrzv_ms*zvl0)
    else
      zdvdr = 0.0_fp
    endif
    zdcmdr = orbrzv_ms*zvl0*(dg(1) - g*db(1)/b)/b - orbrzv_qs*dpsi(1)/zc
    zdcmdr = zdcmdr + orbrzv_ms*g*zdvdr/b
    zdcmdr = zdcmdr + dsign(zepslon, zdcmdr)

    zs = orbrzv_tcmt - tcmtp
    zerr = dabs(zs); zerr = zerr/(dabs(orbrzv_tcmt) + zepslon)
    if(zerr <= zerr_max) return

    zr0 = zr0 + zs/zdcmdr

    if(dabs(zr0 - orbrzv_rc) < 1.0e-3_fp*orbrzv_rc) then
      return
    else if(zr0 < orbrzv_rc) then
      iflag = -1
      return
    else if(zr0 > orbrzv_prmax + zdr) then
      iflag = 1
      return 
    endif

    !print *, "cnt =", cnt,zr0,zvl0,orbrzv_rho,zs/zdcmdr,zr0-zs/zdcmdr
    if(zr0 > orbrzv_rmax .or. zr0 < orbrzv_rmin .or. &
     & zz0 > orbrzv_zmax .or. zz0 < orbrzv_zmin) then
       call errorlog(' orbrz_find_rmid: out of range,cnt=',cnt)
       call errorlog('                  zvl0=',zvl0)
       call errorlog('                  orbrzv_rho=',orbrzv_rho)
       call errorlog('                  zs/zdcmdr=',zs/zdcmdr)
       call errorlog('                  zr0-zs/zdcmdr=',zr0-zs/zdcmdr)
       call errorlog('                  orbrzv_zz(1)=',orbrzv_zz(1))
       call errorlog('                  orbrzv_zz(2)=',orbrzv_zz(2))
       call errorlog('                  orbrzv_zz(3)=',orbrzv_zz(3))

      !close(unit=22222)
       call bad_exit('? out of range r or z in find_rvll0')
    endif
  enddo

  call warnlog(' orbrz_find_rmid: too many iteration,orbrzv_zz(1)=',orbrzv_zz(1))
  call warnlog('                                     orbrzv_zz(2)=',orbrzv_zz(2))
  call warnlog('                                     zerr=',zerr)
  call warnlog('                                     zerr_max=',zerr_max)
  call warnlog('                                     cnt_max=',cnt_max)
  iflag = cnt_max

end subroutine orbrz_find_rmid

subroutine orbrz_findcrz(r, z, psi0, b0, err_max, cnt, cnt_max)

  use ezspline, only : ezspline_interp2,ezspline_gradient2,&
       ezspline_derivative2
  use orbrzv

  implicit none
  real(fp), intent(out) :: r, z
  integer, intent(out) :: cnt
  real(fp), intent(in) :: psi0, b0, err_max
  integer, intent(in) :: cnt_max
  integer :: ier
  real(fp) :: s1, s2, err, err1, err2, &
            & b, r_inv, r2b_inv, dr, dz, dbdr, dbdz, det, &
            & psi, dpsi(1:2), g, dg(1:2), dpdrr, dpdzz, dpdrz

  do cnt = 1, cnt_max
    call ezspline_interp2(orbrzv_psi_spo, r, z, psi, ier)
    call ezspline_gradient2(orbrzv_psi_spo, r, z, dpsi, ier)
    call ezspline_derivative2(orbrzv_psi_spo, 2, 0, r, z, dpdrr, ier)
    call ezspline_derivative2(orbrzv_psi_spo, 0, 2, r, z, dpdzz, ier)
    call ezspline_derivative2(orbrzv_psi_spo, 1, 1, r, z, dpdrz, ier)

    call ezspline_interp2(orbrzv_g_spo, r, z, g, ier)
    call ezspline_gradient2(orbrzv_g_spo, r, z, dg, ier)

    r_inv = 1.0_fp/r
    b = dsqrt(dpsi(2)*dpsi(2) + g*g + dpsi(1)*dpsi(1))*r_inv

    s1 = psi0 - psi
    s2 = b0 - b

    err1 = s1/psi0
    err2 = s2/b0
    err1 = dabs(err1)
    err2 = dabs(err2)
    err = dmax1(err1, err2)

    if(err <= err_max) return

    r2b_inv = r_inv*r_inv/b
    dbdr = r2b_inv*(dpsi(1)*dpdrr + dpsi(2)*dpdrz + g*dg(1)) - b*r_inv
    dbdz = r2b_inv*(dpsi(2)*dpdzz + dpsi(1)*dpdrz + g*dg(2));

    det = dpsi(1)*dbdz - dpsi(2)*dbdr
    dr =  dbdz*s1 - dpsi(2)*s2
    dz = -dbdr*s1 + dpsi(1)*s2
    r = r + dr/det
    z = z + dz/det
  end do

end subroutine orbrz_findcrz
