clean up

mostly all cleaned up and it works. I just need to write a wrapper for SNOPT still.