#include <AccelStepper.h>
#include <Servo.h>
#include <math.h>
#include <Stepper.h>

// Define the stepper motor and the pins that is connected to
AccelStepper stepper1(1, 2, 5); // (Typeof driver: with 2 pins, STEP, DIR)
AccelStepper stepper2(1, 3, 6);
AccelStepper stepper3(1, 4, 7);
AccelStepper stepper6(1, 8, 11);

// Define step constant
#define MotorInterfaceType 4

// Creates an instance
// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence
AccelStepper Stepper4(MotorInterfaceType, 41, 39, 37, 35);
AccelStepper Stepper5(MotorInterfaceType, 33, 31, 29, 27);
const int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution

Servo gripperServo1;  // create servo object to control a servo
Servo gripperServo2;  // create servo object to control a servo

double high, pose, pinches_bool;

const float onestep_indegree = 0.45;
const float onedegree_insteps = 2.22222;
const float high_motion_tosteps = 100; //1mm = 100steps
const float rail_motion_tosteps = 200/154;  //1mm = 1.3steps
const float closing_arms_belt_tosteps = 200/40;  //40mm = 200 steps

// for your motor
void setup() {

  Serial.begin(115200);

  // Stepper motors max speed
  stepper1.setMaxSpeed(4000);
  stepper1.setAcceleration(2000);
  stepper2.setMaxSpeed(4000);
  stepper2.setAcceleration(2000);
  stepper3.setMaxSpeed(4000);
  stepper3.setAcceleration(2000);
  stepper6.setMaxSpeed(4000);
  stepper6.setAcceleration(2000);

  // set the maximum speed, acceleration factor,
	// initial speed and the target position
	Stepper4.setMaxSpeed(4000.0);
	Stepper4.setAcceleration(50.0);
	Stepper4.setSpeed(400);
  Stepper5.setMaxSpeed(4000.0);
	Stepper5.setAcceleration(50.0);
	Stepper5.setSpeed(400);

  gripperServo1.attach(10);
  gripperServo2.attach(9);
  // initial servo value - open gripper

}

void loop() {
  //QUESTA PARTE È DA SOSTITUIRE CON LA COMUNICAZIONE DELL'ANTENNA!!!
  //--------------------------------------------------------------------------
  Serial.println("Please define the 3 entities for mobile robot: high, pose, loading on/off, (e.g. d/h/am/ac/f,int/bool):");
  while (!Serial.available()); // Wait for input
  String command = Serial.readStringUntil('\n');
  //--------------------------------------------------------------------------
  //IN USCITA DALLA COMUNICAZIONE DELL ANTENNA DEVE VENIRE FUORI UNA STRINGA DEL TIPO "120,200,90,50"
  Serial.println("Requested commands:");
  Serial.println(command);
  
  const char* commandchar = command.c_str();
  char *delimiter = ",";
  char* angles_str = strtok(commandchar, delimiter);
  char *command_type = angles_str;
  int command_int = atoi(strtok(NULL, delimiter));

  if (String(command_type) == String("d")){
    move_mobilerobot_rail_to(command_int);
  } else if(String(command_type) == String("h")){
    move_mobilerobothigh_to(command_int);
  } else if(String(command_type) == String("am")){
    move_mobilerobot_movearms(command_int);
  } else if (String(command_type) == String("ac")){
    mobilerobot_close_arms(command_int);
  } else if(String(command_type) == String("f")){
    open_close_hand(command_int);
  }
  Serial.println("position reached!");
  delay(500);
}


void move_mobilerobothigh_to(double high_des) {
  
  double steps1_des = int(high_des * high_motion_tosteps);
  double steps2_des = int(high_des * high_motion_tosteps);  

  stepper1.moveTo(steps1_des);
  stepper2.moveTo(steps2_des);

  while (stepper1.currentPosition() != steps1_des || stepper2.currentPosition() != steps2_des) {
    stepper1.run();
    stepper2.run();
  }
  delay(500);
}

void move_mobilerobot_rail_to(double pose_des){
  double steps3_des = int(pose_des * rail_motion_tosteps);
  stepper3.moveTo(steps3_des);
  while (stepper3.currentPosition() != steps3_des) {
    stepper3.run();
  }
  delay(500);
}

void move_mobilerobot_movearms(double loading_bool) {
  double steps_des = 0;
  if (loading_bool == 1) {
    steps_des = 200;
  } else {
    steps_des = -200;
  }

  Stepper4.moveTo(steps_des);
  Stepper5.moveTo(steps_des);
  
  while (Stepper4.currentPosition() != steps_des || Stepper5.currentPosition() != steps_des){
	  // Move the motor one step
	  Stepper4.run();
    Stepper5.run();
  }
  delay(1000);
}

void mobilerobot_close_arms(double motion_des){
  double steps6_des = int(motion_des * closing_arms_belt_tosteps);
  stepper6.moveTo(steps6_des);
  while (stepper6.currentPosition() != steps6_des) {
    stepper6.run();
  }
  delay(500);
}

void open_close_hand(double on_off){
  if (on_off == 1){
    gripperServo1.write(180);
    gripperServo2.write(180);
  } else {
    gripperServo1.write(0);
    gripperServo2.write(0);
  }
  delay(1000);
}