Various bugfixes from moving the code

This code was originally in py_cgwfs_ao.py, needs some adaptation.