finished up algorithm, further testing needed