#include "Movement.h"
#include "Debug.h"
#include "Config.h"

static Movement *instance;

Movement *Movement::getInstance()
{
  if (!instance)
  {
    instance = new Movement();
  };
  return instance;
};

void Movement::reportEncoders()
{
  Serial.print(COMM_REPORT_ENCODER_SCALED);
  Serial.print(" X");
  Serial.print((float)encoderX.currentPosition() / (float)stepsPerMm[0]);
  Serial.print(" Y");
  Serial.print((float)encoderY.currentPosition() / (float)stepsPerMm[1]);
  Serial.print(" Z");
  Serial.print((float)encoderZ.currentPosition() / (float)stepsPerMm[2]);
  CurrentState::getInstance()->printQAndNewLine();

  Serial.print(COMM_REPORT_ENCODER_RAW);
  Serial.print(" X");
  Serial.print(encoderX.currentPositionRaw());
  Serial.print(" Y");
  Serial.print(encoderY.currentPositionRaw());
  Serial.print(" Z");
  Serial.print(encoderZ.currentPositionRaw());
  CurrentState::getInstance()->printQAndNewLine();

}

void Movement::getEncoderReport()
{
  serialBuffer += COMM_REPORT_ENCODER_SCALED;
  serialBuffer += " X";
  serialBuffer += (float)encoderX.currentPosition() / (float)stepsPerMm[0];
  serialBuffer += " Y";
  serialBuffer += (float)encoderY.currentPosition() / (float)stepsPerMm[1];
  serialBuffer += " Z";
  serialBuffer += (float)encoderZ.currentPosition() / (float)stepsPerMm[2];
  serialBuffer += CurrentState::getInstance()->getQAndNewLine();

  serialBuffer += COMM_REPORT_ENCODER_RAW;
  serialBuffer += " X";
  serialBuffer += encoderX.currentPositionRaw();
  serialBuffer += " Y";
  serialBuffer += encoderY.currentPositionRaw();
  serialBuffer += " Z";
  serialBuffer += encoderZ.currentPositionRaw();
  serialBuffer += CurrentState::getInstance()->getQAndNewLine();

}

void Movement::reportStatus(MovementAxis *axis, int axisStatus)
{  
  serialBuffer += COMM_REPORT_CMD_STATUS;
  serialBuffer += " ";
  serialBuffer += axis->channelLabel;
  serialBuffer += axisStatus;
  serialBuffer += CurrentState::getInstance()->getQAndNewLine();

  //Serial.print(COMM_REPORT_CMD_STATUS);
  //Serial.print(" ");
  //Serial.print(axis->label);
  //Serial.print(axisStatus);
  //CurrentState::getInstance()->printQAndNewLine();
}

void Movement::reportCalib(MovementAxis *axis, int calibStatus)
{
  Serial.print(COMM_REPORT_CALIB_STATUS);
  Serial.print(" ");
  Serial.print(axis->channelLabel);
  Serial.print(calibStatus);
  CurrentState::getInstance()->printQAndNewLine();
}

void Movement::checkAxisSubStatus(MovementAxis *axis, int *axisSubStatus)
{
  int newStatus = 0;
  bool statusChanged = false;

  if (axis->isAccelerating())
  {
    newStatus = COMM_REPORT_MOVE_STATUS_ACCELERATING;
  }

  if (axis->isCruising())
  {
    newStatus = COMM_REPORT_MOVE_STATUS_CRUISING;
  }

  if (axis->isDecelerating())
  {
    newStatus = COMM_REPORT_MOVE_STATUS_DECELERATING;
  }

  if (axis->isCrawling())
  {
    newStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
  }

  // if the status changes, send out a status report
  if (*axisSubStatus != newStatus && newStatus > 0)
  {
    statusChanged = true;
  }
  *axisSubStatus = newStatus;

  if (statusChanged)
  {
    reportStatus(axis, *axisSubStatus);
  }
}

//const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds

Movement::Movement()
{

  // Initialize some variables for testing

  motorMotorsEnabled = false;

  motorConsMissedSteps[0] = 0;
  motorConsMissedSteps[1] = 0;
  motorConsMissedSteps[2] = 0;

  motorLastPosition[0] = 0;
  motorLastPosition[1] = 0;
  motorLastPosition[2] = 0;

  motorConsEncoderLastPosition[0] = 0;
  motorConsEncoderLastPosition[1] = 0;
  motorConsEncoderLastPosition[2] = 0;

  // Create the axis controllers

  axisX = MovementAxis();
  axisY = MovementAxis();
  axisZ = MovementAxis();

  axisX.channelLabel = 'X';
  axisY.channelLabel = 'Y';
  axisZ.channelLabel = 'Z';

  // Create the encoder controller

  encoderX = MovementEncoder();
  encoderY = MovementEncoder();
  encoderZ = MovementEncoder();

  // Load settings

  loadSettings();

  motorMotorsEnabled = false;
}

void Movement::loadSettings()
{

  /**/ //Serial.println("== load pin numbers ==");

  // Load motor settings

  axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, E_STEP_PIN, E_DIR_PIN, E_ENABLE_PIN);
  axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, 0, 0, 0);
  axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, 0, 0, 0);

  axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE;
  axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
  axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;

  /**/ //Serial.println("== load motor settings ==");

  loadMotorSettings();

  // Load encoder settings

  /**/ //Serial.println("== load encoder settings ==");

  loadEncoderSettings();

  /**/ //Serial.println("== load mdl encoder settings ==");

  encoderX.loadMdlEncoderId(_MDL_X1);
  encoderY.loadMdlEncoderId(_MDL_Y);
  encoderZ.loadMdlEncoderId(_MDL_Z);

  /**/ //Serial.println("== load encoder pin numbers ==");

  encoderX.loadPinNumbers(X_ENCDR_A, X_ENCDR_B, X_ENCDR_A_Q, X_ENCDR_B_Q);
  encoderY.loadPinNumbers(Y_ENCDR_A, Y_ENCDR_B, Y_ENCDR_A_Q, Y_ENCDR_B_Q);
  encoderZ.loadPinNumbers(Z_ENCDR_A, Z_ENCDR_B, Z_ENCDR_A_Q, Z_ENCDR_B_Q);

  /**/ //Serial.println("== load encoder load settings 2 ==");

  encoderX.loadSettings(motorConsEncoderType[0], motorConsEncoderScaling[0], motorConsEncoderInvert[0]);
  encoderY.loadSettings(motorConsEncoderType[1], motorConsEncoderScaling[1], motorConsEncoderInvert[1]);
  encoderZ.loadSettings(motorConsEncoderType[2], motorConsEncoderScaling[2], motorConsEncoderInvert[2]);

}

