Fixed plotting math and added comments.

I simulated the changes I made in MATLAB, but I haven't tested it yet.