Adds minor speedup changes
Fixes discretization bug. Adds some diagnostic tools. Adds minor fixes.