// MacroMaster 5000000 - https://eih.no/macromaster/
// Automatic slider for focus stacking - created by Eivind Hansen, prosject started in 2023, but will it ever end...?
// Based on Arduino UNO, final product is utilizing a ATMEGA 328P-AU TQFP-32 chip

const float fwVerison = 0.56;  // firmware version

//********** Library Includes - Start *************
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 64     // OLED display height, in pixels
#define OLED_RESET -1        // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3D  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
#include <AccelStepper.h>
#include <SimpleRotary.h>
//********** Library Includes - End *************

//********** Initializing OLED - Start *************
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//********** Initializing OLED - End *************

//********** Pin Designations - Start *************
const int stepPin = 13;      // Output pin for step at stepper driver - pin mode is set by AccelStepper lib.
const int dirPin = 12;       // Output pin for direction selection at stepper driver  - pin mode is set by AccelStepper lib.
const int trigger_1 = 10;    // Output pin for triggering camera - https://eih.no/wp-content/uploads/2023/03/MM5M_Camera_Interface_typical.jpg
const int trigger_2 = 3;     // Output pin for triggering camera - https://eih.no/wp-content/uploads/2023/03/MM5M_Camera_Interface_typical.jpg
const int homeSwitch = 5;    // Input pin for homing sensor
const int softButton_1 = 4;  // used for button 1
const int softButton_2 = 2;  // used for button 2
const int buzzer = 9;        // Output pin for passive buzzer
const int pwrLED = A3;       // LED in power button
const int MS1 = A1;          // MS1 pin on stepper driver  - for setting microsteps
const int MS2 = A2;          // MS2 pin on stepper driver  - for setting microsteps
// const int SDA = A4		// SDA pin for i2c to OLED display - this is defined by the Wire liberary hence commented out
// const int SCL = A4		// SCL pin for i2c to OLED display - this is defined by the Wire liberary hence commented out
//********** Pin Designations - End *************


//********** Declaring of Global Variables - Start *************
//bool manualMode = false;
//bool globalSettingMode = false;
bool startSet = false;
bool endSet = false;
//bool focusSet = false;
int adress = 0;  // Used for keeping track of menu system
int menuShift = 0;

float ctr = 1;     //Value form rotary encoder
byte lastDir = 0;  //Memory of last direction of encoder operation
char encoderMAX;
char encoderMIN;
enum modeStates { manual_mode,
                  run_mode,
                  auto_mode,
                  semi_mode,
                  setup_mode,
                  edit_setup_mode,
                  abort_mode
};
enum modeStates mode;

enum softKeyConfig { Run_Settings,
                     Blank_Abort,
                     Blank_Blank,
                     Blank_Settings,
                     SetStart_SetEnd,
                     ReSetStart_SetEnd,
                     SetStart_ReSetEnd,
                     Homing_Settings,
                     Exit_Save
};


// Variables related to homing and safety
bool homingDone = false;          // Homing status
const unsigned int limitMIN = 0;  // [STEPS] Minimum position
long limitMAX = 0;                // [STEPS] defined in resMode() i.e.: for 2 mm pitch leadscrew at 1/8 microstepping (120 mm / 2 mm )* 1600 = 96000

// Varaibles for speeds and accelerations
int MAXspeed_M1 = 4000;       // Max speed for stepper M1
int Acceleration_M1 = 10000;  // Acceleration for stepper M1

// Variables related to physical features for hardware
const int leadScrewPitch = 2;  // Pitch for actual leadscrew in mm
int fullRot = 0;               // defined in resMode() - Defines how many steps needed for a full rotation. Motor 200 steps s^-1 -> microstepping 1/16 -> 200*16=3200

// Variabled realted to focus stacking function
//long focusDist = 0;    //[STEPS] Variable for storing the calculated distance of travel for given start & end position
long realEndPos = 0;               //[STEPS] Variable for storing calculated real end position
unsigned int numberPics = 0;       //[#] Variable for storing calculated number of pictures needed
unsigned long focusStepSteps = 0;  //[STEPS] Variable for storing calculated number of steps for each picture - derived form variable focusStepMM
bool stacingDirection = true;      // Shows the direction of travel during the stacking sequenze, startPos -> endPos = true, endPos -> startPos = false


