WIP: some more fixes and function converted from C to Cython