#include <Arduino.h>
#include <U8x8lib.h>

#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif

U8X8_SSD1306_128X64_NONAME_HW_I2C u8x8(/* reset=*/U8X8_PIN_NONE);

#define row1 0
#define row2 2
#define row3 4
#define row4 6

#include <EEPROM.h>

#include "FastAccelStepper.h"
FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

#define dir  5
#define enable 6
#define step 9

#define btn_bwd A0
#define btn_fwd A1
#define btn_minus A2
#define btn_plus A3
#define btn_mode 7
#define btn_entrance 10

#define hallsensor1 A6
#define hallsensor2 A7
#define signal_out 11
#define led 13

bool bwdbtnstate = false, fwdbtnstate = false, btnflag = false;
byte mode = 1;

#define analogvalue 350
int op_speed = 2000;  // Operation-Mode while Filament is buffered.
int btn_speed = 2000; // Button-Mode.
unsigned int acceleration = 10000; // 16000
#define accel_incdec 500
#define speed_incdec 100
#define btndelay 250


void setup() {

  op_speed  = EepromRead(0);
  btn_speed = EepromRead(2);
  acceleration = EepromRead(4);
  //delay(500);

  engine.init();
  stepper = engine.stepperConnectToPin(step);
  if (stepper) {
    stepper->setDirectionPin(dir);
    stepper->setEnablePin(enable);
    stepper->setAutoEnable(true);
  }

  u8x8.begin();
  // u8x8.setFont(u8x8_font_px437wyse700b_2x2_r);
  u8x8.setFont(u8x8_font_chroma48medium8_r);
  
  Display();

  pinMode(btn_bwd, INPUT_PULLUP);
  pinMode(btn_fwd, INPUT_PULLUP);
  pinMode(btn_entrance, INPUT_PULLUP);
  pinMode(btn_minus, INPUT_PULLUP);
  pinMode(btn_plus, INPUT_PULLUP);
  pinMode(btn_mode, INPUT_PULLUP);
  pinMode(hallsensor1, INPUT);
  pinMode(hallsensor2, INPUT);

  pinMode(signal_out, OUTPUT);
  pinMode(led, OUTPUT);
} // --------------- Setup-Klammer ----------------------------------------------


