Cleaned up Code/Comments

Added in queue/Function pointer system.  Tested, worked 100%.  Needs to
be tested in real time.