Fix FORPI bug

Also adds test case
Add tests