/*
 *  short explanation
 */

#include <Arduino.h>                    // main arduino library
#include <Ponoor_PowerSTEP01Library.h>  // stepper motor shield library
#include <SPI.h>                        // Arduino SPI library
#include <EEPROM.h>                     // Arduino EEPROM library
#include "Pin_Definitions.h"            // pin definitions
#include "M034.h"                       // include header file for project

// string with manual for printing over UART
const String manual = "\nM034 stepper motor controll\n"
                "protocol structure: command parameter e.g. movl 200\n"
                "commands (in small letters):\n"
                "help: print instruction manual\n"
                "\n"

                "Basic Motor Instructions (without encoder):\n"
                "movl (move left): spin x steps counterclockwise\n"
                "movr (move right): spin x steps clockwise\n"
                "\n"

                "Other motor Instructions (without encoder):\n"
                "getp (get position): get current motor position\n"
                "movp (move to position): move motor to position uint32_t x\n"
                "setn (set null): set current position as motor null position (home)\n"
                "smpo (set motor position): save current motor position (moved steps) as position x {0-2}\n"
                "gtmp (go to motor position): go to saved motor position x {0-2}\n"
                "\n"

                "Motor configuration:\n"
                "setm (set mode): set mode to: 0 - voltage mode; 1 - current mode\n"
                "sets (set microstep size):\n"
                  "\tOptions: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128\n"
                  "\tSetS parameter = 1 for 1; 2 for 1/2; ...\n"
                  "\tIn current mode the maximum microstepping resolution is 1/16\n"
                  "\tIf changed, Null and saved positions get lost\n"
                "\n"

                "Encoder Instructions:\n"
                "gete (get encoder position): get current encoder singleturn position (average over x)\n"
                "sepo (set encoder position): save current encoder position as position x {0-2}\n"
                "gtep (go to encoder position): go to saved encoder position x {0-2}\n"
                "sepe (save encoder positions to EEPROM): backup saved encoder positions in EEPROM\n"
                "reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM\n"
                "init (initialize): load encoder positions from EEPROM, move to encoder 0 position and set it to motor position null\n"
                "\n\n"
;


/**********************************************************************
 *                          constant definitions                      *
 **********************************************************************/
const uint32_t SERIAL_BAUD_RATE = 921600;   // Baud Rate of serial output
                                            // if changed, must also be changed in platformio.ini: monitor_speed
const SPISettings SPI_ENC_Settings(2600000, MSBFIRST, SPI_MODE3); // SPI settings for stepper motor encoder
const uint8_t POSITION_COUNT = 3;           // legth of position array
const uint32_t ENCODER_AV_COUNT = 1024;     // Number of encoder positions over which to average
const uint32_t MOTORSTEPS = 200;            // number of full steps wich the motor has

// EEPROM (test)
// const 

// small neutron-switch
// motor power settings. Complete explanation in setup. Tested with 16 microsteps
const byte CM_RUN_TVAL = 20;                // motor power settings for current mode
const byte CM_ACC_TVAL = 32;
const byte CM_DEC_TVAL = 32;
const byte CM_HOLD_TVAL = 16;
const float CM_MAX_SPEED = 1000.0;
const byte VM_RUN_TVAL = 16;                // motor power settings for voltage mode
const byte VM_ACC_TVAL = 24;
const byte VM_DEC_TVAL = 24;
const byte VM_HOLD_TVAL = 28;
const float VM_MAX_SPEED = 1000.0;

// big neutron-switch
// motor power settings. Complete explanation in setup. Tested with 16 microsteps
/*
const byte CM_RUN_TVAL = 30;                // motor power settings for current mode
const byte CM_ACC_TVAL = 40;
const byte CM_DEC_TVAL = 40;
const byte CM_HOLD_TVAL = 24;
const float CM_MAX_SPEED = 1000.0;
const byte VM_RUN_TVAL = 16;                // motor power settings for voltage mode
const byte VM_ACC_TVAL = 24;
const byte VM_DEC_TVAL = 24;
const byte VM_HOLD_TVAL = 28;
const float VM_MAX_SPEED = 1000.0;
*/

/**********************************************************************
 *                          global variables                          *
 **********************************************************************/

