Fix crash
Some asserts
Use linear search for out of road points