Use for each loop
Remove unnecessary ctor
Add debug draw, code clean up