long positionMotor[POSITION_COUNT] = {(long)0, (long)1062, (long)-1067};  // array to save motor positions
int32_t positionEncoder[POSITION_COUNT] = {}; // array to save encoder positions
uint16_t eeAddress = 0;                       // Location of next free EEPROM address
uint32_t motorMicrosteps = 16;                // number of microsteps wich currently used
String message;         // String for buffering vallidation message
String errorMessage;   // String for buffering error message


/**********************************************************************
 *                          function declaration                      *
 **********************************************************************/
void controlUart(uint8_t port);    // check if uart communication is incomming on one of the UART's. If this is the case, read and execute incomming command
int32_t averageEncPosition(uint32_t count);         // read and build avverage of current encoder position
void goToEncoderPosition(uint32_t encoderPosition); // gtep (go to encoder position): go to saved encoder position x {0-2}


/**********************************************************************
 *                          instance generation                       *
 **********************************************************************/
HardwareSerial Serial6 (D0, D1);                // create Serial instance for optical links on USART6
SPIClass SPI_Encoder(PIN_SPI_5_MOSI, PIN_SPI_5_MISO, PIN_SPI_5_CLK);  // create SPI instance for encoder on SPI5
//SPIClass * SPI_Encoder_ptr = &SPI_Encoder;      // pointer to SPI_Encoder
//NanotecAbsolutEncoder encoder(SPI_Encoder_ptr);    // create encoder instance
NanotecAbsolutEncoder encoder(&SPI_Encoder);    // create encoder instance
powerSTEP driver(0, nCS_PIN, nSTBY_nRESET_PIN); // powerSTEP library instance, parameters are distance from the end of a daisy-chain of drivers, !CS pin, !STBY/!Reset pin




/**********************************************************************
 *                          setup                                     *
 **********************************************************************/
