Having a working version of heirarchical state machine that matches the book's psuedo code. But I know of at least two logical bugs that occur in this code, so I will modify the code in the next commit away from the psuedo code to fix those bugs.