Added initial working algorithm