const float fwVersion = 0.1;  // 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 0x3C  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
#define EN_PIN 4
#include <AccelStepper.h>
#include <SimpleRotary.h>
#include <EEPROM.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 = 12;      // Output pin for step at stepper driver - pin mode is set by AccelStepper lib.
const int dirPin = 11;       // Output pin for direction selection at stepper driver  - pin mode is set by AccelStepper lib.
const int softButton_1 = 2;  // used for button 1
const int softButton_2 = 3;  // used for button 2
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 MS3 = A3;          // MS3 pin on stepper driver  - for setting microsteps
// 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 = 0;     //Value form rotary encoder
int menuIndex = 0;
unsigned long lastRotaryTime = 0;
static int lastMenuIndex = -1;

byte lastDir = 0;  //Memory of last direction of encoder operation
byte encoderEditMAX = 4;
byte encoderEditMIN = 0;
byte encoderMenuMAX = 2;
byte encoderMenuMIN = 0;

bool editingValue = false;
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_Auto,
                     Homing_Settings_Manu,
                     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 = 1000;       // Max speed for stepper M1
int Acceleration_M1 = 100;  // 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
int shutterTriggerTimer = 500;  //[ms] Amount if time signal to shutter trigger is high
int drehzahl = 1000;         //[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;
bool encoderMode =false;
bool richtung = true;

unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output 
int ledState = HIGH;        // the current state of the output pin
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

//********** 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 *************
//45x45px
const unsigned char PIC[] PROGMEM = {
	0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00, 0x03, 0xe0, 0x3e, 0x00, 0x00, 0x00, 0x0f, 0x00, 0x07, 
	0x80, 0x00, 0x00, 0x18, 0x7f, 0xf0, 0xc0, 0x00, 0x00, 0x63, 0xc0, 0x1e, 0x30, 0x00, 0x00, 0xce, 
	0x0f, 0x83, 0x98, 0x00, 0x01, 0x98, 0xff, 0xf8, 0xcc, 0x00, 0x03, 0x13, 0x80, 0x0e, 0x66, 0x00, 
	0x06, 0x00, 0x3f, 0xe3, 0x9b, 0x00, 0x0c, 0x98, 0xf0, 0x78, 0xc9, 0x80, 0x09, 0x33, 0x80, 0x0e, 
	0x64, 0xc0, 0x13, 0x66, 0x3f, 0xe3, 0x36, 0x40, 0x36, 0xc0, 0x20, 0x39, 0x9b, 0x60, 0x24, 0x99, 
	0x0f, 0x8c, 0xc9, 0x20, 0x6d, 0xb3, 0x3f, 0xe6, 0x6d, 0xb0, 0x69, 0x26, 0xe0, 0x3b, 0x24, 0xb0, 
	0x4b, 0x6d, 0x80, 0x0d, 0xb6, 0x90, 0x5a, 0x49, 0x00, 0x04, 0x92, 0xd0, 0xd2, 0xdb, 0x00, 0x06, 
	0xda, 0x58, 0x92, 0xd2, 0x00, 0x02, 0x5a, 0x48, 0x96, 0x96, 0x00, 0x03, 0x4b, 0x48, 0x96, 0x96, 
	0x07, 0x03, 0x4b, 0x48, 0x96, 0x96, 0x05, 0x83, 0x4b, 0x48, 0x96, 0x96, 0x07, 0x03, 0x4b, 0x48, 
	0x96, 0x96, 0x02, 0x03, 0x4b, 0x48, 0x92, 0xd2, 0x00, 0x02, 0x5a, 0x48, 0xd2, 0xdb, 0x00, 0x06, 
	0xda, 0x58, 0x5a, 0x49, 0x00, 0x04, 0x92, 0xd0, 0x4b, 0x6d, 0x80, 0x0d, 0xb6, 0x90, 0x69, 0x26, 
	0xe0, 0x39, 0x24, 0xb0, 0x6d, 0xb3, 0x3f, 0xe2, 0x6d, 0xb0, 0x24, 0x99, 0x8f, 0x8c, 0xc9, 0x20, 
	0x36, 0xcc, 0xe0, 0x39, 0x9b, 0x60, 0x13, 0x66, 0x3f, 0xe3, 0x36, 0x40, 0x09, 0x33, 0x80, 0x0e, 
	0x64, 0xc0, 0x0c, 0x98, 0xf0, 0x78, 0xc9, 0x80, 0x06, 0xce, 0x3f, 0xe3, 0x9b, 0x00, 0x03, 0x33, 
	0x80, 0x0e, 0x66, 0x00, 0x01, 0x98, 0xff, 0xf8, 0xcc, 0x00, 0x00, 0xce, 0x0f, 0x83, 0x98, 0x00, 
	0x00, 0x63, 0xc0, 0x1e, 0x30, 0x00, 0x00, 0x38, 0x7f, 0xf0, 0xe0, 0x00, 0x00, 0x0f, 0x00, 0x07, 
	0x80, 0x00, 0x00, 0x03, 0xe0, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00
};

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

