algorithm works, however p,q and q are wrong