This function needs a complete rewrite. Will do it after your PR is done.