Got the algorihm working for greyscale, color next. Working on CV stuff now