byte directionPinDrill1 = 12;

byte stepPinDrill1 = 13;

byte directionPinDrill2 = 10;

byte stepPinDrill2 = 11;

byte directionPinX = 8;

byte stepPinX = 9;

int sensorDrill1 = A2;

int sensorDrill2 = A1;

int sensorX = A0;

int detailSensor = A3;

int drillButton1 = A4;

int drillButton2 = A5;

int dowelSwitch = 2;

int detectDetailSwitch = 3;


//-------------------------------------------------------------------------


float stepsPerMillimeter = 50;

float millimetersPerSecondDrill1 = 30;

float millimetersPerSecondDrill2 = 30;

float millimetersPerSecondX = 30;

float workingArea = stepsPerMillimeter * 2800;

float minDistanceCouplerFromBase = stepsPerMillimeter * 32;

float standardDistanceCouplerFromBase = stepsPerMillimeter * 64;

float maxDistanceCouplerFromBase = stepsPerMillimeter * 96;

float maxDistanceBetweenCouplers = stepsPerMillimeter * 480;

float distanceDowelFromCoupler = stepsPerMillimeter * 32;

float distanceSensorFromDrills = stepsPerMillimeter * 50;

float distanceFromHomeSensors = stepsPerMillimeter * 10;

float couplerHoleDept = stepsPerMillimeter * 50;

float dowelHoleDept = stepsPerMillimeter * 30;

float intervalBetweenActions = 1000;

float intervalBetweenStepsDrill1 = 1000000 / (millimetersPerSecondDrill1 * stepsPerMillimeter) / 2;

float intervalBetweenStepsDrill2 = 1000000 / (millimetersPerSecondDrill2 * stepsPerMillimeter) / 2;

float intervalBetweenStepsX = 1000000 / (millimetersPerSecondX * stepsPerMillimeter) / 2;


//-------------------------------------------------------------------------


int selectedMotorStepPin; 

int selectedMotorDirectionPin;

int selectedMotorInterval; 

float position;

float startPosition;

float endPosition;

float detailLength;

int segmentsCount;

float distanceCouplerFromBase;

float distanceBetweenCouplers;

int couplersCount;

int currentDrill;


//-------------------------------------------------------------------------


int selectMotorPropertiesFunction (int motor) {

  switch (motor) {

    case 1:

      selectedMotorStepPin = stepPinDrill1; 
      
      selectedMotorDirectionPin = directionPinDrill1;
      
      selectedMotorInterval = intervalBetweenStepsDrill1; 

    break;

    case 2:

      selectedMotorStepPin = stepPinDrill2; 
      
      selectedMotorDirectionPin = directionPinDrill2;
      
      selectedMotorInterval = intervalBetweenStepsDrill2; 

    break;

    case 3:

      selectedMotorStepPin = stepPinX; 
      
      selectedMotorDirectionPin = directionPinX;
      
      selectedMotorInterval = intervalBetweenStepsX; 

    break;
    
  }

}


//-------------------------------------------------------------------------


int moveFunction (int direction, int Motor) {

  selectMotorPropertiesFunction(Motor);

  digitalWrite(selectedMotorDirectionPin, direction);

  digitalWrite(selectedMotorStepPin, HIGH); 

  delayMicroseconds(selectedMotorInterval);

  digitalWrite(selectedMotorStepPin, LOW); 

  delayMicroseconds(selectedMotorInterval);

  if (direction == HIGH and Motor == 3) { position++; } 

  else if (direction == LOW and Motor == 3) { position--; } 

}


//-------------------------------------------------------------------------


int moveByDistanceFunction (int direction, float distance) {

  for (int x = 0; x < distance; x++) {

    moveFunction(direction, 3);

  }

}


//-------------------------------------------------------------------------


int goToPositionFunction (float newPosition) {

  float distance = newPosition - position;

  int direction;

  if (distance > 0) { direction = HIGH; } 

  else if (distance < 0) { direction = LOW; } 

  moveByDistanceFunction (direction, distance);

}


//-------------------------------------------------------------------------



int drillingFunction (int drill, float holeDept) {

  for (int x = 0; x < holeDept; x++) {

    moveFunction(HIGH, drill);

  }

  for (int x = 0; x < holeDept; x++) {

    moveFunction(LOW, drill);

  }

  Serial.println(position / stepsPerMillimeter);

}

