Fixed bug with calculating Z coords

Also moved caching to method
Moved saving