Bug fixes and code cleanup.

Signed-off-by: Arica Liu <aricaliu@wpi.edu>