Wrote a better algorithm that should be unbeatable + tests

Signed-off-by: danjamin <e051ba3be@gmail.com>