module pf_mod_optimization
  use pfasst
  use pf_mod_dtype
  use pf_mod_utils
  use pf_mod_mpi
  use pf_mod_ndarray_oc
  use pf_mod_parallel_oc
  use pf_my_level
  use feval
  use probin, only: solve_y
  use solutions
  implicit none

  real(pfdp), parameter, private :: armijoDecrease = 1e-4
  real(pfdp), parameter, private :: minStepSize    = 1e-6
  real(pfdp), parameter, private :: maxStepSize    = 1e6
  real(pfdp), parameter, private :: stepIncFactor  = 10
  real(pfdp), parameter, private :: c2             = 0.9
  integer,    parameter, private :: maxIterZoom    = 20

contains

  subroutine evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objective, L2NormUSq, savedAdjoint, predictAdj)
    type(pf_pfasst_t),         intent(inout) :: pf
    type(pf_ndarray_oc_t),     intent(inout) :: q1
    real(pfdp),                intent(in   ) :: dt, alpha
    integer,                   intent(in   ) :: nsteps
    logical,                   intent(in   ) :: predict
    real(pfdp),                intent(  out) :: objective, L2NormUSq
    real(pfdp),                intent(inout) :: savedAdjoint(:,:)
    logical, optional,         intent(in   ) :: predictAdj
    integer             :: m
    real(pfdp), pointer :: obj(:)
    real(pfdp) :: t(pf%levels(pf%nlevels)%nnodes)
    logical    :: predictAdjoint

    predictAdjoint = .true.
    if (present(predictAdj) ) predictAdjoint = predictAdj

    pf%state%itcnt   = 0  ! reset iteration count here, not inside pf_pfasst_block_oc
                          ! how to count skipped sweeps for state?

    solve_y = .true.

    if (pf%rank .eq. 0) then
      call initial(q1, 0.0_pfdp, dt) !dt should be given by rank !initial_rd
    else
      q1%yflatarray = 0.0_pfdp
    end if
    if ((pf%rank .eq. 0) .or. predict) then
      call pf%levels(pf%nlevels)%q0%copy(q1, 1) !do this only for rank 0 or predict = true?
    end if

!     if (do_mixed .eq. 1 ) then
!       q1%pflatarray = 0.0_pfdp
!       call pf%levels(pf%nlevels)%qend%copy(q1, 2)
!       if( predict .eqv. .false. .and. predictAdjoint .eqv. .true. ) then
!         ! call predictor just for adjoint part
!         call pf_predictor(pf, pf%rank*dt, dt, 2)
! !           do m = 1, pf%levels(pf%nlevels)%nnodes
! !             call pf%levels(pf%nlevels)%encap%unpack(pf%levels(pf%nlevels)%Q(m), savedAdjoint(m,:), 2)
! !           end do
! !
! !           t = pf%rank*dt + dt*pf%levels(pf%nlevels)%nodes
! !           call pf%levels(pf%nlevels)%sweeper%evaluate_all(pf%levels(pf%nlevels), t, 2)
!       end if
!     end if

    if (pf%rank .eq. 0) print *, ' *********  solve state **************'

!     if (do_mixed .eq. 1) then
!        call pf_pfasst_block(pf, dt, nsteps, predict, 0)
!        ! need to save values in adjoint component of Q(m)!
! !        if(pf%rank .eq. 0) print *, 'saving adjoint'
!        do m = 1, pf%levels(pf%nlevels)%nnodes
!          call pf%levels(pf%nlevels)%Q(m)%pack(savedAdjoint(m,:), 2)
!        end do
! !        print *, pf%rank, maxval(savedAdjoint(1,:))
!     else
       call pf_pfasst_block_oc(pf, dt, nsteps, predict, 1)
!     end if

!     if (pf%rank .eq. 0) print *, ' *********  compute objective **************'

    allocate(obj(pf%levels(pf%nlevels)%nnodes))
    do m = 1, pf%levels(pf%nlevels)%nnodes
!       if (pf%rank .eq. 0) print *, m
      call objective_function(pf%levels(pf%nlevels)%ulevel%sweeper, pf%levels(pf%nlevels)%Q(m), &
                                    product(pf%levels(pf%nlevels)%lev_shape), m, obj(m))
    end do
    objective = 0.0
    do m = 1, pf%levels(pf%nlevels)%nnodes-1
      objective = objective + &
                 (obj(m)+obj(m+1))*(pf%levels(pf%nlevels)%nodes(m+1)-pf%levels(pf%nlevels)%nodes(m))*dt
    end do
    objective = 0.5*objective !0.5 for trapezoidal rule
    call control_L2Q(pf%levels(pf%nlevels)%ulevel%sweeper, dt, pf%levels(pf%nlevels)%nodes, &
                                  product(pf%levels(pf%nlevels)%lev_shape), L2NormUSq)
    objective = 0.5*objective + 0.5*alpha*L2NormUSq
    deallocate(obj)
  end subroutine evaluate_objective

  ! assumes state solution in Q(:), component 1, of pf object
  ! how to do that dimension independent? this is just for 1d because of the gradient array
  subroutine evaluate_gradient(pf, q1, dt, nsteps, predict, gradient, LinftyNormGrad, L2NormGradSq, savedAdjoint)
    type(pf_pfasst_t),        intent(inout) :: pf
    type(pf_ndarray_oc_t), target, intent(inout) :: q1
    real(pfdp),               intent(in   ) :: dt
    integer,                  intent(in   ) :: nsteps
    logical,                  intent(in   ) :: predict
    real(pfdp),               intent(  out) :: gradient(:,:), LinftyNormGrad, L2NormGradSq
    real(pfdp),               intent(inout) :: savedAdjoint(:,:)
    integer :: m
    integer :: dest, source, ierror, stat(MPI_STATUS_SIZE)
    real(pfdp), allocatable :: terminal(:), tosend(:)
    real(pfdp) :: t(pf%levels(pf%nlevels)%nnodes)

    solve_y = .false.

    pf%state%itcnt   = 0  ! reset iteration count here, not inside pf_pfasst_block_oc
                          ! how to count skipped sweeps for state?

    allocate(terminal(size(savedAdjoint,2)))
!     allocate(tosend(size(savedAdjoint,2)))

    !call initial(q1, pf%rank*dt, (pf%rank+1)*dt)
    q1%pflatarray = 0.0_pfdp ! only if no final time term in objective, otherwise this is nonzero, and terminal needs to be added to this

    terminal = 0.0_pfdp

!     if (do_mixed .eq. 1 ) then
!
!        if(pf%rank /= pf%state%last) then
!           source = modulo(pf%rank+1, pf%comm%nproc)
!           call mpi_recv(terminal, size(savedAdjoint,2), MPI_REAL8, &
!                         source, 1, pf%comm%comm, stat, ierror)
! !           print *, pf%rank, maxval(terminal)
!        end if
!
!        savedAdjoint(1,:) = savedAdjoint(1,:) + terminal
!
!        if (pf%rank /= pf%state%first) then
!           dest = modulo(pf%rank-1, pf%comm%nproc)
!           call mpi_send(savedAdjoint(1,:), size(savedAdjoint,2), MPI_REAL8, &   !mpi_send(savedAdjoint(1,:),...
!                         dest, 1, pf%comm%comm, stat, ierror)
!        end if
!
!        q1%pflatarray = terminal
!
!        ! shift solution values computed simultaneously with state by 'terminal'
!        ! would be sufficient to do this only if rank /= last
!        call pf%levels(pf%nlevels)%Q(1)%unpack(savedAdjoint(1,:), 2)
!        do m = 2, pf%levels(pf%nlevels)%nnodes
!           savedAdjoint(m,:) = savedAdjoint(m,:) + terminal
!           call pf%levels(pf%nlevels)%Q(m)%unpack(savedAdjoint(m,:), 2)
!           savedAdjoint(m,:) = savedAdjoint(m,:) - terminal
!        end do
!        savedAdjoint(1,:) = savedAdjoint(m,:) - terminal  ! reset, to use as initial guess in warm start
!
!        t = pf%rank*dt + dt*pf%levels(pf%nlevels)%nodes
!        call pf%levels(pf%nlevels)%ulevel%sweeper%evaluate_all(pf%levels(pf%nlevels), t, 2)
!
!        ! restrict Q(m) from finest to coarser levels; evaluates on coarse levels
!        call restrict_for_adjoint(pf, 2);
!     end if

    !if (((pf%rank+1)*dt >= Tfin) .or. predict) then
    !  call pf%levels(pf%nlevels)%encap%copy(pf%levels(pf%nlevels)%qend, c_loc(q1), 2)
    !end if
    call pf%levels(pf%nlevels)%qend%copy(q1, 2)
    ! do this only for final step or predict = true?
    !call restrict_for_adjoint(pf, 1)

    if (pf%rank .eq. 0) print *, '*********  solve adjoint *************'
!     call mpi_barrier(pf%comm%comm, ierror)
!     if (do_mixed .eq. 1) then
!       call pf_pfasst_block(pf, dt, nsteps, .false., 2) ! do not predict if do_mixed
      ! try predict here instead of copying: this should be similar to solving state and adjoint completely separate
!     else
      if(predict) pf%q0_style = 1
      call pf_pfasst_block_oc(pf, dt, nsteps, predict, 2) !predict
      pf%q0_style = 0
!     end if

    if (pf%rank .eq. 0) print *, '*********  compute gradient *************'

    do m = 1, pf%levels(pf%nlevels)%nnodes
      call pf%levels(pf%nlevels)%Q(m)%pack(gradient(m,:), 2)
     ! if (do_mixed .eq. 1 )  gradient(m,:) = gradient(m,:) + savedAdjoint(m,:) ! in this variant, we solve the full adjoint, so no adding is needed
    end do

    call construct_gradient(pf%levels(pf%nlevels)%ulevel%sweeper, gradient, pf%levels(pf%nlevels)%nodes, &
                                      LinftyNormGrad, L2NormGradSq)

!     deallocate(tosend)
    deallocate(terminal)
  end subroutine evaluate_gradient

  subroutine armijo_step(pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, &
                         globObj, globDirXGrad, objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepSize, stepTooSmall)
    type(pf_pfasst_t),         intent(inout) :: pf
    type(pf_ndarray_oc_t), target,  intent(inout) :: q1
    real(pfdp),                intent(in   ) :: dt, alpha, globObj, globDirXGrad
    integer,                   intent(in   ) :: nsteps
    integer,                   intent(inout) :: itersState, itersAdjoint
    logical,                   intent(in   ) :: predict
    real(pfdp),                intent(in   ) :: searchDir(:,:)
    real(pfdp),                intent(  out) :: objectiveNew, L2NormUSq
    real(pfdp),                intent(inout) :: stepSize, LinftyNormGrad, L2NormGradSq, gradient(:,:), savedAdjoint(:,:)
    logical,                   intent(inout) :: stepTooSmall

    real(pfdp) :: globObjNew, directionTimesGradient, globDirXGradNew
    integer    :: l, ierror
    !real(pfdp) :: armijoDecrease ! this should be in a structure global to the module, and set in
    !                             ! something like init_optimization, along with other parameters
    !real(pfdp) :: minStepSize
    !armijoDecrease = 1e-4
    !minStepSize    = 1e-6

    do
      call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, stepSize)
      !restrict control
      do l = pf%nlevels-1,1,-1
	call restrict_control(pf%levels(l)%ulevel%sweeper, pf%levels(l+1)%ulevel%sweeper)
      end do

      call evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objectiveNew, L2NormUSq, savedAdjoint)
      itersState = itersState + pf%state%itcnt

      call mpi_allreduce(objectiveNew, globObjNew, 1, MPI_REAL8, MPI_SUM, pf%comm%comm, ierror)
      if(pf%rank == 0) print *, stepSize, 'objectiveNew (L2) = ', globObjNew

      if (globObjNew < globObj + armijoDecrease*stepSize*globDirXGrad) then
        ! evaluate gradient to be consistent with wolfe_powell_step
        call evaluate_gradient(pf, q1, dt, nsteps, predict, gradient, LinftyNormGrad, L2NormGradSq, savedAdjoint)
        itersAdjoint = itersAdjoint + pf%state%itcnt
        return
      end if

       call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, -stepSize)
       ! no need to restrict here, coarse levels get overwritten above
       stepSize = 0.5 * stepSize
       if (stepSize < minStepSize) then
         stepTooSmall = .true.
         if (pf%rank .eq. 0) print *, 'no step found, stopping'
         !call write_control(pf%levels(pf%nlevels)%ctx, k, "u_sdc_split_final")
         return
       end if
     end do
   end subroutine armijo_step

