get_interpolation_grid_indices Subroutine

subroutine get_interpolation_grid_indices(pos, ind, input_coords)
    !+ Find closest [[libfida:inter_grid]] indices `ind` to position `pos`
    real(Float64),  dimension(3), intent(in)  :: pos
        !+ Position [cm]
    integer(Int32), dimension(3), intent(out) :: ind
        !+ Closest indices to position
    integer, intent(in), optional             :: input_coords
        !+ Indicates coordinate system of `pos`. Beam grid (0), machine (1) and cylindrical (2)

    real(Float64),  dimension(3) :: mini, differentials, loc
    integer(Int32), dimension(3) :: maxind
    integer :: i, ics

    if(present(input_coords)) then
        ics = input_coords
    else
        ics = 2
    endif

    if(ics.eq.1) then
        loc(1) = sqrt(pos(1)*pos(1) + pos(2)*pos(2))
        loc(2) = pos(3)
        loc(3) = atan2(pos(2),pos(1))
    endif

    if(ics.eq.2) then
        loc(1) = pos(1)
        loc(2) = pos(2)
        loc(3) = pos(3)
    endif

    maxind(1) = inter_grid%nr
    maxind(2) = inter_grid%nz
    maxind(3) = inter_grid%nphi

    mini(1) = minval(inter_grid%r)
    mini(2) = minval(inter_grid%z)
    mini(3) = minval(inter_grid%phi)

    differentials(1) = inter_grid%dr
    differentials(2) = inter_grid%dz
    differentials(3) = inter_grid%dphi

    do i=1,3
        ind(i) = floor((loc(i)-mini(i))/differentials(i)) + 1
        if (ind(i).gt.maxind(i)) ind(i)=maxind(i)
        if (ind(i).lt.1) ind(i)=1
    enddo

end subroutine get_interpolation_grid_indices