Algorithm revised and commented. I hope its fine now :)