Fix bug in algorithm... Casting issues...