void setup() {
  // Prepare pins
  pinMode(nSTBY_nRESET_PIN, OUTPUT);// for stepper motor shield
  pinMode(nCS_PIN, OUTPUT);         // for stepper motor shield
  pinMode(PIN_SPI_1_MOSI, OUTPUT);  // SPI1 MOSI for stepper motor shield
  pinMode(PIN_SPI_1_MISO, INPUT);   // SPI1 MISO for stepper motor shield
  pinMode(PIN_SPI_1_CLK, OUTPUT);   // SPI1 CLK for stepper motor shield

  pinMode(PIN_SPI_5_MOSI, OUTPUT);  // SPI5 MOSI: not nessesary for our encoder
  pinMode(PIN_SPI_5_MISO, INPUT);   // SPI5 MISO from encoder
  pinMode(PIN_SPI_5_CLK, OUTPUT);   // SPI5 CLK for encoder

  pinMode(LED_GREEN, OUTPUT);       // Green LED on Nucleo-144
  pinMode(LED_BLUE, OUTPUT);        // Blue LED on Nucleo-144
  pinMode(LED_RED, OUTPUT);         // Red LED on Nucleo-144

  pinMode(PIN_RGB_LED_R, OUTPUT);   // Red part of the RGB LED
  pinMode(PIN_RGB_LED_G, OUTPUT);   // Green part of the RGB LED
  pinMode(PIN_RGB_LED_B, OUTPUT);   // Blue part of the RGB LED

  pinMode(PIN_BUTTON_POS1, INPUT_FLOATING);//  Push button for position controll
  pinMode(PIN_BUTTON_POS2, INPUT_FLOATING);//  Push button for position controll
  pinMode(PIN_BUTTON_POS3, INPUT_FLOATING);//  Push button for position controll

  pinMode(PIN_LEMO_POS1, INPUT_PULLDOWN);//  TTL input on Lemo for position controll
  pinMode(PIN_LEMO_POS2, INPUT_PULLDOWN);//  TTL input on Lemo for position controll
  pinMode(PIN_LEMO_POS3, INPUT_PULLDOWN);//  TTL input on Lemo for position controll

  pinMode(PIN_LEMO_READY, OUTPUT);    // TTL output for Ready Signal when Positioning finished

  Serial.begin(SERIAL_BAUD_RATE);     // Start serial UART on USB
  Serial6.begin(SERIAL_BAUD_RATE);    // Start serial UART on optical links

  SPI_Encoder.begin();  // Start SPI for stepper motor encoder
  SPI_Encoder.beginTransaction(SPI_ENC_Settings); // set correct settings

  
  
  // initialise stepper motor shield
  // Reset powerSTEP and set CS
  digitalWrite(nSTBY_nRESET_PIN, HIGH);
  digitalWrite(nSTBY_nRESET_PIN, LOW);
  digitalWrite(nSTBY_nRESET_PIN, HIGH);
  digitalWrite(nCS_PIN, HIGH);

  SPI.begin();                      // Start SPI1 for stepper shield
  SPI.setDataMode(SPI_MODE3);       // set correct SPI mode
  
  // Configure powerSTEP
  driver.SPIPortConnect(&SPI); // give library the SPI port (only the one on an Uno)
  
  driver.configSyncPin(BUSY_PIN, 0); // use SYNC/nBUSY pin as nBUSY, 
                                     // thus syncSteps (2nd paramater) does nothing
                                     
  driver.configStepMode(STEP_FS_16); // 1/16 microstepping, full steps = STEP_FS,
  //driver.configStepMode(STEP_FS); // 1/16 microstepping, full steps = STEP_FS,
                                // options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128
                                // In current mode the maximum microstepping resolution is 1/16
                                
  driver.setMaxSpeed(1000.0); // max speed in units of full steps/s (15.25 to 15610)
  driver.setFullSpeed(2000.0); // full steps/s threshold for disabling microstepping
  //driver.setAcc(1800); // full steps/s^2 acceleration
  //driver.setDec(1800); // full steps/s^2 deceleration
  driver.setAcc(900); // full steps/s^2 acceleration
  driver.setDec(900); // full steps/s^2 deceleration
  
  driver.setSlewRate(SR_520V_us); // faster may give more torque (but also EM noise),
                                  // options are: 114, 220, 400, 520, 790, 980(V/us)
                                  
  driver.setOCThreshold(16); // over-current threshold for the 2.8A NEMA23 motor
                            // used in testing. If your motor stops working for
                            // no apparent reason, it's probably this. Start low
                            // and increase until it doesn't trip, then maybe
                            // add one to avoid misfires. Can prevent catastrophic
                            // failures caused by shorts
  driver.setOCShutdown(OC_SD_ENABLE); // shutdown motor bridge on over-current event
                                      // to protect against permanant damage
  
  driver.setPWMFreq(PWM_DIV_1, PWM_MUL_0_75); // 16MHz*0.75/(512*1) = 23.4375kHz 
                            // power is supplied to stepper phases as a sin wave,  
                            // frequency is set by two PWM modulators,
                            // Fpwm = Fosc*m/(512*N), N and m are set by DIV and MUL,
                            // options: DIV: 1, 2, 3, 4, 5, 6, 7, 
                            // MUL: 0.625, 0.75, 0.875, 1, 1.25, 1.5, 1.75, 2
                            
  driver.setVoltageComp(VS_COMP_DISABLE); // no compensation for variation in Vs as
                                          // ADC voltage divider is not populated
                                          
  driver.setSwitchMode(SW_USER); // switch doesn't trigger stop, status can be read.
                                 // SW_HARD_STOP: TP1 causes hard stop on connection 
                                 // to GND, you get stuck on switch after homing
                                      
  driver.setOscMode(INT_16MHZ); // 16MHz internal oscillator as clock source

  // KVAL registers set the power to the motor by adjusting the PWM duty cycle,
  // use a value between 0-255 where 0 = no power, 255 = full power.
  // Start low and monitor the motor temperature until you find a safe balance
  // between power and temperature. Only use what you need
  
  driver.setRunTVAL(CM_RUN_TVAL);
  driver.setAccTVAL(CM_ACC_TVAL);
  driver.setDecTVAL(CM_DEC_TVAL);
  driver.setHoldTVAL(CM_HOLD_TVAL);
  driver.setCurrentMode();         // set current mode
  /*
  driver.setMaxSpeed(1000.);
  driver.setRunKVAL(VM_RUN_TVAL);
  driver.setAccKVAL(VM_ACC_TVAL);
  driver.setDecKVAL(VM_DEC_TVAL);
  driver.setHoldKVAL(VM_HOLD_TVAL);
  */

  driver.setParam(ALARM_EN, 0x8F); // disable ADC UVLO (divider not populated),
                                   // disable stall detection (not configured),
                                   // disable switch (not using as hard stop)

  driver.getStatus(); // clears error flags
  // end of stepper motor shield initialisation
  
  
  // init
  EEPROM.get(eeAddress, positionEncoder); // restore encoderpositions from EEPROM

  // led greeting
  digitalWrite(PIN_RGB_LED_R, HIGH); // set all off (common anode)
  digitalWrite(PIN_RGB_LED_G, HIGH);
  digitalWrite(PIN_RGB_LED_B, HIGH);
  digitalWrite(PIN_RGB_LED_R, LOW);// red on
  delay(300);
  digitalWrite(PIN_RGB_LED_R, HIGH); // red off
  digitalWrite(PIN_RGB_LED_G, LOW);// blue on
  delay(300);
  digitalWrite(PIN_RGB_LED_G, HIGH); // blue off
  digitalWrite(PIN_RGB_LED_B, LOW);// green on
  delay(300);
  digitalWrite(PIN_RGB_LED_B, HIGH); // green off

  Serial.print(manual);     // print instruction manual on USB UART
  Serial6.print(manual);    // print instruction manual on optical UART

  digitalWrite(PIN_RGB_LED_G, LOW); // Turn green LED on (display setup done)
} // end of setup()




