Fixed typo.
Removed unneeded conversion.
Minor changes to code.

TODO: Something seems fishy about compare_pi_pj.
Fixed compare_pi_pj.

Also did some more inlining and added more tests.