Context switch: Remove duplicated logic to find the next context

We first try to find a context that has a pid greater than the current context. If no such context exists, we go through the list of context again to find a runnable context with a lower pid.

In this change we refactor the code that was almost identical and we cache the first runnable context with a lower pid to avoid going through the context list a second time if no runnable context with greater pid was found.