debugged the current code, has not tested on hardware