New algorithm

Still has problems