fix bugs, and make it compiled. Should be tested with uart