Cleaned up plotting and added testcases to verify