Ready for Touchpad, refactor

Probably doesn't work with gyro right now, still a lot TODO things