// Variables with valus assigned by user
float manuelStep = 0.5;   //[mm] step size when joging in manual mode
long startPos = 0;        //[STEPS] Varaible for storing start position, data in steps
long endPos = 0;          //[STEPS] Variable for storing end position, data in steps
float focusStepMM = 0.5;  //[mm] Varaible for setting the travel distance for each picture, given in mm
int dwell_1 = 1000;       //[ms] Dealy used after a move before taking a new picture, to let the system settle after movement
int dwell_2 = 0;          //[ms] Dealy used between trigger_1 & trigger_2. 0 = no delay, positiv vale delays trigger_2 given amount after trigger_1
// negativ value delays trigger_1 given amount after trigger_2. Depends on camera configuration for focus and shutter
int shutterTriggerTimer = 500;  //[ms] Amount if time signal to shutter trigger is high
int shutterSpeed = 500;         //[ms] Additional delay added after triggers are set high, to compensate for slower shutter speeds
byte numberOfPics = 1;          // number of pics taken for each step
bool highResMode = false;
//********** Declaring of Global Variables - End *************

//********** Creates an Instance of Stepper Motor - Start *************
AccelStepper Stepper_M1(AccelStepper::DRIVER, stepPin, dirPin);
SimpleRotary rotary(7, 6, 8);  // Pin A, Pin B, Button Pin
//********** Creates an Instance of Stepper Motor - End *************


//********** OLED Grapix - Start *************
// 'EiH_512x512_', 45x45px
const unsigned char EiH[] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00,
  0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0,
  0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00,
  0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x00, 0x00,
  0x00, 0x06, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x06, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x06, 0x00, 0xc0,
  0x00, 0x00, 0x10, 0x06, 0x00, 0xc0, 0x00, 0x00, 0x98, 0x06, 0x00, 0xc0, 0x00, 0x0f, 0x88, 0x06,
  0x00, 0xc0, 0x0f, 0xfe, 0x00, 0x06, 0x01, 0x80, 0x7f, 0xf0, 0x00, 0x06, 0x01, 0x80, 0x7c, 0x00,
  0x00, 0x06, 0x01, 0x80, 0x0c, 0x00, 0x0c, 0x06, 0x01, 0x80, 0x0c, 0x00, 0x0c, 0x06, 0x01, 0x80,
  0x0c, 0x00, 0x0c, 0x06, 0x01, 0x80, 0x0c, 0x06, 0x0c, 0x06, 0x01, 0x80, 0x0c, 0x3c, 0x0c, 0x06,
  0x01, 0x80, 0x0f, 0xf8, 0x0c, 0x06, 0x01, 0xf0, 0x7f, 0x80, 0x0c, 0x06, 0x3f, 0xf0, 0x7c, 0x00,
  0x0c, 0xff, 0xff, 0x80, 0x0c, 0x00, 0x0c, 0xff, 0x81, 0x80, 0x06, 0x00, 0x0c, 0x46, 0x01, 0x80,
  0x06, 0x00, 0x8c, 0x06, 0x01, 0x80, 0x06, 0x00, 0xc4, 0x06, 0x01, 0x80, 0x06, 0x00, 0xc6, 0x06,
  0x01, 0x80, 0x06, 0x01, 0xc6, 0x06, 0x01, 0x80, 0x03, 0x07, 0x86, 0x06, 0x01, 0x80, 0x03, 0xfe,
  0x06, 0x06, 0x03, 0x80, 0x00, 0xf0, 0x00, 0x06, 0x01, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

//********** OLED Grapix - End *************



