// Need to define pins for servos

#include <Keypad.h>
#include <Servo.h>

String memory = "";

Servo B; // Create a "Servo" object called "B"
Servo A; // Create a "Servo" object called "A"
Servo G; // Create a "Servo" object called "G"
Servo nF; // Create a "Servo" object called "F"
Servo E; // Create a "Servo" object called "E"
Servo D; // Create a "Servo" object called "D"
Servo C; // Create a "Servo" object called "C"

float off = 0.0; // Variable where the arm's position will be stored (in degrees)
float on = 90.0; // Variable used for the arm's position step

/* Keypad setup */
const byte KEYPAD_ROWS = 4;
const byte KEYPAD_COLS = 3;
byte rowPins[KEYPAD_ROWS] = {22, 21, 20, 19};
byte colPins[KEYPAD_COLS] = {18, 17, 16};
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'A', 'B', 'C'},
  {'D', 'E', 'F'},
  {' ', 'G', ' '},
  {'b', '=', '#'}
};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);

void setup() {
  // put your setup code here, to run once:

  B.attach(2); // Attach the B servo to the pin 2
  B.write(off); // Initialize the B arm's position to 0 (rightmost)

  A.attach(3); 
  A.write(off); 

  G.attach(4); 
  G.write(off); 

  nF.attach(5); 
  nF.write(off); 

  E.attach(6); 
  E.write(off); 

  D.attach(7); 
  D.write(off); 

  C.attach(8); 
  C.write(off); 

  Serial1.begin(115200);
  Serial1.println("Type in a note!");
}

void loop() {
  // put your main code here, to run repeatedly:

  char key = keypad.getKey();

  if (key != NO_KEY) {
    switch (key) {
      case 'A': 
        Serial1.println("A");
        B.write(on);
        A.write(on);
        G.write(off);
        nF.write(off);
        E.write(off);
        D.write(off);
        C.write(off);
        break;
      case 'B': 
        Serial1.println("B");
        B.write(on);
        A.write(off);
        G.write(off);
        nF.write(off);
        E.write(off);
        D.write(off);
        C.write(off);
        break;
      case 'C': 
        Serial1.println("C");
        B.write(off);
        A.write(on);
        G.write(off);
        nF.write(off);
        E.write(off);
        D.write(off);
        C.write(off);
        break;
      case 'D': 
        Serial1.println("D");
        B.write(on);
        A.write(on);
        G.write(on);
        nF.write(on);
        E.write(on);
        D.write(on);
        C.write(off);
        break;
      case 'E': 
        Serial1.println("E");
        B.write(on);
        A.write(on);
        G.write(on);
        nF.write(on);
        E.write(on);
        D.write(off);
        C.write(off);
        break;
      case 'F': 
        Serial1.println("F");
        B.write(on);
        A.write(on);
        G.write(on);
        nF.write(on);
        E.write(off);
        D.write(off);
        C.write(off);
        break;
      case 'G': 
        Serial1.println("G");
        B.write(on);
        A.write(on);
        G.write(on);
        nF.write(off);
        E.write(off);
        D.write(off);
        C.write(off);
        break;
    }
  }
  //  if (key != NO_KEY) {
  //    Serial1.println(key);
  //    memory = memory + key;
  //  }
  
  //delay(1000); // this speeds up the simulation
  //String memory = "";
}