#include <AccelStepper.h>
#include <MultiStepper.h>

//global_val
int received_FB_Steps, received_LR_Steps = 0;
int directionMultiplier = 1; // cw
bool FB_runallowed, LR_runallowed = false;



//USED PINS
static const uint8_t JOYSTIC_LR = 16; //right=1023, left=0
static const uint8_t JOYSTIC_FB = 17; //up   =1023, down=0
static const uint8_t JOYSTIC_BT = 7;  //not pressed = 1

static const uint8_t POTI_LR = 14;
static const uint8_t POTI_FB = 15;

static const uint8_t DRIVER_EN = 8;
static const uint8_t STEPPER_LR_STEP = 3;
static const uint8_t STEPPER_LR_DIR = 4;
static const uint8_t STEPPER_FB_STEP = 6;
static const uint8_t STEPPER_FB_DIR = 5;

#define MS1 9 
#define MS2 10 
#define MS3 11 


#define motorInterfaceTyp 1
#define stepsPerRevolution 3200
AccelStepper FB_stepper(1, STEPPER_FB_STEP, STEPPER_FB_DIR);
AccelStepper LR_stepper(1, STEPPER_LR_STEP, STEPPER_FB_DIR);

//Max values
static const short maxPotiLR = 900;// left
static const short minPotiLR = 45;// right 

static const short maxPotiFB = 900;// at the front
static const short minPotiFB = 45; // at the Back
char receivedCommand;
bool newData = false;

// Functions
void check_serial();
void Poti_val();
void Steps_val();
void Run_FB_stepper();
void homing_FB_stepper();
void Backward();
void Forward();
void Left();
void Right();
void goHome();
void joystik_play();
void move_FB_LR_stepper();

void setup() {
  // put your setup code here, to run once:
  //Define type of connection
  pinMode(JOYSTIC_LR, INPUT);
  pinMode(JOYSTIC_FB, INPUT);
  pinMode(POTI_LR, INPUT);
  pinMode(POTI_FB, INPUT);
  pinMode(JOYSTIC_BT, INPUT);

  pinMode(DRIVER_EN, OUTPUT);
  pinMode(STEPPER_LR_STEP, OUTPUT);
  pinMode(STEPPER_LR_DIR, OUTPUT);
  pinMode(STEPPER_FB_STEP, OUTPUT);
  pinMode(STEPPER_FB_DIR, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);

  digitalWrite(MS1,HIGH);
  digitalWrite(MS2,HIGH);
  digitalWrite(MS3,HIGH);


  /*FB_stepper.setMaxSpeed(200);
  FB_stepper.setAcceleration(400);
  FB_stepper.disableOutputs();
  LR_stepper.setMaxSpeed(200);
  LR_stepper.setAcceleration(400);
  LR_stepper.disableOutputs();

  //enable stepper driver
  digitalWrite(DRIVER_EN, false);*/

  Serial.begin(9600);
  // homing_steppers();
}

void loop() {
  // put your main code here, to run repeatedly:

 /*check_serial(); // check the Serial command for code execution
 Run_FB_LR_stepper(); // for testing 
 joystik_play();// control the movement of the robot using the joystick
 goHome();// go home when the joystick button ist pressed
*/
 // Set the spinning direction clockwise:
  digitalWrite(STEPPER_FB_DIR, HIGH);

  // Spin the stepper motor 1 revolution slowly:
  for (int i = 0; i < stepsPerRevolution; i++) {
    // These four lines result in 1 step:
    digitalWrite(STEPPER_FB_STEP, HIGH);
    delayMicroseconds(2000);
    digitalWrite(STEPPER_FB_STEP, LOW);
    delayMicroseconds(2000);
  }

  delay(1000);

  // Set the spinning direction counterclockwise:
  digitalWrite(STEPPER_FB_DIR, LOW);

  // Spin the stepper motor 1 revolution quickly:
  for (int i = 0; i < stepsPerRevolution; i++) {
    // These four lines result in 1 step:
    digitalWrite(STEPPER_FB_STEP, HIGH);
    delayMicroseconds(1000);
    digitalWrite(STEPPER_FB_STEP, LOW);
    delayMicroseconds(1000);
  }

  delay(1000);

  // Set the spinning direction clockwise:
  digitalWrite(STEPPER_FB_DIR, HIGH);

  // Spin the stepper motor 5 revolutions fast:
  for (int i = 0; i < 5 * stepsPerRevolution; i++) {
    // These four lines result in 1 step:
    digitalWrite(STEPPER_FB_STEP, HIGH);
    delayMicroseconds(500);
    digitalWrite(STEPPER_FB_STEP, LOW);
    delayMicroseconds(500);
  }

  delay(1000);

  // Set the spinning direction counterclockwise:
  digitalWrite(STEPPER_FB_DIR, LOW);

  //Spin the stepper motor 5 revolutions fast:
  for (int i = 0; i < 5 * stepsPerRevolution; i++) {
    // These four lines result in 1 step:
    digitalWrite(STEPPER_FB_STEP, HIGH);
    delayMicroseconds(500);
    digitalWrite(STEPPER_FB_STEP, LOW);
    delayMicroseconds(500);
  }

  delay(1000);
  
}

