Fixed firmware. Littered with debug comments for now