/************************************************************************************
 * 
 * Simple ELS V1.0.1 für ESP32 Dualcore
 * 
 * (c)2021 Frohnix Bastelbude - Frank Frohnert
 * 
 * Kommerzielle Verwendung/Nutzung & Verkauf ist ausdrücklich untersagt.
 * 
 * Um die Software nutzen zu können muss die Arduino Library TMC5160 von tommag
 * installiert werden:
 * 
 *  https://github.com/tommag/TMC5160_Arduino
 *  
 *  und die ESP32 Encoder Library
 *  https://github.com/madhephaestus/ESP32Encoder/
 *  
 *  V1.0.1:
 *  - "Zustellung" in "Vorschub" umbenannt
 *  
 *************************************************************************************/ 






#include <Preferences.h>
#include <Arduino.h>
#include <ESP32Encoder.h>
#include <U8g2lib.h>
#include <SPI.h>
#include <Wire.h>
#include <TMC5160.h>

# define ENCODER_A 14 // Drehzahlencoder Pin A
# define ENCODER_B 27 // Drehzahlencoder Pin B
# define BUTLEFT 12   // Links fahren
# define BUTRIGHT 25  // Rechts fahren
# define BUTSTILL 26  // Keine fahrt
# define BUTZERO 33   // 0 setzen, schnellfahrt zu 0
# define BUTTARGET 32 // Ziel setzen, schnellfahrt zu Ziel

int VERSION1=1;
int VERSION2=0;
int VERSION3=1;

// UPM Encoder
ESP32Encoder encoder;
int64_t encoderLastUpm   = 0;     // letzte Position des Encoders (UPM) in Steps *4 (Quad)
int64_t encoderAct    = 0;      // aktue Position des Encoders in Steps *4 (Quad)
int     encoderSteps  = 600; // volle Steps des Encoders
int     encoderDoSteps = 0; // Schritte, die der Encoder gemacht hat
int64_t encoderLastSteps = 0; // Tempvariable für encoderDoSteps
int64_t encoderManTicks = 0; // schritte für eine mauelle Änderung der Position

// UPM
int     upm           = 0; // Umdrehung pro Minute
int     upmMillisMeasure = 500; // Messzeit für UPM in Millisekunden
int     upmMillisTemp = 0; // Tempspeicher für millis
int64_t maxUpm = 0; // maximale, fehlerfreie UPM für Drehbank. Wird errechnet aus dem Vorschub

// EEPROMVars
float threadPitch = 1.5; // Gewindesteigung in mm
float spindleMmPerRound = 0.1; // Leitspindelweg pro umdrehung in mm
int motorAcc = 8000; // Beschleunigung
int motorDec = 16000; // Bremsen
int motorMaxSpeed = 2500; // Höchstgeschwindigkeit
int globalScaler = 98;
float backlash = 0;
bool posLimits = false; // innerhalb von 0 - Ziel bewegen
bool targetAbsolute = true; // Ziel ist Absolut
bool motorDirection = false;
bool encoderDirection = false;

// MainVars
int   stepperSteps = 200; // Schrittmotor Schritte pro Umdrehung
float stepperStepsPerEncoderSteps = 0; // Leitspindel Schrittmotor Schritte pro Drehschritt (errechnet sich aus stepper Steps, threadPitch und spindleMmPerRound)
float stepperPos = 0; // Sollposition des Steppers
float mmPos = 0; // Sollposition in mm
float stepperTarget = 0; // Zielposition des Steppers
float mmTarget = 0; // Zielposition in mm
float motorPos = 0; // Motorposition in Steps
float motorPosMm = 0; // motorposition in mm
float encoderDeg = 0; // Winkel des encoders;
bool isSetupEnter = false;
bool isSetup = false;
int setupPage = 0;
int posManDir = 0; // richtung für manuelles verschieben
int posManDirTmp = 0; // alter Wert von PosManDir
int motorManAcc = 0; // Beschleunigung im manuellem Mode
int64_t posDiff = 0; // Absolut Differenz motor Sollwert zu Istwert

float motorPositionOnSetup = 0;
float ping = 0;
float pong = 0;
bool pingPong = false;
int64_t pingPongPause = 0;


int64_t milliTicker = 100;
int64_t milliTickerTmp = 0;

int64_t milliTicker2 = 100;
int64_t milliTickerTmp2 = 0;

int driveDirection = 0; // Spindel Laufrichtung 0= stop, 1=positiv , -1 = negativ
bool waitForStart=false;

// RestVars
int xAni = 0;
bool butLeftPressed = false;
bool butRightPressed = false;
int64_t butLeftRightPressedCount = 0;
bool butStillPressed = false;
int64_t butStillPressedCount = 0;
bool butZeroPressed = false;
int64_t butZeroPressedCount = 0;
bool butTargetPressed = false;
int64_t butTargetPressedCount = 0;

bool goZero = false; // Fahr schnell zu 0
bool goTarget = false; // Fahr schnell zu Ziel


bool lastLR = false; // für richtungsänderungsdetektion (false=L, true=R)
bool aktLR = false; // für richtungsänderungsdetektion (false=L, true=R)
bool driveLR = false;

int64_t encoderCalcBase = 0;

int messSteps=0;
int64_t milliSim=0;

bool zeroDeg=false; // wird auf true gesetzt wenn 0° Punkt erreicht
int64_t encoderISRCount=0;
bool dispZeroDeg=false;
int64_t backlashPos=0;
bool correctZeroPos=false;
bool correctTargetPos=false;
int lastBacklashAction=0;
float oldMotorPos=0;
int lastMotorAction=0;
bool noBacklash=false;
int64_t encoderAbs=0;
int64_t raendelVersatz=60;
int64_t raendelVersatzSum=0;
bool startRaendel=false;
bool raendelMode=false;  // <----- nicht aktivieren, lässt sich nicht stoppen.