void ISR_Abort() {
  mode = abort_mode;
}



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

  //********** Defining pinModes - Start *************
  // DDRD &= (0 << DDD2) &  (1 << DDD3) &  (0 << DDD4) &  (0 << DDD5) ; //  B00001010; //pinMode pins 0-7, 0=input, 1=output
  // //PORTD |= (1 << PORTD2) &  (0 << PORTD3) &  (1 << PORTD4) &  (0 << PORTD5) ; //
  // PORTD = B11010111;
  // DDRC &= 1 << DDC1 & 1 << DDC2 & 1 << DDC3;
  // DDRB &= 1 << DDB1 & 1 << DDB2 & 1 << DDB4 & 1 << DDB5;


  pinMode(homeSwitch, INPUT);
  pinMode(softButton_1, INPUT_PULLUP);
  pinMode(softButton_2, INPUT_PULLUP);
  pinMode(trigger_1, OUTPUT);
  pinMode(trigger_2, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(pwrLED, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  //********** Defining pinModes - End *************

  digitalWrite(pwrLED, HIGH);  // Turn on led in power button
  delay(500);                  // Alow i2c to start OK
  mode = auto_mode;

  //********** Initializing the Stepper Motor - Start *************
  Stepper_M1.setMaxSpeed(MAXspeed_M1);             // Max speed for stepper
  Stepper_M1.setAcceleration(Acceleration_M1);     // Acceleration for stepper
  Stepper_M1.setPinsInverted(true, false, false);  // Inverting the direction of rotation with the first argument set to true.
  //********** Initializing the Stepper Motor - End *************

  rotary.setDebounceDelay(2);  //Debounce delay for rotary function of encoder

  //********** Do Not Start if Display is Not Ready - Start *************
  if (!display.begin(SSD1306_SWITCHCAPVCC)) {
    ;
    for (;;)
      ;  // Don't proceed, loop forever
  }
  //********** Do Not Start if Display is Not Ready - End *************

  display.clearDisplay();
  display.display();
  resMode(8);  // Sets default resolution mode to 1/8 microstepping
  splash();    //shows splash screen
}

void loop() {
  if (homingDone) Encoder1();
  if (!homingDone && (mode != setup_mode)) dispalyNoReference();
  if ((!digitalRead(softButton_1)) && homingDone && startSet && endSet) stackRun();
  if ((!digitalRead(softButton_2)) && (mode == auto_mode)) setupMenu();
}



void resMode(int MS) {
  // sets microstepping mode for stepper driver
  switch (MS) {
    case 16:  // 1/16 microstepping
      digitalWrite(MS1, HIGH);
      digitalWrite(MS2, HIGH);
      fullRot = 3200;
      break;

    case 8:  // 1/8 microstepping
      digitalWrite(MS1, LOW);
      digitalWrite(MS2, LOW);
      fullRot = 1600;
      break;
  }
  //calculates ned limitMAX valeu based in microstepping selection
  limitMAX = ((120 / long(leadScrewPitch)) * fullRot);
}

void buzz(int tune) {  //Function for creating buzzing tones
  switch (tune) {
    case 1:
      tone(buzzer, 800, 400);
      delay(500);
      tone(buzzer, 800, 400);
      // delay(500);
      break;

    case 2:
      tone(buzzer, 800, 400);
      delay(500);
      tone(buzzer, 800, 400);
      delay(500);
      tone(buzzer, 1000, 150);
      delay(150);
      tone(buzzer, 1200, 150);
      // delay(200);
      break;
  }
}


void shutterRelease() {
  // funksjon for å ta flere bilder pr steg?
  int temp_pic_cnt = 1;
  displayStatus(F("In postion!"));
  displaySoftKeys(Blank_Abort);
  delay(dwell_1);  // Seteling time for system oscilations

  while (temp_pic_cnt <= numberOfPics) {  // loop utill all pictures are taken for given positon
    // Logic for whitch trigger to trigger first
    if (dwell_2 > 0) {  // Trigger 1 first, then trigger 2
      digitalWrite(trigger_1, HIGH);
      delay(dwell_2);
      digitalWrite(trigger_2, HIGH);
    } else if (dwell_2 < 0) {  // Trigger 2 first, then trigger 1
      digitalWrite(trigger_2, HIGH);
      delay(dwell_2 * -1);
      digitalWrite(trigger_1, HIGH);
    } else {  // Trigger both at same time
      digitalWrite(trigger_1, HIGH);
      digitalWrite(trigger_2, HIGH);
    }

    displayStatus(F("Shutter triggered!"));
    displaySoftKeys(Blank_Abort);

    delay(shutterTriggerTimer);  // Amount of thime trigger signals are high
    digitalWrite(trigger_1, LOW);
    digitalWrite(trigger_2, LOW);

    if (shutterSpeed > shutterTriggerTimer) {  // Additional delay if shutter speed is slower than amount of time triggers are keep high
      delay(shutterSpeed - shutterTriggerTimer);
    }

    temp_pic_cnt++;
  }


 // numberPics--;
  //displayStatus(F("Repositioning!"), F("Abort!"), F(""));
  displayStatus(F("Repositioning!"));
  displaySoftKeys(Blank_Abort);
}

void homing() {
  //displayStatus(F("Homing!"), F(""), F(""));
  displayStatus(F("Homing!"));
  displaySoftKeys(Blank_Abort);
  mode = auto_mode;
  attachInterrupt(digitalPinToInterrupt(softButton_2), ISR_Abort, RISING);

  Stepper_M1.setCurrentPosition(0);  // Set current position to 0
  Stepper_M1.moveTo(mmToSteps(30));  // Move 30 mm to make sure that carrier is at the correct side of the homing sensor
  while (!(Stepper_M1.distanceToGo() == 0) && (mode == auto_mode)) Stepper_M1.run();
  Stepper_M1.setCurrentPosition(0);                                            // Set current position to 0
  Stepper_M1.moveTo(-limitMAX);                                                // Move distance grater than max travle in direction of homing switch
  while (!(digitalRead(homeSwitch)) && (mode == auto_mode)) Stepper_M1.run();  // Move until homeing switch is activated
  // displayStatus("Reference found!", "    --    ", "    --    ");
  Stepper_M1.setCurrentPosition(mmToSteps(20));  //Set current position to 20 mm, this is the homing position, which is 20 mm offsett from end travel
  Stepper_M1.setMaxSpeed(2000);
  Stepper_M1.setAcceleration(5000);
  Stepper_M1.moveTo(0);
  while (!(Stepper_M1.distanceToGo() == 0) && (mode == auto_mode)) Stepper_M1.run();
  // Set default speed and acceleration
  Stepper_M1.setMaxSpeed(MAXspeed_M1);
  Stepper_M1.setAcceleration(Acceleration_M1);
  // set homing status to TRUE
  if (mode == auto_mode) {
    homingDone = true;
    buzz(1);
    delay(500);
    displayStatus(F("Auto mode!"));
    displaySoftKeys(Blank_Settings);
  } else if (mode == abort_mode) dispalyNoReference();

  detachInterrupt(digitalPinToInterrupt(softButton_2));
}




//*** Reading encoder values ***
void Encoder1() {
  int rDir = rotary.rotate();
  byte Lang = rotary.pushType(1000);  //threshold for difference of a short vs long press on button
  // Check direction
  if (rDir == 1) {
    // CW
    switch (mode) {
      case setup_mode:
        ctr--;
        if (ctr < 5 && ctr >= encoderMIN) {
          menuShift++;
          if (menuShift >= 0) menuShift = 0;
        }

        if (ctr <= encoderMIN) ctr = encoderMIN;  //Min value of ctr check
        adress = ctr;

        // if (adress <= 5 && adress > encoderMIN ) menuShift++;
        break;

      case edit_setup_mode:
        if (adress == 1) ctr = ctr + 0.05;
        if (adress == 2) ctr = ctr + 50;
        if (adress == 3) ctr++;
        if (adress == 4) ctr = ctr + 50;
        if (adress == 5) ctr = ctr + 50;
        if (adress == 6) ctr++;
        if (adress == 7) ctr++;
        if (adress == 8) ctr = ctr + 100;
        if (adress == 9) ctr = ctr + 100;
        break;

      case manual_mode:
        ctr = ctr + manuelStep;
        break;

      default:
        ctr++;
        break;
    }

    lastDir = rDir;
    eActionRot();
  }

  if (rDir == 2) {
    // CCW
    switch (mode) {
      case setup_mode:
        ctr++;
        if (ctr > 5 && ctr <= encoderMAX) {
          menuShift--;
          if (menuShift <= ((encoderMAX - 5) * -1)) menuShift = ((encoderMAX - 5) * -1);
        }

        if (ctr >= encoderMAX) ctr = encoderMAX;  //Max value of ctr check

        adress = ctr;
        // if (adress > 5 && adress <= encoderMAX+1) menuShift--;


        break;

      case edit_setup_mode:
        if (adress == 1) ctr = ctr - 0.05;
        if (adress == 2) ctr = ctr - 50;
        if (adress == 3) ctr--;
        if (adress == 4) ctr = ctr - 50;
        if (adress == 5) ctr = ctr - 50;
        if (adress == 6) ctr--;
        if (adress == 7) ctr--;
        if (adress == 8) ctr = ctr - 100;
        if (adress == 9) ctr = ctr - 100;
        break;

      case manual_mode:
        ctr = ctr - manuelStep;
        break;

      default:
        ctr--;
        break;
    }

    lastDir = rDir;
    eActionRot();
  }
  if (Lang == 1) {
    eActionShort();
  }
  if (Lang == 2) {
    eActionLong();
  }
}

void eActionRot() {

  switch (mode) {

    case setup_mode:
      displaySetupMenu();
      break;

    case edit_setup_mode:
      if (adress == 1) focusStepMM = ctr;
      if (adress == 2) shutterSpeed = ctr;
      if (adress == 3) numberOfPics = ctr;
      if (adress == 4) dwell_1 = ctr;
      if (adress == 5) dwell_2 = ctr;
      if (adress == 6) shutterTriggerTimer = ctr;
      if (adress == 7) highResMode = ctr;
      if (adress == 8) MAXspeed_M1 = ctr;
      if (adress == 9) Acceleration_M1 = ctr;
      displaySetupMenu();
      break;
  }
}


void eActionShort() {

  // if ((mode == manual_mode) && (mode != setup_mode)) {
  //   if (manuelStep == 0.5) {
  //     displayStatus(F("Joging (0,1 mm)"));
  //     displaySoftKeys(SetStart_SetEnd);
  //     manuelStep = 0.1;
  //   } else {
  //     manuelStep = 0.5;
  //     displayStatus(F("Joging (0,5 mm)"));
  //     displaySoftKeys(SetStart_SetEnd);
  //   }
  // }


  switch (mode) {

    // **** Manual Mode ****
    case manual_mode:
      if (manuelStep == 0.5) {
        displayStatus(F("Joging (0,1 mm)"));
        displaySoftKeys(SetStart_SetEnd);
        manuelStep = 0.1;
      } else {
        manuelStep = 0.5;
        displayStatus(F("Joging (0,5 mm)"));
        displaySoftKeys(SetStart_SetEnd);
      }
      break;
    // **** Manual Mode End ****

    // **** Setup Mode ****
    case setup_mode:
      mode = edit_setup_mode;
      switch (adress) {
        case 1:
          //adress = 1;
          ctr = focusStepMM;
          break;
        case 2:
          //adress = 2;
          ctr = shutterSpeed;
          break;
        case 3:
          //adress = 3;
          ctr = numberOfPics;
          break;
        case 4:
          //adress = 4;
          ctr = dwell_1;
          break;
        case 5:
          //adress = 5;
          ctr = dwell_2;
          break;
        case 6:
          //adress = 6;
          ctr = shutterTriggerTimer;
          break;
        case 7:
          //adress = 7;
          ctr = highResMode;
          break;
        case 8:
          //adress = 8;
          ctr = MAXspeed_M1;
          break;
        case 9:
          //adress = 9;
          ctr = Acceleration_M1;
          break;
      }
      break;
    // **** Setup Mode End****

    // ****Edit Setup Mode ****
    case edit_setup_mode:
      if (adress == 1) ctr = 1;
      if (adress == 2) ctr = 2;
      if (adress == 3) ctr = 3;
      if (adress == 4) ctr = 4;
      if (adress == 5) ctr = 5;
      if (adress == 6) ctr = 6;
      if (adress == 7) ctr = 7;
      if (adress == 8) ctr = 8;
      if (adress == 9) ctr = 9;
      mode = setup_mode;
      break;
      // ****Edit Setup Mode End ****
  }
}


void eActionLong() {
  // Serial.println("LONG");
  if (mode == manual_mode) {
    mode = auto_mode;
    if (startSet && endSet) {
      displayStatus(F("Ready!"));
      displaySoftKeys(Run_Settings);
    }

  } else {
    mode = manual_mode;
    // Serial.println("Manual mode");
    displayStatus(F("Manual mode!"));
    displaySoftKeys(SetStart_SetEnd);
    manuelMove(Stepper_M1.currentPosition());
  }
}


void manuelMove(long sled) {
  ctr = 0;
  displayStatus(F("Joging (0,5 mm)"));
  displaySoftKeys(SetStart_SetEnd);

  while (mode == manual_mode) {
    Encoder1();
    Stepper_M1.moveTo(sled + mmToSteps(ctr));
    if ((Stepper_M1.targetPosition() >= limitMIN) && (Stepper_M1.targetPosition() <= limitMAX)) {
      Stepper_M1.run();
    } else if (Stepper_M1.targetPosition() <= limitMIN) {
      sled = limitMIN;
      ctr = 0;
      if (Stepper_M1.currentPosition() == limitMIN) {
        displayStatus(F("End of travel!"));
        displaySoftKeys(SetStart_SetEnd);
      }
    } else if (Stepper_M1.targetPosition() >= limitMAX) {
      sled = limitMAX;
      ctr = 0;
      if (Stepper_M1.currentPosition() == limitMAX) {
        displayStatus(F("End of travel!"));
        displaySoftKeys(SetStart_SetEnd);
      }
    }

    if ((Stepper_M1.currentPosition() == limitMIN + 1) || (Stepper_M1.currentPosition() == limitMAX - 1)) {
      displayStatus(F("Joging..."));
      displaySoftKeys(SetStart_SetEnd);
    }

    if ((!digitalRead(softButton_1) && softKey())) {
      while (!digitalRead(softButton_1))
        ;
      setPos(true);
    } else if ((!digitalRead(softButton_2) && softKey())) {
      while (!digitalRead(softButton_2))
        ;
      setPos(false);
    }
  }
}




void setPos(bool pos) {

  if (pos) {
    startPos = Stepper_M1.currentPosition();
    startSet = true;
    //delay(20);
  }
  if (!pos) {
    endPos = Stepper_M1.currentPosition();
    endSet = true;
    //delay(20);
  }
  displayStatus(F("Position set!"));
  displaySoftKeys(SetStart_SetEnd);
  delay(800);
  updateRunParameters();
}

void updateRunParameters() {
  focusStepSteps = mmToSteps(focusStepMM);

  if (startSet && endSet) {
    if (startPos > endPos) {
      stacingDirection = false;
      //focusDist = startPos - endPos;
      //numberPics = ceil((startPos - endPos) / focusStepSteps);
      if (((startPos - endPos) % focusStepSteps) != 0) {
        numberPics = ((startPos - endPos) / focusStepSteps) + 1;
      } else numberPics = ((startPos - endPos) / focusStepSteps);

    } else {
      stacingDirection = true;
      // focusDist = endPos - startPos;
      //numberPics = ceil(focusDist / focusStepSteps);
      if (((endPos - startPos) % focusStepSteps) != 0) {
        numberPics = ((endPos - startPos) / focusStepSteps) + 1;
      } else numberPics = ((endPos - startPos) / focusStepSteps);
    }


    if (stacingDirection) {
      realEndPos = startPos + (numberPics * focusStepSteps);
      if (realEndPos > limitMAX) {
        endSet = false;
      }
    } else {
      realEndPos = startPos - (numberPics * focusStepSteps);
      if (realEndPos < limitMIN) {
        endSet = false;
      }
    }
    if (!endSet && (mode != run_mode)) {
      displayStatus(F("End pos. out of range!"));
      displaySoftKeys(SetStart_ReSetEnd);
    };
    if ((mode == manual_mode) && endSet && startSet) {
      displayStatus(F("Start & end pos. set!"));
      displaySoftKeys(SetStart_SetEnd);
    }
  }
}


void stackRun() {
  mode = run_mode;
  updateRunParameters();
  displayStatus(F("Initializing!"));
  displaySoftKeys(Blank_Abort);
  long tempDist = startPos;
  attachInterrupt(digitalPinToInterrupt(softButton_2), ISR_Abort, RISING);

  switch (stacingDirection) {
    case true:
      Stepper_M1.moveTo(startPos - mmToSteps(1));
      while (!(Stepper_M1.distanceToGo() == 0) && (mode == run_mode)) Stepper_M1.run();
      while (Stepper_M1.currentPosition() < realEndPos && (mode == run_mode)) {
        Stepper_M1.moveTo(tempDist);
        while (!(Stepper_M1.distanceToGo() == 0) && (mode == run_mode)) Stepper_M1.run();
        shutterRelease();
        tempDist = tempDist + focusStepSteps;
        numberPics--;
      }
      break;

    case false:
      Stepper_M1.moveTo(startPos + mmToSteps(1));
      while (!(Stepper_M1.distanceToGo() == 0) && (mode == run_mode)) Stepper_M1.run();
      while (Stepper_M1.currentPosition() > realEndPos && (mode == run_mode)) {
        Stepper_M1.moveTo(tempDist);
        while (!(Stepper_M1.distanceToGo() == 0) && (mode == run_mode)) Stepper_M1.run();
        shutterRelease();
        tempDist = tempDist - focusStepSteps;
        numberPics--;
      }
      break;
  }

  if (mode == run_mode) {
    numberPics = 0;
    displayStatus(F("Finish!"));
    displaySoftKeys(Blank_Blank);
    Stepper_M1.moveTo(startPos);
    while (!(Stepper_M1.distanceToGo() == 0)) Stepper_M1.run();
    buzz(2);
    updateRunParameters();
    mode = auto_mode;
    displayStatus(F("Ready!"));
    displaySoftKeys(Run_Settings);
  } else if (mode == abort_mode) {
    displayStatus(F("Aborted!"));
    displaySoftKeys(Run_Settings);
    mode = auto_mode;
  }
  detachInterrupt(digitalPinToInterrupt(softButton_2));
}




//**** Conversions ****
// Function that converts [mm] to [steps] - argument in is a float in mm, returns a float with corelating number of steps
float mmToSteps(float mm) {
  return ((mm / leadScrewPitch) * fullRot);
}

// Function that converts [steps] to [mm]  - argument in is a  long with number of steps, returns a float with corelating distance in  mm
float stepsToMM(long steps) {
  return ((steps / float(fullRot)) * leadScrewPitch);
}


//**** Display ****

void splash() {
  display.setTextSize(0);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text
  display.clearDisplay();
  display.drawBitmap((SCREEN_WIDTH - 45) / 2, (SCREEN_HEIGHT - 50) / 2, EiH, 45, 45, 1);
  display.setCursor(7, 54);
  display.print(F("MacroMaster 5000000"));
  display.setCursor(1, 1);
  display.print(F("v"));
  display.print(fwVerison);
  display.display();
  delay(4000);

}  //*** END splash screen ***


// Display status info during operation - argument in is a string printet at bottom of screen
// Current position is also printet for each call
void displayStatus(String info) {
  display.setTextSize(0);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text
  display.clearDisplay();

  if (homingDone) {
    display.setCursor(1, 1);
    display.print(F("Pos  :"));
    display.print(stepsToMM(Stepper_M1.currentPosition()), 3);
  }

  display.drawFastHLine(1, display.height() - 20, display.width() - 2, WHITE);
  display.setCursor(1, display.height() - 18);
  display.print(info);

  if (startSet) {
    display.setCursor(1, 10);
    display.print(F("S.Pos:"));
    display.print(stepsToMM(startPos), 3);
  }
  if (endSet) {
    display.setCursor(1, 20);
    display.print(F("E.Pos:"));
    display.print(stepsToMM(realEndPos), 3);
    display.setCursor(1, 30);
    display.print(F("Steps:"));
    display.print(numberPics);
  }
  display.display();
}

void dispalyNoReference() {
  displayStatus(F("Homing required!"));
  displaySoftKeys(Homing_Settings);

  display.drawBitmap((SCREEN_WIDTH - 45) / 2, (SCREEN_HEIGHT - 65) / 2, EiH, 45, 45, 1);
  /*   display.setTextSize(0);       // Normal 1:1 pixel scale
    display.setTextColor(WHITE);  // Draw white text
    display.setCursor(1, 35);
    display.print(F(" MacroMaster 5000000")); */
  display.display();

  while (1) {
    if (!digitalRead(softButton_1) && softKey()) {
      homing();
      break;
    }
    if (!digitalRead(softButton_2) && softKey()) {
      setupMenu();
      break;
    }
  }
}



void setupMenu() {
  mode = setup_mode;
  ctr = 1;
  adress = 1;
  menuShift = 0;
  displaySetupMenu();

  while (mode == setup_mode || edit_setup_mode) {

    if (!digitalRead(softButton_1) && softKey()) {  // Exit
      mode = auto_mode;
      if (homingDone) {
        displayStatus(F("Auto mode!"));
        mode = auto_mode;
        displaySoftKeys(Blank_Settings);
      }
      break;
    }
    if (!digitalRead(softButton_2) && softKey()) buzz(2);  // Save

    //if (softKeyPressed(softButton_2)) buzz(2);

    Encoder1();
  }
}

bool softKey() {
  //debounce button press
  //uint16_t delayIfYouAreUsingCrappyButtons = 250;
  static unsigned long timeStamp = 0;
  // static uint16_t state = 0;
  //static bool skState = false;
  //state = (state << 1) | digitalRead(softButton_1) | 0xfe00;
  // Serial.println(state,BIN);
  if ((timeStamp + 100 < millis())) {
    timeStamp = millis();
    return true;
  }
  return false;
}

/* bool softKey1() {
  //debounce button press
  uint16_t delayIfYouAreUsingCrappyButtons = 250;
  static unsigned long timeStamp = 0;
  static uint16_t state = 0;
  //static bool skState = false;
  state = (state << 1) | digitalRead(softButton_1) | 0xfe00;
  // Serial.println(state,BIN);
  if ((state == 0xff00) && (timeStamp + delayIfYouAreUsingCrappyButtons < millis())) {
    timeStamp = millis();
    return (state == 0xff00);
  }
  return false;
  } */

/* bool softKey2() {
  //debounce button press
  uint16_t delayIfYouAreUsingCrappyButtons = 250;
  static unsigned long timeStamp = 0;
  static uint16_t state = 0;
  //static bool skState = false;
  state = (state << 1) | digitalRead(softButton_2) | 0xfe00;
  // Serial.println(state,BIN);
  if ((state == 0xff00) && (timeStamp + delayIfYouAreUsingCrappyButtons < millis())) {
    timeStamp = millis();
    return (state == 0xff00);
  }
  return false;
  } */

void displaySetupMenu() {
  // Serial.println(ctr);
  // Serial.println(menuShift);
  // Serial.println(adress);

  encoderMAX = 9;
  encoderMIN = 1;


  display.setTextSize(1);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text
  display.clearDisplay();

  // Menu Item 1
  display.setCursor(1, 1 + (11 * menuShift));
  display.print(F("Focus Step   : "));
  if (adress == 1) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(focusStepMM, 3);
  display.setTextColor(WHITE);


  // Menu Item 2
  display.setCursor(1, 12 + (11 * menuShift));
  display.print(F("Shutter Speed: "));
  if (adress == 2) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(shutterSpeed);
  display.setTextColor(WHITE);

  // Menu Item 3
  display.setCursor(1, 23 + (11 * menuShift));
  display.print(F("Numbr of Pics: "));
  if (adress == 3) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(numberOfPics);
  display.setTextColor(WHITE);

  // Menu Item 4
  display.setCursor(1, 34 + (11 * menuShift));
  display.print(F("Settling Time: "));
  if (adress == 4) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(dwell_1);
  display.setTextColor(WHITE);

  // Menu Item 5
  display.setCursor(1, 45 + (11 * menuShift));
  display.print(F("Triggr Offset: "));
  if (adress == 5) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(dwell_2);
  display.setTextColor(WHITE);

  //Menu Item 6
  display.setCursor(1, 56 + (11 * menuShift));
  display.print(F("Shutter Hold: "));
  if (adress == 6) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(shutterTriggerTimer);
  display.setTextColor(WHITE);

  //Menu Item 7
  display.setCursor(1, 67 + (11 * menuShift));
  display.print(F("High Res Mode: "));
  if (adress == 7) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(highResMode);
  display.setTextColor(WHITE);

  //Menu Item 8
  display.setCursor(1, 78 + (11 * menuShift));
  display.print(F("Max Speed    : "));
  if (adress == 8) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(MAXspeed_M1);
  display.setTextColor(WHITE);

  // Menu Item 9
  display.setCursor(1, 89 + (11 * menuShift));
  display.print(F("Acceleration : "));
  if (adress == 9) display.setTextColor(BLACK, WHITE);  // 'inverted' text
  display.print(Acceleration_M1);
  display.setTextColor(WHITE);

  displaySoftKeys(Exit_Save);
  display.display();
}

void displaySoftKeys(int SoftKeyType) {
  // SoftKey1 text location start at 0 and end within 9 [0123456789]
  // SoftKey2 text location end at 9 and start within 0 [0123456789]
  String SoftKey1;
  String SoftKey2;

  switch (SoftKeyType) {
    case Run_Settings:
      SoftKey1 = F("Run");
      SoftKey2 = F("  Settings");
      break;
    case Blank_Abort:
      SoftKey1 = F("");
      SoftKey2 = F("    Abort!");
      break;
    case Blank_Blank:
      SoftKey1 = F("");
      SoftKey2 = F("");
      break;
    case Blank_Settings:
      SoftKey1 = F("");
      SoftKey2 = F("  Settings");
      break;
    case SetStart_SetEnd:
      SoftKey1 = F("Set Start");
      SoftKey2 = F("   Set End");
      break;
    case ReSetStart_SetEnd:
      SoftKey1 = F("Re-Set Start");
      SoftKey2 = F("   Set End");
      break;
    case SetStart_ReSetEnd:
      SoftKey1 = F("Set Start");
      SoftKey2 = F("Re-Set End");
      break;
    case Homing_Settings:
      SoftKey1 = F("Homing");
      SoftKey2 = F("  Settings");
      break;
    case Exit_Save:
      SoftKey1 = F("Exit");
      SoftKey2 = F("      Save");
      break;
  }

  display.setTextSize(0);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text

  //draw boxes for soft keys
  display.fillRect(0, display.height() - 9, display.width(), display.height() - 9, BLACK);

  display.fillRoundRect(-2, display.height() - 9, 65, 11, 3, WHITE);
  display.fillRoundRect(65, display.height() - 9, 65, 11, 3, WHITE);
  //location for soft key text
  display.setTextColor(BLACK);
  display.setCursor(1, display.height() - 8);  //S1 location
  display.print(SoftKey1);
  display.setCursor(67, display.height() - 8);  //S2 location
  display.print(SoftKey2);
  display.setTextColor(WHITE);
  display.display();
}