/*
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>. 
*/

#define SerialPort Serial1
const byte PIN_ENC_A = 5;
const byte PIN_ENC_B = 13;
const byte PIN_ENC_X = A3;
const byte PIN_BTN_A = A0;
const byte PIN_BTN_B = A1;
const byte PIN_BTN_C = A2;
const byte PIN_LED_A = A4;
const byte PIN_LED_B = A5;
//const byte PIN_LED_C = RXLED;

void setup() {
  pinMode(PIN_ENC_A, INPUT);
  pinMode(PIN_ENC_B, INPUT);  
  pinMode(PIN_ENC_X, INPUT);
  pinMode(PIN_BTN_A, INPUT);  
  pinMode(PIN_BTN_B, INPUT);  
  pinMode(PIN_BTN_C, INPUT);  
  pinMode(PIN_LED_A, OUTPUT);  
  pinMode(PIN_LED_B, OUTPUT);  
//  pinMode(PIN_LED_C, OUTPUT);  
  SerialPort.begin(9600);
}

char encoderDelta() {
  static char last_state = 0;
  static word rotation = 0;
  static word rotation_mem = 0;
  char state = (digitalRead(PIN_ENC_A)) << 1 | digitalRead(PIN_ENC_B);
  char delta;
  state = state ^ (state >> 1);
  if(state == last_state) delta = 0;
  if(state == ((last_state + 1) & 3)) delta = 1;
  if(state == ((last_state + 3) & 3)) delta = -1;
  last_state = state;
  rotation += delta;
  char xdelta = rotation - rotation_mem;
  xdelta = abs(xdelta) > 1 ? ((xdelta > 0) ? 1 : -1) : 0;
  rotation_mem += xdelta;
  return xdelta;
}

void loop() {
  static word rotation = 0;
  char delta = encoderDelta();
  if(delta) {
    rotation += delta;
    SerialPort.print("Rotation: ");
    SerialPort.println(rotation);
  }

  static byte buttons = 0;
  byte buttonbits = ((!digitalRead(PIN_BTN_A)) << 2)
                  | ((!digitalRead(PIN_BTN_B)) << 1)
                  | ((!digitalRead(PIN_BTN_C)) << 0);

  if(buttonbits != buttons) {
    SerialPort.print("Buttons: ");
    SerialPort.write((buttonbits & 4) ? 'A' : '-');
    SerialPort.write((buttonbits & 2) ? 'B' : '-');
    SerialPort.write((buttonbits & 1) ? 'C' : '-'); 
    SerialPort.println("");
  }
  buttons = buttonbits;

  while(SerialPort.available()) {
    byte x = SerialPort.read();
    digitalWrite(PIN_LED_A, !(x & 1));
    digitalWrite(PIN_LED_B, !(x & 2));  
  }
//   SerialPort.write("Hello");
}