// *********************************** SPI Display ****************************************
U8G2_SSD1306_128X64_NONAME_F_4W_HW_SPI oledSPI(U8G2_R0, /* cs=*/ 5, /* dc=*/ 16, /* reset=*/ 17);

// *********************************** SPI TMC5160 ****************************************
TMC5160_SPI motor = TMC5160_SPI(4); //Use default SPI peripheral and SPI settings.
Preferences preferences;
TaskHandle_t Task1; // Dualcore
TMC5160::PowerStageParameters powerStageParams; // defaults.
TMC5160::MotorParameters motorParams;

void IRAM_ATTR encoderISR() { 
  encoderAbs= abs(encoder.getCount()/4)%(encoderSteps);
  if(encoderAbs==0){
    zeroDeg=true;
    dispZeroDeg=true;
  }
  if(encoderAbs==raendelVersatzSum && startRaendel==false && raendelMode==true ){
    startRaendel=true;
    raendelVersatzSum=raendelVersatzSum+raendelVersatz;
  }
  if(raendelVersatzSum>=encoderSteps){
    raendelVersatzSum=0;
  }
}

void setup() {
  Serial.begin(115200);
  Serial.print("Simple ELS V");
  Serial.print(VERSION1);
  Serial.print(".");
  Serial.print(VERSION2);
  Serial.print(".");
  Serial.println(VERSION3);
  
  preferences.begin("simpleELS", false);
  threadPitch          = preferences.getFloat("threadPitch", threadPitch);
  spindleMmPerRound    = preferences.getFloat("spindleMmPR", spindleMmPerRound);
  motorAcc             = preferences.getInt("motorAcc", motorAcc);
  motorDec             = preferences.getInt("motorDec", motorDec);
  motorMaxSpeed        = preferences.getInt("motorMaxSpeed", motorMaxSpeed);
  globalScaler         = preferences.getInt("globalScaler", globalScaler);
  backlash             = preferences.getFloat("backlash", backlash);
  posLimits            = preferences.getBool("posLimits", posLimits);
  targetAbsolute       = preferences.getBool("targetAbs", targetAbsolute);
  motorDirection       = preferences.getBool("motorDir", motorDirection);
  encoderDirection     = preferences.getBool("encoderDir", encoderDirection);
  encoderSteps         = preferences.getInt("encoderStp", encoderSteps);
  
  ESP32Encoder::useInternalWeakPullResistors = UP;
  encoder.attachFullQuad(ENCODER_A, ENCODER_B);
  encoder.clearCount();
  pinMode(BUTLEFT,   INPUT_PULLUP);
  pinMode(BUTRIGHT,  INPUT_PULLUP);
  pinMode(BUTSTILL,  INPUT_PULLUP);
  pinMode(BUTZERO,   INPUT_PULLUP);
  pinMode(BUTTARGET, INPUT_PULLUP);

  pinMode(4, OUTPUT);






  xTaskCreatePinnedToCore(
    Task1code, /* Function to implement the task */
    "Task1", /* Name of the task */
    10000,  /* Stack size in words */
    NULL,  /* Task input parameter */
    2,  /* Priority of the task */
    &Task1,  /* Task handle. */
    0); /* Core where the task should run */





  powerStageParams.drvStrength = 0;
  powerStageParams.bbmTime = 8;
  powerStageParams.bbmClks = 0;
  motorParams.globalScaler = globalScaler; // 150 max?
  motorParams.irun = 31;
  motorParams.ihold = 20;
  motorParams.freewheeling = TMC5160_Reg::FREEWHEEL_NORMAL;
  motorParams.pwmOfsInitial = 30;  //36
  motorParams.pwmGradInitial = 0;  //73

  // init motor
  // ---------------------
  motor.begin(powerStageParams, motorParams, TMC5160::NORMAL_MOTOR_DIRECTION);
  motor.setModeChangeSpeeds(1, 0, 16000); // 400,0,1500
  motor.setRampMode(TMC5160::POSITIONING_MODE);
  motor.setMaxSpeed(motorMaxSpeed);
  motor.setRampSpeeds(0, 0.1, 100); //Start, stop, threshold speeds  
  motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1
  motor.setCurrentPosition(0, true);

  // --------------------- 
}



