Cleaned Code

New method for wrap around. TODO: Snap to grid