Have a look at the code posted in this thread, in particular post #16 I think there might be something you can use in there, a tidy little ring buffer perhaps.
http://www.picbasic.co.uk/forum/showthread.php?t=4972