PI-algoritme by Bert Bergs