First step

Improved movement
Added debug gizmos