//-------------------------------------------------------------------------


int goToHomeFunction() {

  while (digitalRead(sensorDrill1) == LOW) { moveFunction(LOW, 1); }

  while (digitalRead(sensorDrill2) == LOW) { moveFunction(LOW, 2); }

  while (digitalRead(sensorX) == LOW) { moveFunction(LOW, 3); }


  while (digitalRead(sensorDrill1) == HIGH) { moveFunction(HIGH, 1); }

  while (digitalRead(sensorDrill2) == HIGH) { moveFunction(HIGH, 2); }

  while (digitalRead(sensorX) == HIGH) { moveFunction(HIGH, 3); }

  moveByDistanceFunction(HIGH, distanceFromHomeSensors + distanceSensorFromDrills);

  for (int x = 0; x < distanceFromHomeSensors; x++) {

    moveFunction(HIGH, 1);

    moveFunction(HIGH, 2);

  }

  position = 0;

  currentDrill = 0;

}


//-------------------------------------------------------------------------


int detectDetailFunction() {
  
  goToPositionFunction(0);
  
  while (digitalRead(detailSensor) == LOW and position < workingArea) {

    moveFunction(HIGH, 3); 

  }

  startPosition = position;
  
  delay(intervalBetweenActions);

  while (digitalRead(detailSensor) == HIGH and position < workingArea) {

    moveFunction(HIGH, 3); 

  }

  endPosition = position;

  detailLength = endPosition - startPosition;
  
  Serial.println(" ");
  Serial.println("------------------------detect detail-------------------------");
  Serial.println(" ");
  Serial.print("Detail length "); Serial.println(detailLength / stepsPerMillimeter);
  Serial.print("Start position "); Serial.print(startPosition / stepsPerMillimeter);
  Serial.print("End position "); Serial.println(endPosition / stepsPerMillimeter);

  
}


//-------------------------------------------------------------------------


int createCouplersDistanceFunction () {

  startPosition = 0;

  endPosition = 675 * stepsPerMillimeter;

  position = endPosition;

  detailLength = 675 * stepsPerMillimeter;

  if (detailLength - minDistanceCouplerFromBase * 2 <= maxDistanceBetweenCouplers) {
    
    distanceCouplerFromBase = minDistanceCouplerFromBase;

    segmentsCount = 1;
    
  } else if (detailLength - maxDistanceCouplerFromBase * 2 <= maxDistanceBetweenCouplers) {
    
    distanceCouplerFromBase = maxDistanceCouplerFromBase;

    segmentsCount = 1;
    
  } else if (detailLength - standardDistanceCouplerFromBase * 2 > maxDistanceBetweenCouplers) {
        
    distanceCouplerFromBase = standardDistanceCouplerFromBase;

    segmentsCount = ceil((detailLength - distanceCouplerFromBase * 2) / maxDistanceBetweenCouplers);
    
  }

  distanceBetweenCouplers = (detailLength - distanceCouplerFromBase * 2) / segmentsCount;

  couplersCount = segmentsCount + 1;
  
  Serial.println(" ");
  Serial.println("------------------------Coupler Distance-------------------------");
  Serial.println(" ");
  Serial.print("Detail length "); Serial.println(detailLength / stepsPerMillimeter);
  Serial.print("segments Count "); Serial.println(segmentsCount);
  Serial.print("Couplers Count "); Serial.println(ceil(couplersCount)); 
  Serial.print("Coupler From Base "); Serial.println(distanceCouplerFromBase / stepsPerMillimeter);
  Serial.print("Between Couplers "); Serial.println(distanceBetweenCouplers / stepsPerMillimeter);
  
}


//-------------------------------------------------------------------------


