#include <EEPROM.h>

// Stepper driver pins
#define DIR  6
#define STP  7

// 3 push buttons
#define RUN    11
#define CAL    12
#define STR    4

// Switch
#define HOME   9

// 2 LEDs
#define RunLed  10
#define CalLed  5

#define stepsPerRev  200   // How many steps per rev
#define retSpeed     1500  // Speed of the motor retracting in run mode (Lower = Faster)
#define extSpeed     2000  // Speed of the motor extending in run mode (Lower = Faster)
#define minCalSpeed  5000  // Minimum speed in calibration mode
#define maxCalSpeed  2000  // Maximum speed in calibration mode

#define Retracted  1
#define Extended   2

unsigned long retractedSteps = 50;   // Steps taken in reversev after hitting the home switch
unsigned long extendedSteps = 1500;  // Take this many steps to reach extended position (Default), unused if reading from memory
unsigned long currPosition = 0;
unsigned long lastButtonPress = 0;

int retDir = LOW;   // Direction of motor when retracting (Change according to requirement LOW or HIGH)
int extDir = HIGH;  // Direction of motor when extending (Change according to requirement LOW or HIGH)
int Position = 0;
int maxReading = 550;
int minReading = 490;

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

  pinMode(DIR, OUTPUT);
  pinMode(STP, OUTPUT);

  pinMode(RUN, INPUT_PULLUP);
  pinMode(CAL, INPUT_PULLUP);
  pinMode(STR, INPUT_PULLUP);
  pinMode(HOME, INPUT_PULLUP);

  pinMode(RunLed, OUTPUT);
  pinMode(CalLed, OUTPUT);

  Serial.println("Initializing...");
  retractMotor();  // Retract motor at start

  // Clear the EEPROM for the first time, after uploading and ruuning this code once comment out the below 4 lines:
  for (int i = 0 ; i < EEPROM.length() ; i++) {
    EEPROM.write(i, 0);
  }
  delay(1000);
  //------------------------- END

  extendedSteps = readLongFromEEPROM(10);  // Read the last value saved in the Arduino memory
  Serial.print("Extended steps: ");        // Print the value read on serial monitor
  Serial.println(extendedSteps);
}

void loop() {
  if (digitalRead(RUN) == LOW) {  // If run button is pressed
    digitalWrite(RunLed, LOW);  // ON

    if (millis() - lastButtonPress > 50) {
      Serial.println("Run Button pressed!");
      // Toggle the motor position
      if (Position == Extended) {  // If extended
        retractMotor();  // Retract
      }
      else {  // If retracted
        extendMotor();  // Extend
      }
    }
    // Remember last button press event
    lastButtonPress = millis();
  }

  if (digitalRead(CAL) == LOW) {  // If calibrate button is pressed
    digitalWrite(CalLed, LOW);  // ON

    if (millis() - lastButtonPress > 50) {
      Serial.println("Calibrate Button pressed!");
      // Run the calibrate function
      calibratePosition();
    }
    // Remember last button press event
    lastButtonPress = millis();
  }

  digitalWrite(RunLed, HIGH);  // OFF
  digitalWrite(CalLed, HIGH);  // OFF
}

void retractMotor() {  // Function to retract the motor
  Serial.println("Retracting Motor.");
  digitalWrite(DIR, retDir);  // Set the motor direction to retracted
  while (digitalRead(HOME) == HIGH) {  // While switch not pressed
    // Send pulses
    digitalWrite(STP, HIGH);
    delayMicroseconds(retSpeed);
    digitalWrite(STP, LOW);
    delayMicroseconds(retSpeed);
  }

  delay(1000); // Slight delay

  digitalWrite(DIR, extDir);  // Set the motor direction to extended
  for (int i = 0; i < retractedSteps; i++) {  // Take 50 steps away from the home switch
    // Send pulses
    digitalWrite(STP, HIGH);
    delayMicroseconds(retSpeed);
    digitalWrite(STP, LOW);
    delayMicroseconds(retSpeed);
  }
  Serial.println("Motor at home position.");
  Position = Retracted;
  currPosition = retractedSteps;
}

void extendMotor() {  // Function to extend the motor
  Serial.println("Extending Motor.");
  digitalWrite(DIR, extDir);  // Set the motor direction to extended
  for (int i = 0; i < extendedSteps; i++) {  // Take the required steps away from the home position
    // Send pulses
    digitalWrite(STP, HIGH);
    delayMicroseconds(extSpeed);
    digitalWrite(STP, LOW);
    delayMicroseconds(extSpeed);
  }
  Serial.println("Motor at extended position.");
  Position = Extended;
  currPosition = extendedSteps;
}

void calibratePosition() {  // Function to calibrate the motor position
  int motorSpeed = 0;
  while (1) {
    int joystickReading = analogRead(A0);

    if (joystickReading > maxReading) {  // Retract
      motorSpeed = map(joystickReading, 550, 1000, minCalSpeed, maxCalSpeed);
      //motorSpeed = constrain(motorSpeed, minCalSpeed, maxCalSpeed);
      Serial.println(motorSpeed);
      digitalWrite(DIR, retDir);  // Set the motor direction to retracted
      if (digitalRead(HOME) == HIGH) {  // If home buttton not pressed
        // Send pulses
        digitalWrite(STP, HIGH);
        delayMicroseconds(motorSpeed);
        digitalWrite(STP, LOW);
        delayMicroseconds(motorSpeed);
        currPosition--;
      }
    }
    else if (joystickReading < minReading) {  // Extend
      motorSpeed = map(joystickReading, 490, 50, minCalSpeed, maxCalSpeed);
      //motorSpeed = constrain(motorSpeed, minCalSpeed, maxCalSpeed);
      Serial.println(motorSpeed);
      digitalWrite(DIR, extDir);  // Set the motor direction to extended
      // Send pulses
      digitalWrite(STP, HIGH);
      delayMicroseconds(motorSpeed);
      digitalWrite(STP, LOW);
      delayMicroseconds(motorSpeed);
      currPosition++;
    }

    if (digitalRead(STR) == LOW) {  // If store Button pressed
      if (currPosition > retractedSteps) {  // Min extended position should be more than home position
        Serial.println("New extended position stored.");
        extendedSteps = currPosition;
        Position = Extended;
        writeLongIntoEEPROM(10, extendedSteps);  // Save new value on position 10 and onwards
        Serial.print("New extended steps: ");
        Serial.println(extendedSteps);
        for (int i = 0; i < 3; i++) {  // Flash the calibrate LED for 3 sec when new position stored
          digitalWrite(CalLed, LOW);
          delay(500);
          digitalWrite(CalLed, HIGH);
          delay(500);
        }
        break;  // Break out of the calibrate function
      }
      else {
        Serial.println("Invalid position.");
      }
    }
  }
}

void writeLongIntoEEPROM(int address, long number) {  // Function to save long value in eeprom
  EEPROM.write(address, (number >> 24) & 0xFF);
  EEPROM.write(address + 1, (number >> 16) & 0xFF);
  EEPROM.write(address + 2, (number >> 8) & 0xFF);
  EEPROM.write(address + 3, number & 0xFF);
}

long readLongFromEEPROM(int address) {  // Function to read long value from eeprom
  return ((long)EEPROM.read(address) << 24) +
         ((long)EEPROM.read(address + 1) << 16) +
         ((long)EEPROM.read(address + 2) << 8) +
         (long)EEPROM.read(address + 3);
}