First shot at mean shift algorithm. Currently doesnt work