Added a few const constraints
Tidied includes up
Merged loops
Added -h/--help switch