/*
  Stepper Motor Demonstration 4
  Stepper-Demo4.ino
  Demonstrates NEMA 17 Bipolar Stepper with A4988 Driver

  DroneBot Workshop 2018
  https://dronebotworkshop.com
*/
#include <EEPROM.h>
// Define Constants

// Connections to A4988 1 filament extruder
const int dirPin = 2;  // Direction 1
const int stepPin = 3; // Step 1
// Connections to A4988 2 filament selector
const int dirPin2 = 4;  // Direction 2
const int stepPin2 = 5; // Step 2
//Microswitch
const int buttonPin = 6; //microswitch
int mode = 0;
int count = 0;
int precount = 0; 
int home = 0;
int steps = 0;
//no filament in ptfe tube to extruder
int filament1 = 0;
int filament2 = 0;
int filament3 = 0;
int filament4 = 0;
int currentFilament = 0;
// Motor steps per rotation
const int STEPS_PER_REV = 200;

void checkFilamentState() {
   filament1 = EEPROM.read(0);
   filament2 = EEPROM.read(1);
   filament3 = EEPROM.read(2);
   filament4 = EEPROM.read(3);
   home = EEPROM.read(4);
   currentFilament = EEPROM.read(5);
}  


void retractCurrentFilament(){
    if(filament1 == 1){
      if (home > 0){
    //set motor 2 direction  clockwise
    digitalWrite(dirPin2, HIGH);
    // Spin motor 2 home steps slowly
    for (int x = 0; x < home; x++) {
     digitalWrite(stepPin2, HIGH);
     delayMicroseconds(3000);
     digitalWrite(stepPin2, LOW);
     delayMicroseconds(3000);
    }
    home = 0;
    EEPROM.update(4, home);
  }
  
  // Set motor direction anti clockwise
  digitalWrite(dirPin, LOW);
  
  // Spin motor one 2 rotations slowly
  for (int x = 0; x < STEPS_PER_REV * 2; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
  // Pause for one second
  delay(1000);
  filament1 = 0;
  EEPROM.update(0, filament1);
  currentFilament = 0;
  EEPROM.update(5,currentFilament); 
  // Set motor 2 direction counterclockwise
  digitalWrite(dirPin2, LOW);
  // Spin motor 2 a quarter rotations quickly
   for (int x = 0; x < 50; x++) {
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(5000);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(5000);
  }
  home = 50;
  EEPROM.update(4, home);
 }
    if(filament2 == 1){
      
    }
    if(filament3 == 1){
      
    }
    if(filament4 == 1){
      
    }
  
}
void setup() {
// Setup the pins as Outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  //setup switch as input
  pinMode(buttonPin, INPUT_PULLUP);
  digitalWrite(stepPin, LOW);
  digitalWrite(stepPin2, LOW);
  digitalWrite(buttonPin, HIGH);
  delay(1000);
  checkFilamentState();
  
}
void loop() {
  if (digitalRead(buttonPin) == HIGH) {
    if (count == 1){
      mode = 1;
      precount = count;
      count = 0;
      
    }
     if (count == 2){
      mode = 2;
      precount = count;
      count = 0;
      
    }
    if (count == 5){
      mode = 1;
      precount = count;
      count = 0;
    }
     if (count == 7){
      mode = 7;
      precount = count;
      count = 0;
    }
    if (precount != 1 and precount != 2 and precount != 5 and precount != 7){
       mode = 0;
       count = 0;
    }
   
   // if (count != 5){
   //   mode = 0;
   //   count = 0;
  //  }
  }
  if (digitalRead(buttonPin) == LOW) {
    mode = 0 ;
    precount = 0;
    digitalWrite(dirPin2, HIGH);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(10);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(10);
    delay(100);
    digitalWrite(dirPin2, LOW);
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(10);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(10);
    delay(400);
    count = count + 1;
    
  }
  if (mode == 1){  //select filament 1 and extrude
 // if (currentFilament != 1 || currentFilament != 0){
 //   retractCurrentFilament();
 // }
  if (home > 0){
    //set motor 2 direction  clockwise
    digitalWrite(dirPin2, HIGH);
    // Spin motor 2 home steps slowly
    for (int x = 0; x < home; x++) {
     digitalWrite(stepPin2, HIGH);
     delayMicroseconds(3000);
     digitalWrite(stepPin2, LOW);
     delayMicroseconds(3000);
    }
    home = 0;
    EEPROM.update(4, home);
  }
  
  // Set motor direction clockwise
  digitalWrite(dirPin, HIGH);
  
  // Spin motor one 2 rotations slowly
  for (int x = 0; x < STEPS_PER_REV * 2; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
  // Pause for one second
  delay(1000);
  filament1 = 1;
  EEPROM.update(0, filament1);
  currentFilament = 1;
  EEPROM.update(5, currentFilament);
  // Set motor 2 direction counterclockwise
  digitalWrite(dirPin2, LOW);
  // Spin motor 2 a quarter rotations quickly
   for (int x = 0; x < 50; x++) {
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(5000);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(5000);
  }
  home = 50;
  EEPROM.update(4, home);
  count = 0;
  mode = 0;
  }
  if (mode == 2){  //select filament 2 and extrude
  if (currentFilament != 2 || currentFilament != 0){
    retractCurrentFilament();
  }
  if (home != 50){
    if(home > 50){
    //set motor 2 direction  clockwise
    digitalWrite(dirPin2, HIGH);
    steps = home - 50;
    }
     if(home < 50){
    //set motor 2 direction anti clockwise
    digitalWrite(dirPin2, LOW);
    steps = 50 - home;
    }
    // Spin motor 2 home steps slowly
    for (int x = 0; x < steps; x++) {
     digitalWrite(stepPin2, HIGH);
     delayMicroseconds(3000);
     digitalWrite(stepPin2, LOW);
     delayMicroseconds(3000);
    }
    home = 50;
    EEPROM.update(4, home);
  }
  
  // Set motor direction clockwise
  digitalWrite(dirPin, HIGH);
  
  // Spin motor one 2 rotations slowly
  for (int x = 0; x < STEPS_PER_REV * 2; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
  // Pause for one second
  delay(1000);
  filament2 = 1;
  EEPROM.update(1, filament2);
  currentFilament = 2;
  EEPROM.update(5, currentFilament);
  // Set motor 2 direction clockwise
  digitalWrite(dirPin2, HIGH);
  // Spin motor 2 a quarter rotations quickly
   for (int x = 0; x < 50; x++) {
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(5000);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(5000);
  }
  home = 0;
  EEPROM.update(4, home);
  /*
  // Set motor direction counterclockwise
  digitalWrite(dirPin, LOW);

  // Spin motor two rotations quickly
  for (int x = 0; x < (STEPS_PER_REV * 2); x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
 */
  
  count = 0;
  mode = 0;
  }
  if (mode == 7){
    // Set motor 2 direction clockwise
    digitalWrite(dirPin2, HIGH);
    // Spin motor 2 one rotation slowly
   for (int x = 0; x < STEPS_PER_REV ; x++) {
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(3000);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(3000);
   }
   home = 0;
   EEPROM.update(4, home);
   count = 0;
   mode = 0;
  // Pause for one second
  //delay(1000);
  }
  //mode = 0;
  
}