int64_t getEncoderCount() {
  if (encoderDirection == false) {
    return encoderCalcBase + encoder.getCount();
  } else {
    return encoderCalcBase - encoder.getCount();
  }
}
void setEncoderDirection(bool dir) {
  encoderCalcBase = encoder.getCount();
  encoder.setCount(0);
  encoderLastSteps = encoderCalcBase;
  encoderDirection = dir;
}
float motorGetCurrentPosition(){
  float a= motor.getCurrentPosition();
  if(isnan(a)){
    return 0;
  }else{
    return a;
  }
}
void loop() {  
  
  // Differenz der Soll-/Istwert schritte des Motors berechnen
  posDiff = abs(motorGetCurrentPosition() - motor.getTargetPosition());

  // Aktuelle Drehzahlencoder Position
  encoderLastSteps = encoderAct;
  encoderAct = getEncoderCount();

  // Simulator
  /*
  if(millis()>=milliSim+1){
    milliSim=milliSim+1;
    encoderAct=encoderAct+20;
  }
  */
  // max UPM errechnen
  maxUpm = (motorMaxSpeed * 60) / ((spindleMmPerRound / threadPitch) * 200);

  // Stepperschritte pro Drehzahlencoder Schritt berechnen
  stepperStepsPerEncoderSteps = (spindleMmPerRound / (threadPitch / stepperSteps)) / (encoderSteps * 4);

  // Winkel berechnen
  encoderDeg = float(encoderAct % (encoderSteps * 4)) * 360 / (encoderSteps * 4);
  if (encoderDeg < 0) {
    encoderDeg = 360 + encoderDeg;
  }
  //if((stepperPos==0 || stepperPos==stepperTarget) && zeroDeg==false){
  //  encoderLastSteps=encoderAct;
  //}

  if(stepperPos==0 || stepperPos==stepperTarget){    
    waitForStart=true;
  }
  if(zeroDeg==true && waitForStart==true && (stepperPos==0 || stepperPos==stepperTarget)){
    waitForStart=false;    
  }
  if(zeroDeg==true){
    Serial.println(1000);
    zeroDeg=false;
  }
  

  // Sollschritte für stepper errechnen
  if (encoderAct != encoderLastSteps && driveDirection != 0 && isSetup == false) { // schrittdifferenz des Drehzahlencoders auswerten
    if(waitForStart==false){
      encoderDoSteps = (encoderAct - encoderLastSteps) * driveDirection;
    }else{
      encoderDoSteps=0;
    }
    
    if (posLimits == true) {
      if(stepperPos+(encoderDoSteps*stepperStepsPerEncoderSteps)<0 && stepperTarget>0){       
        encoderDoSteps=0;
        correctZeroPos=true;        
      }else
      if(stepperPos+(encoderDoSteps*stepperStepsPerEncoderSteps)>stepperTarget && stepperTarget>0){         
        encoderDoSteps=0;
        correctTargetPos=true;      
      }else
     if(stepperPos+(encoderDoSteps*stepperStepsPerEncoderSteps)<stepperTarget && stepperTarget<0){         
        encoderDoSteps=0; 
        correctTargetPos=true;      
      }else
      if(stepperPos+(encoderDoSteps*stepperStepsPerEncoderSteps)>0 && stepperTarget<0){         
        encoderDoSteps=0;
        correctZeroPos=true;        
      }        
    } 
      
  } else {
    encoderDoSteps = 0;
  }
  if(isSetup==true){
    driveDirection=0;
  }
  //encoderLastSteps = encoderAct;

  // Handverstellung als encoderDoSteps addieren/subtrahieren
  if(posManDir!=0){
    encoderManTicks = 2000;
    if (posDiff < encoderManTicks) {
      motor.setAccelerations((motorAcc/20), motorDec, 15, round(float(motorDec)/3*2));
      encoderDoSteps = (encoderManTicks * posManDir);
    }    
  }
  if(posManDirTmp!=0 && posManDir==0){    
    motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1   
    killMotor();    
  }
  posManDirTmp=posManDir;
  
  // Drehzahl berechnen
  if (millis() >= upmMillisTemp + upmMillisMeasure) {
    upm = (abs(encoderAct - encoderLastUpm) * 60000 / (millis() - upmMillisTemp)) / (encoderSteps * 4);
    encoderLastUpm = encoderAct;
    upmMillisTemp = upmMillisTemp + upmMillisMeasure;
  }

  // Schrittmotor Schritte und mm aktualisieren
  if (encoderDoSteps != 0) { // Step und Pos Daten auffrischen
    stepperPos = stepperPos + (stepperStepsPerEncoderSteps * encoderDoSteps);
  }

  // Rändeln
  if(raendelMode==true){
    if(startRaendel==true && goTarget==false && goZero==false){
      if(stepperPos==0){
        goTarget=true;
      }
      if(stepperPos==stepperTarget){
        goZero=true;
      }      
    }
  }
  

  // Schritte Sollwert zum Motor Senden
  if (isSetup == false) {
    if (goTarget == false && goZero == false) {
      if(backlash!=0){
        if(noBacklash==false){
          doBacklash();
        }else{
          noBacklash=false;
        }
        
      }
      
      
      if(correctZeroPos==false && correctTargetPos==false){
        motor.setTargetPosition(stepperPos);
      }else{
        if(correctZeroPos==true){
          motor.setTargetPosition(0);
          while(motorGetCurrentPosition()!=0 && motor.getCurrentSpeed()!=0){  }
          doBacklashZero();
          /*
          if(stepperTarget<0){          
            //doBacklash(-1);
          }
          if(stepperTarget>0){
            //doBacklash(1);
          }          
          stepperPos=0;*/
        }
        if(correctTargetPos==true){
          motor.setTargetPosition(stepperTarget);
          while(motorGetCurrentPosition()!=stepperTarget && motor.getCurrentSpeed()!=0){  }
          doBacklashTarget();          
          /*
          if(stepperTarget<0){          
            //doBacklash(1);
          }
          if(stepperTarget>0){
            //doBacklash(-1);
          }          
          motor.setTargetPosition(stepperTarget);
          while(motor.getCurrentSpeed()!=0){  }
          stepperPos=stepperTarget;*/ 
        }
        correctZeroPos=false;
        correctTargetPos=false;
        driveDirection=0;
        
      }
      mmPos = stepperPos * threadPitch / stepperSteps;
      mmTarget = stepperTarget * threadPitch / stepperSteps;
      motorPos=motorGetCurrentPosition();
      if(motorPos!=0){
        motorPosMm=motorPos * threadPitch / stepperSteps;
      }else{
        motorPosMm=0;
      }
      messSteps++;
      if(messSteps>20){ 
        
        /*     
        Serial.print(stepperPos);
        Serial.print(", ");        
        Serial.print(motor.getCurrentPosition());
        Serial.print(", ");        
        Serial.print(motor.getTargetPosition());
        Serial.print(", ");        
        Serial.println(stepperPos-motor.getCurrentPosition());
        
       
        /*
        float diff=float(upm)/threadPitch*spindleMmPerRound*470/float(motorAcc);
        Serial.print(abs(stepperPos-motor.getCurrentPosition()));
        Serial.print(", ");
        Serial.print(motor.getCurrentSpeed());
        Serial.print(", ");
        Serial.print(upm);
        Serial.print(", ");
        Serial.print(diff);
        Serial.print(" -> ");
        Serial.println(diff-abs(stepperPos-motor.getCurrentPosition()));
        */
        
        messSteps=0;
      }
    } else {

      if (goZero == true && stepperPos!=0) {
        doBacklash();        
        motor.setTargetPosition(0);
      }else{
        goZero=false;
      }
      if (goTarget == true && stepperPos!=stepperTarget) { 
        doBacklash();       
        motor.setTargetPosition(stepperTarget);        
      }else{
        goTarget=false;
      }
      if(motorGetCurrentPosition()!=0){
        mmPos = motorGetCurrentPosition() * threadPitch / stepperSteps;
      }else{
        mmPos=0;
      }
      motorPosMm=mmPos;
      
      
      if (goTarget == true && motorGetCurrentPosition()==stepperTarget) {        
        doBacklashTarget();        
        goTarget=false;
        stepperPos=stepperTarget;
        startRaendel=false;
        
      }
      if (goZero == true && motorGetCurrentPosition()==0) {
        doBacklashZero();
        goZero=false;
        stepperPos=0;
        startRaendel=false;
       
      }      
    }
  }
  motorPos=motorGetCurrentPosition();
  if(motorPos!=oldMotorPos){
    if(motorPos>oldMotorPos){lastMotorAction=1;}
    if(motorPos<oldMotorPos){lastMotorAction=-1;}
    oldMotorPos=motorPos;
  }
  
}
void doBacklashZero(){
  stepperPos = 0;
  encoderDoSteps=0;
  if(stepperTarget<0 && lastMotorAction==1){
    doBacklashRight();
  }
  if(stepperTarget>0 && lastMotorAction==-1){
    doBacklashLeft();
  } 
  if(stepperTarget==0){
    if(stepperPos<0 && lastMotorAction==-1){
      doBacklashRight();       
    }
    if(stepperPos>0 && lastMotorAction==1){
      doBacklashLeft();       
    }
  }
  motorPos=0;
  oldMotorPos=0;
  stepperPos=0; 
}

