Added more serial debug, refactoring