// https://wokwi.com/projects/404328671355451393 this sketch/sim
// https://wokwi.com/projects/399355387445360641
// for https://forum.arduino.cc/t/limit-switch-issue-for-stepper-motor-application/1265784/12
//
// based on sim https://wokwi.com/projects/388661915235241985
// and https://github.com/waspinator/AccelStepper/tree/master/examples/Bounce
// adding a state machine to have an un-balanced CW-CCW movement
// and a limited number of cycle counts for
//

//
// Bounce.pde -- adapted for state machine
// -*- mode: C++ -*-
//
// Make a single stepper bounce from one limit to another
//
// Copyright (C) 2012 Mike McCauley
// $Id: Random.pde,v 1.1 2011/01/05 01:51:01 mikem Exp mikem $

#include <AccelStepper.h> // https://www.airspayce.com/mikem/arduino/AccelStepper/index.html

// Define a stepper and the pins it will use
AccelStepper stepper(1, 10, 7); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5

const byte LeftLimitSwitchPin = A1;
const byte RightLimitSwitchPin = A2;
const byte HomePin = A3;
const byte ledR = 2, ledL = 12;
enum States {IDLE, CCW, CW, CCW_HOMING, CW_SEEKING, BACK_OFF_CCW};
char *stateNames[] = {"Idle", "CCW", "CW", "CCW_HOMING", "CW_SEEKING", "BACK_OFF_CCW"};
int state = IDLE;
long leftPos = -50; // left end of planned motion relative to 0 in AccelStepper
long rightPos = 300; // right end of planned motion
long rightLimitSwitchPos_acc = 1e6;
int cycleCount = 2;
int currentCycle = 0;
bool homed = false;  // state variable for whether the endstops are known

const long X_LIM_POS = 0;  // limit switch location in accelStepper coord after homing
// for Simulation:
const long X_LIM_L_POS_boot = -25 ; // limit switch location in boot coord for sim
const long X_LIM_R_POS_boot = 300 ; // limit switch location in boot coord for sim
const int LIMIT_SWITCH_WIDTH = 500 ; // active length of switch in boot coordinates
long offsetAccToBoot = 0; // difference from bootup coordinates from accelStepper 0
bool simLeftSwitchTriggered = 0, simRightSwitchTriggered = 0;


void setup()
{
  Serial.begin(115200);
  pinMode(LeftLimitSwitchPin, INPUT_PULLUP);
  pinMode(RightLimitSwitchPin, INPUT_PULLUP);
  pinMode(HomePin, INPUT_PULLUP);
  stepper.setMaxSpeed(50); // increased after homed
  stepper.setAcceleration(100);
  stepper.moveTo(0); //CW
  state = 0;
  Serial.print("0cw:" );
  pinMode(ledR, OUTPUT);
  pinMode(ledL, OUTPUT);
  //Serial.print("Acceleration:");
  // Serial.print(stepper.getAcceleration());
}


