removed smoothing because it was changing the goal
reorganized some code
added some zrot stuff