/*void check_serial()
{
  if(Serial.available() > 0)
  {
    receivedCommand = Serial.read();
    newData =true;
    if (newData == true)
    {
      switch (receivedCommand)
      {
        case 'P':
        Poti_val();
        break;

        case 'S':
        Steps_val();
        break;

        case'M':
        move_FB_LR_stepper();
        break;
      }
    }
    newData = false;
  }
}
void Poti_val()
{
  Serial.print("Poti front;Back value = ");
  Serial.println(analogRead(POTI_FB));
  Serial.print("Joystick front;Back value = ");
  Serial.println(analogRead(JOYSTIC_FB));
  Serial.println("_______________________");
  Serial.print("Poti left;right value = ");
  Serial.println(analogRead(POTI_LR));
  Serial.print("Joystick left;right value = ");
  Serial.println(analogRead(JOYSTIC_LR));
  
}
void Steps_val()
{
  Serial.print("FB_stepper Steps: ");
  Serial.println(-FB_stepper.currentPosition());
   Serial.print("LR_stepper Steps: ");
  Serial.println(-LR_stepper.currentPosition());
}
void Run_FB_LR_stepper()
{
  if (FB_runallowed == true && LR_runallowed == true)
  {
    FB_stepper.enableOutputs();
    FB_stepper.run();
    LR_stepper.enableOutputs();
    LR_stepper.run();
  }
  else
  {
    LR_stepper.disableOutputs();
    LR_stepper.disableOutputs();
    return;
  }
}
void homing_steppers()
{
  while( analogRead(POTI_FB) > 420)
  {
    if(analogRead(JOYSTIC_FB)>500 && analogRead(JOYSTIC_FB)< 510)
    {
     Backward();
    }
  }
   while ( analogRead(POTI_FB )<420)
   {
     if( analogRead(JOYSTIC_FB)>500 && analogRead(JOYSTIC_FB)< 510)
     {
      Forward();
     }
   }
   while (analogRead(POTI_LR) > 420)
   {
    if ( analogRead(JOYSTIC_FB)>500 && analogRead(JOYSTIC_FB)< 510)
    {
      Left();
    }
   }
   while (analogRead(POTI_LR) < 420)
   {
    if ( analogRead(JOYSTIC_FB)>500 && analogRead(JOYSTIC_FB)< 510)
    {
      Right();
    }
   }
  FB_stepper.setCurrentPosition(0);
  LR_stepper.setCurrentPosition(0);
  Serial.println(" at starting point! ");
}
void Forward()
{
  digitalWrite(STEPPER_FB_DIR, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, false);
}

void Backward()
{
  digitalWrite(STEPPER_FB_DIR, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_FB_STEP, false);
}
void Left()
{
  digitalWrite(STEPPER_LR_DIR, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, false);
}
void Right()
{
  digitalWrite(STEPPER_LR_DIR, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, false);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, true);
  delayMicroseconds(1);
  digitalWrite(STEPPER_LR_STEP, false);
}
void goHome()
{
  if(digitalRead(JOYSTIC_BT)== 0)
  {
    homing_steppers();
  }
}
void joystik_play()
{
   if(analogRead(JOYSTIC_FB) > 502 + 100 && analogRead(POTI_FB) < maxPotiFB)
  { 
   Forward();
  }
  else if(analogRead(JOYSTIC_FB) < 502 - 100 && analogRead(POTI_FB) > minPotiFB)
  {
   Backward();
  } 
    
  if(analogRead(JOYSTIC_LR) > 502 + 100 && analogRead(POTI_LR) > minPotiLR)
  {
   Left();
  }
  else if(analogRead(JOYSTIC_LR) < 502 - 100 && analogRead(POTI_LR) < maxPotiLR)
  {
   Right();
  }
  delayMicroseconds(5);
}


void move_FB_LR_stepper()
{
  if( minPotiFB < analogRead(POTI_FB)< maxPotiFB)
  {
 
    if ( analogRead( POTI_FB) == minPotiFB || analogRead(POTI_FB) == maxPotiFB)
    {
      
      FB_stepper.disableOutputs();
      FB_stepper.stop(); 
    }
    else
    {
      FB_runallowed = true;
      LR_runallowed = true;
      received_FB_Steps = Serial.parseInt();
      received_LR_Steps = Serial.parseInt();
      FB_stepper.move(-1 * received_FB_Steps);
      LR_stepper.move(-1 * received_LR_Steps);
      
    }
  }
  
}*/