int selectDrillFunction(int drill) {

  switch (drill) {

    case 0:

      if (currentDrill = 1) { 
        
        moveByDistanceFunction (HIGH, distanceSensorFromDrills); 

        position = position - distanceSensorFromDrills;
        
      } else if (currentDrill = 2) {
        
        moveByDistanceFunction (LOW, distanceSensorFromDrills); 

        position = position + distanceSensorFromDrills;

      }

      currentDrill = 0;

    break;

    case 1:

      if (currentDrill = 0) { 
        
        moveByDistanceFunction (LOW, distanceSensorFromDrills); 

        position = position + distanceSensorFromDrills;
        
      } else if (currentDrill = 2) {
        
        moveByDistanceFunction (LOW, distanceSensorFromDrills * 2); 

        position = position + distanceSensorFromDrills * 2;

      }

      currentDrill = 1;

    break;

    case 2:

      if (currentDrill = 0) { 
        
        moveByDistanceFunction (HIGH, distanceSensorFromDrills); 

        position = position - distanceSensorFromDrills;
        
      } else if (currentDrill = 1) {
        
        moveByDistanceFunction (HIGH, distanceSensorFromDrills * 2); 

        position = position - distanceSensorFromDrills * 2;

      }

      currentDrill = 2;

    break;
    
  }

}


//-------------------------------------------------------------------------


int dowelsDrillingFunction(int direction) {

  selectDrillFunction(2);

  moveByDistanceFunction (direction, distanceCouplerFromBase - distanceDowelFromCoupler);

  for (int x = 1; x <= segmentsCount; x++) {

    moveByDistanceFunction (direction, distanceDowelFromCoupler * 2);

    drillingFunction (2, dowelHoleDept);

    moveByDistanceFunction (direction, distanceBetweenCouplers - distanceDowelFromCoupler * 2);

    drillingFunction (2, dowelHoleDept);

  }

}


//-------------------------------------------------------------------------


int couplersDrillingFunction(int direction, int drill) {

  selectDrillFunction(drill);

  moveByDistanceFunction (direction, distanceCouplerFromBase);

  drillingFunction (drill, couplerHoleDept);

  for (int x = 0; x < segmentsCount; x++) {
  
    moveByDistanceFunction (direction, distanceBetweenCouplers);

    drillingFunction (drill, couplerHoleDept);

  }

  moveByDistanceFunction (direction, distanceCouplerFromBase);

}


//-------------------------------------------------------------------------


int mainControllerFunction(int drill, int dowelSwitch, int detectDetailSwitch) {

  if (detectDetailSwitch == HIGH or detailLength == 0) { detectDetailFunction(); }

  createCouplersDistanceFunction ();

  int couplersDirection;

  int dowelsDirection;

  if (position == startPosition) { couplersDirection = HIGH; dowelsDirection = LOW; } 
  
  else if (position == endPosition) { couplersDirection = LOW; dowelsDirection = HIGH; }


  if (detailLength >= minDistanceCouplerFromBase * 3) {

    couplersDrillingFunction(couplersDirection, drill);

  }

  if (dowelSwitch == HIGH and detailLength >= distanceDowelFromCoupler + (minDistanceCouplerFromBase + distanceDowelFromCoupler ) * 2) { 
    
    dowelsDrillingFunction(dowelsDirection);
    
  }
  
  goToPositionFunction(startPosition);

}


//-------------------------------------------------------------------------


void setup() {


  Serial.begin(9600);

  pinMode(stepPinDrill1, OUTPUT); 

  pinMode(directionPinDrill1, OUTPUT); 

  pinMode(stepPinDrill2, OUTPUT); 

  pinMode(directionPinDrill2, OUTPUT);

  pinMode(stepPinX, OUTPUT); 

  pinMode(directionPinX, OUTPUT);

  pinMode(sensorX, INPUT);

  pinMode(sensorDrill1, INPUT);

  pinMode(sensorDrill2, INPUT);
  
  pinMode(detailSensor, INPUT);

  pinMode(drillButton1, INPUT);

  pinMode(drillButton2, INPUT);

  pinMode(dowelSwitch, INPUT);

  pinMode(detectDetailSwitch, INPUT);

  goToHomeFunction();
  
}


//-------------------------------------------------------------------------


void loop() {

  if (digitalRead(drillButton1) == HIGH) {

    mainControllerFunction(1, digitalRead(dowelSwitch), digitalRead(detectDetailSwitch));

  } else if (digitalRead(drillButton2) == HIGH) {

    mainControllerFunction(2, digitalRead(dowelSwitch), digitalRead(detectDetailSwitch));

  }

}