working version, for modes 1 and 2, cleaned up a little. mode 3 still needs to be setup, but working for now.