void ISR_Abort() {
  mode = abort_mode;
}
////////////////////////////////////////////SETUP/////////////////////////////////////////////////////////

void setup() {
  Serial.begin(115200);
  pinMode(7, INPUT_PULLUP); // CLK
  pinMode(6, INPUT_PULLUP); // DT
  pinMode(8, INPUT_PULLUP); // SW
  pinMode(softButton_1, INPUT_PULLUP);
  pinMode(softButton_2, INPUT_PULLUP);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  pinMode(EN_PIN, OUTPUT);  
  //********** Defining pinModes - End *************

  delay(500);                  // Alow i2c to start OK
  mode = auto_mode;

  //********** Initializing the Stepper Motor - End *************

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

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

  display.clearDisplay();
  display.display();
  resMode(1);  // Sets default resolution mode to 1/8 microstepping
  splash();    //shows splash screen
}
////////////////////////////////////////////loop/////////////////////////////////////////////////////////
void loop() {
  Encoder1();
  if (mode != setup_mode) displayNoReference();
  //if ((!digitalRead(softButton_2)) && (mode == auto_mode)) setupMenu();
}

////////////////////////////////////////////loop-END/////////////////////////////////////////////////////////

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

    case 8:  // 1/8 microstepping
      digitalWrite(MS1, LOW);
      digitalWrite(MS2, LOW);
      digitalWrite(MS3, LOW);
      fullRot = 1600;
      break;

     case 1:  // Full
      digitalWrite(MS1, HIGH);
      digitalWrite(MS2, HIGH);
      digitalWrite(MS3, HIGH);
      fullRot = 200;
      break;
  }
  //calculates ned limitMAX valeu based in microstepping selection
  limitMAX = ((120 / long(leadScrewPitch)) * fullRot);
}

void starten() {
  digitalWrite(EN_PIN, LOW);
  displayStatus(F("dreht automatisch."));
  displaySoftKeys(Blank_Abort);
  mode = auto_mode;
  attachInterrupt(digitalPinToInterrupt(softButton_2), ISR_Abort, RISING);
  Stepper_M1.setCurrentPosition(0);
  Stepper_M1.setMaxSpeed(drehzahl); 
  Stepper_M1.setAcceleration(Acceleration_M1);
  Stepper_M1.setPinsInverted(richtung, false, false);
  Stepper_M1.moveTo(17400);
  Stepper_M1.run();
  while (!(Stepper_M1.distanceToGo() == 0) && (mode == auto_mode)) Stepper_M1.run();
  Stepper_M1.setCurrentPosition(0);
  // set homing status to TRUE
  if (mode == auto_mode) {
    homingDone = true;
    delay(500);
    displayStatus(F("Auto mode!"));
    displaySoftKeys(Blank_Settings);
    
  } else if (mode == abort_mode) displayNoReference();
  detachInterrupt(digitalPinToInterrupt(softButton_2));
  Stepper_M1.disableOutputs();
  digitalWrite(EN_PIN, HIGH); 
}

