Rafactor to remove duplication in the implimentation of the movement stages of the algorithm
Add support to color theresholding from the trackbars, and delete useless code