Fixed p and q being the same
Improved and got semi-working