#if defined(FARMDUINO_EXP_V20)
  void Movement::initTMC2130()
  {
    axisX.initTMC2130();
    axisY.initTMC2130();
    axisZ.initTMC2130();
  }

  void Movement::loadSettingsTMC2130()
  {
    /**/
    int motorCurrentX;
    int stallSensitivityX;
    int microStepsX;

    int motorCurrentY;
    int stallSensitivityY;
    int microStepsY;

    int motorCurrentZ;
    int stallSensitivityZ;
    int microStepsZ;

    motorCurrentX = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X);
    stallSensitivityX = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X);
    microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X);
    //axisX.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);

    motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y);
    stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y);
    microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y);
    //axisY.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);

    motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z);
    stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z);
    microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z);
    //axisZ.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);

    motorCurrentX = 600;
    stallSensitivityX = 0;
    microStepsX = 0;

    motorCurrentY = 600;
    stallSensitivityY = 0;
    microStepsY = 0;

    motorCurrentZ = 600;
    stallSensitivityZ = 0;
    microStepsZ = 0;

    TMC2130X->push();
    TMC2130X->toff(3);
    TMC2130X->tbl(1);
    TMC2130X->hysteresis_start(4);
    TMC2130X->hysteresis_end(-2);
    TMC2130X->rms_current(motorCurrentX); // mA
    TMC2130X->microsteps(microStepsX);
    TMC2130X->diag1_stall(1);
    TMC2130X->diag1_active_high(1);
    TMC2130X->coolstep_min_speed(0xFFFFF); // 20bit max
    TMC2130X->THIGH(0);
    TMC2130X->semin(5);
    TMC2130X->semax(2);
    TMC2130X->sedn(0b01);
    TMC2130X->sg_stall_value(stallSensitivityX);

    TMC2130Y->push();
    TMC2130Y->toff(3);
    TMC2130Y->tbl(1);
    TMC2130Y->hysteresis_start(4);
    TMC2130Y->hysteresis_end(-2);
    TMC2130Y->rms_current(motorCurrentY); // mA
    TMC2130Y->microsteps(microStepsY);
    TMC2130Y->diag1_stall(1);
    TMC2130Y->diag1_active_high(1);
    TMC2130Y->coolstep_min_speed(0xFFFFF); // 20bit max
    TMC2130Y->THIGH(0);
    TMC2130Y->semin(5);
    TMC2130Y->semax(2);
    TMC2130Y->sedn(0b01);
    TMC2130Y->sg_stall_value(stallSensitivityY);

    TMC2130Z->push();
    TMC2130Z->toff(3);
    TMC2130Z->tbl(1);
    TMC2130Z->hysteresis_start(4);
    TMC2130Z->hysteresis_end(-2);
    TMC2130Z->rms_current(motorCurrentZ); // mA
    TMC2130Z->microsteps(microStepsZ);
    TMC2130Z->diag1_stall(1);
    TMC2130Z->diag1_active_high(1);
    TMC2130Z->coolstep_min_speed(0xFFFFF); // 20bit max
    TMC2130Z->THIGH(0);
    TMC2130Z->semin(5);
    TMC2130Z->semax(2);
    TMC2130Z->sedn(0b01);
    TMC2130Z->sg_stall_value(stallSensitivityZ);

    TMC2130E->push();
    TMC2130E->toff(3);
    TMC2130E->tbl(1);
    TMC2130E->hysteresis_start(4);
    TMC2130E->hysteresis_end(-2);
    TMC2130E->rms_current(600); // mA
    TMC2130E->microsteps(0);
    TMC2130E->diag1_stall(1);
    TMC2130E->diag1_active_high(1);
    TMC2130E->coolstep_min_speed(0xFFFFF); // 20bit max
    TMC2130E->THIGH(0);
    TMC2130E->semin(5);
    TMC2130E->semax(2);
    TMC2130E->sedn(0b01);
    TMC2130E->sg_stall_value(0);

  }

#endif

void Movement::test()
{
  axisX.enableMotor();

  //axisY.test();

  //axisX.enableMotor();
  //axisX.setMotorStep();
  //delayMicroseconds(10);
  //axisX.setMotorStep();
  //delayMicroseconds(10);

  //digitalWrite(X_STEP_PIN, HIGH);
  //delayMicroseconds(10);
  //digitalWrite(X_STEP_PIN, LOW);
  //delayMicroseconds(10);

  //axisX.setMotorStepWriteTMC2130();
  //axisX.test();

  //Serial.print("R99");
  //Serial.print(" mot x = ");
  //Serial.print(axisX.currentPosition());
  //Serial.print(" enc x = ");
  //Serial.print(encoderX.currentPosition());
  //Serial.print("\r\n");

  //Serial.print("R99");
  //Serial.print(" mot y = ");
  //Serial.print(axisY.currentPosition());
  //Serial.print(" enc y = ");
  //Serial.print(encoderY.currentPosition());
  //Serial.print("\r\n");

  //Serial.print("R99");
  //Serial.print(" mot z = ");
  //Serial.print(axisZ.currentPosition());
  //Serial.print(" enc z = ");
  //Serial.print(encoderZ.currentPosition());
  //Serial.print("\r\n");

  // read changes in encoder
  //encoderX.readEncoder();
  //encoderY.readEncoder();
  //encoderZ.readEncoder();
  //reportPosition();

  //bool test = axisX.endStopMin();
  //Serial.print("R99");
  //Serial.print(" test = ");
  //Serial.print(test);
  //Serial.print("\r\n");
}

void Movement::test2()
{
  
  axisX.setMotorStep();
  //CurrentState::getInstance()->printPosition();
  //encoderX.test();
  //encoderY.test();
  //encoderZ.test();
}

/**
 * xDest - destination X in steps
 * yDest - destination Y in steps
 * zDest - destination Z in steps
 * maxStepsPerSecond - maximum number of steps per second
 * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
 */