void loop()
{
  simulateHardware();

  bool LeftLimitSwitchActive = digitalRead(LeftLimitSwitchPin) == LOW || simLeftSwitchTriggered;
  bool RightLimitSwitchActive = digitalRead(RightLimitSwitchPin) == LOW || simRightSwitchTriggered;

  // If at the end of travel go to the other end
  switch (state) {
    case IDLE: // IDLE
      ; // do nothing
      if (digitalRead(LeftLimitSwitchPin) == LOW) {
        currentCycle = 0;
        state = CCW;
        Serial.println();
      }
      if (digitalRead(HomePin) == LOW) {
        homed = false;
        stepper.setMaxSpeed(50);
        if (LeftLimitSwitchActive) {
          stepper.move(100);
          state = BACK_OFF_CCW;
        } else {
          stepper.move(-1000);
          state = CCW_HOMING;
        }
      }
      break;
    case CCW: // moving CCW
      if (stepper.distanceToGo() == 0 // finished motion?
          //|| limitSwitchActive // active limit switch
         )
      {
        currentCycle += 1;
        Serial.print(currentCycle);
        if (currentCycle <= cycleCount) {
          stepper.moveTo(rightPos);
          state = CW;
        } else {
          state = IDLE;
        }
      }
      break;
    case CW: // moving CW
      if (stepper.distanceToGo() == 0) {
        Serial.print(",");
        stepper.moveTo(leftPos);
        state = CCW;
      }
      break;
    case CCW_HOMING:
      if (LeftLimitSwitchActive) {
        homedLeft();
        stepper.moveTo(rightPos + 1000); // seek other limit
        state = CW_SEEKING;
      }
      break;
    case CW_SEEKING:
      if (RightLimitSwitchActive) {
        homedRight();
        stepper.moveTo(-10);
        state = CCW;
      }
      break;
    case BACK_OFF_CCW:
      if (LeftLimitSwitchActive) {
        homedLeft();
        stepper.moveTo(rightPos + 1000); // seek other limit
        state = CW_SEEKING;
      }
      break;
    default:
      break;
  }
  stepper.run(); // move stepper as needed

  // Output
  updateLeds();
  static int stateLast  = -1;
  if (state != stateLast) {
    posReport();
    stateLast = state;
  }
}

void posReport() {
  long here = stepper.currentPosition();
  Serial.print(millis());
  Serial.print(' ');
  Serial.print(stateNames[state]);
  Serial.print(" Pos_acc:");
  Serial.print(here);
  Serial.print(" Pos_boot:");
  Serial.print(here + offsetAccToBoot);
  Serial.print(homed ? " homed" : " not homed");
  Serial.println();

}

void homedLeft() {
  // Called after hit the LHS limit switch
  Serial.print("HomedLeft:");
  //homed = true;
  //stepper.setMaxSpeed(400);
  long here = stepper.currentPosition();
  stepper.setCurrentPosition(X_LIM_POS);
  long newHere = stepper.currentPosition();
  offsetAccToBoot = here - X_LIM_POS;
  Serial.print(" here_boot:"); Serial.print(here);
  Serial.print(" here_acc:"); Serial.print(newHere);
  Serial.print(" offsetActToBoot:"); Serial.print(offsetAccToBoot);
  Serial.print(" Pos_boot:"); Serial.print(newHere + offsetAccToBoot);
  Serial.println();
}

void homedRight() {
  // Called after hit the RHS limit switch
  Serial.print("HomedRight:");
  homed = true;
  stepper.setMaxSpeed(400);
  stepper.moveTo(0);
  long here = stepper.currentPosition();
  rightLimitSwitchPos_acc = here;
  Serial.print(" here_boot:"); Serial.print(here + offsetAccToBoot);
  Serial.print(" here_acc:"); Serial.print(here);
  Serial.print(" rightLimitSwitchPos_acc:"); Serial.print(rightLimitSwitchPos_acc);
  Serial.println();
}


//
// simulate the externals
//
void updateLeds(void) {
  const uint32_t interval = 10;
  static uint32_t last = 0;
  if (millis() - last >= interval) {
    digitalWrite(ledL, simLeftSwitchTriggered);
    digitalWrite(ledR, simRightSwitchTriggered);
  }
}
void simulateHardware(void) {
  const uint32_t interval = 10;
  const long left = X_LIM_L_POS_boot, right = X_LIM_R_POS_boot, width = LIMIT_SWITCH_WIDTH;

  static uint32_t last = 0;
  if (millis() - last >= interval) {
    long bootPos = stepper.currentPosition() + offsetAccToBoot;
    last += interval;
    simLeftSwitchTriggered = (bootPos <= left && bootPos > left - width)
                             || digitalRead(LeftLimitSwitchPin) == LOW ;
    simRightSwitchTriggered = (bootPos >= right && bootPos < right + width )
                              || digitalRead(RightLimitSwitchPin) == LOW;
  }
}