/**********************************************************************
 *                          loop                                      *
 **********************************************************************/
void loop() {
  if(Serial.available() != 0){  // check if uart communication is incomming on USB UART
    controlUart(3);            // If this is the case, read and execute incomming command
  }
  if(Serial6.available() != 0){ // check if uart communication is incomming on optical UART
    controlUart(6);            // If this is the case, execute command
  }

  if(digitalRead(PIN_LEMO_POS1)==HIGH){   // if TTL position 1 input is HIGH
    goToEncoderPosition(0);               // drive motor to saved encoder position 1
  }
  if(digitalRead(PIN_LEMO_POS2)==HIGH){   // same for position 2
    goToEncoderPosition(1);
  }
  if(digitalRead(PIN_LEMO_POS3)==HIGH){   // and position 3
    goToEncoderPosition(2);
  }

  if(digitalRead(PIN_BUTTON_POS1)==HIGH){ // if button position 1 is HIGH
    delay(200);                           // wait, so that reset could be done before motor is moved
    goToEncoderPosition(0);               // drive motor to saved encoder position 1
  }
  if(digitalRead(PIN_BUTTON_POS2)==HIGH){ // same for position 2
    delay(200);
    goToEncoderPosition(1);
  }
  if(digitalRead(PIN_BUTTON_POS3)==HIGH){ // same for position 3
    delay(200);
    goToEncoderPosition(2);
  }    
} // end of loop()








/**********************************************************************
 *                          functions                                 *
 **********************************************************************/