//*** Reading encoder values ***
void Encoder1() {
  int rDir = rotary.rotate();

  // Separate Entprellung für Encoder-Taster
  static unsigned long lastButtonPress = 0;
  const unsigned long debounceTime = 50;
  bool buttonPressed = false;

static bool lastRawState = HIGH;
bool rawState = digitalRead(8);

if (lastRawState == HIGH && rawState == LOW && (millis() - lastButtonPress > debounceTime)) {
  buttonPressed = true;
  lastButtonPress = millis();
  Serial.println(F("Button clicked"));
}

lastRawState = rawState;

  // ============================ MODE: setup_mode ============================
  if (mode == setup_mode) {
    if (rDir == 1 && menuIndex <= 2) { // CW → Menü nach unten
      menuIndex++;
      if (menuIndex > encoderMenuMAX) menuIndex = encoderMenuMAX;
      if (menuIndex > menuShift + 2) {
        menuShift = menuIndex - 2;
      }
      adress = menuIndex;
      if (menuIndex != lastMenuIndex || mode == edit_setup_mode) {
        displaySetupMenu();
        lastMenuIndex = menuIndex;
      }
    }

    if (rDir == 2 && menuIndex > 0) { // CCW → Menü nach oben
      menuIndex--;
      if (menuIndex < encoderMenuMIN) menuIndex = encoderMenuMIN;
      if (menuIndex < menuShift) {
        menuShift = menuIndex;
      }
      adress = menuIndex;
      if (menuIndex != lastMenuIndex || mode == edit_setup_mode) {
        displaySetupMenu();
        lastMenuIndex = menuIndex;
      }
    }

    if (buttonPressed) {
      // Klick → Wechsel in Wertbearbeitungsmodus
      mode = edit_setup_mode;

      // Initialwert in ctr laden
      switch (menuIndex) {
        case 0: ctr = encoderMode ? 1 : 0; break;
        case 1: ctr = drehzahl; break;
        case 2: ctr = richtung ? 1 : 0; break;
        default: break;
      }
      displaySetupMenu();
    }
  }

  // ============================ MODE: edit_setup_mode ============================
  else if (mode == edit_setup_mode) {
    if (rDir == 1) { // CW → Wert erhöhen
      switch (menuIndex) {
        case 0: ctr = 1; break;
        case 1: ctr += 100; if (ctr > 1000) ctr = 1000; break;
        case 2: ctr = 1; break;
        default: break;
      }
      displaySetupMenu();
    }

    if (rDir == 2) { // CCW → Wert verringern
      switch (menuIndex) {
        case 0: ctr = 0; break;
        case 1: ctr -= 100; if (ctr < 100) ctr = 100; break;
        case 2: ctr = 0; break;
        default: break;
      }
      displaySetupMenu();
    }

    if (buttonPressed) {
      // Klick → Wert speichern und zurück zu setup_mode
      switch (menuIndex) {
        case 0: encoderMode = (ctr >= 1); break;
        case 1: drehzahl = ctr; break;
        case 2: richtung = (ctr >= 1); break;
        default: break;
      }
      mode = setup_mode;
      displaySetupMenu();
    }
  }
}


//**** Display ****
void splash() {
  display.setTextSize(1);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text
  display.clearDisplay();
  display.drawBitmap((SCREEN_WIDTH - 45) / 2, (SCREEN_HEIGHT - 50) / 2, PIC, 45, 45, 1);
  display.setCursor(7, 54);
  display.print(F("Manne Schmidt"));
  display.setCursor(1, 1);
  display.print(F("v"));
  display.print(fwVersion);
  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();
  display.drawFastHLine(1, display.height() - 20, display.width() - 2, WHITE);
  display.setCursor(1, display.height() - 18);
  display.print(info);
  display.display();
}

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

  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()) {
    // Werte übernehmen & ins Home-Menü zurück
    // (optional EEPROM-Speichern kannst du hier noch ergänzen)

    // Falls man gerade im Edit-Modus war, Änderungen übernehmen
    if (mode == edit_setup_mode) {
      switch (menuIndex) {
        case 0: encoderMode = (ctr >= 1); break;
        case 1: drehzahl = ctr; break;
        case 2: richtung = (ctr >= 1); break;
        default: break;
      }
  }

  mode = auto_mode;
  displayNoReference();
  break;
}



    Encoder1();
  }
}

