Improve router

A bunch of commits to improve the router robustness. Mostly fix const, add comments