Initialize submarine state more explicitly
Make output parameters pointers, test input/output in main