//Libraries//
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <AFMotor.h>
#include <Servo.h>
#include <AccelStepper.h>

//Stepper Motors Definitions//
#define ShouDirPin 39
#define ShouStepPin 37
#define ElbDirPin 34
#define ElbStepPin 35
#define motorInterfaceType 1

//Stepper Motors//
AccelStepper Shoulder = AccelStepper(motorInterfaceType, ShouStepPin, ShouDirPin);
AccelStepper Elbow = AccelStepper(motorInterfaceType, ElbStepPin, ElbDirPin);

//Motor Shield Motors//
AF_DCMotor motor1(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor motor3(3, MOTOR12_64KHZ); // create motor #3, 64KHz pwm
AF_DCMotor motor4(4, MOTOR12_64KHZ); // create motor #4, 64KHz pwm

//Servo Motors//
Servo servo1;
Servo servo2;

//Vex Motor A connections//
int enA = 44;
int in1 = 30;
int in2 = 31;
//Vex Motor B connections//
int enB = 45;
int in3 = 33;
int in4 = 32;

//Other//
int wrist_1;
int wrist_2;
int elbowPot;
int shoulderPot;
int dir;
int VexMotAspeed = 0;
int Array[11];
int randNum;
int Stepperspeed = pow(10,5);

//Transeiver//
const byte address[6] = "00005";
RF24 radio(28, 29);
//////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
//Servo setup//
  servo1.attach(9);
  servo2.attach(10);
  
//set up Serial library at 9600 bps//
  Serial.begin(9600);
  Serial.println("Motor test!");

//Motor shield setup//
  motor1.setSpeed(150);
  motor2.setSpeed(150);
  motor3.setSpeed(200);
  motor4.setSpeed(255);

//Transeiver setup//
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();

//Acceleration and speed setup for stepper motors//
  Elbow.setMaxSpeed(Stepperspeed);
  Elbow.setAcceleration(Stepperspeed);
  Shoulder.setMaxSpeed(Stepperspeed);
  Shoulder.setAcceleration(Stepperspeed);

//Set all the motor control pins to outputs//
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

//Turn off motors - Initial state//
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
//Checks if transeivers are connected and runs the control cade if it is//
  if (radio.available()) {
    
//Reads values from controls system//
    radio.read(&Array, sizeof(Array));

//Arrays from the controls system//
    Array[0]; // Pot #1
    Array[1]; // Pot #2
    Array[2]; // Pot #3
    Array[3]; // Pot #4
    Array[4]; // Button #1
    Array[5]; // Button #2
    Array[6]; // Button #3
    Array[7]; // Button #4
    Array[8]; // Button #5
    Array[9]; // Button #6
    Array[10]; // Switch

//Mapping values from the control system to fit the robot//
    wrist_1 = map(Array[0], 0, 1023, 0, 180);
    wrist_2 = map(Array[1], 0, 1023, 0, 180);
    shoulderPot = map(Array[3], 0, 1023, 0, -3200);

    if (Array[2] <= 30)
        elbowPot = map(Array[2], 15, 30, 10000, 0);
    else if (Array[2] > 30)
        elbowPot = 0;
    else if (Array [2] < 15)
        elbowPot = 10000;

//Prints values for analysis//
    Serial.print(" Mega PotVal_1: ");
    Serial.print(wrist_1);
    Serial.print(" Mega PotVal_2: ");
    Serial.print(wrist_2);
    Serial.print(" Mega PotVal_3: ");
    Serial.print(elbowPot);
    Serial.print(" Mega PotVal_4: ");
    Serial.print(shoulderPot);
    Serial.print(" Button_1: ");
    Serial.print(Array[4]);
    Serial.print(" Button_2: ");
    Serial.print(Array[5]);
    Serial.print(" Button_3: ");
    Serial.print(Array[6]);
    Serial.print(" Button_4: ");
    Serial.print(Array[7]);
    Serial.print(" Button_5: ");
    Serial.print(Array[8]);
    Serial.print(" Button_6: ");
    Serial.print(Array[9]);
    Serial.print(" Switch: ");
    Serial.println(Array[10]);

//DC motors if statements that uses the values from the control system//
    if (Array[10] == 0) dir = 1;
    else dir = 0;

    if (Array [9] == 1)
      vexMotA();     
    else{
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);}

    if (Array [8] == 1)
      vexMotB();
    else{
      digitalWrite(in3, LOW);
      digitalWrite(in4, LOW);}
      
    if (Array [7] == 1)
      indexFinger();
    else
      motor2.run(RELEASE);
      
    if (Array [6] == 1)
      midFinger();
    else
      motor3.run(RELEASE);     

    if (Array [5] == 1)
      ringFinger();
    else
      motor1.run(RELEASE);
      
    if (Array [4] == 1)
      pinkyFinger();
    else
      motor4.run(RELEASE);

//Servo and stepper motor 
    servo1.write(wrist_1);
    servo2.write(wrist_2);
    Shoulder.moveTo(shoulderPot);
    Shoulder.runToPosition();
    Elbow.moveTo(elbowPot);
    Elbow.runToPosition();

    delay(10);
  }
//If the transeivers do not connect, then run the rock-paper-scissors code//
  else RockPaperScissors();
}