int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled,
                                 unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
                                 bool xHome, bool yHome, bool zHome)
{

  /**/ //Serial.println("AAA");
  /**/ //test();

  long xDest = xDestScaled * stepsPerMm[0];
  long yDest = yDestScaled * stepsPerMm[1];
  long zDest = zDestScaled * stepsPerMm[2];

  unsigned long currentMillis = 0;
  unsigned long timeStart = millis();

  serialMessageNr = 0;
  serialMessageDelay = 0;

  int incomingByte = 0;
  int error = 0;
  bool emergencyStop = false;
  unsigned int commandSpeed[3];

  // if a speed is given in the command, use that instead of the config speed

  if (xMaxSpd > 0 && xMaxSpd < speedMax[0])
  {
    commandSpeed[0] = xMaxSpd;
  }
  else
  {
    commandSpeed[0] = speedMax[0];
  }

  if (yMaxSpd > 0 && yMaxSpd < speedMax[1])
  {
    commandSpeed[1] = yMaxSpd;
  }
    else
  {
    commandSpeed[1] = speedMax[1];
  }

  if (zMaxSpd > 0 && zMaxSpd < speedMax[2])
  {
    commandSpeed[2] = zMaxSpd;
  }
    else
  {
    commandSpeed[2] = speedMax[2];
  }

  axisX.setMaxSpeed(commandSpeed[0]);
  axisY.setMaxSpeed(commandSpeed[1]);
  axisZ.setMaxSpeed(commandSpeed[2]);

  // Load coordinates into axis class

  long sourcePoint[3] = {0, 0, 0};
  sourcePoint[0] = CurrentState::getInstance()->getX();
  sourcePoint[1] = CurrentState::getInstance()->getY();
  sourcePoint[2] = CurrentState::getInstance()->getZ();

  long currentPoint[3] = {0, 0, 0};
  currentPoint[0] = CurrentState::getInstance()->getX();
  currentPoint[1] = CurrentState::getInstance()->getY();
  currentPoint[2] = CurrentState::getInstance()->getZ();

  long destinationPoint[3] = {0, 0, 0};
  destinationPoint[0] = xDest;
  destinationPoint[1] = yDest;
  destinationPoint[2] = zDest;

  motorConsMissedSteps[0] = 0;
  motorConsMissedSteps[1] = 0;
  motorConsMissedSteps[2] = 0;

  motorConsMissedStepsPrev[0] = 0;
  motorConsMissedStepsPrev[1] = 0;
  motorConsMissedStepsPrev[2] = 0;

  motorLastPosition[0] = currentPoint[0];
  motorLastPosition[1] = currentPoint[1];
  motorLastPosition[2] = currentPoint[2];

  // Load coordinates into motor control
  // Report back coordinates if target coordinates changed

  if (axisX.loadCoordinates(currentPoint[0], destinationPoint[0], xHome))
  {
    Serial.print(COMM_REPORT_COORD_CHANGED_X);
    Serial.print(" X");
    Serial.print(axisX.destinationPosition() / stepsPerMm[0]);
    CurrentState::getInstance()->printQAndNewLine();
  }

  if (axisY.loadCoordinates(currentPoint[1], destinationPoint[1], yHome))
  {
    Serial.print(COMM_REPORT_COORD_CHANGED_Y);
    Serial.print(" Y");
    Serial.print(axisY.destinationPosition() / stepsPerMm[1]);
    CurrentState::getInstance()->printQAndNewLine();
  }

  if (axisZ.loadCoordinates(currentPoint[2], destinationPoint[2], zHome))
  {
    Serial.print(COMM_REPORT_COORD_CHANGED_Z);
    Serial.print(" Z");
    Serial.print(axisZ.destinationPosition() / stepsPerMm[2]);
    CurrentState::getInstance()->printQAndNewLine();
  }

  // Prepare for movement

  axisX.movementStarted = false;
  axisY.movementStarted = false;
  axisZ.movementStarted = false;

  storeEndStops();
  reportEndStops();

  axisX.setDirectionAxis();
  axisY.setDirectionAxis();
  axisZ.setDirectionAxis();

  // Enable motors

  axisSubStep[0] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
  axisSubStep[1] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
  axisSubStep[2] = COMM_REPORT_MOVE_STATUS_START_MOTOR;

  reportStatus(&axisX, axisSubStep[0]);
  reportStatus(&axisY, axisSubStep[1]);
  reportStatus(&axisZ, axisSubStep[2]);

  enableMotors();

  // Start movement

  axisActive[0] = true;
  axisActive[1] = true;
  axisActive[2] = true;

  if (xHome || yHome || zHome)
  {
    if (!xHome) { axisX.deactivateAxis(); }
    if (!yHome) { axisY.deactivateAxis(); }
    if (!zHome) { axisZ.deactivateAxis(); }

    axisActive[0] = xHome;
    axisActive[1] = yHome;
    axisActive[2] = zHome;
  }

  axisX.checkMovement();
  axisY.checkMovement();
  axisZ.checkMovement();

  axisX.setTicks();
  axisY.setTicks();
  axisZ.setTicks();

  emergencyStop = CurrentState::getInstance()->isEmergencyStop();

  unsigned long loopCounts = 0;

  // Let the interrupt handle all the movements
  while ((axisActive[0] || axisActive[1] || axisActive[2]) && !emergencyStop)
  {
    #if defined(FARMDUINO_V14)
    checkEncoders();
    #endif

    /**/
    if (loopCounts % 1000 == 0)
    {

      //Serial.print("R99");
      //Serial.print(" missed step ");
      //Serial.print(motorConsMissedSteps[1]);
      //Serial.print(" axis pos ");
      //Serial.print(axisY.currentPosition());
      //Serial.print("\r\n");

      //Serial.print("X - ");
      //axisX.test();

      //Serial.print("Y - ");
      //axisY.test();

    }
    loopCounts++;
    

    checkAxisSubStatus(&axisX, &axisSubStep[0]);
    checkAxisSubStatus(&axisY, &axisSubStep[1]);
    checkAxisSubStatus(&axisZ, &axisSubStep[2]);

    axisX.checkTiming();
    axisY.checkTiming();
    axisZ.checkTiming();

    if (axisX.isStepDone())
    {
      axisX.checkMovement();
      //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
      axisX.resetStepDone();
    }

    if (axisY.isStepDone())
    {
      axisY.checkMovement();
      //checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]);
      axisY.resetStepDone();
    }

    if (axisZ.isStepDone())
    {
      axisZ.checkMovement();
      //checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]);
      axisZ.resetStepDone();
    }

    if (axisX.isAxisActive() && motorConsMissedSteps[0] > motorConsMissedStepsMax[0])
    {
      axisX.deactivateAxis();
      serialBuffer += "R99";
      serialBuffer += " deactivate motor X due to missed steps";
      serialBuffer += "\r\n";

      if (xHome)
      {
        encoderX.setPosition(0);
        axisX.setCurrentPosition(0);
      }
      else
      {
        error = ERR_STALL_DETECTED;
      }
    }

    if (axisY.isAxisActive() && motorConsMissedSteps[1] > motorConsMissedStepsMax[1])
    {
      axisY.deactivateAxis();
      serialBuffer += "R99";
      serialBuffer += " deactivate motor Y due to missed steps";
      serialBuffer += "\r\n";
      
      if (yHome)
      {
        encoderY.setPosition(0);
        axisY.setCurrentPosition(0);
      }
      else
      {
        error = ERR_STALL_DETECTED;
      }
    }

    if (axisZ.isAxisActive() && motorConsMissedSteps[2] > motorConsMissedStepsMax[2])
    {
      axisZ.deactivateAxis();

      serialBuffer += "R99";
      serialBuffer += " deactivate motor Z due to missed steps";
      serialBuffer += "\r\n";

      if (zHome)
      {
        encoderZ.setPosition(0);
        axisZ.setCurrentPosition(0);        
      }
      else
      {
        error = ERR_STALL_DETECTED;
      }
    }

    if (axisX.endStopAxisReached(false))
    {
      axisX.setCurrentPosition(0);
      encoderX.setPosition(0);
    }

    if (axisY.endStopAxisReached(false))
    {
      axisY.setCurrentPosition(0);
      encoderY.setPosition(0);
    }

    if (axisZ.endStopAxisReached(false))
    {
      axisZ.setCurrentPosition(0);
      encoderZ.setPosition(0);
    }

    axisActive[0] = axisX.isAxisActive();
    axisActive[1] = axisY.isAxisActive();
    axisActive[2] = axisZ.isAxisActive();

    currentPoint[0] = axisX.currentPosition();
    currentPoint[1] = axisY.currentPosition();
    currentPoint[2] = axisZ.currentPosition();

    CurrentState::getInstance()->setX(currentPoint[0]);
    CurrentState::getInstance()->setY(currentPoint[1]);
    CurrentState::getInstance()->setZ(currentPoint[2]);

    storeEndStops();

    // Check timeouts
    if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000)))
    {
      serialBuffer += COMM_REPORT_TIMEOUT_X;
      serialBuffer += "\r\n";
      serialBuffer += "R99 timeout X axis\r\n";
      error = ERR_TIMEOUT;
    }
    if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[1] * 1000) || (millis() < timeStart && millis() > timeOut[1] * 1000)))
    {
      serialBuffer += COMM_REPORT_TIMEOUT_Y;
      serialBuffer += "\r\n";
      serialBuffer += "R99 timeout Y axis\r\n";
      error = ERR_TIMEOUT;
    }
    if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[2] * 1000) || (millis() < timeStart && millis() > timeOut[2] * 1000)))
    {
      serialBuffer += COMM_REPORT_TIMEOUT_Z;
      serialBuffer += "\r\n";
      serialBuffer += "R99 timeout Z axis\r\n";
      error = ERR_TIMEOUT;
    }

    // Check if there is an emergency stop command
    if (Serial.available() > 0)
    {
      incomingByte = Serial.read();
      if (incomingByte == 69 || incomingByte == 101)
      {
        serialBuffer += "R99 emergency stop\r\n";

        Serial.print(COMM_REPORT_EMERGENCY_STOP);
        CurrentState::getInstance()->printQAndNewLine();

        emergencyStop = true;

        axisX.deactivateAxis();
        axisY.deactivateAxis();
        axisZ.deactivateAxis();

        axisActive[0] = false;
        axisActive[1] = false;
        axisActive[2] = false;

        error = ERR_EMERGENCY_STOP;
      }
    }

    if (error != 0)
    {
      serialBuffer += "R99 error\r\n";

      axisActive[0] = false;
      axisActive[1] = false;
      axisActive[2] = false;
    }

    // Send the serial buffer one character per cycle to keep motor timing more accuracte
    serialBufferSendNext();

    // Periodically send message still active
    currentMillis++;
    serialMessageDelay++;

    if (serialMessageDelay > 300 && serialBuffer.length() == 0 && serialBufferSending == 0)
    {

      //Serial.print("Y-");
      //axisY.test();/**/

      serialMessageDelay = 0;

      switch(serialMessageNr)
      {
        case 0:
          serialBuffer += COMM_REPORT_CMD_BUSY;
          serialBuffer += CurrentState::getInstance()->getQAndNewLine();
          break;
        case 1:
          serialBuffer += CurrentState::getInstance()->getPosition();
          serialBuffer += CurrentState::getInstance()->getQAndNewLine();
          #if defined(FARMDUINO_V14)
            getEncoderReport();
          #endif
          break;

        case 2:
          #if defined(FARMDUINO_EXP_V20)
          
          serialBuffer += "R89";
          serialBuffer += " X";
          serialBuffer += axisX.getLoad();
          serialBuffer += " Y";
          serialBuffer += axisY.getLoad();
          serialBuffer += " Z";
          serialBuffer += axisZ.getLoad();
          serialBuffer += CurrentState::getInstance()->getQAndNewLine();
          
          #endif
          break;

        default:
          break;
      }

      serialMessageNr++;

      #if !defined(FARMDUINO_EXP_V20)
      if (serialMessageNr > 1)
      {
        serialMessageNr = 0;
      }
      #endif

      #if defined(FARMDUINO_EXP_V20)
      
      if (serialMessageNr > 2)
      {
        serialMessageNr = 0;
      }
      #endif

      serialBufferSending = 0;
      
      if (debugMessages /*&& debugInterrupt*/)
      {

				Serial.print("R99");
				Serial.print(" missed step ");
				Serial.print(motorConsMissedSteps[1]);
				Serial.print(" encoder pos ");
				Serial.print(encoderY.currentPosition());
				Serial.print(" axis pos ");
				Serial.print(axisY.currentPosition());
				Serial.print("\r\n");
      }
    }
  }

  serialBufferEmpty();
  Serial.print("R99 stopped\r\n");

  // Send feedback for homing

  if (xHome && !error && !emergencyStop)
  {
    Serial.print(COMM_REPORT_HOMED_X);
    CurrentState::getInstance()->printQAndNewLine();
  }

  if (yHome && !error && !emergencyStop)
  {
    Serial.print(COMM_REPORT_HOMED_Y);
    CurrentState::getInstance()->printQAndNewLine();
  }

  if (zHome && !error && !emergencyStop)
  {
    Serial.print(COMM_REPORT_HOMED_Z);
    CurrentState::getInstance()->printQAndNewLine();
  }

  // Stop motors

  axisSubStep[0] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
  axisSubStep[1] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
  axisSubStep[2] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;

  reportStatus(&axisX, axisSubStep[0]);
  reportStatus(&axisY, axisSubStep[1]);
  reportStatus(&axisZ, axisSubStep[2]);

  disableMotors();

  // Report end statuses

  currentPoint[0] = axisX.currentPosition();
  currentPoint[1] = axisY.currentPosition();
  currentPoint[2] = axisZ.currentPosition();

  CurrentState::getInstance()->setX(currentPoint[0]);
  CurrentState::getInstance()->setY(currentPoint[1]);
  CurrentState::getInstance()->setZ(currentPoint[2]);

  storeEndStops();
  reportEndStops();
  reportPosition();
  reportEncoders();

  // Report axis idle

  axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE;
  axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
  axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;

  reportStatus(&axisX, axisSubStep[0]);
  reportStatus(&axisY, axisSubStep[1]);
  reportStatus(&axisZ, axisSubStep[2]);

  disableMotors();

  if (emergencyStop)
  {
    CurrentState::getInstance()->setEmergencyStop();
    error = ERR_EMERGENCY_STOP;
  }

  Serial.print("R99 error ");
  Serial.print(error);
  Serial.print("\r\n");


  // Return error
  CurrentState::getInstance()->setLastError(error);

  return error;
}

