Implemented test function (untested), a few fixes
Merge branch 'colortrack' of https://github.com/jlfranco/gts into colortrack