Fix bug in inner loop and add swap function