Added test code and BMS testing