Improve efficency

Also cleaned up the tests and demo program.