Fixed version of the CSP algorithm.

The one from the original repository did not seem to work...