Ported code from c++ to python