/*
This robotic arm is the same as my other one but now it must use stepper motors for 
the hip, shoulder and elbow jounts, and servos for the wrist an gripper (finger/thumb)

All movements must be controlled by joysticks (2 in total) and buttons (3 in total)
joystick 1 will move hip and shoulder
joystick 2 will elbow and wrist
button 1 is for recording (recording == !recording)
button 2 is for playback (playback == !playback)
and button 3 is for resetting position of the wrist to the middle

*/

#include <Arduino.h>
#include <AccelStepper.h>
#include <Servo.h>

// Define pins for joystick and buttons
#define JOYSTICK1_X_PIN A0
#define JOYSTICK1_Y_PIN A1
#define JOYSTICK2_X_PIN A4
#define JOYSTICK2_Y_PIN A5

#define RECORD_BUTTON_PIN A2
#define PLAYBACK_BUTTON A3


// Define pins for stepper drivers
#define STEPPER1_STEP_PIN 2
#define STEPPER1_DIR_PIN 3
#define STEPPER2_STEP_PIN 4
#define STEPPER2_DIR_PIN 5
#define STEPPER3_STEP_PIN 6
#define STEPPER3_DIR_PIN 7
//#define RESET 13 // Pin 13 connected to RST pin
//#define SLEEP 12     // Pin 12 connected to SLEEP pin

// Define pins for servos
#define SERVO1_PIN 9
#define SERVO2_PIN 10

// Define variables for stepper drivers
AccelStepper stepper1(AccelStepper::DRIVER, STEPPER1_STEP_PIN, STEPPER1_DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN);
AccelStepper stepper3(AccelStepper::DRIVER, STEPPER3_STEP_PIN, STEPPER3_DIR_PIN);

// Define variables for servos
Servo servo1;
Servo servo2;

// Define variables for joystick and buttons
int joystick1XVal = 0;
int joystick1YVal = 0;
int joystick2XVal = 0;
int joystick2YVal = 0;
bool recordButton = false;
bool playbackButton = false;

// Define variables for recording and playing back movements
const int MAX_MOVEMENTS = 10; // Maximum number of movements to record
int recordedMovements[MAX_MOVEMENTS][7]; // Array to store recorded movements
int currentMovementIndex = 0; // Index of the current movement being recorded or played back

void setup() {
  // Set up pins for stepper drivers
  pinMode(STEPPER1_STEP_PIN, OUTPUT);
  pinMode(STEPPER1_DIR_PIN, OUTPUT);
  pinMode(STEPPER2_STEP_PIN, OUTPUT);
  pinMode(STEPPER2_DIR_PIN, OUTPUT);
  pinMode(STEPPER3_STEP_PIN, OUTPUT);
  pinMode(STEPPER3_DIR_PIN, OUTPUT);

  /*
    pinMode(SLEEP, OUTPUT);

    digitalWrite(SLEEP, HIGH);  // Wake up EasyDriver
     delay(5);  // Wait for EasyDriver wake up

     pinMode(RESET, OUTPUT);
    digitalWrite(RESET, HIGH);
  */

  // Set up pins for servos
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  // Set up pins for joystick and buttons
  pinMode(JOYSTICK1_X_PIN, INPUT);
  pinMode(JOYSTICK1_Y_PIN, INPUT);
  pinMode(JOYSTICK2_X_PIN, INPUT);
  pinMode(JOYSTICK2_Y_PIN, INPUT);

  pinMode(RECORD_BUTTON_PIN, INPUT_PULLUP);
  pinMode(PLAYBACK_BUTTON, INPUT_PULLUP);

  // Set up stepper drivers
  stepper1.setMaxSpeed(200);
  stepper2.setMaxSpeed(200);
  stepper3.setMaxSpeed(200);
}

void loop() {
  // Read joystick and button states
  joystick1XVal = analogRead(JOYSTICK1_X_PIN);
  joystick1YVal = analogRead(JOYSTICK1_Y_PIN);
  joystick2XVal = analogRead(JOYSTICK2_X_PIN);
  joystick2YVal = analogRead(JOYSTICK2_Y_PIN);


  recordButton = digitalRead(RECORD_BUTTON_PIN);
  playbackButton = digitalRead(PLAYBACK_BUTTON);

  // Move the robot arm based on joystick and button states
  int stepper1Steps = map(joystick1XVal, 0, 1023, -200, 200);
  int stepper2Steps = map(joystick1YVal, 0, 1023, -200, 200);
  int stepper3Steps = map(joystick2XVal, 0, 1023, -200, 200); // *****

  
  stepper1.move(stepper1Steps);
  stepper2.move(stepper2Steps);
  stepper3.move(stepper3Steps);
  stepper1.run();
  stepper2.run();
  stepper3.run();

  // Record or play back movements
  if (recordButton && !playbackButton) {
    // Record a movement
    if (currentMovementIndex < MAX_MOVEMENTS) {
      recordedMovements[currentMovementIndex][0] = joystick1XVal;
      recordedMovements[currentMovementIndex][1] = joystick1YVal;
      recordedMovements[currentMovementIndex][2] = joystick2XVal; // *****
      recordedMovements[currentMovementIndex][3] = joystick2YVal;

      recordedMovements[currentMovementIndex][4] = recordButton;
      recordedMovements[currentMovementIndex][5] = playbackButton;
      recordedMovements[currentMovementIndex][6] = 0; // Placeholder for servo positions
      currentMovementIndex++;


    }

  } else if (!recordButton && playbackButton) {
    // Play back recorded movements
    for (int i = 0; i < currentMovementIndex; i++) {
      joystick1XVal = recordedMovements[i][0];
      joystick1YVal = recordedMovements[i][1];
      joystick2XVal = recordedMovements[i][2]; // *****
      joystick2YVal = recordedMovements[i][3];


      recordButton = recordedMovements[i][4];
      playbackButton = recordedMovements[i][5];


      int servo1Pos = recordedMovements[i][6];
      int servo2Pos = 180 - servo1Pos; // Servo 2 is mounted in opposite direction to servo 1
      servo1.write(servo1Pos);
      servo2.write(servo2Pos);
      stepper1Steps = map(joystick1XVal, 0, 1023, -200, 200);
      stepper2Steps = map(joystick1YVal, 0, 1023, -200, 200);
      stepper3Steps = map(joystick2YVal, 0, 1023, -200, 200);

      
      stepper1.moveTo(stepper1Steps);
      stepper2.moveTo(stepper2Steps);
      stepper3.moveTo(stepper3Steps);
      while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning()) {
        stepper1.run();
        stepper2.run();
        stepper3.run();
      }
      delay(500); // Delay between movements
    }
  }

  // Control servos based on joystick position
  int servo1Pos = map(joystick1XVal, 0, 1023, 0, 180);
  int servo2Pos = 180 - servo1Pos; // Servo 2 is mounted in opposite direction to servo 1
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);
}