Basic algorithm, needs optimization