void doBacklashTarget(){
  stepperPos = stepperTarget;
  encoderDoSteps=0;
  if(stepperTarget<0 && lastMotorAction==-1){          
    doBacklashLeft();
  }
  if(stepperTarget>0 && lastMotorAction==1){
    doBacklashRight();
  }
  if(stepperTarget==0){
    if(stepperPos<0 && lastMotorAction==-1){
      doBacklashRight();       
    }
    if(stepperPos>0 && lastMotorAction==1){
      doBacklashLeft();       
    }
  }        
  motor.setTargetPosition(stepperTarget);
  while(motor.getCurrentSpeed()!=0){  }
  motorPos=stepperTarget;
  oldMotorPos=stepperTarget;
  stepperPos=stepperTarget;  
}

void doBacklashLeft(){  
  lastMotorAction=1;
  lastBacklashAction=-1;  
  doBacklash();
}
void doBacklashRight(){  
  lastMotorAction=-1;
  lastBacklashAction=1;  
  doBacklash();
}

void doBacklash() {    
    if (lastMotorAction==1 && lastBacklashAction!=lastMotorAction) {    
      backlashPos=motorGetCurrentPosition();
      motor.setCurrentPosition(backlashPos - backlash);
      motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1
      motor.setTargetPosition(backlashPos);
      while(backlashPos!=motorGetCurrentPosition() && motor.getCurrentSpeed()!=0){  }
      motorPos=motorGetCurrentPosition();
      oldMotorPos=motorPos;
      lastMotorAction=1;
      //stepperPos=motor.getCurrentPosition();
      stepperPos=backlashPos;      
      lastBacklashAction=lastMotorAction;
    }
    if (lastMotorAction==-1 && lastBacklashAction!=lastMotorAction) {    
      backlashPos=motorGetCurrentPosition();
      motor.setCurrentPosition(backlashPos + backlash);
      motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1
      motor.setTargetPosition(backlashPos);
      while(backlashPos!=motorGetCurrentPosition()  && motor.getCurrentSpeed()!=0){  }
      motorPos=motorGetCurrentPosition();
      oldMotorPos=motorPos;
      lastMotorAction=-1;
      //stepperPos=motorGetCurrentPosition();
      stepperPos=backlashPos;      
      lastBacklashAction=lastMotorAction;
    }  
}

void killMotor(){
    noBacklash=true;
    float mpDiff=motorPos-oldMotorPos;    
    motor.stop();
    while(motor.getCurrentSpeed()!=0){}
    stepperPos=motorGetCurrentPosition();    
    motor.setMaxSpeed(motorMaxSpeed);
    motor.setRampSpeeds(0, 0.1, 100); //Start, stop, threshold speeds  
    motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1 
    motor.setTargetPosition(stepperPos);
    while(motor.getCurrentSpeed()!=0){}    
    stepperPos=motorGetCurrentPosition();       
    encoderDoSteps=0;
    motorPos=stepperPos;
    oldMotorPos=stepperPos+mpDiff;   
}
// ********************************************************************************************************************************************************************

