Added test harness, fixed small bugs