int Blank = 0;
//Begin Pin Designation
int STBY = 11; //standby
int PWMA = 10; //Speed control

// {Pin1, Pin2, Position}
int motorPins[5][3] = {{13, 12, 0}, {8, 9, 0}, {7, 6, 0}, {5, 6, 0}, {2, 3, 0}};

int inByte = Serial.read();

//End Pin Designation

//Define Pin Designations for Fingers
void setup() {
    pinMode(STBY, OUTPUT);
    pinMode(PWMA, OUTPUT);
    for (int i = 0; i  < 5; i++){
        pinMode(motorPins[i][0], OUTPUT);
        pinMode(motorPins[i][1], OUTPUT);
        move(i, 255, 1, true);
        motorPins[i][3] = 1;
    }
    delay(2300); //go for 1 second
    //digitalWrite(STBY, LOW);
    Standby(); //stop
    Serial.begin(9600);   // We'll send debugging information via the Serial monitor
    int incomingByte = 0;
}

void move(int motor, int speed, int direction) {
  move(motor, speed, direction, false);
}

void move(int motor, int speed, int direction, boolean override) {
    if (!override && direction == motorPins[motor][2]) {
        digitalWrite(motorPins[motor][0], LOW);
        digitalWrite(motorPins[motor][1], LOW);
        return;
    }

    boolean inPin1 = LOW;
    boolean inPin2 = HIGH;

    if (direction == 1) {
        inPin1 = HIGH;
        inPin2 = LOW;
    }

    digitalWrite(STBY, HIGH); //disable standby
    Serial.print("Motor ");
    Serial.print(motor);
    Serial.print(" Is moving, POS is set to ");
    Serial.println(motorPins[motor][2]);
    digitalWrite(motorPins[motor][0], inPin1);
    digitalWrite(motorPins[motor][1], inPin2);
    analogWrite(PWMA, speed);
}

void Standby() {
    //enable standby
    digitalWrite(STBY, LOW);
}

void SyPrint(){
    for (int i = 1; i  < 5; i++){
        Serial.print("Motor POS " );
        Serial.print(i);
        Serial.print(" = " );
        Serial.print(motorPins[i][2]);
        Serial.print(" ");
    }

    Serial.println(" ");
}


void loop(void) {
    Standby(); //stopE
    if (Serial.available() > 0) {

        int inByte = Serial.read();
        switch (inByte) {

            case 'A'://Close
                for (int i = 1; i< 5; i++){
                    move(i, 255, 0);
                }
                delay(2100); //go for 1 second
                //Standby(); //stopE
                SyPrint();
                break;

            case 'B': //Open
                for (int i = 2; i< 6; i++){
                    move(i, 255, 1);
                }
                delay(2300); //go for 1 second
                //Standby(); //stop
                SyPrint();
                break;

            case 'C':// Slow Close
                for (int i = 2; i< 6; i++){
                    move(i, 155, 0);
                }
                delay(3000); //go for 1 secondB
                //Standby(); //stop
                break;

            case 'D'://Point
                for (int i = 2; i< 5; i++){
                    move(i, 255, 0);
                }
                move(5, 255, 1);
                delay(2300); //go for 1 second
                //Standby(); //stop
                break;

            case 'E': //LongHorns
                move(5, 255, 0); //motor 5, full speed, close //speed set
                move(4, 255, 1); //motor 2, full speed, close
                move(3, 255, 0); //motor 3, full speed, close
                move(2, 255, 1); //motor 4, full speed, close
                delay(2300); //go for 1 second
                //Standby(); //stop

                break;

                // default: // turn all the motors off:
                //  Standby(); //stop
        }
    }
}