re-write TP handler, removed index variable, made it a bit easier to understand.

Signed-off-by: Sleaker <sleaker@gmail.com>
Merge pull request #2160 from Sleaker/master

Handle TP Command a bit better