void Task1code( void * parameter) {

  oledSPI.begin();
  attachInterrupt(digitalPinToInterrupt(ENCODER_A), encoderISR, FALLING);
  for (;;) {
    int klickCountFactor=1; // Multiplikator für die KlickCounter
    oledSPI.clearBuffer();
    if (isSetupEnter == false && isSetup == false) {
      displayMain(upm, motorPosMm, mmTarget, spindleMmPerRound, encoderDeg, maxUpm);
    }
    if (isSetupEnter == true) {
      displaySetupEnter();
      if (isSetup == false) {
        motorPositionOnSetup = motorGetCurrentPosition();
      }
    }
    if (isSetup == true) {
      if (setupPage < 0) {
        setupPage = 11;
      }
      if (setupPage > 11) {
        setupPage = 0;
      }
      displaySetup(setupPage);
    }


    oledSPI.sendBuffer();

    if (digitalRead(BUTLEFT) == LOW) {
      butLeftPressed = true;
      if (isSetup == false) {
        butLeftRightPressedCount++;
        if (butLeftRightPressedCount > 20 * klickCountFactor) {
          posManDir = -1;
          driveDirection=0;
        }
      }
    }
    if (digitalRead(BUTRIGHT) == LOW) {
      butRightPressed = true;
      if (isSetup == false) {
        butLeftRightPressedCount++;
        if (butLeftRightPressedCount > 20 * klickCountFactor) {
          posManDir = 1;
          driveDirection=0;
        }
      }
    }
    if (digitalRead(BUTSTILL) == LOW) {
      butStillPressed = true;
      butStillPressedCount++;
      if (butStillPressedCount > 50 * klickCountFactor) {
        if (isSetup == false) {
          isSetupEnter = true;
        }
        if (isSetup == true) {
          isSetup = false;
          butStillPressedCount = 0;
          setValues();
        }
      }
    }
    if (digitalRead(BUTZERO) == LOW) {
        delay(5);
        if (digitalRead(BUTZERO) == LOW) {
          butZeroPressed = true;
          if (isSetup == false) {
            butZeroPressedCount++;
            if (butZeroPressedCount > 25 * klickCountFactor) {
              if (targetAbsolute == true) {
                stepperTarget = stepperTarget - stepperPos;
              }             
              stepperPos = 0;
              encoderDoSteps=0;              
              motor.setCurrentPosition(0);
              oldMotorPos=0;  
            }
          }
      }
    }
    if (digitalRead(BUTTARGET) == LOW) {
      delay(5);
      if (digitalRead(BUTTARGET) == LOW) {
        butTargetPressed = true;
        if (isSetup == false) {        
          butTargetPressedCount++;
          if (butTargetPressedCount > 25 * klickCountFactor) {
            stepperTarget = stepperPos;                 
          }
        }
      }
    }

    if (digitalRead(BUTZERO) == HIGH && butZeroPressed == true) {
      delay(5);
      if (digitalRead(BUTZERO) == HIGH) {      
        butZeroPressed = false;
        driveDirection=0;
        if (isSetup == false) {
          if (butZeroPressedCount <= 25 * klickCountFactor) {
            if(stepperTarget>0){
              if(goZero==false){            
                goZero = true;
              }else{
                goZero=false;  
                killMotor();                               
              }
            }
            if(stepperTarget<0){
              if(goTarget==false){            
                goTarget = true;
              }else{
                goTarget=false;
                killMotor();                                               
              }
            }
            if(stepperTarget==0 && stepperPos>0){
              if(goZero==false){            
                goZero = true;
              }else{
                goZero=false;  
                killMotor();                               
              }
            }
          }
          butZeroPressedCount = 0;
        } else {
          setupPage--;
          if (setupPage < 0) {
            setupPage = 11;
          }
        }
        milliTicker2 = 300;
      }
    }
    if (digitalRead(BUTTARGET) == HIGH && butTargetPressed == true) {
      delay(5);
      if (digitalRead(BUTTARGET) == HIGH) {
        butTargetPressed = false; 
        driveDirection=0;     
        if (isSetup == false) {
          if (butTargetPressedCount <= 25 * klickCountFactor) {
            if(stepperTarget>0){
              if(goTarget==false){            
                goTarget = true;
              }else{
                goTarget=false;
                killMotor();                                               
              }
            }
            if(stepperTarget<0){
              if(goZero==false){            
                goZero = true;
              }else{
                goZero=false;  
                killMotor();                               
              }
            }
            if(stepperTarget==0 && stepperPos<0){
              if(goTarget==false){            
                goTarget = true;
              }else{
                goTarget=false;  
                killMotor();                               
              }
            }
          }
          butTargetPressedCount = 0;
        } else {
          setupPage++;
          if (setupPage > 11) {
            setupPage = 0;
          }
        }
        milliTicker2 = 300;
      }
    }

    if (digitalRead(BUTLEFT) == HIGH && butLeftPressed == true) {      
      if (butLeftRightPressedCount <= 20 * klickCountFactor) { 
        if(posLimits==true){
          if((stepperPos<=stepperTarget && stepperPos>0 && stepperTarget>0) || (stepperPos<=0 && stepperPos>stepperTarget && stepperTarget<0)){
            driveDirection = -1;
          }
        }else{
          driveDirection= -1;
        }
        
      }
      butLeftPressed = false;
      posManDir = 0;
      butLeftRightPressedCount = 0;
      milliTicker2 = 300;
    }
    if (digitalRead(BUTRIGHT) == HIGH && butRightPressed == true) {      
      if (butLeftRightPressedCount <= 20 * klickCountFactor) {
        if(posLimits==true){
          if((stepperPos>=0 && stepperPos<stepperTarget && stepperTarget>0) || (stepperPos>=stepperTarget && stepperPos<0 && stepperTarget<0)){
            driveDirection = 1;
          }
        }else{
          driveDirection= 1;
        }  
          
                
      }
      butRightPressed = false;
      posManDir = 0;
      butLeftRightPressedCount = 0;
      milliTicker2 = 300;
    }
    if (digitalRead(BUTSTILL) == HIGH && butStillPressed == true) {      
      driveDirection = 0;
      butStillPressed = false;
      if (isSetupEnter == true) {
        isSetup = true;
        isSetupEnter = false;
      }
    }
    if (digitalRead(BUTSTILL) == HIGH) {
      butStillPressedCount = 0;
    }


    delay(20);
  }
}


