Adding more code

more code written, not tested on HW yet