!    subroutine zoom(stepLowIn, stepHighIn, stepSizeIn, stepSizeOut, &
!                    pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, &
!                    globObj, globDirXGrad, objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepTooSmall, globObjLowIn)
!      type(pf_pfasst_t),         intent(inout) :: pf
!      type(pf_ndarray_oc_t), target,  intent(inout) :: q1
!      real(pfdp),                intent(in   ) :: dt, alpha, globObj, globDirXGrad
!      integer,                   intent(in   ) :: nsteps
!      integer,                   intent(inout) :: itersState, itersAdjoint
!      logical,                   intent(in   ) :: predict
!      real(pfdp),                intent(in   ) :: searchDir(:,:)
!      real(pfdp),                intent(  out) :: objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepSizeOut
!      real(pfdp),                intent(in   ) :: stepSizeIn, stepLowIn, stepHighIn, globObjLowIn
!      real(pfdp),                intent(inout) :: gradient(:,:), savedAdjoint(:,:)
!      logical,                   intent(inout) :: stepTooSmall
!
!      real(pfdp) :: directionTimesGradient, globObjNew, globDirXGradNew
!      real(pfdp) :: stepSize, stepLow, stepHigh, globObjLow
!      integer    :: l, ierror, iter
!
!      stepSize = stepSizeIn
!      stepLow = stepLowIn
!      stepHigh = stepHighIn
!      globObjLow = globObjLowIn
!
!      iter = 0
!      if (pf%rank .eq. 0) print *, 'in zoom'
!
!      do
!        call update_control(pf%levels(pf%nlevels)%ctx, searchDir, -stepSize)
!        stepSize = 0.5_pfdp*(stepLow+stepHigh)
!        iter = iter + 1
!        if (stepSize < minStepSize .or. iter > maxIterZoom) then
!          stepTooSmall = .true.
!          if (pf%rank .eq. 0) print *, 'no step found (< min or > maxIter), stopping'
!          !call write_control(pf%levels(pf%nlevels)%ctx, k, "u_sdc_split_final")
!          stepSizeOut = stepSize
!          return
!        end if
!
!        call update_control(pf%levels(pf%nlevels)%ctx, searchDir, stepSize)
!         !restrict control
!         do l = pf%nlevels-1,1,-1
!    	  call restrict_control(pf%levels(l)%ctx, pf%levels(l+1)%ctx)
!         end do
!
!         call evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objectiveNew, L2NormUSq, savedAdjoint)
!         itersState = itersState + pf%state%itcnt
!         call mpi_allreduce(objectiveNew, globObjNew, 1, MPI_REAL8, MPI_SUM, pf%comm%comm, ierror)
!         if(pf%rank == 0) print *, stepSize, 'objectiveNew (L2) = ', globObjNew
!
!         if ( (globObjNew > globObj + armijoDecrease*stepSize*globDirXGrad) .or. &
!              (globObjNew >= globObjLow) )                                       then
!           stepHigh = stepSize
!           if (pf%rank .eq. 0) print *, 'set new stepHigh to stepSize; stepHigh = ', stepHigh, 'stepLow = ', stepLow
!         else
!           call evaluate_gradient(pf, q1, dt, nsteps, predict, gradient, LinftyNormGrad, L2NormGradSq, savedAdjoint)
!           itersAdjoint = itersAdjoint + pf%state%itcnt
!           directionTimesGradient = compute_scalar_prod(searchDir, gradient, pf%levels(pf%nlevels)%nodes, dt)
!           call mpi_allreduce(directionTimesGradient, globDirXGradNew, 1, MPI_REAL8, MPI_SUM, &
!                              pf%comm%comm, ierror)
!           if (pf%rank .eq. 0) print *, 'globDirXGradNew = ', globDirXGradNew
!
!
!           if (abs(globDirXGradNew) <= -c2*globDirXGrad) then
!             stepSizeOut = stepSize
!             return
!           end if
!
!           if (globDirXGradNew*(stepHigh-stepLow) >= 0) then
!             if(pf%rank == 0) print *, stepSize, stepHigh, stepLow
!             stepHigh = stepLow
!             if (pf%rank .eq. 0) print *, 'set new stepHigh to stepLow; stepHigh = ', stepHigh, 'stepLow = ', stepLow, 'stepSize = ', stepSize
!           end if
!           stepLow = stepSize
!           if (pf%rank .eq. 0) print *, 'set new stepLow; stepHigh = ', stepHigh, 'stepLow = ', stepLow, 'stepSize = ', stepSize
!           globObjLow = globObjNew
!         end if
!       end do
!    end subroutine zoom
!
!
!    subroutine wolfe_powell_step(pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, &
!                                 globObj, globDirXGrad, objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepSize, stepTooSmall)
!     type(pf_pfasst_t),         intent(inout) :: pf
!     type(pf_ndarray_oc_t), target,  intent(inout) :: q1
!     real(pfdp),                intent(in   ) :: dt, alpha, globObj, globDirXGrad
!     integer,                   intent(in   ) :: nsteps
!     integer,                   intent(inout) :: itersState, itersAdjoint
!     logical,                   intent(in   ) :: predict
!     real(pfdp),                intent(in   ) :: searchDir(:,:)
!     real(pfdp),                intent(  out) :: objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq
!     real(pfdp),                intent(inout) :: stepSize, gradient(:,:), savedAdjoint(:,:)
!     logical,                   intent(inout) :: stepTooSmall
!
!     integer    :: l, ierror, iter
!     real(pfdp) :: prevStepSize, directionTimesGradient, globDirXGradNew, globObjNew, globObjPrev
!     !, maxStepSize, minStepSize, stepIncFactor, armijoDecrease, c2,
!     prevStepSize   = 0.0_pfdp
!     !maxStepSize    = 1e6
!     !minStepSize    = 1e-6
!     !stepIncFactor  = 10
!     !armijoDecrease = 1e-4
!     !c2             = 0.9
!     globObjPrev = globObj
!     iter = 1
!
!     do
!       call update_control(pf%levels(pf%nlevels)%ctx, searchDir, stepSize)
!       !restrict control
!       do l = pf%nlevels-1,1,-1
! 	call restrict_control(pf%levels(l)%ctx, pf%levels(l+1)%ctx)
!       end do
! !       call dump_control(pf%levels(pf%nlevels)%ctx, pf, 'u1')
!
!       call evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objectiveNew, L2NormUSq, savedAdjoint)
!       itersState = itersState + pf%state%itcnt
!       call mpi_allreduce(objectiveNew, globObjNew, 1, MPI_REAL8, MPI_SUM, pf%comm%comm, ierror)
!       if(pf%rank == 0) print *, stepSize, 'objectiveNew (L2) = ', globObjNew
!
!       if ( (globObjNew > globObj + armijoDecrease*stepSize*globDirXGrad) .or. &
!            ( globObjNew >= globObjPrev .and. iter > 1) )                   then
!         call zoom(prevStepSize, stepSize, stepSize, stepSize, &
!                   pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, globObj, globDirXGrad, &
!                   objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepTooSmall, globObjPrev)
!         return
!       end if
!
!       if (pf%rank .eq. 0) print *, 'evaluate new gradient'
!       call evaluate_gradient(pf, q1, dt, nsteps, predict, gradient, LinftyNormGrad, L2NormGradSq, savedAdjoint)
!       itersAdjoint = itersAdjoint + pf%state%itcnt
!       directionTimesGradient = compute_scalar_prod(searchDir, gradient, pf%levels(pf%nlevels)%nodes, dt)
!       call mpi_allreduce(directionTimesGradient, globDirXGradNew, 1, MPI_REAL8, MPI_SUM, &
!                          pf%comm%comm, ierror)
!       if (pf%rank .eq. 0) print *, 'globDirXGradNew = ', globDirXGradNew
!       if (abs(globDirXGradNew) <= -c2*globDirXGrad) then
!         return
!       end if
!
!       if (globDirXGradNew >= 0) then
!         call zoom(stepSize, prevStepSize, stepSize, stepSize, &
!                   pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, globObj, globDirXGrad, &
!                   objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepTooSmall, globObjNew)
!         return
!       end if
!
!       globObjPrev  = globObjNew
!       prevStepSize = stepSize
!       stepSize     = stepIncFactor*stepSize
!       iter = iter + 1
!       if (stepSize > maxStepSize) then
!          stepTooSmall = .true.
!          if (pf%rank .eq. 0) print *, 'no step found (> max), stopping'
!          !call write_control(pf%levels(pf%nlevels)%ctx, k, "u_sdc_split_final")
!          return
!        end if
!        call update_control(pf%levels(pf%nlevels)%ctx, searchDir, -prevStepSize)
!     end do
!
!    end subroutine wolfe_powell_step

  subroutine strong_wolfe_step(pf, q1, dt, nsteps, itersState, itersAdjoint, predict, searchDir, gradient, savedAdjoint, alpha, &
                          globObj, globDirXGrad, objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq, stepSize, stepTooSmall)
    type(pf_pfasst_t),         intent(inout) :: pf
    type(pf_ndarray_oc_t), target,  intent(inout) :: q1
    real(pfdp),                intent(in   ) :: dt, alpha, globObj, globDirXGrad
    integer,                   intent(in   ) :: nsteps
    integer,                   intent(inout) :: itersState, itersAdjoint
    logical,                   intent(in   ) :: predict
    real(pfdp),                intent(in   ) :: searchDir(:,:)
    real(pfdp),                intent(  out) :: objectiveNew, L2NormUSq, LinftyNormGrad, L2NormGradSq
    real(pfdp),                intent(inout) :: stepSize, gradient(:,:), savedAdjoint(:,:)
    logical,                   intent(inout) :: stepTooSmall
    integer    :: l, ierror
    real(pfdp) :: low, high
    real(pfdp) :: directionTimesGradient, globObjNew, globDirXGradNew
    logical    :: first

    first = .true.

    low  = 0.0_pfdp
    high = stepSize
    !high = 1.0_pfdp
    !if (stepSize > high) high = stepSize

    do
      call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, high) !stepSize)
      !restrict control
      do l = pf%nlevels-1,1,-1
	call restrict_control(pf%levels(l)%ulevel%sweeper, pf%levels(l+1)%ulevel%sweeper)
      end do