void displaySetup(int page) {
  if (page == 0) {
    spindleMmPerRound = displaySetupPartFloat("Vorschub", "mm/Umdr.", spindleMmPerRound, 0.01, 10.00, 1,0.01);
  }
  if (page == 1) {
    targetAbsolute = displaySetupPart("Zielverhalten", "Ziel bei Nullpunktaenderung", targetAbsolute, "  Absolut", "   Relativ");
  }
  if (page == 2) {
    posLimits = displaySetupPart("Fahrverhalten", "Bew. nur zw. 0 und Ziel", posLimits, "  Inaktiv", "     Aktiv");
  }
  if (page == 3) {
    motorAcc = displaySetupPart("Mot. Beschl.", "Schritte/sek", motorAcc, 100, 16700, 100, 200);
  }
  if (page == 4) {
    motorDec = displaySetupPart("Mot. Bremse", "Schritte/sek", motorDec, 100, 16700, 100, 200);
  }
  if (page == 5) {
    motorMaxSpeed = displaySetupPart("Mot. Geschw.", "UpM", motorMaxSpeed, 100, 3000, 1, 2,0.3);
  }
  if (page == 6) {
    globalScaler = displaySetupPart("Motorstrom", "A", globalScaler, 1, 230, 1, 5, 0.01211);
  }
  if (page == 7) {
    backlash = displaySetupPartFloat("Spindelspiel", "Schritte", backlash, 0, 400, 1,0.1);
    // backlashautomatik
    // setCurrentPosition
    // getCurrentPosition
    // setTargetPosition
    // getTargetPosition
    ping = motorPositionOnSetup;
    pong = motorPositionOnSetup + backlash;

    if (motor.getCurrentSpeed() == 0 && backlash > 0 && millis() > pingPongPause) {
      pingPong = !pingPong;
      pingPongPause = millis() + 500;
    }
    if (pingPong == true) {
      motor.setTargetPosition(ping);
    } else {
      motor.setTargetPosition(pong);
    }
  }
  if (page == 8) {
    motorDirection = displaySetupPart("Drehrichtung", "Drehrichtung des Motors", motorDirection, "   Links", "   Rechts");
  }
  if (page == 9) {
    encoderDirection = displaySetupPart("Messrichtung", "Drehrichtung des Encoders", encoderDirection, "   Links", "   Rechts");
  }
  if (page == 10) {
    encoderSteps = displaySetupPart("Encoder Aufl.", "Schritte", encoderSteps,100,1000,1,2);
  }
  if (page == 11) {
    threadPitch = displaySetupPartFloat("Leitsp.Steigung", "mm", threadPitch, 0.5, 10, 1,0.01);
  }

}
// float displaySetupPart(String title, String valueUnit, float value, float minValue, float maxValue, float factor, float minStep)
// bool  displaySetupPart(String title, String description, bool value, String value0Name, String value1Name)
// int   displaySetupPart(String title, String valueUnit, int value, int minValue, int maxValue, int stepChangeSmall, int stepChangeLarge)
// int   displaySetupPart(String title, String valueUnit, int value, int minValue, int maxValue, int stepChangeSmall, int stepChangeLarge, float factor)


void setValues() {
  detachInterrupt(digitalPinToInterrupt(ENCODER_A));
  delay(250);
  if (motorDirection == false) {
    motor.begin(powerStageParams, motorParams, TMC5160::NORMAL_MOTOR_DIRECTION);
  } else {
    motor.begin(powerStageParams, motorParams, TMC5160::INVERSE_MOTOR_DIRECTION);
  }
  motor.setMaxSpeed(motorMaxSpeed);
  //motor.setAcceleration(motorAcc);
  motor.setAccelerations(motorAcc, motorDec, round(float(motorAcc)/3*2), round(float(motorDec)/3*2)); //AMAX, DMAX, A1, D1
  motorParams.globalScaler = globalScaler;
  motor.setRampSpeeds(0, 0.1, 100); //Start, stop, threshold speeds  
  
  setEncoderDirection(encoderDirection);
  preferences.putFloat("threadPitch", threadPitch);
  preferences.putFloat("spindleMmPR", spindleMmPerRound);
  preferences.putInt("motorAcc", motorAcc);
  preferences.putInt("motorDec", motorDec);
  preferences.putInt("motorMaxSpeed", motorMaxSpeed);
  preferences.putInt("globalScaler", globalScaler);
  preferences.putFloat("backlash", backlash);
  preferences.putBool("posLimits", posLimits);
  preferences.putBool("targetAbs", targetAbsolute);
  preferences.putBool("motorDir", motorDirection);
  preferences.putBool("encoderDir", encoderDirection);
  preferences.putInt("encoderStp", encoderSteps);
   
  delay(250);
  attachInterrupt(digitalPinToInterrupt(ENCODER_A), encoderISR, FALLING);
}