void Movement::serialBufferEmpty()
{
  while (serialBuffer.length() > 0)
  {
    serialBufferSendNext();
  }
}

void Movement::serialBufferSendNext()
{
  // Send the next char in the serialBuffer
  if (serialBuffer.length() > 0)
  {

    if (serialBufferSending < serialBuffer.length())
    {
      //Serial.print("-");
      switch (serialBuffer.charAt(serialBufferSending))
      {
      case 13:
        Serial.print("\r\n");
        break;
      case 10:
        break;
      default:
        Serial.print(serialBuffer.charAt(serialBufferSending));
        break;
      }

      serialBufferSending++;
    }
    else
    {
      // Reset length of buffer when done
      serialBuffer = "";
      serialBufferSending = 0;
    }
  }
}

//
// Calibration
//

int Movement::calibrateAxis(int axis)
{

  // Load motor and encoder settings

  loadMotorSettings();
  loadEncoderSettings();

  //unsigned long timeStart             = millis();

  bool movementDone = false;

  int paramValueInt = 0;
  int stepsCount = 0;
  int incomingByte = 0;
  int error = 0;

  bool invertEndStops = false;
  int parEndInv;
  int parNbrStp;

  float *missedSteps;
  int *missedStepsMax;
  long *lastPosition;
  float *encoderStepDecay;
  bool *encoderEnabled;
  int *axisStatus;
  long *axisStepsPerMm;

  // Prepare for movement

  storeEndStops();
  reportEndStops();

  // Select the right axis
  MovementAxis *calibAxis;
  MovementEncoder *calibEncoder;

  switch (axis)
  {
  case 0:
    calibAxis = &axisX;
    calibEncoder = &encoderX;
    parEndInv = MOVEMENT_INVERT_ENDPOINTS_X;
    parNbrStp = MOVEMENT_AXIS_NR_STEPS_X;
    invertEndStops = endStInv[0];
    missedSteps = &motorConsMissedSteps[0];
    missedStepsMax = &motorConsMissedStepsMax[0];
    lastPosition = &motorLastPosition[0];
    encoderStepDecay = &motorConsMissedStepsDecay[0];
    encoderEnabled = &motorConsEncoderEnabled[0];
    axisStatus = &axisSubStep[0];
    axisStepsPerMm = &stepsPerMm[0];
    break;
  case 1:
    calibAxis = &axisY;
    calibEncoder = &encoderY;
    parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;
    parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y;
    invertEndStops = endStInv[1];
    missedSteps = &motorConsMissedSteps[1];
    missedStepsMax = &motorConsMissedStepsMax[1];
    lastPosition = &motorLastPosition[1];
    encoderStepDecay = &motorConsMissedStepsDecay[1];
    encoderEnabled = &motorConsEncoderEnabled[1];
    axisStatus = &axisSubStep[1];
    axisStepsPerMm = &stepsPerMm[1];
    break;
  case 2:
    calibAxis = &axisZ;
    calibEncoder = &encoderZ;
    parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z;
    parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z;
    invertEndStops = endStInv[2];
    missedSteps = &motorConsMissedSteps[2];
    missedStepsMax = &motorConsMissedStepsMax[2];
    lastPosition = &motorLastPosition[2];
    encoderStepDecay = &motorConsMissedStepsDecay[2];
    encoderEnabled = &motorConsEncoderEnabled[2];
    axisStatus = &axisSubStep[2];
    axisStepsPerMm = &stepsPerMm[2];
    break;
  default:
    Serial.print("R99 Calibration error: invalid axis selected\r\n");
    error = 1;
    CurrentState::getInstance()->setLastError(error);
    return error;
  }

  // Preliminary checks

  if (calibAxis->endStopMin() || calibAxis->endStopMax())
  {
    Serial.print("R99 Calibration error: end stop active before start\r\n");
    error = 1;
    CurrentState::getInstance()->setLastError(error);
    return error;
  }

  Serial.print("R99");
  Serial.print(" axis ");
  Serial.print(calibAxis->channelLabel);
  Serial.print(" move to start for calibration");
  Serial.print("\r\n");

  *axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR;
  reportStatus(calibAxis, axisStatus[0]);

  // Move towards home
  calibAxis->enableMotor();
  
  /**/
  //calibAxis->setDirectionHome();
  calibAxis->setDirectionAway();

  calibAxis->setCurrentPosition(calibEncoder->currentPosition());

  stepsCount = 0;
  *missedSteps = 0;
  movementDone = false;

  motorConsMissedSteps[0] = 0;
  motorConsMissedSteps[1] = 0;
  motorConsMissedSteps[2] = 0;

  *axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
  reportStatus(calibAxis, axisStatus[0]);

  reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_HOME);

  while (!movementDone && error == 0)
  {

    #if defined(FARMDUINO_V14)
      checkEncoders();
    #endif

    checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]);

    // Check if there is an emergency stop command
    if (Serial.available() > 0)
    {
      incomingByte = Serial.read();
      if (incomingByte == 69 || incomingByte == 101)
      {
        Serial.print("R99 emergency stop\r\n");
        movementDone = true;
        CurrentState::getInstance()->setEmergencyStop();
        Serial.print(COMM_REPORT_EMERGENCY_STOP);
        CurrentState::getInstance()->printQAndNewLine();
        error = 1;
      }
    }

    // Move until any end stop is reached or the motor is skipping. That end should be the far end stop. First, ram the end at high speed.

    /**/
    //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax))
    //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
    if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
    {

      calibAxis->setStepAxis();
      

      delayMicroseconds(100000 / speedHome[axis] / 2);

      stepsCount++;
      if (stepsCount % (speedHome[axis] * 3) == 0)
      {
        // Periodically send message still active
        Serial.print(COMM_REPORT_CMD_BUSY);
        CurrentState::getInstance()->printQAndNewLine();
      }

      if (debugMessages)
      {
        if (stepsCount % (speedHome[axis] / 6) == 0 /*|| *missedSteps > 3*/)
        {
          Serial.print("R99");
          Serial.print(" step count ");
          Serial.print(stepsCount);
          Serial.print(" missed steps ");
          Serial.print(*missedSteps);
          Serial.print(" max steps ");
          Serial.print(*missedStepsMax);
          Serial.print(" cur pos mtr ");
          Serial.print(calibAxis->currentPosition());
          Serial.print(" cur pos enc ");
          Serial.print(calibEncoder->currentPosition());
          Serial.print("\r\n");
        }
      }

      calibAxis->resetMotorStep();
      delayMicroseconds(100000 / speedHome[axis] / 2);
    }
    else
    {
      movementDone = true;
      Serial.print("R99 movement done\r\n");

      // If end stop for home is active, set the position to zero
      if (calibAxis->endStopMin())
      {
        invertEndStops = true;
      }
    }
  }

  reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_END);

  Serial.print("R99");
  Serial.print(" axis ");
  Serial.print(calibAxis->channelLabel);
  Serial.print(" at starting point");
  Serial.print("\r\n");

  // Report back the end stop setting

  if (error == 0)
  {
    if (invertEndStops)
    {
      paramValueInt = 1;
    }
    else
    {
      paramValueInt = 0;
    }

    Serial.print("R23");
    Serial.print(" ");
    Serial.print("P");
    Serial.print(parEndInv);
    Serial.print(" ");
    Serial.print("V");
    Serial.print(paramValueInt);
    //Serial.print("\r\n");
    CurrentState::getInstance()->printQAndNewLine();
  }

  // Store the status of the system

  storeEndStops();
  reportEndStops();

  // Move into the other direction now, and measure the number of steps

  Serial.print("R99");
  Serial.print(" axis ");
  Serial.print(calibAxis->channelLabel);
  Serial.print(" calibrating length");
  Serial.print("\r\n");

  stepsCount = 0;
  movementDone = false;
  *missedSteps = 0;

  /**/
  //calibAxis->setDirectionAway();
  calibAxis->setDirectionHome();

  calibAxis->setCurrentPosition(calibEncoder->currentPosition());

  motorConsMissedSteps[0] = 0;
  motorConsMissedSteps[1] = 0;
  motorConsMissedSteps[2] = 0;

  long encoderStartPoint = calibEncoder->currentPosition();
  long encoderEndPoint = calibEncoder->currentPosition();

  while (!movementDone && error == 0)
  {

    #if defined(FARMDUINO_V14)
       checkEncoders();
    #endif

    checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]);

    // Check if there is an emergency stop command
    if (Serial.available() > 0)
    {
      incomingByte = Serial.read();
      if (incomingByte == 69 || incomingByte == 101)
      {
        Serial.print("R99 emergency stop\r\n");
        movementDone = true;
        CurrentState::getInstance()->setEmergencyStop();
        Serial.print(COMM_REPORT_EMERGENCY_STOP);
        CurrentState::getInstance()->printQAndNewLine();
        error = 1;
      }
    }

    // Ignore the missed steps at startup time
    if (stepsCount < 10)
    {
      *missedSteps = 0;
    }

    // Move until the end stop is at the home position by detecting the other end stop or missed steps are detected
    /**/
    //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
    //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax))
    if (((!invertEndStops && !calibAxis->endStopMin()) || (invertEndStops && !calibAxis->endStopMax())) && !movementDone && (*missedSteps < *missedStepsMax))

    {

      calibAxis->setStepAxis();
      stepsCount++;

      //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);

      delayMicroseconds(100000 / speedHome[axis] / 2);

      if (stepsCount % (speedHome[axis] * 3) == 0)
      {
        // Periodically send message still active
        Serial.print(COMM_REPORT_CMD_BUSY);
        //Serial.print("\r\n");
        CurrentState::getInstance()->printQAndNewLine();

        Serial.print("R99");
        Serial.print(" step count: ");
        Serial.print(stepsCount);
        Serial.print("\r\n");
      }

      calibAxis->resetMotorStep();
      delayMicroseconds(100000 / speedHome[axis] / 2);
    }
    else
    {
      Serial.print("R99 movement done\r\n");
      movementDone = true;
    }
  }

  Serial.print("R99");
  Serial.print(" axis ");
  Serial.print(calibAxis->channelLabel);
  Serial.print(" at end point");
  Serial.print("\r\n");

  encoderEndPoint = calibEncoder->currentPosition();

  // if the encoder is enabled, use the encoder data instead of the step count

  if (encoderEnabled)
  {
    stepsCount = abs(encoderEndPoint - encoderStartPoint);
  }

  // Report back the end stop setting

  if (error == 0)
  {
    Serial.print("R23");
    Serial.print(" ");
    Serial.print("P");
    Serial.print(parNbrStp);
    Serial.print(" ");
    Serial.print("V");
    Serial.print((float)stepsCount);
    CurrentState::getInstance()->printQAndNewLine();
  }

  *axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
  reportStatus(calibAxis, axisStatus[0]);

  calibAxis->disableMotor();

  storeEndStops();
  reportEndStops();

  switch (axis)
  {
  case 0:
    CurrentState::getInstance()->setX(stepsCount);
    break;
  case 1:
    CurrentState::getInstance()->setY(stepsCount);
    break;
  case 2:
    CurrentState::getInstance()->setZ(stepsCount);
    break;
  }

  reportPosition();

  *axisStatus = COMM_REPORT_MOVE_STATUS_IDLE;
  reportStatus(calibAxis, axisStatus[0]);

  reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_IDLE);

  CurrentState::getInstance()->setLastError(error);
  return error;
}

