Fix some bugs in reimplementation of function