Fixed Worm-Display Algorithm

Fixed the annoying worm-curve bug and simplified the algorithm for
displaying the worm on the screen.