int debugPrintCount = 0;

// Check encoder to verify the motor is at the right position
void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled)
{
#if !defined(FARMDUINO_EXP_V20)
  if (*encoderEnabled)
  {
    bool stepMissed = false;

    if (debugMessages)
    {
		  //debugPrintCount++;
		  //if (debugPrintCount % 50 == 0)
		  {
			  Serial.print("R99");
			  Serial.print(" encoder pos ");
			  Serial.print(encoder->currentPosition());
        Serial.print(" last enc ");
        Serial.print(*encoderLastPosition);
        Serial.print(" axis pos ");
			  Serial.print(axis->currentPosition());
			  Serial.print(" last pos ");
			  Serial.print(*lastPosition);
			  Serial.print(" move up ");
			  Serial.print(axis->movingUp());
			  Serial.print(" missed step cons ");
			  Serial.print(motorConsMissedSteps[0]);
			  Serial.print(" missed step ");
			  Serial.print(*missedSteps);
			  //Serial.print(" encoder X pos ");
			  //Serial.print(encoderX.currentPosition());
			  //Serial.print(" axis X pos ");
			  //Serial.print(axisX.currentPosition());
			  Serial.print(" decay ");
			  Serial.print(*encoderStepDecay);
			  Serial.print(" enabled ");
			  Serial.print(*encoderEnabled);
			  Serial.print("\r\n");
		  }
		  
    }

    // Decrease amount of missed steps if there are no missed step
    if (*missedSteps > 0)
    {
      (*missedSteps) -= (*encoderStepDecay);
    }
    
    // Check if the encoder goes in the wrong direction or nothing moved
    if ((axis->movingUp() && *encoderLastPosition > encoder->currentPositionRaw()) ||
        (!axis->movingUp() && *encoderLastPosition < encoder->currentPositionRaw()))
    {
      stepMissed = true;
    }

    if (stepMissed && *missedSteps < 32000)
    {
      (*missedSteps)++;
    }

    *encoderLastPosition = encoder->currentPositionRaw();
    *lastPosition = axis->currentPosition();

    //axis->resetStepDone();

    if (*encoderUseForPos)
    {
      axis->setCurrentPosition(encoder->currentPosition());
    }
  }
#endif

#if defined(FARMDUINO_EXP_V20)

  /**/
  /*
  Serial.print("R99");
  Serial.print(" XXX ");
  Serial.print(" cur pos ");
  Serial.print(axis->currentPosition());
  Serial.print(" last pos ");
  Serial.print(*lastPosition);
  */

  if (*encoderEnabled) {
    if (axis->stallDetected()) {
      // In case of stall detection, count this as a missed step
      (*missedSteps)++;
      axis->setCurrentPosition(*lastPosition);
    }
    else {
      // Decrease amount of missed steps if there are no missed step
      if (*missedSteps > 0)
      {
        (*missedSteps) -= (*encoderStepDecay);
      }
      *lastPosition = axis->currentPosition();
      encoder->setPosition(axis->currentPosition());
    }
  }

  //Serial.print(" new last pos ");
  //Serial.print(*lastPosition);
  //Serial.print(" en pos ");
  //Serial.print(encoder->currentPosition());
  //Serial.print("\r\n");


#endif

}

