Initial commit: working algorithm