#include <Servo.h>

#define step1Pin 4
#define dir1Pin 5
#define step2Pin 2
#define dir2Pin 3
#define serPot1Pin A0
#define serPot2Pin A1
#define serPot3Pin A2
#define stepperPot1Pin A3
#define stepperPot2Pin A4
#define servo1Pin 9
#define servo2Pin 10
#define servo3Pin 11


const int microStepStepper1 = 1; //Full step is 1, Half step is 2, Quater step is 4, and so forth
const int microStepStepper2 = 1; //Full step is 1, Half step is 2, Quater step is 4, and so forth
int offsetStepper1 = 2;
int offsetStepper2 = 2;
const int stepSpeed = 10;
int serPot1Mapped;
int serPot2Mapped;
int serPot3Mapped;
int stepperPot1Mapped;
int stepperPot2Mapped;
int serPot1Target;
int serPot2Target;
int serPot3Target;
int stepperPot1Target;
int stepperPot2Target;

Servo elbowJointServo;
Servo wristJointServo;
Servo handJointServo;

void setup() {
  Serial.begin(9600);

  pinMode(dir1Pin, OUTPUT);
  pinMode(step1Pin, OUTPUT);
  pinMode(dir2Pin, OUTPUT);
  pinMode(step2Pin, OUTPUT);

  elbowJointServo.attach(servo1Pin);
  wristJointServo.attach(servo2Pin);
  handJointServo.attach(servo3Pin);

  while (offsetStepper1 > 0) {
    offsetStepper1--;
    digitalWrite(dir1Pin, HIGH);
    digitalWrite(step1Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step1Pin, LOW);
    delay(stepSpeed);
  }
  
  while (offsetStepper2 > 0) {
    offsetStepper2--;
    digitalWrite(dir2Pin, HIGH);
    digitalWrite(step2Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step2Pin, LOW);
    delay(stepSpeed);
  }
}

void loop() {
  serPot1Mapped = map(analogRead(serPot1Pin), 0, 1023, -1000, 1000);
  serPot2Mapped = map(analogRead(serPot2Pin), 0, 1023, -1000, 1000);
  serPot3Mapped = map(analogRead(serPot3Pin), 0, 1023, -1000, 1000);
  stepperPot1Mapped = map(analogRead(stepperPot1Pin), 0, 1023, 10, -10);
  stepperPot2Mapped = map(analogRead(stepperPot2Pin), 0, 1023, -10, 10);

  //Servo Control
  if (serPot1Mapped < -1) {
    serPot1Target = serPot1Target + (serPot1Mapped/100);
  }
  if (serPot1Mapped > 1) {
    serPot1Target = serPot1Target + (serPot1Mapped/100);
  }
  if (serPot1Target < 0) {
    serPot1Target = 0;
  }
  if (serPot1Target > 180) {
    serPot1Target = 180;
  }
  if (serPot2Mapped < -1) {
    serPot2Target = serPot2Target + (serPot2Mapped/100);
  }
  if (serPot2Mapped > 1) {
    serPot2Target = serPot2Target + (serPot2Mapped/100);
  }
  if (serPot2Target < 0) {
    serPot2Target = 0;
  }
  if (serPot2Target > 180) {
    serPot2Target = 180;
  }
  if (serPot3Mapped < -1) {
    serPot3Target = serPot3Target + (serPot3Mapped/100);
  }
  if (serPot3Mapped > 1) {
    serPot3Target = serPot3Target + (serPot3Mapped/100);
  }
  if (serPot3Target < 0) {
    serPot3Target = 0;
  }
  if (serPot3Target > 180) {
    serPot3Target = 180;
  }

  //Stepper Control
  if (stepperPot1Mapped < -5) {
    digitalWrite(dir1Pin, LOW);
    digitalWrite(step1Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step1Pin, LOW);
    delay(stepSpeed);
  }
  if (stepperPot1Mapped > 5) {
    digitalWrite(dir1Pin, HIGH);
    digitalWrite(step1Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step1Pin, LOW);
    delay(stepSpeed);
  }
  if (stepperPot2Mapped < -5) {
    digitalWrite(dir2Pin, LOW);
    digitalWrite(step2Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step2Pin, LOW);
    delay(stepSpeed);
  }
  if (stepperPot2Mapped > 5) {
    digitalWrite(dir2Pin, HIGH);
    digitalWrite(step2Pin, HIGH);
    delay(stepSpeed);
    digitalWrite(step2Pin, LOW);
    delay(stepSpeed);
  }

  elbowJointServo.write(serPot1Target);
  wristJointServo.write(serPot2Target);
  handJointServo.write(serPot3Target);
}