float displaySetupPartFloat(String title, String valueUnit, float value, float minValue, float maxValue, float factor, float minStep) {
  if (value < minValue) {
    value = minValue;
  }
  if (value > maxValue) {
    value = maxValue;
  }
  char valueBuf[8];
  int valueRight = 0;
  float factorValue = value * factor;
  if (butLeftPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value - minStep;
    } else {
      value = value - minStep*2;
    }
    if (value < minValue) {
      value = minValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }
  if (butRightPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value + minStep;
    } else {
      value = value + minStep*2;
    }
    if (value > maxValue) {
      value = maxValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }



  value = round(value * 100) / 100;
  factorValue = round(factorValue * 100) / 100;
  String valueString = String(factorValue);
  valueString.toCharArray(valueBuf, 8);
  valueRight = oledSPI.getStrWidth(valueBuf);
  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(0, 12);
  oledSPI.print(title);
  oledSPI.setFont(u8g2_font_helvR10_tf);
  oledSPI.setCursor(40 - valueRight, 34);
  oledSPI.print(value);
  oledSPI.setCursor(52, 32);
  oledSPI.setFont(u8g2_font_helvR08_tf);
  oledSPI.print(valueUnit);
  oledSPI.setCursor(20, 51);
  oledSPI.print("MIN");
  oledSPI.setCursor(20, 64);
  oledSPI.print(minValue);
  oledSPI.setCursor(86, 51);
  oledSPI.print("MAX");
  oledSPI.setCursor(86, 64);
  oledSPI.print(maxValue);
  oledSPI.drawHLine(0, 19, 128);
  oledSPI.drawHLine(0, 38, 128);
  oledSPI.drawVLine(64, 38, 26);


  return value;
}
bool displaySetupPart(String title, String description, bool value, String value0Name, String value1Name) {

  if (butLeftPressed == true) {
    value = false;
  }
  if (butRightPressed == true) {
    value = true;
  }

  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(0, 12);
  oledSPI.print(title);
  oledSPI.setFont(u8g2_font_helvR10_tf);
  oledSPI.setCursor(0, 50);
  oledSPI.print(value0Name);
  oledSPI.setCursor(64, 50);
  oledSPI.print(value1Name);
  oledSPI.setCursor(0, 32);
  oledSPI.setFont(u8g2_font_helvR08_tf);
  oledSPI.print(description);
  oledSPI.drawHLine(0, 19, 128);
  if (value == true) {
    oledSPI.drawHLine(67, 52, 60);
  } else {
    oledSPI.drawHLine(0, 52, 60);
  }


  return value;
}
int displaySetupPart(String title, String valueUnit, int value, int minValue, int maxValue, int stepChangeSmall, int stepChangeLarge) {
  if (value < minValue) {
    value = minValue;
  }
  if (value > maxValue) {
    value = maxValue;
  }
  char valueBuf[8];
  int valueRight = 0;
  if (butLeftPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value - stepChangeSmall;
    } else {
      value = value - stepChangeLarge;
    }
    if (value < minValue) {
      value = minValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }
  if (butRightPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value + stepChangeSmall;
    } else {
      value = value + stepChangeLarge;
    }
    if (value > maxValue) {
      value = maxValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }

  String valueString = String(value);
  valueString.toCharArray(valueBuf, 8);
  valueRight = oledSPI.getStrWidth(valueBuf);
  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(0, 12);
  oledSPI.print(title);
  oledSPI.setFont(u8g2_font_helvR10_tf);
  oledSPI.setCursor(40 - valueRight, 34);
  oledSPI.print(value);
  oledSPI.setCursor(52, 32);
  oledSPI.setFont(u8g2_font_helvR08_tf);
  oledSPI.print(valueUnit);
  oledSPI.setCursor(20, 51);
  oledSPI.print("MIN");
  oledSPI.setCursor(20, 64);
  oledSPI.print(minValue);
  oledSPI.setCursor(86, 51);
  oledSPI.print("MAX");
  oledSPI.setCursor(86, 64);
  oledSPI.print(maxValue);
  oledSPI.drawHLine(0, 19, 128);
  oledSPI.drawHLine(0, 38, 128);
  oledSPI.drawVLine(64, 38, 26);


  return value;
}

int displaySetupPart(String title, String valueUnit, int value, int minValue, int maxValue, int stepChangeSmall, int stepChangeLarge, float factor) {
  if (value < minValue) {
    value = minValue;
  }
  if (value > maxValue) {
    value = maxValue;
  }
  char valueBuf[8];
  int valueRight = 0;
  float factorValue = value * factor;
  if (butLeftPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value - stepChangeSmall;
    } else {
      value = value - stepChangeLarge;
    }
    if (value < minValue) {
      value = minValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }
  if (butRightPressed == true && millis() > milliTickerTmp2 + milliTicker2) {
    if (milliTicker2 > 5) {
      value = value + stepChangeSmall;
    } else {
      value = value + stepChangeLarge;
    }
    if (value > maxValue) {
      value = maxValue;
    }
    milliTicker2 = milliTicker2 / 1.05;
    milliTickerTmp2 = millis();
  }

  String valueString = String(factorValue);
  valueString.toCharArray(valueBuf, 8);
  valueRight = oledSPI.getStrWidth(valueBuf);
  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(0, 12);
  oledSPI.print(title);
  oledSPI.setFont(u8g2_font_helvR10_tf);
  oledSPI.setCursor(40 - valueRight, 34);
  oledSPI.print(valueString);
  oledSPI.setCursor(52, 32);
  oledSPI.setFont(u8g2_font_helvR08_tf);
  oledSPI.print(valueUnit);
  oledSPI.setCursor(20, 51);
  oledSPI.print("MIN");
  oledSPI.setCursor(20, 64);
  oledSPI.print(float(minValue * factor));
  oledSPI.setCursor(86, 51);
  oledSPI.print("MAX");
  oledSPI.setCursor(86, 64);
  oledSPI.print(float(maxValue * factor));
  oledSPI.drawHLine(0, 19, 128);
  oledSPI.drawHLine(0, 38, 128);
  oledSPI.drawVLine(64, 38, 26);


  return value;
}


