Add demo program, update readme + fix minor logic errors