void Movement::loadMotorSettings()
{

  // Load settings

  homeIsUp[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
  homeIsUp[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y);
  homeIsUp[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z);

  speedMax[0] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_X);
  speedMax[1] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Y);
  speedMax[2] = ParameterList::getInstance()->getValue(MOVEMENT_MAX_SPD_Z);

  speedMin[0] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_X);
  speedMin[1] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Y);
  speedMin[2] = ParameterList::getInstance()->getValue(MOVEMENT_MIN_SPD_Z);

  speedHome[0] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_SPEED_X);
  speedHome[1] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_SPEED_Y);
  speedHome[2] = ParameterList::getInstance()->getValue(MOVEMENT_HOME_SPEED_Z);

  stepsAcc[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_X);
  stepsAcc[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Y);
  stepsAcc[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEPS_ACC_DEC_Z);

  motorInv[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X));
  motorInv[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y));
  motorInv[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z));

  endStInv[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X);
  endStInv[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y);
  endStInv[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z);

  endStInv2[0] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_2_ENDPOINTS_X);
  endStInv2[1] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_2_ENDPOINTS_Y);
  endStInv2[2] = ParameterList::getInstance()->getValue(MOVEMENT_INVERT_2_ENDPOINTS_Z);

  endStEnbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_X));
  endStEnbl[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_Y));
  endStEnbl[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_ENABLE_ENDPOINTS_Z));

  timeOut[0] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
  timeOut[1] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Y);
  timeOut[2] = ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_Z);

  motorKeepActive[0] = ParameterList::getInstance()->getValue(MOVEMENT_KEEP_ACTIVE_X);
  motorKeepActive[1] = ParameterList::getInstance()->getValue(MOVEMENT_KEEP_ACTIVE_Y);
  motorKeepActive[2] = ParameterList::getInstance()->getValue(MOVEMENT_KEEP_ACTIVE_Z);

  motorMaxSize[0] = ParameterList::getInstance()->getValue(MOVEMENT_AXIS_NR_STEPS_X);
  motorMaxSize[1] = ParameterList::getInstance()->getValue(MOVEMENT_AXIS_NR_STEPS_Y);
  motorMaxSize[2] = ParameterList::getInstance()->getValue(MOVEMENT_AXIS_NR_STEPS_Z);

  motor2Inv[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_INVERT_X));
  motor2Inv[1] = false;
  motor2Inv[2] = false;

  motor2Enbl[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_SECONDARY_MOTOR_X));
  motor2Enbl[1] = false;
  motor2Enbl[2] = false;

  motorStopAtHome[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_HOME_X));
  motorStopAtHome[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_HOME_Y));
  motorStopAtHome[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_HOME_Z));

  motorStopAtMax[0] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_MAX_X));
  motorStopAtMax[1] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_MAX_Y));
  motorStopAtMax[2] = intToBool(ParameterList::getInstance()->getValue(MOVEMENT_STOP_AT_MAX_Z));

  stepsPerMm[0] = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X);
  stepsPerMm[1] = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y);
  stepsPerMm[2] = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z);

  if (stepsPerMm[0] < 1) 
  {
    stepsPerMm[0] = 1;
  }

  if (stepsPerMm[1] < 1)
  {
    stepsPerMm[1] = 1;
  }

  if (stepsPerMm[2] < 1)
  {
    stepsPerMm[2] = 1;
  }

  CurrentState::getInstance()->setStepsPerMm(stepsPerMm[0], stepsPerMm[1], stepsPerMm[2]);

  axisX.loadMotorSettings(speedMax[0], speedMin[0], speedHome[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], endStInv2[0], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[0], motor2Inv[0], endStEnbl[0], motorStopAtHome[0], motorMaxSize[0], motorStopAtMax[0]);
  axisY.loadMotorSettings(speedMax[1], speedMin[1], speedHome[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], endStInv2[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1], motorStopAtHome[1], motorMaxSize[1], motorStopAtMax[1]);
  axisZ.loadMotorSettings(speedMax[2], speedMin[2], speedHome[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], endStInv2[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2], motorStopAtHome[2], motorMaxSize[2], motorStopAtMax[2]);

  /**/