!       call dump_control(pf%levels(pf%nlevels)%ctx, pf, 'u1')

      call evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objectiveNew, L2NormUSq, savedAdjoint, first)
      first = .false.

      itersState = itersState + pf%state%itcnt
      call mpi_allreduce(objectiveNew, globObjNew, 1, MPI_REAL8, MPI_SUM, pf%comm%comm, ierror)
      if(pf%rank == 0) print *, high, 'objectiveNew (L2) = ', globObjNew ! *, stepSize,

      call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, -high) !-stepSize)

      if (globObjNew < globObj + armijoDecrease*stepSize*globDirXGrad) then
        high = 2.0*high
      else ! Armijo condition not satisfied, exit loop and reduce stepsize
        exit
      end if
    end do

    do
      stepSize = 0.5*(low+high)

      if (stepSize < minStepSize ) then
        stepTooSmall = .true.
        if (pf%rank .eq. 0) print *, 'no step found (< min), stopping'
        return
      end if

      call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, stepSize)
      do l = pf%nlevels-1,1,-1
	call restrict_control(pf%levels(l)%ulevel%sweeper, pf%levels(l+1)%ulevel%sweeper)
      end do

      call evaluate_objective(pf, q1, dt, nsteps, predict, alpha, objectiveNew, L2NormUSq, savedAdjoint, first)
      itersState = itersState + pf%state%itcnt
      call mpi_allreduce(objectiveNew, globObjNew, 1, MPI_REAL8, MPI_SUM, pf%comm%comm, ierror)
      if(pf%rank == 0) print *, stepSize, 'objectiveNew (L2) = ', globObjNew

      if (globObjNew >= globObj + armijoDecrease*stepSize*globDirXGrad) then
        call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, -stepSize)
        high = stepSize
        cycle
      end if

      ! now Armijo is satisfied again
      call evaluate_gradient(pf, q1, dt, nsteps, predict, gradient, LinftyNormGrad, L2NormGradSq, savedAdjoint)
      itersAdjoint = itersAdjoint + pf%state%itcnt
      directionTimesGradient = compute_scalar_prod(searchDir, gradient, pf%levels(pf%nlevels)%nodes, dt)
      call mpi_allreduce(directionTimesGradient, globDirXGradNew, 1, MPI_REAL8, MPI_SUM, &
                         pf%comm%comm, ierror)

      if (pf%rank .eq. 0) print *, 'globDirXGradNew = ', globDirXGradNew, '-c2*globDirXGrad', -c2*globDirXGrad

!       if (abs(globDirXGradNew) <= -c2*globDirXGrad) then  ! strong Wolfe conditions satisfied
!         return
!       end if

      if (globDirXGradNew < c2*globDirXGrad) then
!         if(pf%rank == 0) print *, stepSize, high, low
        low = stepSize
        if (pf%rank .eq. 0) print *, 'set new stepLow;  stepHigh = ', high, 'stepLow = ', low, 'stepSize = ', stepSize
      elseif (globDirXGradNew > -c2*globDirXGrad) then
        high = stepSize
        if (pf%rank .eq. 0) print *, 'set new stepHigh; stepHigh = ', high, 'stepLow = ', low, 'stepSize = ', stepSize
      else
        exit
      end if

      call update_control(pf%levels(pf%nlevels)%ulevel%sweeper, searchDir, -stepSize)
    end do

  end subroutine strong_wolfe_step

end module pf_mod_optimization


text:
      PROGRAM INITIALIZE
      IMPLICIT NONE

      open(unit=23,file="parameters.inc")
      call setpara('param_card.dat')  !first call to setup the paramaters
      close(23)

      end

ext: f


!!Quick routine to wrap the SLALIB moon position function.
!!It is inaccurate but fine for QUIJOTE accuracy. It downsamples
!! the data by 100x!

subroutine refro(zd, hm, tdk, pmb, rh, wl, phi, tlr, eps, ref, len_zd)
  implicit none
  integer, intent(in) :: len_zd
  real*8, intent(in) :: zd(len_zd)
  real*8, intent(out) :: ref(len_zd)
  real*8, intent(in) :: hm ! height
  real*8, intent(in) :: tdk ! ambient temp (K)
  real*8, intent(in) :: pmb ! pressure (mb)
  real*8, intent(in) :: rh ! relative humidity (0-1)
  real*8, intent(in) :: wl ! effective wavelength (um)
  real*8, intent(in) :: tlr ! latitude of observer
  real*8, intent(in) :: phi ! temperature lapse rate (K/m)
  real*8, intent(in) :: eps ! precision required (radian)

  !f2py real*8 zd, hm, tdk, pmb, rh, wl, phi, tlr, eps, ref
  !f2py integer len_zd

  integer :: i

  do i=1, len_zd
     call sla_refro(zd(i), hm, tdk, pmb, rh, wl, phi, tlr, eps, ref(i))
  enddo

end subroutine refro