void displaySetupEnter() {
  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(0, 40);
  oledSPI.print("Einstellungen");
}
void displayMain(int upm, float actPosition, float ziel, float steigung, float deg, int maxUpm) {
  char upmBuf[8];
  char maxUpmBuf[8];
  char posBuf[20];
  char zielBuf[20];
  char steigungBuf[20];
  char degBuf[20];
  u8g2_uint_t xRight;
  int x = 0;
  int y = 0;

  String upmString = String(upm);
  String maxUpmString = String(maxUpm);
  if(actPosition!=0){actPosition = round(actPosition * 100) / 100;}else{actPosition=0;}
  //actPosition = round(actPosition * 100) / 100;
  if(ziel!=0){ziel = round(ziel * 100) / 100;}else{ziel=0;}
  steigung = round(steigung * 100) / 100;
  deg = round(deg * 100) / 100;
  upmString.toCharArray(upmBuf, 8);
  maxUpmString.toCharArray(maxUpmBuf, 8);
  dtostrf(actPosition, 4, 2, posBuf);
  dtostrf(ziel, 4, 2, zielBuf);
  dtostrf(steigung, 2, 2, steigungBuf);
  dtostrf(deg, 3, 2, degBuf);
  if(dispZeroDeg==true || deg==0){
    if(stepperPos==stepperTarget || stepperPos==0){
      oledSPI.drawDisc(2,2,2);
    }else{
      oledSPI.drawCircle(2,2,2);
    }
    dispZeroDeg=false;
  }
  
  oledSPI.setFont(u8g2_font_helvB24_tf);
  xRight = oledSPI.getStrWidth(upmBuf);
  oledSPI.setCursor(80 - xRight, 24);
  oledSPI.print(upm);
  oledSPI.setFont(u8g2_font_helvB12_tf);
  oledSPI.setCursor(85, 19);
  oledSPI.print("UPM");
  oledSPI.drawLine(0, 26, 128, 26);
  oledSPI.drawLine(42, 26, 42, 51);
  oledSPI.drawLine(86, 26, 86, 51);
  oledSPI.drawLine(0, 51, 128, 51);
  oledSPI.setFont(u8g2_font_open_iconic_all_2x_t);

  x = 13;
  y = 28;
  oledSPI.drawTriangle(x + 0, y + 0, x + 0, y + 12, x + 6, y + 6);
  oledSPI.drawTriangle(x + 13, y + 0, x + 13, y + 12, x + 7, y + 6);
  oledSPI.drawVLine(x + 6, y + 1, 11);
  x = 57;
  y = 28;
  oledSPI.drawTriangle(x + 7, y + 0, x + 7, y + 12, x + 13, y + 6);
  oledSPI.drawVLine(x + 13, y + 1, 11);
  if(posLimits==false){
    oledSPI.drawBox(x + 0, y + 4, 7, 5);
  }else{
    oledSPI.drawBox(x -31 , y + 4, 38, 5);
  }  
  x = 100;
  y = 28;
  oledSPI.drawTriangle(x + 0, y + 12, x + 12, y + 12, x + 12, y + 0);
  
  if(targetAbsolute==true){
    oledSPI.setFont(u8g2_font_helvB08_tf);
    oledSPI.setCursor(74, 38); oledSPI.print("R");
  }
  oledSPI.setFont(u8g2_font_helvR08_tf);
  xRight = oledSPI.getStrWidth(posBuf);
  oledSPI.setCursor(39 - xRight, 50); oledSPI.print(posBuf);
  xRight = oledSPI.getStrWidth(zielBuf);
  oledSPI.setCursor(83 - xRight, 50); oledSPI.print(zielBuf);
  xRight = oledSPI.getStrWidth(steigungBuf);
  oledSPI.setCursor(120 - xRight, 50); oledSPI.print(steigungBuf);
  //xRight=oledSPI.getStrWidth(degBuf);
  //oledSPI.setCursor(120-xRight,64); oledSPI.print(degBuf);
  //oledSPI.setCursor(123,64); oledSPI.print(char(176));
  if (maxUpm < 2500) {
    xRight = oledSPI.getStrWidth(maxUpmBuf);
    oledSPI.setCursor(70 - xRight, 64); oledSPI.print("Max. UPM:");
    oledSPI.setCursor(125 - xRight, 64); oledSPI.print(maxUpmBuf);
  }
  x = xAni;
  y = 54;

  if (driveDirection == 1 ) {
    oledSPI.drawTriangle(x + 0, y + 0, x + 0, y + 10, x + 5, y + 5);
    if (xAni > 10) {
      oledSPI.drawTriangle(x - 10, y + 0, x - 10, y + 10, x - 5, y + 5);
    }
    if (xAni > 20) {
      oledSPI.drawTriangle(x - 20, y + 0, x - 20, y + 10, x - 15, y + 5);
    }
    xAni++;
    if (xAni >= 30) {
      xAni = 20;
    }
  }
  if (driveDirection == -1) {
    oledSPI.drawTriangle(x + 5, y + 0, x + 5, y + 10, x + 0, y + 5);
    if (xAni < 20) {
      oledSPI.drawTriangle(x + 15, y + 0, x + 15, y + 10, x + 10, y + 5);
    }
    if (xAni < 10) {
      oledSPI.drawTriangle(x + 25, y + 0, x + 25, y + 10, x + 20, y + 5);
    }
    xAni--;
    if (xAni <= 0) {
      xAni = 10;
    }
  }
}