bool softKey() {
  //debounce button press
  //uint16_t delayIfYouAreUsingCrappyButtons = 250;
  static unsigned long timeStamp = 0;
  if ((timeStamp + 50 < millis())) {
    timeStamp = millis();
    return true;
  }
  return false;
}

void displaySetupMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  for (int i = 0; i < 5; i++) {
    int menuItemIndex = i + menuShift;
    int y = i * 10;

    // Zeilenhervorhebung
    if (menuItemIndex == menuIndex) {
      display.setCursor(0, y);
      display.print("> ");
    } else {
      display.setCursor(0, y);
      display.print("  ");
    }

    switch (menuItemIndex) {
      case 0:
        display.print("Encoder: ");
        if (mode == edit_setup_mode && menuItemIndex == menuIndex) {
          display.print(ctr ? "[ON]" : "[OFF]");
        } else {
          display.print(encoderMode ? "ON" : "OFF");
        }
        break;

      case 1:
        display.print("Speed: ");
        if (mode == edit_setup_mode && menuItemIndex == menuIndex) {
          display.print("[");
          display.print((int)ctr);
          display.print("]");
        } else {
          display.print((int)drehzahl);
        }
        break;

      case 2:
        display.print("Dir: ");
        if (mode == edit_setup_mode && menuItemIndex == menuIndex) {
          display.print(ctr ? "[CW]" : "[CCW]");
        } else {
          display.print(richtung ? "CW" : "CCW");
        }
        break;
    }
  }

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


void displaySoftKeys(int SoftKeyType) {
  const __FlashStringHelper* SoftKey1;
  const __FlashStringHelper* 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_Auto:
      SoftKey1 = F("Auto/Start");
      SoftKey2 = F("  Settings");
      break;
    case Homing_Settings_Manu:
      SoftKey1 = F("drehen");
      SoftKey2 = F("  Settings");
      break;
    case Exit_Save:
      SoftKey1 = F("Exit");
      SoftKey2 = F("      Save & Exit");
      break;
  }

  display.setTextSize(0);       // Normal 1:1 pixel scale
  display.setTextColor(WHITE);  // Draw white text
  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);
  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();
}

void startenManu() {
  digitalWrite(EN_PIN, LOW);
  displayStatus(F("dreht manuell."));
  displaySoftKeys(Blank_Abort);
  mode = manual_mode;
  attachInterrupt(digitalPinToInterrupt(softButton_2), ISR_Abort, RISING);
  Stepper_M1.setMaxSpeed(MAXspeed_M1);   
  Stepper_M1.setPinsInverted(richtung, false, false);
  Stepper_M1.setSpeed(1000);
 
while ((mode == manual_mode) && (digitalRead(softButton_1) == LOW)) {
  Stepper_M1.runSpeed();   
}
  detachInterrupt(digitalPinToInterrupt(softButton_2));
  Stepper_M1.disableOutputs();
  digitalWrite(EN_PIN, HIGH);
}

void displayNoReference() {
  display.clearDisplay(); // Alles löschen, damit nichts überlappt
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);

  // Text oben anzeigen
  if (encoderMode) {
    display.println(F("Drehscheibe steht."));
  }

  // Bild in der Mitte anzeigen
  display.drawBitmap((SCREEN_WIDTH - 45) / 2, (SCREEN_HEIGHT - 66) / 2, PIC, 45, 45, 1);

  // Softkeys am unteren Rand zeichnen
  if (encoderMode) {
    displaySoftKeys(Homing_Settings_Auto);
  } else {
    displaySoftKeys(Homing_Settings_Manu);
  }

  // Jetzt alles auf dem Display anzeigen
  display.display();

  // Eingabeschleife
  while (1) {
    if (!digitalRead(softButton_1) && softKey()) {
      if (encoderMode) {
        starten();  // Auto
      } else {
        startenManu();  // Manuell
      }
      break;
    }

    if (!digitalRead(softButton_2) && softKey()) {
      setupMenu();
      break;
    }
  }
}