Fixed parser bug with sub stp command.
Added debug code and changed braces to BSD style in TokenParser.h