/*
#if defined(FARMDUINO_EXP_V20)
  loadSettingsTMC2130();
#endif
*/
  primeMotors();
}

bool Movement::intToBool(int value)
{
  if (value == 1)
  {
    return true;
  }
  return false;
}

void Movement::loadEncoderSettings()
{

  // Load encoder settings

  motorConsMissedStepsMax[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X);
  motorConsMissedStepsMax[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y);
  motorConsMissedStepsMax[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z);

  motorConsMissedStepsDecay[0] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_X);
  motorConsMissedStepsDecay[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y);
  motorConsMissedStepsDecay[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z);

  motorConsMissedStepsDecay[0] = motorConsMissedStepsDecay[0] / 100;
  motorConsMissedStepsDecay[1] = motorConsMissedStepsDecay[1] / 100;
  motorConsMissedStepsDecay[2] = motorConsMissedStepsDecay[2] / 100;

  motorConsMissedStepsDecay[0] = min(max(motorConsMissedStepsDecay[0], 0.01), 99);
  motorConsMissedStepsDecay[1] = min(max(motorConsMissedStepsDecay[1], 0.01), 99);
  motorConsMissedStepsDecay[2] = min(max(motorConsMissedStepsDecay[2], 0.01), 99);

  motorConsEncoderType[0] = ParameterList::getInstance()->getValue(ENCODER_TYPE_X);
  motorConsEncoderType[1] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Y);
  motorConsEncoderType[2] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Z);

  motorConsEncoderScaling[0] = ParameterList::getInstance()->getValue(ENCODER_SCALING_X);
  motorConsEncoderScaling[1] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Y);
  motorConsEncoderScaling[2] = ParameterList::getInstance()->getValue(ENCODER_SCALING_Z);

  motorConsEncoderUseForPos[0] = ParameterList::getInstance()->getValue(ENCODER_USE_FOR_POS_X);
  motorConsEncoderUseForPos[1] = ParameterList::getInstance()->getValue(ENCODER_USE_FOR_POS_Y);
  motorConsEncoderUseForPos[2] = ParameterList::getInstance()->getValue(ENCODER_USE_FOR_POS_Z);

  motorConsEncoderInvert[0] = ParameterList::getInstance()->getValue(ENCODER_INVERT_X);
  motorConsEncoderInvert[1] = ParameterList::getInstance()->getValue(ENCODER_INVERT_Y);
  motorConsEncoderInvert[2] = ParameterList::getInstance()->getValue(ENCODER_INVERT_Z);

  if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_X) == 1)
  {
    motorConsEncoderEnabled[0] = true;
  }
  else
  {
    motorConsEncoderEnabled[0] = false;
  }

  if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Y) == 1)
  {
    motorConsEncoderEnabled[1] = true;
  }
  else
  {
    motorConsEncoderEnabled[1] = false;
  }

  if (ParameterList::getInstance()->getValue(ENCODER_ENABLED_Z) == 1)
  {
    motorConsEncoderEnabled[2] = true;
  }
  else
  {
    motorConsEncoderEnabled[2] = false;
  }
}

