Working Sequential Version

Working Sequential Algorithm, working Opencv, not sure about CUDA