#include <EEPROM.h>
// Define Constants
#define home_addr 0

#define F1_addr 1
#define F2_addr 2
#define F3_addr 3
#define F4_addr 4

#define current_addr 5
#define last_addr 6

// Connections to A4988 1 filament extruder
#define dirPin 6  // Direction 1
#define stepPin 3 // Step 1

// Connections to A4988 2 filament selector
#define dirPin2 5  // Direction 2
#define stepPin2 2 // Step 2

//Microswitch
#define buttonPin A2 //microswitch
#define enablePin 5

int mode = 0;
int count = 0;
int precount = 0; 
int home = false;
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;
int last_change = 0;

// Motor steps per rotation
#define STEPS_PER_REV 200

void checkFilamentState() {
   filament1 = EEPROM.read(F1_addr);
   filament2 = EEPROM.read(F2_addr);
   filament3 = EEPROM.read(F3_addr);
   filament4 = EEPROM.read(F4_addr);
   home = EEPROM.read(home_addr);
   currentFilament = EEPROM.read(current_addr);
   last_change = EEPROM.read(last_addr);
}  

void retractCurrentFilament(){
  Serial.println("Retract");
  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 go_home(){
  //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 = true;
  EEPROM.update(home_addr, home);
}

void setup() {
  Serial.begin(115200);
  
  // Setup the pins as Outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(enablePin, OUTPUT);
  
  //setup switch as input
  pinMode(buttonPin, INPUT_PULLUP);
  digitalWrite(stepPin, LOW);
  digitalWrite(stepPin2, LOW);
  digitalWrite(buttonPin, HIGH);
  digitalWrite(enablePin, HIGH);
  
  delay(1000);
  checkFilamentState();
  Serial.println(last_change);
  
}

void loop() {
  if (digitalRead(buttonPin) == HIGH) {
    if (count == 1){
      mode = 1;
      precount = count;
      count = 0;
      Serial.println("count = 1");
    }
     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 (count == 10){
      mode = 10;
      precount = count;
      count = 0;
      Serial.println("Home");
      go_home();
    }
    // if (precount != 1 and precount != 2 and precount != 5 and precount != 7){}
    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);
    if(count < 10){
      count++;
    }
    Serial.println(count);
    
  }



  /* 
  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;
  */
  
}