void loop() {

  Buttons();

  if (btnflag == true) {
    stepper->setSpeedInHz(btn_speed);  //stepper->setSpeedInUs(speed);
  } else {
    stepper->setSpeedInHz(op_speed);
  }
  stepper->setAcceleration(acceleration);

  Hallsensors();

}  // ------------------------------------------ Loop-Klammer ------------------------
void Buttons() {

  // --- Button Bwd -----------------------------------------------------------------
  if (digitalRead(btn_bwd) == LOW) {
    btnflag = true;
    bwdbtnstate = true;
    stepper->runForward();  //reversed by TMC2208
  } else if (digitalRead(btn_bwd) == HIGH && bwdbtnstate == true) {
    stepper->stopMove();
    btnflag = false;
    bwdbtnstate = false;
  }


  // --- Button Fwd -----------------------------------------------------------------
  if (digitalRead(btn_fwd) == LOW) {
    btnflag = true;
    fwdbtnstate = true;
    stepper->runBackward();  //reversed by TMC2208
  } else if (digitalRead(btn_fwd) == HIGH && fwdbtnstate == true) {
    stepper->stopMove();
    btnflag = false;
    fwdbtnstate = false;
  }


  // --- Button Mode -----------------------------------------------------------------
  if (digitalRead(btn_mode) == LOW) {
    mode++;
    if (mode > 3) {
      mode = 1;
    }
    delay(btndelay);
    Display();
  }


  // --- Button Minus (-)  -----------------------------------------------------------------
  if (digitalRead(btn_minus) == LOW) {
    switch (mode) {
      case 1:
        op_speed = op_speed - speed_incdec;
        break;
      case 2:
        btn_speed = btn_speed - speed_incdec;
        break;
      case 3:
        acceleration = acceleration - accel_incdec;
        break;
    }
    Display();
    delay(btndelay);
  }

  // --- Button Plus (+) -------------------------------------------------------------------
  if (digitalRead(btn_plus) == LOW) {
    switch (mode) {
      case 1:
        op_speed = op_speed + speed_incdec;
        break;
      case 2:
        btn_speed = btn_speed + speed_incdec;
        break;
      case 3:
        acceleration = acceleration + accel_incdec;
        break;
    }
    Display();
    delay(btndelay);
  }



  // ---  Entrance Switch -----------------------------------------------------------------
  if (digitalRead(btn_entrance) == LOW) {
    digitalWrite(signal_out, LOW);  // Signal must be Active-LOW
    digitalWrite(led, HIGH);
  } else {
    digitalWrite(signal_out, HIGH);
    digitalWrite(led, LOW);
  }


  // --- Buttons Eeprom Save (Push + and - together) -----------------------------------------
  if (digitalRead(btn_minus) == LOW && digitalRead(btn_plus) == LOW) {
    EepromWrite(0, op_speed);  // Adress, variable
    EepromWrite(2, btn_speed);
    EepromWrite(4, acceleration);
    u8x8.clear();  // Clear Display
    u8x8.setInverseFont(1);
    u8x8.drawString(0, row3, "! Eeprom saved !");
    u8x8.setInverseFont(0);
    delay(1000);
    u8x8.clear();  // Clear Display
    Display();
    
  }
}
void Hallsensors() {

  int sensor1 = analogRead(hallsensor1), sensor2 = analogRead(hallsensor2);

  // --- Hallsensor 1 -----------------------------------------------------------------
  if (sensor1 < analogvalue && sensor2 > analogvalue && btnflag == false) {
    stepper->forceStop();  // stepper->stopMove();
    u8x8.setCursor(0, row4);
    u8x8.print("             ");
  }

  // --- Hallsensor 2 -----------------------------------------------------------------
  if (sensor1 > analogvalue && sensor2 < analogvalue && btnflag == false) {
    stepper->runBackward();  // reversed by TMCC2208

    // u8x8.clear();            // Clear Display
    u8x8.setInverseFont(1);  // Inverse Font
    u8x8.setCursor(0, row4);
    u8x8.print("  ..Buffering.. ");
    u8x8.setInverseFont(0);
  }
}
// Here I'll be converting the tiny4koled code with u8g2 to work with the OLED display.
void Display() {

  if (op_speed <= 100) {
    op_speed = 100;
  }
  if (btn_speed <= 100) {
    btn_speed = 100;
  }
  // u8x8.clear();  // Clear Display
  u8x8.drawString(1, row1, "Op.Speed");

  u8x8.drawString(1, row2, "Btn.Speed");

  u8x8.drawString(1, row3, "Accel.");

  int noRow = 12; // Set X axis to dislay value
  switch (mode) {
    case 1:
      u8x8.setInverseFont(1);
      u8x8.setCursor(noRow, row1);
      u8x8.print(op_speed / 100);
      u8x8.setInverseFont(0);
      u8x8.print("  ");
      u8x8.setCursor(noRow, row2);
      u8x8.print(btn_speed / 100);
      u8x8.setCursor(noRow - 2, row3);
      u8x8.print(acceleration);
      u8x8.print("  ");

      break;
    case 2:
      u8x8.setCursor(noRow, row1);
      u8x8.print(op_speed / 100);
      u8x8.setInverseFont(1);
      u8x8.setCursor(noRow, row2);
      u8x8.print(btn_speed / 100);
      u8x8.setInverseFont(0);
      u8x8.print("  ");
      u8x8.setCursor(noRow - 2, row3);
      u8x8.print(acceleration);
      break;
    case 3:
      u8x8.setCursor(noRow, row1);
      u8x8.print(op_speed / 100);
      u8x8.setCursor(noRow, row2);
      u8x8.print(btn_speed / 100);
      u8x8.setInverseFont(1);
      u8x8.setCursor(noRow - 2, row3);
      u8x8.print(acceleration);
      u8x8.setInverseFont(0);
      u8x8.print("  ");

      break;
  }
}
void EepromWrite(int address, unsigned int number) {
  EEPROM.update(address, number >> 8);
  EEPROM.update(address + 1, number & 0xFF);
}

unsigned int EepromRead(int address) {
  return (EEPROM.read(address) << 8) + EEPROM.read(address + 1);
}