Fix bugs, add functional test