Fix some bugs and OMP optimization