// read and execute incomming UART command
void controlUart(uint8_t port){
  String input;           // String for buffering read data
  String parameterStr;    // STring for extracted parameter
  uint32_t parameter = 0;     // variable for storing command parameter
  
  
  switch(port){           // Read from the port where the communication is coming from
    case 3:
      input = Serial.readStringUntil('\n');  // read till line feed
      break;
    case 6:
      input = Serial6.readStringUntil('\n');  // read till line feed
      break;
  }
  input.trim(); // remove leading and trailing whitespace
  parameterStr = input.substring(5);      // extract parameter: read from sign 5
  parameter = atoi(parameterStr.c_str()); // convert String into int
  
  // search for controll string. If found execute command.
  // movl (move left): spin x steps counterclockwise
  if(input.startsWith("movl")){
    driver.move(REV, parameter);
    while(driver.busyCheck());
    driver.softStop();
    while(driver.busyCheck());
    message = "Motor is moved counterclockwise. Steps: ";
    message += (String) parameter;
  }
  
  // movr (move right): spin x steps clockwise
  else if(input.startsWith("movr")){
      driver.move(FWD, parameter);
      while(driver.busyCheck());
      driver.softStop();
      while(driver.busyCheck());
      message = "Motor is moved clockwise. Steps: ";
      message += (String) parameter;
  }
  
  // movp (move to position): move motor to position uint32_t x
  else if(input.startsWith("movp")){    
    driver.goTo(parameter);
    while(driver.busyCheck());
    driver.softStop();
    while(driver.busyCheck());
    message = "Motor is moved to position: ";
    message += (String) parameter;
  }

  // gtmp (go to motor position): go to saved motor position x {0-2}
  else if(input.startsWith("gtmp")){
    if(parameter<POSITION_COUNT){
      driver.goTo(positionMotor[parameter]);
      while(driver.busyCheck());
      driver.softStop();
      while(driver.busyCheck());
      message = "Motor is moved to saved position: ";
      message += (String) parameter;
    }
    else{
      errorMessage = "Parameter out of range";
    }
  }

  // gtep (go to encoder position): go to saved encoder position x {0-2}
  else if(input.startsWith("gtep")){
    if(parameter<POSITION_COUNT){
      goToEncoderPosition(parameter); // Separated so that this can also be called up via the buttons and the TTL inputs.

      /*
      int32_t currentPosition;
      int32_t steps2Drive = 1;
      uint8_t i = 0;            // loop counter
      message = "Motor is moved to saved position: ";
      message += (String) parameter;
      message += ". Moved steps: ";
      while(steps2Drive != 0){  // move motor to position  till the position is reached, because it is not reached exactly every time
        currentPosition = averageEncPosition(ENCODER_AV_COUNT);   // read singleturn position
        steps2Drive = int32_t ((double)(currentPosition - positionEncoder[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
        if(steps2Drive<0){
          driver.move(REV, ((unsigned long) (steps2Drive * -1)));
        }
        else{
          driver.move(FWD, (unsigned long) steps2Drive);
        }
        while(driver.busyCheck());
        driver.softStop();
        while(driver.busyCheck());
        delay(10);
        message += (String) steps2Drive;
        message += ("; ");
        i++;
        if(i==10){
          errorMessage = ("positioning not possible. Check neutron switch. Maybe motor parameter must be updated");
          break;
        }
      }
      */
    }
  }
  
  // smpo (set motor position): save current motor position (moved steps) as position x {0-2}
  else if(input.startsWith("smpo")){    
    if(parameter<POSITION_COUNT){
      positionMotor[parameter] = driver.getPos();
      message = "Current position is saved as motor position: ";
      message += (String) parameter;
    }
    else{
      errorMessage = "Parameter out of range";
    }
  }

  // sepo (set encoder position): save current encoder position as position x {0-2}
  else if(input.startsWith("sepo")){    
    if(parameter<POSITION_COUNT){
      positionEncoder[parameter] = averageEncPosition(ENCODER_AV_COUNT);   // read singleturn position
      message = "Current position is saved as encoder position: ";
      message += (String) parameter;
    }
    else{
      errorMessage = "Parameter out of range";
    }
  }
  
  // getp (get position): get current motor position
  else if(input.startsWith("getp")){
    long position = driver.getPos();
    message = "Current position is: ";
    message = (String) position;
  }
  
  // setn (set null): set current position as motor null position (home)
  else if(input.startsWith("setn")){
    driver.resetPos();
    message = "Current position is set to 0";
  }
  
  // setm (set mode): set mode to: 0 - voltage mode; 1 - current mode
  else if(input.startsWith("setm")){
    if(parameter==0){
      // Settings at 24V
      driver.softHiZ();
      driver.setMaxSpeed(VM_MAX_SPEED);
      driver.setRunTVAL(VM_RUN_TVAL);
      driver.setAccTVAL(VM_ACC_TVAL);
      driver.setDecTVAL(VM_DEC_TVAL);
      driver.setHoldTVAL(VM_HOLD_TVAL);
      driver.setVoltageMode();
      message = ("Voltage mode is set");
    }
    else if(parameter==1){
      driver.softHiZ();
      driver.setMaxSpeed(CM_MAX_SPEED);
      driver.setRunKVAL(CM_RUN_TVAL);
      driver.setAccKVAL(CM_ACC_TVAL);
      driver.setDecKVAL(CM_DEC_TVAL);
      driver.setHoldKVAL(CM_HOLD_TVAL);
      driver.setCurrentMode();
      message = ("Current mode is set");
    }
    else errorMessage = "Parameter out of range";
  }
  
  // sets (set microstep size):
  // Options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128
  // SetS parameter = 1 for 1; 2 for 1/2; ...
  // In current mode the maximum microstepping resolution is 1/16
  // If changed, Null and saved positions get lost
  else if(input.startsWith("sets")){      
    if(parameter==1){
      driver.softHiZ();
      driver.configStepMode(STEP_FS);     // full steps
      message = "Full step is set";
    }
    else if(parameter==2){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_2);   // 2 microsteps
      message = "microstep size 1/2 is set";
    }
    else if(parameter==4){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_4);   // 4 microsteps
      message = "microstep size 1/4 is set";
    }
    else if(parameter==8){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_8);   // 8 microsteps
      message = "microstep size 1/8 is set";
    }
    else if(parameter==16){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_16);  // 16 microsteps
      message = "microstep size 1/16 is set";
    }
    else if(parameter==32){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_32);  // 32 microsteps
      message = "microstep size 1/32 is set";
    }
    else if(parameter==64){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_64);  // 64 microsteps
      message = "microstep size 1/64 is set";
    }
    else if(parameter==128){
      driver.softHiZ();
      driver.configStepMode(STEP_FS_128); // 128 microsteps
      message = "microstep size 1/128 is set";
    }
    else{
      errorMessage = "wrong parameter for sets";
    }
    message += "\nposition lost, new initialisation needed";
  }
  
  // gete (get encoder position): get current encoder singleturn position (average over x)
  else if(input.startsWith("gete")){
    if(parameter>0){
      message = (String) averageEncPosition(parameter);  // read and build avverage of current encoder position
    }
    else{
      errorMessage = "Parameter out of range";
    }
  }
  
  // sepe (save encoder positions to EEPROM): backup saved encoder positions in EEPROM
  else if(input.startsWith("sepe")){
    EEPROM.put(eeAddress, positionEncoder);           // put data to EEPROM
    message = "Encoder positions writen to EEPROM: "; // build output string
    for(int i =0; i<3; i++){
      message += (String) positionEncoder[i];
      message += ("; ");
    }
  }

  // reep (reed EEPROM encoder positions): reed encoder singleturn positions from EEPROM
  else if(input.startsWith("reep")){
    int32_t positionEncoderEeprom[3] = {0,0,0};   // for read EEPROM data
    EEPROM.get(eeAddress, positionEncoderEeprom); // reed encoder positions from EEPROM
    message = "Encoder positions in EEPROM: ";    // build output string
    for(int i =0; i<3; i++){
      message += (String) positionEncoderEeprom[i];
      message += ("; ");
    }
  }
  
  // init (initialize): load encoder positions from EEPROM, move to encoder 0 position and set it to motor position null
  else if(input.startsWith("init")){
    int32_t homePosition;
    int32_t currentPosition;
    int32_t steps2Drive = 1;
    uint8_t i = 0;            // loop counter
    message = "Moved steps to EEPROM encoder home position: ";
    driver.softStop();        // hold motor
    driver.move(FWD, 10);     // move a little bit so that the later move begins at a good position
    while(driver.busyCheck());
    driver.move(REV, 10);     // move back so that a second init returns 0 moved steps
    while(driver.busyCheck());
    driver.softStop();        // hold motor
    while(driver.busyCheck());
    delay(100);               // wait till motor stands still
    //EEPROM.get(eeAddress, homePosition);  // (old) read home position from EEPROM
    EEPROM.get(eeAddress, positionEncoder); // restore encoderpositions from EEPROM
    homePosition = positionEncoder[0];      // copy position 0 to homePosition for manipulation

    
    while(steps2Drive != 0){  // move motor to position null till the position is reached, because null is not reached exactly every time
      currentPosition = averageEncPosition(ENCODER_AV_COUNT);   // read singleturn position
      steps2Drive = int32_t ((double)(currentPosition - homePosition) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
      driver.resetPos();        // set motor driver position 0
      driver.goTo(steps2Drive); // move calculated steps
      while(driver.busyCheck());
      driver.softStop();
      while(driver.busyCheck());
      delay(10);
      message += (String) steps2Drive;  // build output string
      message += ("; ");
      i++;
      if(i==10){
        errorMessage = ("init not possible. Check neutron switch. Maybe motor parameter must be updated");
        break;
      }
    }

    driver.resetPos();        // set motor driver position 0
    while(driver.busyCheck());
  }
  
  // help: print instruction manual
  else if(input.startsWith("help")){
    message = manual;
  }
  
  // hold: test how much hold power is needed
  else if(input.startsWith("hold")){
    delay(1000);
    driver.setHoldKVAL(24);
    delay(2000);
    driver.setHoldKVAL(16);
    delay(2000);
    message = ("hold test done");
  }

  // command unknown
  else{
    errorMessage = "Command unknown or parameter is missing. Send 'help' to get the manual again";
  }

  if(message.length() != 0){  // if message available, print on the port on which command was receiverd
    switch(port){
      case 3:
        Serial.println(message);
        break;
      case 6:
        Serial6.println(message);
        break;
    }
  }
  if(errorMessage.length() != 0){  // if error message available, print on the port on which command was receiverd
    switch(port){
      case 3:
        Serial.println(errorMessage);
        break;
      case 6:
        Serial6.println(errorMessage);
        break;
    }
  }
  return;
} // end of controlUart()