unsigned long Movement::getMaxLength(unsigned long lengths[3])
{
  unsigned long max = lengths[0];
  for (int i = 1; i < 3; i++)
  {
    if (lengths[i] > max)
    {
      max = lengths[i];
    }
  }
  return max;
}

void Movement::enableMotors()
{
  motorMotorsEnabled = true;

  axisX.enableMotor();
  axisY.enableMotor();
  axisZ.enableMotor();

  delay(100);
}

void Movement::disableMotorsEmergency()
{
  motorMotorsEnabled = false;

  axisX.disableMotor();
  axisY.disableMotor();
  axisZ.disableMotor();
}

void Movement::disableMotors()
{
  motorMotorsEnabled = false;

  if (motorKeepActive[0] == 0) { axisX.disableMotor(); }
  if (motorKeepActive[1] == 0) { axisY.disableMotor(); }
  if (motorKeepActive[2] == 0) { axisZ.disableMotor(); }

  delay(100);
}

void Movement::primeMotors()
{
  if (motorKeepActive[0] == 1) { axisX.enableMotor(); } else { axisX.disableMotor(); }
  if (motorKeepActive[1] == 1) { axisY.enableMotor(); } else { axisY.disableMotor(); }
  if (motorKeepActive[2] == 1) { axisZ.enableMotor(); } else { axisZ.disableMotor(); }
}

bool Movement::motorsEnabled()
{
  return motorMotorsEnabled;
}

bool Movement::endStopsReached()
{

  if (axisX.endStopsReached() ||
      axisY.endStopsReached() ||
      axisZ.endStopsReached())
  {
    return true;
  }
  return false;
}

void Movement::storePosition()
{

#if !defined(FARMDUINO_EXP_V20)
  if (motorConsEncoderEnabled[0])
  {
    CurrentState::getInstance()->setX(encoderX.currentPosition());
  }
  else
  {
    CurrentState::getInstance()->setX(axisX.currentPosition());
  }

  if (motorConsEncoderEnabled[1])
  {
    CurrentState::getInstance()->setY(encoderY.currentPosition());
  }
  else
  {
    CurrentState::getInstance()->setY(axisY.currentPosition());
  }

  if (motorConsEncoderEnabled[2])
  {
    CurrentState::getInstance()->setZ(encoderZ.currentPosition());
  }
  else
  {
    CurrentState::getInstance()->setZ(axisZ.currentPosition());
  }
#endif

#if defined(FARMDUINO_EXP_V20)
  CurrentState::getInstance()->setX(axisX.currentPosition());
  CurrentState::getInstance()->setY(axisY.currentPosition());
  CurrentState::getInstance()->setZ(axisZ.currentPosition());
#endif

}

void Movement::reportEndStops()
{
  CurrentState::getInstance()->printEndStops();
}

void Movement::reportPosition()
{
  CurrentState::getInstance()->printPosition();
}

void Movement::storeEndStops()
{
  CurrentState::getInstance()->storeEndStops();
}

void Movement::setPositionX(long pos)
{
  axisX.setCurrentPosition(pos);
  encoderX.setPosition(pos);
}

void Movement::setPositionY(long pos)
{
  axisY.setCurrentPosition(pos);
  encoderY.setPosition(pos);
}

void Movement::setPositionZ(long pos)
{
  axisZ.setCurrentPosition(pos);
  encoderZ.setPosition(pos);
}

// Handle movement by checking each axis
void Movement::handleMovementInterrupt(void)
{
  // No need to check the encoders for Farmduino 1.4
  #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
    checkEncoders();
  #endif

  // handle motor timing

  axisX.incrementTick();
  axisY.incrementTick();
  axisZ.incrementTick();

}

void Movement::checkEncoders()
{
  // read encoder pins using the arduino IN registers instead of digital in
  // because it used much fewer cpu cycles

  // A=16/PH1 B=17/PH0 AQ=31/PC6 BQ=33/PC4
  encoderX.checkEncoder(
    ENC_X_A_PORT   & ENC_X_A_BYTE,
    ENC_X_B_PORT   & ENC_X_B_BYTE,
    ENC_X_A_Q_PORT & ENC_X_A_Q_BYTE,
    ENC_X_B_Q_PORT & ENC_X_B_Q_BYTE);

  // A=23/PA1 B=25/PA3 AQ=35/PC2 BQ=37/PC0
  encoderY.checkEncoder(
    ENC_Y_A_PORT   & ENC_Y_A_BYTE,
    ENC_Y_B_PORT   & ENC_Y_B_BYTE,
    ENC_Y_A_Q_PORT & ENC_Y_A_Q_BYTE,
    ENC_Y_B_Q_PORT & ENC_Y_B_Q_BYTE);

  // A=27/PA5 B=29/PA7 AQ=39/PG2 BQ=41/PG0
  encoderZ.checkEncoder(
    ENC_Z_A_PORT   & ENC_Z_A_BYTE,
    ENC_Z_B_PORT   & ENC_Z_B_BYTE,
    ENC_Z_A_Q_PORT & ENC_Z_A_Q_BYTE,
    ENC_Z_B_Q_PORT & ENC_Z_B_Q_BYTE);
}