subroutine rdplan(jd, np, lon, lat, ra, dec, diam, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: lon
  real*8, intent(in) :: lat
  integer, intent(in) :: np
  real*8, intent(in) :: jd(len_bn)
  real*8, intent(out) :: diam(len_bn)
  real*8, intent(out) :: ra(len_bn)
  real*8, intent(out) :: dec(len_bn)

  !f2py integer len_bn
  !f2py real*8 lon, lat
  !f2py real*8 ra,dec,jd, diam

  !integer :: i

  real*8 :: pi = 3.14159265359

  integer :: step = 100
  integer :: kup,k
  kup = len_bn/step

  do k=1, len_bn
     call sla_rdplan(jd(k),np,lon*pi/180.0,lat*pi/180.0,ra(k),dec(k),diam(k))
  enddo

end subroutine rdplan

subroutine planet(jd, np, dist, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  integer, intent(in) :: np
  real*8, intent(in) :: jd(len_bn)
  real*8, intent(out) :: dist(6,len_bn)

  !f2py integer len_bn
  !f2py real*8 jd, dist

  integer :: jstat

  !real*8 :: pi = 3.14159265359

  integer :: step = 100
  integer :: kup,k
  kup = len_bn/step

  !mask = 1.0d0

  do k=1, len_bn
     call sla_planet(jd(k),np,dist(:,k),jstat)
  enddo

end subroutine planet

subroutine h2e(az, el, mjd, lon, lat, ra, dec, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: lon
  real*8, intent(in) :: lat
  real*8, intent(in) :: az(len_bn)
  real*8, intent(in) :: el(len_bn)
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(out) :: ra(len_bn)
  real*8, intent(out) :: dec(len_bn)

  interface
     real*8 FUNCTION sla_gmst(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_gmst
  end interface

  interface
     real*8 FUNCTION sla_dranrm(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_dranrm
  end interface

  !f2py integer len_bn
  !f2py real*8 lon, lat
  !f2py real*8 ra,dec,mjd

  integer :: i

  real*8 :: gmst

  do i=1, len_bn
     call sla_dh2e(az(i), el(i), lat, ra(i), dec(i))
     gmst = sla_gmst(mjd(i))
     ra(i) = gmst + lon - ra(i)
     ra(i) = sla_dranrm(ra(i))
  enddo

end subroutine h2e

subroutine e2h(ra, dec, mjd, lon, lat, az, el,lha, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: lon
  real*8, intent(in) :: lat
  real*8, intent(in) :: ra(len_bn)
  real*8, intent(in) :: dec(len_bn)
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(out) :: az(len_bn)
  real*8, intent(out) :: el(len_bn)
  real*8, intent(out) :: lha(len_bn)

  interface
     real*8 FUNCTION sla_gmst(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_gmst
  end interface

  interface
     real*8 FUNCTION sla_dranrm(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_dranrm
  end interface

  !f2pye2h integer len_bn
  !f2py real*8 lon, lat
  !f2py real*8 az,el,mjd

  integer :: i
  real*8 :: gmst

  do i=1, len_bn
     gmst = sla_gmst(mjd(i))
     lha(i) = lon + gmst - ra(i) ! CONVERT TO LHA
     call sla_de2h(lha(i), dec(i), lat, az(i), el(i))
  enddo

end subroutine e2h

subroutine precess(ra, dec,mjd, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(inout) :: ra(len_bn)
  real*8, intent(inout) :: dec(len_bn)

  interface
     real*8 FUNCTION sla_epb(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_epb
  end interface

  !f2py integer len_bn
  !f2py real*8 mjd
  !f2py real*8 ra,dec

  integer :: i
  real*8 :: epoch

  do i=1, len_bn
     epoch = sla_epb(mjd(i))
     call sla_preces('FK5', epoch, 2000D0, ra(i), dec(i))
  enddo

end subroutine precess

subroutine prenut(ra, dec,mjd, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(inout) :: ra(len_bn)
  real*8, intent(inout) :: dec(len_bn)

  real*8 :: v1(3)
  real*8 :: v2(3)
  real*8 :: PM(3,3)

  interface
     real*8 FUNCTION sla_epb(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_epb
  end interface
  interface
     real*8 FUNCTION sla_dranrm(dummy)
     real*8 :: dummy
     END FUNCTION sla_dranrm
  end interface

  !f2py integer len_bn
  !f2py real*8 mjd
  !f2py real*8 ra,dec

  integer :: i
  real*8 :: epoch

  do i=1, len_bn
     call sla_dcs2c(ra(i),dec(i),v1) ! convert to vectors
     epoch = sla_epb(mjd(i))
     print *, ra(i),dec(i)
     call sla_prenut(epoch,2000D0,PM) ! Nutation and precession matrix
     call sla_dmxv(PM,v1,v2)
     call sla_dcc2s(v2,ra(i),dec(i))
     ra(i) = sla_dranrm(ra(i))
  enddo

end subroutine prenut

subroutine precess_year(ra, dec,mjd, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(inout) :: ra(len_bn)
  real*8, intent(inout) :: dec(len_bn)

  interface
     real*8 FUNCTION sla_epb(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_epb
  end interface

  !f2py integer len_bn
  !f2py real*8 mjd
  !f2py real*8 ra,dec

  integer :: i
  real*8 :: epoch

  do i=1, len_bn
     epoch = sla_epb(mjd(i))
     call sla_preces('FK5', 2000D0, epoch, ra(i), dec(i))
  enddo

end subroutine precess_year

subroutine pa(ra, dec,mjd, lon,lat,pang, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: lon
  real*8, intent(in) :: lat
  real*8, intent(in) :: mjd(len_bn)
  real*8, intent(in) :: ra(len_bn)
  real*8, intent(in) :: dec(len_bn)
  real*8, intent(out) :: pang(len_bn)

  interface
     real*8 FUNCTION sla_gmst(mjddummy)
     real*8 :: mjddummy
     END FUNCTION sla_gmst
  end interface

  interface
     real*8 FUNCTION sla_pa(radummy, decdummy, phidummy)
     real*8 :: radummy, decdummy, phidummy
     END FUNCTION sla_pa
  end interface

  !f2py integer len_bn
  !f2py real*8 mjd, lon, lat
  !f2py real*8 ra,dec, pang

  integer :: i
  real*8 :: ha, gmst

  do i=1, len_bn
     gmst = sla_gmst(mjd(i))
     ha = gmst + lon - ra(i)
     pang(i) = sla_pa(ha, dec(i), lat)
  enddo

end subroutine pa

subroutine e2g(ra, dec, gl, gb, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: ra(len_bn)
  real*8, intent(in) :: dec(len_bn)
  real*8, intent(out) :: gl(len_bn)
  real*8, intent(out) :: gb(len_bn)

  !f2py integer len_bn
  !f2py real*8 ra,dec,gl,gb

  integer :: i

  do i=1, len_bn
     call sla_eqgal(ra(i), dec(i), gl(i), gb(i))
  enddo

end subroutine e2g

subroutine g2e(gl, gb, ra, dec, len_bn)
  implicit none

  integer, intent(in) :: len_bn
  real*8, intent(in) :: gl(len_bn)
  real*8, intent(in) :: gb(len_bn)
  real*8, intent(out) :: ra(len_bn)
  real*8, intent(out) :: dec(len_bn)

  !f2py integer len_bn
  !f2py real*8 ra,dec,gl,gb

  integer :: i

  do i=1, len_bn
     call sla_galeq(gl(i), gb(i), ra(i), dec(i))
  enddo

end subroutine g2e


text:
      module besseltab
      integer,parameter :: lbtab=1000
      real*8, parameter :: hbar=6.58211928e-16 ! eV.s
      real*8 xi(lbtab),Sw(lbtab),SwI(lbtab)
      end module besseltab

      subroutine setcolb(blist,colb)
      use wsbb
      real*8 blist(nblist)
      type(sbeam) colb

      colb%gamma=blist(1)
      colb%gambet=blist(2)
      colb%ce=blist(3)
      colb%xangle(1:8)=blist(4:11)
      colb%Benv(1:36)=blist(12:47)
      colb%Benv5(1:25)=blist(48:72)
      colb%v_cen(1:5)=blist(73:77)
      colb%cod(1:6)=blist(78:83)
      colb%Luminosity=blist(84)
      colb%bstrl=int(blist(85)+0.1)
      colb%nslice=int(blist(99)+0.1)
      colb%zslice(1:nslimax*2)=blist(100:99+nslimax*2)

      return
      end

      subroutine storecolb(blist,colb)
      use wsbb
      real*8 blist(nblist)
      type(sbeam) colb

!      blist=>colb%zslice
!      colb%zslice=>blist
      blist(1)=colb%gamma
      blist(2)=colb%gambet
      blist(3)=colb%ce
      blist(4:11)=colb%xangle(1:8)
      blist(12:47)=colb%Benv(1:36)
      blist(48:72)=colb%Benv5(1:25)
      blist(73:77)=colb%v_cen(1:5)
      blist(78:83)=colb%cod(1:6)
      blist(84)=colb%Luminosity
      blist(85)=dble(colb%bstrl)
      blist(99)=dble(colb%nslice)
      blist(100:99+nslimax*2)=colb%zslice(1:nslimax*2)

      return
      end

!     18/01/93 303061342  MEMBER NAME  BEAMBEAM *.FORT     M  E2FORT
      subroutine beambeam(np,x,px,y,py,z,g,dv,spx,spy,spz,
     $     p_in,blist,iturn)
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
!
!  Beam-Beam tracking subroutine with a six dimension symplectic
!  mothod.
!  Beamstrahlung, Multi IPs
!
!                   written by K.Ohmi      2015.9.2
!
!
!
!
!cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc
!        emix  =p_in(22), emiy =(23),     dp=(24),  sigz=(27)
!        nsli  =p_in(28), rne  =(29),
!  Modified to accept spin variables spx, spy , spz by K. O., 11 Dec 2018.
!  As for blist(nblist), modified to use kfromr/rfromk to store/recall integer data, K. O. 12 Dec 2018.
      use besseltab
      use wsbb
      use tfstk
      use tmacro
      use photontable
      use ffs_flag
      implicit none
      real*8 x(np),px(np),y(np),py(np),z(np),g(np),dv(np),
     $     spx(np),spy(np),spz(np)
      real*8 p_in(70),blist(nblist)
      real*8 sqrpi
      integer*4 np,iturn,idummy,bstrhl,nbsemit,ns1,ns2,nsli,nss
c      integer*4 n,i,is,j,jl,jm,ju,iseed,irtc
      integer*4 n,i,is,j,jl,jm,ju,irtc
      LOGICAL*4 icross,tilt,deform,cod
      real*8 a2,a2x,a2y,acx,acy,asp,c1,cdu,d1x,d1y,dux,duy,delg,dLum
      real*8 fx0,fy0,fxy,gamp,gxy,h1,p1,hi,pn,pxy2,pz,pxi,pyi
      real*8 sigz,xx,yy,w1x,w1y
      real*8 cosx,sinx,tanx,sig11x,sig11y,sig12x,sig12y,sqrsig11,sz
      real*8 sint,cost
      real*8 cnbs,cp2bs,dp2bs,cpbs,cubs,dpzbs,dsz,rhoi,rhoi2
      real*8 rnbs,rlum_col,w2x,w2y,xij,xtmp,ucE
      real*8 tran,rgetgl
      type (sad_symdef), pointer :: symd
!      equilavence (blist(1),gamp)

!      type(sbeam), target :: colb
      type(sbeam) colb

      call setcolb(blist,colb)

      bstrhl=colb%bstrl

c      write(*,'(A,2I6,1P,3E12.3)') 'Welcome to beambeam',
c     &     iturn,colb%nslice,colb%ce,colb%gamma
!      write(*,'(4F10.5,I5)') colb%xangle(1),colb%xangle(2),
!     &     colb%xangle(3),colb%xangle(4),colb%nslice
!      write(*,'(5E12.4)') (p_in(i),i=1,30)
!      call m_print(colb%Benv,6)
!      call m_print(colb%Benv5,5)
!      write(*,'(1P,(4D12.5,A)/)') (colb%v_cen(i),i=1,4),' *z'

!     write(*,*) colb
!     blist=>colb
!     begin initialize for preventing compiler warning

      sqrpi=sqrt(pi)
!     end   initialize for preventing compiler warning
      nsli=colb%nslice
      if(nsli .le. 0) return
!
!   Lorentz factor gamma is stored in h0.
!   Factor gammabeta is stored in p0.
!     gamma=blist(i_p0+2)=h0
!     gammabet=blist(i_p0+1)=p0
!
!
!
      icross=.false.
      tilt=.false.
      deform=.false.
      cod=.false.
      if(colb%xangle(1).ne.0.) icross=.true.
      if(colb%xangle(5).ne.0.) tilt=.true.
      do i=1,4
         if(colb%v_cen(i).ne.0.) then
            deform=.true.
            exit
         endif
      enddo

      do i=1,4
         if(colb%cod(i).ne.0.) then
            cod=.true.
            exit
         endif
      enddo

!
!    sigz should be given dynamically for bstrhl
      sigz=p_in(27)

      gamp=colb%gamma
      cnbs=5.*sqrt(3.)*finest*gamp/6.
      cpbs=2*re*gamp*gamp*gamp/3
      cubs=1.5d0*hbar*cveloc*gamp*gamp/am_e

!      write(*,'(A,1P,4E12.4)') 'bst ',gamp,cnbs,cpbs,cubs

      do i=1,np

!   Get Canonical variables
!       g(i)=pn-1.d0   Not good for precision
!       g(i)=g(i)*(g(i)+2.d0)
!   pn=|p|/p0=1+delta
         pn=1.d0+g(i)
         px(i)=px(i)*pn
         py(i)=py(i)*pn
      enddo
!       write(*,*) 'Before BB interation'
!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)

! ### Crossing angle correction ######################################

      sinx=colb%xangle(2)
      cosx=colb%xangle(3)
      tanx=colb%xangle(4)
c      write(*,*) 'xangle ',sinx,cosx,tanx
c      call tfmemcheckprint('beambeam',1,.false.,irtc)

      if(icross) then
         do i=1,np
            pxy2=px(i)**2+py(i)**2
            pn=1.d0+g(i)
            pz=sqrt(pn**2-pxy2)
            h1=pxy2/(pn+pz)
            pz=1.d0/pz
            g(i)=g(i)-tanx*px(i)+tanx*tanx*h1
            px(i)=(px(i)-tanx*h1)/cosx
            py(i)=py(i)/cosx
            pxy2=px(i)**2+py(i)**2
            pz=1.d0/sqrt((1.+g(i))**2-pxy2)
            xx=x(i)
            x(i)=tanx*z(i)+(1.d0+sinx*px(i)*pz)*xx
            y(i)=y(i)+sinx*py(i)*xx*pz
            z(i)=z(i)/cosx-tanx/cosx*h1*xx*pz
         enddo
      endif
!      write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)

!  Closed orbit correction

      if(cod) then
         do i=1,np
            x(i)=x(i)-colb%cod(1)
            px(i)=px(i)+colb%cod(2)
            y(i)=y(i)-colb%cod(3)
            py(i)=py(i)+colb%cod(4)
            z(i)=z(i)+colb%cod(5)
         enddo
      endif

! t_angle rotation

      if(tilt) then
         sint=colb%xangle(5)
         cost=colb%xangle(6)
         do i=1,np
            xtmp=cost*x(i)+sint*y(i)
            y(i)=cost*y(i)-sint*x(i)
            x(i)=xtmp
            xtmp=cost*px(i)+sint*py(i)
            py(i)=cost*py(i)-sint*px(i)
            px(i)=xtmp
         enddo
      endif
!      write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)

!  Slice loop

      nss=max(0,int(rgetgl('BBCUT',idummy)))
      ns1=nss+1
      ns2=nsli-nss
      do 15 i=1,np
         do 20 is=1,nsli
!            if(is .lt. ns1 .or. is .gt. ns2)cycle
!            dsz=(colb%zslice(nslimax+is-1)-colb%zslice(nslimax+is))*
!     &           sigz*0.5
            dsz=(colb%zslice(nslimax+is-1)-colb%zslice(nslimax+is))*
     &           sigz*0.5
            if(is.eq.1) dsz=(5.-colb%zslice(nslimax+is))*sigz*0.5
            if(is.eq.colb%nslice) dsz=(colb%zslice(nslimax+is-1)+5.)*
     &           sigz*0.5

c       write(*,*) ' In loop ',is
            pxy2=px(i)**2+py(i)**2
            sz=(z(i)-colb%zslice(is)*sigz)/2.d0
!
!  exp(-:D:)x

            x(i)=x(i)+px(i)*sz
            y(i)=y(i)+py(i)*sz
            g(i)=g(i)-pxy2*0.25d0
            pn=1.+g(i)
!     center of mass of each slice

!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)
            if(deform) then
               x(i) =x(i)-colb%zslice(is)*sigz*colb%v_cen(1)
               px(i)=px(i)-colb%zslice(is)*sigz*colb%v_cen(2)
               y(i) =y(i)-colb%zslice(is)*sigz*colb%v_cen(3)
               py(i)=py(i)-colb%zslice(is)*sigz*colb%v_cen(4)
            endif
!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)
!   ###########################################################
!  Beam Beam interaction Body
!   ###########################################################
            sig11x=colb%Benv5(1)+2.d0*colb%Benv5(2)*sz
     &           +colb%Benv5(7)*sz**2
            sig12x=colb%Benv5(2)+colb%Benv5(7)*sz
            sig11y=colb%Benv5(13)+2.d0*colb%Benv5(14)*sz
     &           +colb%Benv5(19)*sz**2
            sig12y=colb%Benv5(14)+colb%Benv5(19)*sz

            asp=sqrt(sig11y/sig11x)

!   ##### Luminosity caluculation

!      dLum=0.d0

            xx=x(i)*x(i)/sig11x
            yy=y(i)*y(i)/sig11y
            if(xx.lt.36.d0.and.yy.lt.36.d0) then
               dLum=exp(-xx*0.5)*exp(-yy*0.5)/pi2/sqrt(sig11x*sig11y)
               colb%Luminosity=colb%Luminosity+dLum
            endif
!
!    ##### x-y kick ###########################################
!
!     case: sigma_x = sigma_y (round beam)
            if(abs((sig11x-sig11y)/sig11x).le.1.d-5) then
!      blist(1100)=gamma  blist(1101)=sli   blist(1102)=partn/sli*re
               c1=colb%ce/colb%nslice
               hi=-x(i)**2/2.d0/sig11x-y(i)**2/2.d0/sig11y
               a2=2.d0*(1.d0-exp(hi))/(x(i)**2+y(i)**2)
               fx0=c1*a2*x(i)
               fy0=c1*a2*y(i)
!   pz kick
               gxy=c1*sig12x/sig11x*exp(hi)

!    case: sigma_x > sigma_y
            elseif(sig11x.gt.sig11y) then
               sqrsig11=sqrt(2.d0*(sig11x-sig11y))
               c1=colb%ce*2.d0*sqrpi/sqrsig11/colb%nslice

               acx=abs(x(i))
               acy=abs(y(i))
               d1x=acx/sqrsig11
               d1y=acy/sqrsig11
               call cerrf(d1x,d1y,w1y,w1x)
               hi=-x(i)**2/2.d0/sig11x-y(i)**2/2.d0/sig11y
               call cerrf(d1x*asp,d1y/asp,w2y,w2x)
               a2=exp(hi)
               a2x=a2*w2x
               a2y=a2*w2y
               fx0=c1*(w1x-a2x)
               fy0=c1*(w1y-a2y)
               if(x(i) .lt. 0.d0)fx0=-fx0
               if(y(i) .lt. 0.d0)fy0=-fy0
!   pz kick
               cdu=-1.d0/(sig11x-sig11y)/2.d0
               fxy=x(i)*fx0+y(i)*fy0
               dux=cdu*(fxy+2.d0*colb%ce/colb%nslice*(a2*asp-1.d0))
               duy=-cdu*(fxy+2.d0*colb%ce/colb%nslice*(a2/asp-1.d0))
               gxy=sig12x*dux+sig12y*duy
!         write(*,*) sz,gxy,sig12x,sig12y

!     case: sigma_x < sigma_y
            else
               sqrsig11=sqrt(2.d0*(sig11y-sig11x))
               c1=colb%ce/colb%nslice*2.d0*sqrpi/sqrsig11

               acx=abs(x(i))
               acy=abs(y(i))
               d1x=acx/sqrsig11
               d1y=acy/sqrsig11
               call cerrf(d1y,d1x,w1x,w1y)
               hi=-x(i)**2/2.d0/sig11x-y(i)**2/2.d0/sig11y
               call cerrf(d1y/asp,d1x*asp,w2x,w2y)
               a2=exp(hi)
               a2x=a2*w2x
               a2y=a2*w2y
               fx0=c1*(w1x-a2x)
               fy0=c1*(w1y-a2y)
               if(x(i) .lt. 0.d0)fx0=-fx0
               if(y(i) .lt. 0.d0)fy0=-fy0
!     pz kick
               cdu=-1.d0/(sig11x-sig11y)/2.d0
               fxy=x(i)*fx0+y(i)*fy0
               dux=cdu*(fxy+2.d0*colb%ce/colb%nslice*(a2*asp-1.d0))
               duy=-cdu*(fxy+2.d0*colb%ce/colb%nslice*(a2/asp-1.d0))
               gxy=sig12x*dux+sig12y*duy
            endif
!
            px(i)=px(i)-fx0
            py(i)=py(i)-fy0
            g(i)=g(i)-gxy

!     Beamstrahlung

            if(bstrhl.gt.0) then
               rhoi2=(fx0*fx0+fy0*fy0)/dsz/dsz
               rhoi=sqrt(rhoi2)
               rnbs=cnbs*rhoi*dsz
               dpzbs=cpbs*rhoi2*dsz
               dp2bs=sqrt(cp2bs*rhoi*rhoi2*dsz)
!     write(*,'(I6,1P,3E12.4)') i,rnbs,dpzbs,dp2bs
               ucE=cubs*rhoi
!               write(*,'(2I3,1P,6E12.4)') is,i,rhoi,ucE,dsz,rnbs,fx0,fy0

               if(rnbs.gt.0.5) then
                  write(*,*) 'Nbs is too large',rnbs
                  stop
               endif
!               g(i)=g(i)+dpzbs  !+tgauss(iseed)*dp2bs
!  Poisson generator
c               xx=tran(iseed)
               xx=tran()
!           if(xx.gt.prb) then
!  Binomial generator
               if(xx.lt.rnbs) then
!   Quantum excitation model using 2nd order momemnt
!              g(i)=g(i)-(dpzbs+tgauss(iseed)*dp2bs*sqrt(rnbs))/rnbs
!!!!!              g(i)=g(i)-dpzbs/rnbs  !  both gave the same result
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  K bessel function table
                  xx=xx/rnbs
!     Locate j for SwI()=xx
                  n=lbtab        ! dim of SwI
                  jl=0
                  ju=n+1
 100              if(ju-jl.gt.1) then
                     jm=(ju+jl)/2
                     if((SwI(n).gt.SwI(1)).eqv.(xx.gt.SwI(jm))) then
                        jl=jm
                     else
                        ju=jm
                     endif
                     goto 100
                  endif
                  j=jl
                  xij=xi(j)+(xi(j+1)-xi(j))*(xx-SwI(j))/
     &                 (SwI(j+1)-SwI(j))
                  delg=cubs*rhoi*exp(xij)
!    emitted photon info. is send to SAD.  2019/10/1
                  if(photons) then
                     pxi=px(i)/pn
                     pyi=py(i)/pn
                     p1=colb%gambet*pn
                     h1=gamp*pn
                     write(*,*) x(i),pxi,y(i),pyi,delg
                     call tphotonconv(x(i),pxi,y(i),pyi,delg,0.d0,
     &                    p1,h1,sz,i)
                  endif
!                  write(*,'(I4,1P,7E12.4)')
!     &                 j,xi(j),xi(j+1),xij,SwI(j),SwI(j+1),xx,delg
                  g(i)=g(i)-delg
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
                  nbsemit=nbsemit+1
               endif
!     dpzbsav=dpzbsav+dpzbs/rnbs   ! same as upper formula
!              goto 2
            endif

!     center of mass of each slice
!
!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)
            if(deform) then
               x(i) =x(i)+colb%zslice(is)*sigz*colb%v_cen(1)
               px(i)=px(i)+colb%zslice(is)*sigz*colb%v_cen(2)
               y(i) =y(i)+colb%zslice(is)*sigz*colb%v_cen(3)
               py(i)=py(i)+colb%zslice(is)*sigz*colb%v_cen(4)
            endif
!
!     exp(:D:)x

            pxy2=px(i)**2+py(i)**2
            x(i)=x(i)-px(i)*sz
            y(i)=y(i)-py(i)*sz
            g(i)=g(i)+pxy2*0.25d0
!
 20      enddo
 15   enddo
c      call tfmemcheckprint('beambeam',5,.false.,irtc)

! t_angle rotation

!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)
      if(tilt) then
         do i=1,np
            xtmp=cost*x(i)-sint*y(i)
            y(i)=cost*y(i)+sint*x(i)
            x(i)=xtmp
            xtmp=cost*px(i)-sint*py(i)
            py(i)=cost*py(i)+sint*px(i)
            px(i)=xtmp
         enddo
      endif

!  Closed orbit correction

      if(cod) then
         do i=1,np
            x(i)=x(i)+colb%cod(1)
            px(i)=px(i)-colb%cod(2)
            y(i)=y(i)+colb%cod(3)
            py(i)=py(i)-colb%cod(4)
            z(i)=z(i)-colb%cod(5)
         enddo
      endif

! ###  Crossing angle correction ###################################

!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)
      if(icross) then
         do i=1,np
            pxy2=px(i)**2+py(i)**2
            pn=1.d0+g(i)
            pz=sqrt(pn**2-pxy2)
            h1=pxy2/(pn+pz)
            pz=1.d0/pz
            x(i)=(x(i)-sinx*z(i))/(1.d0+(sinx*(px(i)+sinx*h1))*pz)
            y(i)=y(i)-sinx*py(i)*pz*x(i)
            z(i)=z(i)*cosx+sinx*cosx*h1*pz*x(i)
            h1=h1*cosx**2
            xx=px(i)
            px(i)=xx*cosx+tanx*h1
            py(i)=py(i)*cosx
            g(i)=g(i)+sinx*xx
         enddo
      endif

!       write(*,*) 'After BB interation'
!       write(*,'(2D25.15)') x(1),px(1),y(1),py(1),z(1),g(1)

!   Return to SAD variables

!   pn=|p|/p0=1+delta
c      call tfmemcheckprint('beambeam',7,.false.,irtc)
      call limitnan(x,1.d4)
      call limitnan(px,1.d4)
      call limitnan(y,1.d4)
      call limitnan(py,1.d4)
      call limitnan(z,1.d10)
      call limitnan(g,1.d4)
      do  i=1,np
         pn=1.d0+g(i)
         px(i)=px(i)/pn
         py(i)=py(i)/pn
!     When you change momentum, change dv(i) as
         h1=sqrt(1.d0+(pn*colb%gambet)**2)
         dv(i)=-g(i)*(1.d0+pn)/h1/(h1+pn*colb%gamma)+dvfs
      enddo

!      write(*,*) 'blist(nblist)',blist(nblist)
c      call descr_sad(dfromk(int8(blist(nblist)+0.1d0)),symd)
c      call descr_sad(dfromr(blist(nblist)),symd)
c     modified to use kvlum in wsbb
      if(kvlum%k .ne. 0)then
        call descr_sad(kvlum,symd)
        rlum_col=colb%Luminosity/colb%nslice/np
        if(ktfenanq(rlum_col))then
          rlum_col=0.d0
        endif
        symd%value=dfromr(rlum_col)
!     call tfsetlist(ntfreal,0,rlum_col,iax,1)
!!!!      iax=colb%iax
!!!!      rlist(iax)=rlum_col
      endif
c      call tfmemcheckprint('rlum_col',0,.true.,irtc)
      call LumRead(rlum_col)
c      call tfmemcheckprint('rlum_col',1,.true.,irtc)
!       write(*,*) rlum_col
!

      return
      end subroutine beambeam

!   18/01/93 303072023  MEMBER NAME  BBINIT   *.FORT     M  E2FORT
      subroutine bbinit(p_in,blist)
      use besseltab
      use wsbb
      use tfstk
      use kyparam
      use tmacro
      use photontable
      use ffs_flag
      implicit none
      integer*4 idummy,nsli,i,irtc
!  parameter (nblist=1600,tbuf0=1100,nslimax=500)
      real*8 benv_inv(36),m_sub5(25),v_sub5(5),c_sub
      real*8 m_tmp(36),m_Crs(36),eig5(10)
      real*8 m_B(36),m_R(36),m_H(36),m_HRB(36),m_Emit(36)
      real*8 p_in(70),rne,blist(nblist)
      real*8 rgetgl,gauinv
      real*8 cnbs,cpbs,cubs,gamp
c      type (sad_descriptor) kv
      type (sad_symdef), pointer :: vsymd
!      integer*4 itlookup,itfsymbol
      real*8 sigx,sigy,sigz,az,bz,emiz,dp,pwb0,yy
      character*17 vname
      data vname/'LuminosityMonitor'/
!cccccccccc parameter input

      type(sbeam) colb

!      i8list=colb
!      write(*,*) 'Welcome to bbinit',associated(colb)
!      if(.not.associated(colb)) then
!         allocate(colb)
!         write(*,*) 'colb is allocated'
!      endif

      rne=p_in(ky_NP_BEAM)
      nsli=int(p_in(ky_SLI_BEAM))
      colb%bstrl=p_in(53)

      colb%nslice=nsli
      if(nsli .le. 0) then
c 23 Jan 2020 by KO, modified to use non positive SLICE to disable BB.
c         colb%nslice=1
         colb%nslice=0
      endif
      if(nsli .gt. nslimax) then
         write(*,*)'*******************************'
         write(*,*)' Too many number of slices !!! '
         write(*,*)'    nslice=',nsli
         write(*,*)' Maximum allowable number is ', nslimax
         write(*,*)'*******************************'
         stop
      end if
!        alphax= p_in(1), betax= (2), alphay= (3), betay= (4)
!        r1    = p_in(5), r2   = (6),     r3= (7),   r4 = (8)
!        ex    = p_in(9), epx  =(10),     ey=(11),  epy =(12)
!        zx    =p_in(13), zpx  =(14),     zy=(15),  zpy =(16)
!        dx    =p_in(17), dpx  =(18),     dy=(19),  dpy =(20)
!        emix  =p_in(22), emiy =(23),     dp=(24),  sigz=(27)
!        nsli  =p_in(28), rne  =(29),     bstrl=(53)
!---  |----1----|----2----|----3----|----4----|----5----|----6----|----7
!       write(*,'(//A)')
!     &   '*** Strong Beam setup *****************************'
!       WRITE(*,'(A,1PD12.6)') 'Number of Particles= ',rne
!       WRITE(*,'(A,1PD12.6,A,D12.6,A)')
!     &    'betau      = ',p_in(2) ,'m   betav     = ',p_in(4) ,'m'
!       WRITE(*,'(A,1PD12.6,A,D12.6,A)')
!     &    'etaX       = ',p_in(9) ,'m   etaY      = ',p_in(11),'m'
!       WRITE(*,'(A,1PD12.6,A,D12.6,A)')
!     &    'emittance x= ',p_in(22),'m  emittance y= ',p_in(23),'m'
!       WRITE(*,'(A,1PD12.6,A,D12.6,A)')
!     &    'sigz       = ',p_in(27),'m  sigp       = ',p_in(24),'m'
!       WRITE(*,'(A,F10.2,A)') 'Crossing angle ',p_in(21)*1.D3,' mrad'
!       if(colb%bstrl.eq.1) then
!          WRITE(*,'(A)') 'Beam strahlung ON'
!       else
!          WRITE(*,'(A)') 'Beam strahlung OFF'
!       endif
!
!  Strong beam envelope calculation

      if(p_in(31).gt.0..and.p_in(37).gt.0..and.p_in(42).gt.0..and.
     &     p_in(46).gt.0..and.p_in(49).gt.0..and.p_in(51).gt.0.) then
!         write(*,*) 'Strong beam is constructed from envelope matrix'
      call m_symmet_set_6d(p_in(31),colb%Benv)
      else
!         write(*,*) 'Strong beam is constructed from twiss parameters'
!      p_in(25)=<z pz>
         emiz=p_in(24)**2*p_in(27)**2-p_in(25)**2
         if(emiz.le.0.) then
            write(*,*) ' bb.f : Error in emitance z calculation'
            stop
         endif
         emiz=sqrt(emiz)
         bz=p_in(27)**2/emiz
         az=-p_in(25)/emiz
         call m_B_set_6d(p_in(1),p_in(2),p_in(3),p_in(4),az,bz,m_B)
         call m_R_set_6d(p_in(5),p_in(6),p_in(7),p_in(8),m_R)
         call m_H_set_6d(p_in(9),p_in(10),p_in(11),p_in(12),
     &        p_in(13),p_in(14),p_in(15),p_in(16),m_H)
         call m_Emit_set_6d(p_in(22),p_in(23),p_in(24)*p_in(27),m_Emit)

         call m_mul3_6d(m_H,m_R,m_B,m_HRB)
!       call is_symplectic_6d(blist(m_HRB))

!         call m_print(m_HRB,6)
         call m_sim_tr_6d(m_HRB,m_Emit,colb%Benv)
!      call m_print(blist(m_Emit),6)
      endif
!      write(*,*) ' Beam envelope matrix on the head on frame'
!      call m_print(colb%Benv,6)

      if(p_in(21).ne.0.) then
         call m_Crs_set_6d(p_in(21),m_Crs)
         call m_sim_tr_6d(m_Crs,colb%Benv,colb%Benv)
      endif

!       write(*,*) ' Envelope of Strong beam at IP (Streak image)'
!      call m_print(colb%Benv,6)
!       write(*,*) ' '
      dp=0.d0
      sigx=sqrt(colb%Benv(1))
      sigy=sqrt(colb%Benv(15))
      sigz=sqrt(colb%Benv(29))
!      WRITE(*,'(A,1PD12.6,A)') 'sigma x=',sigx ,'m',
!     &   'sigma y=',sigy,'m','sigma z=',sigz,'m'
!      com=re*partn/2.d0/pi/h0/(sigx+sigy)
!      xix=com*p_in(2)/sigx
!      xiy=com*p_in(4)/sigy
!         write(*,*) 'Convensional beam beam parameters '
!         write(*,'(A,F10.5,A,F10.5/)') 'xix=',xix,'  xiy=',xiy

!   z slicing

!   Weak beam    p0=momentum(eV)  p0+1=p0/m_e=beta gamma  p0+2=gamma

      pwb0=rgetgl('MOMENTUM',idummy)
      colb%gambet=pwb0/am_e
      colb%gamma=sqrt(colb%gambet**2+1.)
!       write(*,*) ' Weak beam momentum '
!       write(*,'(3F15.3/)') (blist(i_p0+i),i=0,2)

!  Crossing angle and fundamental parameter set

      colb%xangle(1)=p_in(21)
      colb%xangle(2)=sin(p_in(21))
      colb%xangle(3)=cos(p_in(21))
      colb%xangle(4)=tan(p_in(21))
      colb%ce=rne*re/colb%gamma
!      WRITE(*,'(A,1P,E12.4,A/)') 'Ne re/gamma ',colb%ce,' mrad'

!  Closed orbit of strong beam

      colb%cod(1)=p_in(17)
      colb%cod(2)=p_in(18)/colb%xangle(3)
      colb%cod(3)=p_in(19)
      colb%cod(4)=p_in(20)/colb%xangle(3)
      colb%cod(5)=p_in(26)/colb%xangle(3)

!     write(*,*) '  Beam displacement'
!       write(*,'(A/,1P,(5D12.5)/)')
!     &    '    dx          dpx         dy         dpy        dz',
!     &            (blist(i_cod+i),i=0,4)
      if(colb%bstrl.gt.0) then
         gamp=colb%gamma
         cnbs=5.*sqrt(3.)*finest*gamp/6.
         cpbs=2*re*gamp*gamp*gamp/3
         cubs=1.5d0*hbar*cveloc*gamp*gamp/am_e
         write(*,'(A,1P,4E12.4)') 'beamstrahlung ',gamp,cnbs,cpbs,cubs
         if(photons) then
            call tsetphotongeo(0.d0,0.d0,0.d0,.true.)
         endif
      endif

!-------------------------------------------------------------------
!         z slicing
!-------------------------------------------------------------------

!    get 5x5 matrix

      call m_inverse_6d(colb%Benv,Benv_inv)
!      call m_print(blist(m_benv_inv),6)
      call m_zslice_5d(Benv_inv,m_sub5,v_sub5,c_sub)
!      call m_print(blist(m_sub5),5)
      call teigen(m_sub5,m_tmp,eig5,5,5)
!      call m_print(blist(m_sub5),5)
      call m_normalize_5d(m_sub5,eig5)
!      call m_print(blist(m_sub5),5)
!      write(*,'(5D12.6)') (blist(eig5+2*i-2),i=1,5)
!      write(*,'(5D12.6)') (blist(eig5+2*i-1),i=1,5)
      do i=1,5
         if(eig5(2*(i-1)+1).lt.0.) then
            write(*,*) 'bbinit : 5dim. beam envelope has zero or',
     &           ' negative eigen values'
             stop
          endif
          eig5(2*(i-1)+1)=1.d0/eig5(2*(i-1)+1)
       enddo
       call m_diag_5d(eig5(1),eig5(3),eig5(5),eig5(7),eig5(9),
     &      colb%Benv5)
       call m_sim_tr_5d(m_sub5,colb%Benv5,colb%Benv5)
       call mv_mul_5d(colb%Benv5,v_sub5,colb%v_cen)
       call v_chg_sign(colb%v_cen,5)
!       write(*,*) 'Strong beam envelope of each slice'
!       call m_print(colb%Benv5,5)
!       write(*,*) 'Its center of mass'
!       write(*,'(1P,(4D12.5,A)/)') (colb%v_cen(i),i=1,4),' *z'
       call benv_norm_axis(colb%Benv5,colb%v_cen,colb%xangle(5))
       colb%xangle(7)=cos(colb%xangle(5))
       colb%xangle(6)=sin(colb%xangle(5))
!      write(*,'(A,F10.2,A/)') ' Tilt angle =',
!     &     blist(i_t_angle)*1.D+3,' mrad'

!      write(*,*) 'Envelope and its center on the tilt frame'
!      call m_print(blist(m_Benv5),5)
!      write(*,'(1P,(4D12.5,A)/)') (blist(v_bcen_fac+i-1),i=1,4),' *z'

!  *****   <x_i>=v_bcen_fac(i)*z   *****
!   beambeam requires m_Benv5, v_bcen_fac, and tilt_angle

!         WRITE(*,*)'DP= ',DP
!ccccccccccc zstar(i)
! Slicing

       do i=1,nsli-1
          yy=1.d0/nsli*i
          colb%zslice(i+nslimax)=gauinv(yy)
          if (i .eq. 1)then
             colb%zslice(i)=-exp(-colb%zslice(i+nslimax)**2/2.d0)/
     &            sqrt(2.d0*pi)*nsli
          else
             colb%zslice(i)=(exp(-colb%zslice(i+nslimax-1)**2/2.d0)
     &        -exp(-colb%zslice(i+nslimax)**2/2.d0))/sqrt(2.d0*pi)*nsli
          endif
       enddo
       if (nsli .eq. 1)then
          colb%zslice(1)=0.d0
       else
          colb%zslice(nsli)=exp(-colb%zslice(nsli+nslimax-1)**2/2.d0)
     &         /sqrt(2.d0*pi)*nsli
       endif
       colb%zslice(nslimax)=100.
       colb%zslice(nslimax+nsli)=100.
!       write(*,*)'************* Slice ***************'
!       write(*,'(A)')'Slice    zc           zboundary   (unit sigz)'
!       write(*,'(20X,F10.5)') colb%zslice(nslimax)
       do i=1,nsli
          colb%zslice(i)=-colb%zslice(i)
          colb%zslice(i+nslimax)=-colb%zslice(i+nslimax)
!          write(*,'(I3,F10.5,10X,F10.5)') i,colb%zslice(i),1./dble(nsli)
!          write(*,'(20X,F10.5)') colb%zslice(i+nslimax)
       enddo
!       write(*,*)'***********************************'

       call mkbesseltab
!
c
c If we return list the allocation should be done in each track.
c that is this if block should be remove to beambeam.
       if(kvlum%k .eq. 0) then
c     modified to use kvlum in wsbb
         kvlum=kxsymbolz(vname,len(vname),vsymd)
         call tflocald(vsymd%value)
c     In the case of only one real variable.
c         blist(nblist)=rfromk(kv%k)
         vsymd%value=dfromr(0.d0)
       endif

!      write(*,*) kv%k

c       call tfmemcheckprint('storecolb',0,.true.,irtc)
       call storecolb(blist,colb)
c       call tfmemcheckprint('storecolb',1,.true.,irtc)

       return
       end subroutine bbinit

      subroutine LumRead(rlum_col)
      use tfstk
      use efun
c      use tfcbk
      implicit none
      type (sad_descriptor) kx,kem
      integer*4 irtc,level,itfdownlevel,isp0
      real*8 rlum_col
      save kem
      data kem%k/0/
      if(itfcontext .le. 0)then
        return
      endif
      if(kem%k .eq. 0)then
        kem=kxsymbolz('LuminosityMonitor',17)
      endif
      call tclrfpe
      levele=levele+1
      isp0=isp
      isp=isp+1
      dtastk(isp)=kem
      isp=isp+1
      rtastk(isp)=rlum_col
c      isp=isp+1
c      itastk(1,isp)=ntfreal
c      vstk(ivstkoffset+isp)=sigx
c      isp=isp+1
c      itastk(1,isp)=ntfreal
c      vstk(ivstkoffset+isp)=sigy
c      isp=isp+1
c      itastk(1,isp)=ntfreal
c      vstk(ivstkoffset+isp)=sigz

      kx=tfefunref(isp0+1,.false.,irtc)
c      call tfdebugprint(kx,'LumRead',1)
      isp=isp0
      level=itfdownlevel()
      return
      end

      subroutine mkbesseltab
!  use physconst
      use besseltab
      implicit none
      integer i
      real*8 x,dx,xnu,ri,rk,rip,rkp,x0,x2,dx6,rk0,rk2
      real*8 Pgtot

!      write(*,*) 'Make S(x) table for radiation (beamstrahlung)'
      dx=0.1d0
      xnu=5./3.
      x=1.1**40
      Sw(1)=0.
      SwI(1)=0.
      do i=1,lbtab
      x0=x
      x=x/1.02
      x2=(x+x0)*0.5
      dx6=(x0-x)/6.
      xi(i)=log(x)
      call bessik(x2,xnu,ri,rk2,rip,rkp)
      call bessik(x,xnu,ri,rk,rip,rkp)
      if(i.gt.1) Sw(i)=Sw(i-1)+(rk0+4*rk2+rk)*dx6
      if(i.gt.1) Pgtot=x2*Sw(i)*(x0-x)
      if(i.gt.1) SwI(i)=SwI(i-1)+Sw(i)*(x0-x)
      rk0=rk
!     write(*,'(1P,6E12.4)') x,bessk0(x),rk,Sw(i),Pgtot,SwI(i)
      enddo
      do i=1,lbtab
         SwI(i)=SwI(i)/SwI(lbtab)
!     write(*,'(I4,1P,6E12.4)') i,xi(i),SwI(i)
      enddo
      return
      end subroutine mkbesseltab

max_stars_repo_name: noboruatkek/SAD

max_issues_repo_name: noboruatkek/SAD

max_forks_repo_name: noboruatkek/SAD


text:
      subroutine wr3w7(cspb,fep,iktmax,iodb,iopg,iopr,iopt,
     $ itermx,jflagb,jxmod,kxmod,ncompb,newin,nodbmx,nopgmx,
     $ noprmx,noptmx,nsq,nsqmax,ntitl,ntitmx,nxmdmx,nxmod,
     $ nxtb,nxtmax,rho,tempc,tdspkg,tdspl,tolbt,toldl,tolsat,
     $ ubasis,uebal,umemb,uphas1,uphas2,uredox,usolb,uspecb,
     $ utitl,xbarb,uxmd24,xlkmod)
c
c     This subroutine writes the EQ3NR input file in compact ("W")
c     format for versions 7.0-7.2. It thus encompasses two version
c     levels.
c
c     This subroutine is called by:
c
c       XCON3/xcon3.f
c
c-----------------------------------------------------------------------
c
      implicit none
c
c-----------------------------------------------------------------------
c
c     Calling sequence variable declarations.
c
      integer iktmax,nodbmx,nopgmx,noprmx,noptmx,nsqmax,ntitmx,nxmdmx,
     $ nxtmax
c
      integer iodb(nodbmx),iopg(nopgmx),iopr(noprmx),iopt(noptmx),
     $ jflagb(nsqmax),jxmod(nxmdmx),kxmod(nxmdmx),ncompb(nxtmax)
c
      integer itermx,newin,nsq,ntitl,nxmod,nxtb
c
      character*80 utitl(ntitmx)
      character*24 ubasis(nsqmax),umemb(iktmax,nxtmax),uphas1(nsqmax),
     $ uphas2(nsqmax),usolb(nxtmax),uspecb(nsqmax),uxmd24(nxmdmx)
      character*24 uebal,uredox
c
      real*8 cspb(nsqmax),xbarb(iktmax,nxtmax),xlkmod(nxmdmx)
c
      real*8 fep,rho,tempc,tdspkg,tdspl,tolbt,toldl,tolsat
c
c-----------------------------------------------------------------------
c
c     Local variable declarations.
c
      integer i,ikb,iktb,j2,n,ns,nxb
      integer ilnobl
c
c-----------------------------------------------------------------------
c
c     Write the new input file.
c
c     Title.
c
      do 110 n = 1,ntitl
        j2 = ilnobl(utitl(n))
        write (newin,1000) utitl(n)(1:j2)
 1000 format(a)
  110 continue
      if (ntitl .lt. ntitmx) write (newin,1010)
 1010 format('endit.')
c
c-----------------------------------------------------------------------
c
c     Temperature.
c
      write (newin,1040) tempc
 1040 format(5x,'tempc= ',1pe12.5)
c
c     Density, total dissolved salts (per kg), and total dissolved
c     salts (per liter).
c
      write (newin,1050) rho,tdspkg,tdspl
 1050 format (7x,'rho= ',1pe12.5,4x,'tdspkg= ',1pe12.5,5x,
     $ 'tdspl= ',1pe12.5)
c
c     Redox parameter and name of redox species defining a controlling
c     couple, if any.
c
      j2 = ilnobl(uredox)
      write (newin,1060) fep,uredox(1:j2)
 1060 format (7x,'fep= ',1pe12.5,4x,'uredox= ',a)
c
c     Convergence tolerances (tolbt and toldl) and saturation
c     tolerance (tolsat).
c
      write (newin,1070) tolbt,toldl,tolsat
 1070 format (5x,'tolbt= ',1pe12.5,5x,'toldl= ',1pe12.5,4x,'tolsat= ',
     $ 1pe12.5)
c
c     Maximum number of iterations.
c
      write (newin,1080) itermx
 1080 format (4x,'itermx= ',i2)
c
c-----------------------------------------------------------------------
c
      write (newin,1090)
 1090 format('*',15x,'1    2    3    4    5    6    7    8    9   10')
c
c     Iopt option switches.
c     Note: iopt(1) = iopt1, etc.
c
      write (newin,1100) (iopt(i), i = 1,10)
 1100 format(2x,'iopt1-10= ',10i5)
c
c     Iopg option switches.
c     Note: iopg(1) = iopg1, etc.
c
      write (newin,1110) (iopg(i), i = 1,10)
 1110 format(2x,'iopg1-10= ',10i5)
c
c     Iopr option switches.
c     Note: iopr(1) = iopr1, etc.
c
      write (newin,1120) (iopr(i), i = 1,20)
 1120 format(2x,'iopr1-10= ',10i5,/1x,'iopr11-20= ',10i5)
c
c     Iodb option switches.
c     Note: iodb(1) = iodb1, etc.
c
      write (newin,1130) (iodb(i), i = 1,10)
 1130 format(2x,'iodb1-10= ',10i5)
c
c-----------------------------------------------------------------------
c
c     Species for electrical balancing.
c
      j2 = ilnobl(uebal)
      write (newin,1150) uebal(1:j2)
 1150 format(5x,'uebal= ',a)
c
c-----------------------------------------------------------------------
c
c     Nxmod options.
c
      write (newin,1200) nxmod
 1200 format(5x,'nxmod= ',i2)
c
      if (nxmod .gt. 0) then
        do 420 n = 1,nxmod
          j2 = ilnobl(uxmd24(n))
          write (newin,1220) uxmd24(n)(1:j2),jxmod(n),kxmod(n),
     $    xlkmod(n)
 1220     format(3x,'species= ',a,/6x,'type= ',i2,14x,'option= ',
     $    i2,14x,'xlkmod= ',1pe12.5)
  420   continue
      endif
c
c-----------------------------------------------------------------------
c
c     Basis species and associated constraints.
c
      do 440 ns = 1,nsq
        j2 = ilnobl(uspecb(ns))
        write (newin,1240) uspecb(ns)(1:j2)
 1240   format('data file master species= ',a)
        j2 = ilnobl(ubasis(ns))
        write (newin,1250) ubasis(ns)(1:j2)
 1250   format(3x,'switch with species= ',a)
        write (newin,1260) jflagb(ns),cspb(ns)
 1260   format(3x,'jflag= ',i2,3x,'csp= ',1pg12.5)
        if (jflagb(ns).ge.17 .and. jflagb(ns).le.18) then
          j2 = ilnobl(uphas1(ns))
          write (newin,1270) uphas1(ns)(1:j2)
 1270     format(2x,'co-ion= ',a)
        elseif (jflagb(ns) .eq. 19) then
          j2 = ilnobl(uphas1(ns))
          write (newin,1272) uphas1(ns)(1:j2)
 1272     format(1x,'mineral= ',a)
        elseif (jflagb(ns) .eq. 20) then
          j2 = ilnobl(uphas2(ns))
          write (newin,1275) uphas1(ns),uphas2(ns)(1:j2)
 1275     format(1x,'sol-sol= ',a24,2x,'end-mem= ',a)
        elseif (jflagb(ns) .eq. 21) then
          j2 = ilnobl(uphas1(ns))
          write (newin,1277) uphas1(ns)(1:j2)
 1277     format(5x,'gas= ',a)
        endif
  440 continue
      write (newin,1280)
 1280 format('endit.')
c
c-----------------------------------------------------------------------
c
c     Mole fractions of solid solutions.
c
      if (iopt(4) .ge. 2) then
        write (newin,1300)
 1300   format('*   Solid solution compositions')
        do 460 nxb = 1,nxtb
          j2 = ilnobl(usolb(nxb))
          write (newin,1310) usolb(nxb)(1:j2)
 1310     format(3x,a)
          iktb = ncompb(nxb)
          do 450 ikb = 1,iktb
            write (newin,1320) umemb(ikb,nxb),xbarb(ikb,nxb)
 1320       format(6x,a24,3x,f10.4)
  450     continue
          write (newin,1330)
 1330     format(6x,'endit.')
  460   continue
        write (newin,1340)
 1340   format(3x,'endit.')
      endif
c
c-----------------------------------------------------------------------
c
  999 continue
      end


text:
!**********************************************************************************************************************************
! LICENSING
! Copyright (C) 2015  National Renewable Energy Laboratory
!
!    SuperController DataExchange, a submodule of openfast
!
! Licensed under the Apache License, Version 2.0 (the "License");
! you may not use this file except in compliance with the License.
! You may obtain a copy of the License at
!
!     http://www.apache.org/licenses/LICENSE-2.0
!
! Unless required by applicable law or agreed to in writing, software
! distributed under the License is distributed on an "AS IS" BASIS,
! WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
! See the License for the specific language governing permissions and
! limitations under the License.
!
!**********************************************************************************************************************************
! File last committed: $Date: $
! (File) Revision #: $Rev: $
! URL: $HeadURL: $
!**********************************************************************************************************************************
MODULE SC_DataEx

! This is a pseudo module used to couple FAST v8 with SuperController; it is considered part of the FAST glue code
   USE FAST_Types
   USE SCDataEx_Types

   IMPLICIT NONE

   PRIVATE

   TYPE(ProgDesc), PARAMETER            :: SC_DX_Ver = ProgDesc( 'SuperController DataExchange', '', '' )

! ==================================================================================================="

      ! ..... Public Subroutines ...................................................................................................

   PUBLIC :: SC_DX_Init                           ! Initialization routine
   PUBLIC :: SC_DX_SetInputs                      ! Glue-code routine to update inputs for SuperController
   PUBLIC :: SC_DX_SetOutputs                     ! Glue-code routine to update inputs to turbine controller from SuperController

CONTAINS
!----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE SC_DX_Init( NumSC2CtrlGlob, NumSC2Ctrl, NumCtrl2SC, SC_DX, ErrStat, ErrMsg )
!..................................................................................................................................
   INTEGER(IntKi),                  INTENT(IN   )  :: NumSC2CtrlGlob
   INTEGER(IntKi),                  INTENT(IN   )  :: NumSC2Ctrl
   INTEGER(IntKi),                  INTENT(IN   )  :: NumCtrl2SC
   TYPE(SCDataEx_Data),             INTENT(INOUT)  :: SC_DX        ! data for the SuperController integration module
   INTEGER(IntKi),                  INTENT(  OUT)  :: ErrStat     ! Error status of the operation
   CHARACTER(*),                    INTENT(  OUT)  :: ErrMsg      ! Error message if ErrStat /= ErrID_None

      ! local variables
   INTEGER(IntKi)                                  :: ErrStat2    ! temporary Error status of the operation
   CHARACTER(ErrMsgLen)                            :: ErrMsg2     ! temporary Error message if ErrStat /= ErrID_None

   CHARACTER(*),   PARAMETER                       :: RoutineName = 'SC_DX_Init'

      ! Initialize variables

   ErrStat = ErrID_None
   ErrMsg  = ""

   IF (NumCtrl2SC > 0) THEN
      CALL AllocPAry( SC_DX%u%toSC, NumCtrl2SC, 'u%toSC', ErrStat2, ErrMsg2 )
      CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
   END IF

   IF (ErrStat >= AbortErrLev) RETURN

      ! make sure the C versions are synced with these arrays
   if (NumCtrl2SC > 0) then
      SC_DX%u%c_obj%toSC_Len = NumCtrl2SC
      SC_DX%u%c_obj%toSC     = C_LOC( SC_DX%u%toSC(1) )
   else
      SC_DX%u%c_obj%toSC_Len = 0
      SC_DX%u%c_obj%toSC     = C_NULL_PTR
   end if

      !............................................................................................
      ! Define system output initializations (set up mesh) here:
      !............................................................................................
   if (NumSC2CtrlGlob > 0) then
      CALL AllocPAry( SC_DX%y%fromSCglob, NumSC2CtrlGlob, 'y%fromSCglob', ErrStat2, ErrMsg2 )
      CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
   end if

   IF (ErrStat >= AbortErrLev) RETURN

      ! make sure the C versions are synced with these arrays
   if (NumSC2CtrlGlob > 0) then
      SC_DX%y%c_obj%fromSCglob_Len = NumSC2CtrlGlob
      SC_DX%y%c_obj%fromSCglob     = C_LOC( SC_DX%y%fromSCglob(1) )
   else
      SC_DX%y%c_obj%fromSCglob_Len = 0
      SC_DX%y%c_obj%fromSCglob     = C_NULL_PTR
   end if

   if (NumSC2Ctrl > 0) then
      CALL AllocPAry( SC_DX%y%fromSC, NumSC2Ctrl, 'y%fromSC', ErrStat2, ErrMsg2 )
      CALL SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName )
   end if

   IF (ErrStat >= AbortErrLev) RETURN

   ! make sure the C versions are synced with these arrays
   if (NumSC2Ctrl > 0) then
      SC_DX%y%c_obj%fromSC_Len = NumSC2Ctrl
      SC_DX%y%c_obj%fromSC     = C_LOC( SC_DX%y%fromSC(1) )
   else
      SC_DX%y%c_obj%fromSC_Len = 0
      SC_DX%y%c_obj%fromSC     = C_NULL_PTR
   end if

   if( (NumSC2CtrlGlob > 0) .or. (NumSC2Ctrl > 0) .or. (NumSC2Ctrl > 0)) then
      SC_DX%p%UseSC = .true.
   else
      SC_DX%p%UseSC = .false.
   end if

   RETURN

END SUBROUTINE SC_DX_Init

!----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE SC_DX_SetInputs(p_FAST, y_SrvD, SC_DX, ErrStat, ErrMsg )
!..................................................................................................................................

   TYPE(FAST_ParameterType),       INTENT(IN    )  :: p_FAST      ! Parameters for the glue code
   TYPE(SrvD_OutputType),          INTENT(IN)      :: y_SrvD      ! The outputs of the ServoDyn module (control)
   TYPE(SCDataEx_Data),            INTENT(INOUT)   :: SC_DX       ! data for the SuperController integration module
   INTEGER(IntKi),                 INTENT(  OUT)   :: ErrStat     ! Error status of the operation
   CHARACTER(*),                   INTENT(  OUT)   :: ErrMsg      ! Error message if ErrStat /= ErrID_None

   ! local variables
   INTEGER(IntKi)                                  :: ErrStat2    ! temporary Error status of the operation
   CHARACTER(ErrMsgLen)                            :: ErrMsg2     ! temporary Error message if ErrStat /= ErrID_None

   CHARACTER(*),   PARAMETER                       :: RoutineName = 'SC_DX_SetInputs'

   ErrStat = ErrID_None
   ErrMsg  = ""

      ! set SuperController inputs
   if (SC_DX%p%UseSC) then
      if (allocated(y_SrvD%toSC).and. associated(SC_DX%u%toSC)) SC_DX%u%toSC = y_SrvD%toSC
   end if

END SUBROUTINE SC_DX_SetInputs
!----------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE SC_DX_SetOutputs(p_FAST, u_SrvD, SC_DX, ErrStat, ErrMsg )
!..................................................................................................................................

   TYPE(FAST_ParameterType),       INTENT(IN    )  :: p_FAST      ! Parameters for the glue code
   TYPE(SrvD_InputType),           INTENT(INOUT)   :: u_SrvD      ! The inputs of the ServoDyn module (control)
   TYPE(SCDataEx_Data),            INTENT(IN   )   :: SC_DX       ! data for the SuperController integration module
   INTEGER(IntKi),                 INTENT(  OUT)   :: ErrStat     ! Error status of the operation
   CHARACTER(*),                   INTENT(  OUT)   :: ErrMsg      ! Error message if ErrStat /= ErrID_None

   ! local variables
   INTEGER(IntKi)                                  :: ErrStat2    ! temporary Error status of the operation
   CHARACTER(ErrMsgLen)                            :: ErrMsg2     ! temporary Error message if ErrStat /= ErrID_None

   CHARACTER(*),   PARAMETER                       :: RoutineName = 'SC_DX_SetOutputs'

   ErrStat = ErrID_None
   ErrMsg  = ""

      ! set SuperController inputs
   if (SC_DX%p%UseSC) then
      if (allocated(u_SrvD%fromSC)    .and. associated(SC_DX%y%fromSC))     u_SrvD%fromSC     = SC_DX%y%fromSC
      if (allocated(u_SrvD%fromSCglob).and. associated(SC_DX%y%fromSCglob)) u_SrvD%fromSCglob = SC_DX%y%fromSCglob
   end if

END SUBROUTINE SC_DX_SetOutputs
!----------------------------------------------------------------------------------------------------------------------------------
END MODULE SC_DataEx
!**********************************************************************************************************************************

max_stars_repo_name: rcorniglion/openfast

max_issues_repo_name: rcorniglion/openfast

max_forks_repo_name: rcorniglion/openfast