! time integrate guiding centor orbit using time step control
! based on 3rd and 4th order runge-kutta comparison
subroutine orbrz4(iorb, istep, irff, ierr)

  use orbrzv
  use nbi_com,only : orbrzv_zzerr_con,orbrzv_rf_option
  use logmod

  implicit none
  real(fp), parameter :: znrc_safety = 0.9_fp, &
                       & znrc_errcon = 1.04976e-3_fp, &
                       & znrc_pgrow = -0.25_fp, &
                       & znrc_pshrnk = -0.333333333333_fp
  ! orbit index, time step, and rf-related flag
  integer, intent(in) :: iorb, istep, irff
  integer, intent(out) :: ierr ! status code, 0 = ok
  real(fp) :: zzerr, ztf, zrfd, zdtr
  integer :: iter, ierr_sum

  ! initialize error flag
  ierr  = 0
  ierr_sum = 0

  !goto 1111

  ! small time step for debug mode
  !if(idebug .ne. 0) orbrzv_tdt = orbrzv_dbg_dt 

  !---------------------------------------------------------------------- 
  ! new time step controller based on the error estimation getting from
  ! comparing 3rd and 4th runge kutta

  ! set trial step guessed from the previous step
  orbrzv_dt = orbrzv_tdt

  do iter = 1, orbrzv_iter_max

    call orbrz_rk4(iorb, istep, orbrzv_dt, ierr)

    if(ierr .ne. 0) then
       call errorlog(' orbrz4: error from orbrz_rk4, iter=',iter)
       ierr_sum = ierr
    endif

    !if(orbrzv_pflag >= 0) then
    if(orbrzv_pflag .eq. 0) then

      ! if particle went small enough time step already, no more adjustment
      !if(orbrzv_dt <= orbrzv_dt_min) then
      !  orbrzv_tdt = orbrzv_dt_min
      !  goto 7777
      !end if

      ! get maximum error and normalize against to orbrzv_zzerr_con
      zzerr = orbrzv_zzerr(0)/orbrzv_zzerr_con

      if(zzerr > 1.0_fp .and. irff == 0) then
        ! first check whether previous time was small enough
        ! and if it is, skip the time step control and goto next step
        if(orbrzv_dt <= orbrzv_dt_min) then
          orbrzv_tdt = orbrzv_dt_min
          goto 7777
        end if
        
        ! if not, restore the previous status 
        ! and repeat the loop using reduced time 
        call orbrz_backto0

        ! reduce time step for next trial
        orbrzv_dt = znrc_safety*orbrzv_dt*(zzerr**znrc_pshrnk)
        orbrzv_dt = dmax1(orbrzv_dt_min, orbrzv_dt)
      else
        ! prepare next step 

        ! 1. increase time step if error is too small
        if(zzerr > znrc_errcon) then
          orbrzv_tdt = znrc_safety*orbrzv_dt*(zzerr**znrc_pgrow)
        else
          orbrzv_tdt = 5.0_fp*orbrzv_dt
        end if
        orbrzv_tdt = dmin1(orbrzv_tdt, orbrzv_dt_max)

        !call orbrz_safe_call_zdtrob(zdtr)
        !orbrzv_tdt = min(orbrzv_tdt, 0.5_fp*zdtr)

        ! 2. estimate the distance to rf-resonance and adjust
        !    time step and goose
        if(orbrzv_rf_option.eq.1 .and. mcrf_on) then
          if(irff == 0) then
            call orbrz_rf_timft(ztf, zrfd, irff)

            if(ztf < 1.0_fp) orbrzv_tdt = orbrzv_tdt*ztf

            !if(zrfd < 1.0_fp .and. orbrzv_rfdv(-1) < 0.1_fp) then
              !goose = dmax1(1.0001_fp, goose*zrfd)
              !if(zrfd < 0.3_fp) goose = 1.0001_fp
            ! goose = 1.0001_fp
            !endif
          else
            orbrzv_tdt = orbrzv_dt
          endif

          orbrzv_tdt = dmax1(orbrzv_dt_min, orbrzv_tdt)
        endif

        goto 7777
      end if
    else
      ! if particle is lost during the trial exit loop immediately
      orbrzv_tdt = orbrzv_dt_min
      goto 7777
    end if

  enddo

  call warnlog(' orbrz4: too many trial at error control loop==>', orbrzv_iter_max)

7777 continue

  ! at this point, orbrzv_dt contains actual time step underwent in this time
  ! and orbrzv_tdt is trial step for next time

  ! report total error
  ierr = ierr_sum

  return

1111 continue
  ! try to go one step
  ! *zz are copied to *zz0
  ! *zz are replaced with new position
  ! if particle is lost during the step, ilost = 1 and *zz are reset
  if(orbrzv_pflag >= 0) then
     call orbrz_safe_call_zdtrob(orbrzv_dt)
     orbrzv_dt = orbrzv_dt
     call orbrz_rk4(iorb, istep, orbrzv_dt, ierr)
  else
     call errorlog(' orbrz4: calling orbrz with lost particle ?')
     return
  end if

end subroutine orbrz4
