improved algorithms
Merge branch 'feature-tspfix' into develop