// read and build avverage of current encoder position
int32_t averageEncPosition(uint32_t count){
  int32_t encoderPositionAv = 0;
  uint32_t i = 0;
  for(i; i<count; i++){
    encoder.readPosition();                   // read position from encoder
    if(encoder.getStatus()==0){               // if status is good,
      encoderPositionAv += encoder.getSingleturn(); //sum up positions
    }
    else{                                     // if status is not good,
      i--;                                    // count down i so that the count is correct
    }
  }
  encoderPositionAv = encoderPositionAv / i;  // build average
  return encoderPositionAv;
} // end of averageEncPosition()


// gtep (go to encoder position): go to saved encoder position x {0-2}
// Separated so that this can also be called up via the buttons and the TTL inputs.
void goToEncoderPosition(uint32_t encoderPosition){
  int32_t currentPosition;
  int32_t steps2Drive = 1;
  uint8_t i = 0;            // loop counter
  uint32_t failure = 0;     // set if failure happen
  digitalWrite(PIN_LEMO_READY, HIGH); // Set ready HIGH (display positioning in progress)
  digitalWrite(PIN_RGB_LED_B, LOW);   // Turn blue LED on (display positioning in progress)
  digitalWrite(PIN_RGB_LED_R, HIGH);  // Turn red LED off
  digitalWrite(PIN_RGB_LED_G, HIGH);  // Turn green LED off
  message = "Motor is moved to saved position: ";
  message += (String) encoderPosition;
  message += ". Moved steps: ";
  while(steps2Drive != 0){  // move motor to position  till the position is reached, because it is not reached exactly every time
    currentPosition = averageEncPosition(ENCODER_AV_COUNT);   // read singleturn position
    steps2Drive = int32_t ((double)(currentPosition - positionEncoder[encoderPosition]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
    if(steps2Drive<0){
      driver.move(REV, ((unsigned long) (steps2Drive * -1)));
    }
    else{
      driver.move(FWD, (unsigned long) steps2Drive);
    }
    while(driver.busyCheck());
    driver.softStop();
    while(driver.busyCheck());
    delay(10);
    message += (String) steps2Drive;
    message += ("; ");
    i++;
    if(i==10){
      errorMessage = ("positioning not possible. Check neutron switch. Maybe motor parameter must be updated");
      failure = 1;
      break;
    }
  }
  if(failure == 0){                     // without failure
    digitalWrite(PIN_LEMO_READY, LOW);  // Set ready LOW (display positioning done)
    digitalWrite(PIN_RGB_LED_G, LOW);   // Turn green LED on (display positioning done)
    digitalWrite(PIN_RGB_LED_B, HIGH);  // Turn blue LED off
  }
  else{                                 // with failure
    //digitalWrite(PIN_LEMO_READY, LOW);  // Set ready LOW (display positioning done)
    digitalWrite(PIN_RGB_LED_R, LOW);   // Turn red LED on (display failure)
    digitalWrite(PIN_RGB_LED_B, HIGH);  // Turn blue LED off
  }
  
} // end of goToEncoderPosition()

