/*
  Forum: https://forum.arduino.cc/t/dc-motorsteuerung-mit-hall-endschalter/1376495/10
  Wokwi: https://wokwi.com/projects/429506925147834369

  2025/04/30
  ec2021

  Anschlüsse

  Blau = H1
  Grün = H2
  Rot  = H3

  Schwarz = S1
  Weiss   = S2

  In dieser Version wird das Erreichen der Magnetkontakte über die Stepperposition und durch Pins simuliert ...
  Mittels des Parameters der Funktion initSimMagnetSwitches(startPos); kann man das Verhalten des Sketches testen
  falls beim Start des Sketches H1, H2, H3 oder keine bekannte Position vorliegt.

*/
#include <MobaTools.h>

const byte s1Pin = 4;
const byte s2Pin = 5;
const byte h1Pin = 6;
const byte h2Pin = 7;
const byte h3Pin = 8;

const byte stepPin = 3;
const byte dirPin = 2;
const int stepsPerRev = 6400;

enum States {UNKNOWN, IDLE, WAIT, GOH1, GOH2, GOH3};
char cState[][10] = {"UNKNOWN", "IDLE", "WAIT", "GOH1", "GOH2", "GOH3"};

States state = UNKNOWN;

MoToStepper stepper1( stepsPerRev, STEPDIR );

struct switchType {
  byte pin;
  byte state = HIGH;
  unsigned long lastChange = 0;
  byte lastState = HIGH;
  void init(byte aPin) {
    pin = aPin;
    pinMode(pin, INPUT_PULLUP);
  }
  boolean was_pressed() {
    byte actState = digitalRead(pin);
    if (actState != state && millis() - lastChange > 100) {
      lastChange = millis();
      state = actState;
      return (state == LOW);
    } else {
      return false;
    }
  }
  boolean closed() {
    byte actState = digitalRead(pin);
    return (actState == LOW);
  }
};

switchType S1, S2, H1, H2, H3;

void setup()
{
  Serial.begin(115200);
  stepper1.attach( stepPin, dirPin );
  stepper1.setSpeed( 100 );
  S1.init(s1Pin);
  S2.init(s2Pin);
  H1.init(h1Pin);
  H2.init(h2Pin);
  H3.init(h3Pin);

  /***************************/
  initSimMagnetSwitches(0);   // Startposition 1 = H1, 2 = H2, 3 = H3, alles andere = Fehlerhafte Position
  /***************************/

  state = UNKNOWN;
  while (state == UNKNOWN) {
    setStartState();          // Der Sketch bleibt in dieser While-Schleife "hängen"´, bis eine der bekannten Positionen
                               // angefahren wird.


    /***************************/
    serialControl();          // Hier eine Möglichkeit zum Verfahren per seriellem Kommando
    // s oder S = Stopp
    // v oder V = Vorwärts
    // r oder R = Rückwärts
    simMagnetSwitches();      // Diese Funktion simuliert die Magnetkontakte anhand der Stepper-Position
    /***************************/
  }
}

void loop() {

  stateMachine();

  /***************************/
  simMagnetSwitches();
  /***************************/

}

void changeStateTo(States newState) {
  state = newState;
  Serial.print("State = ");
  Serial.println(cState[state]);

}

void controlMachine() {
  if (S1.was_pressed()) {
    Serial.println("S1 Pressed");
    stepper1.rotate(1);
    changeStateTo(GOH2);
  }
  if (S2.was_pressed()) {
    Serial.println("S2 Pressed");
    stepper1.rotate(1);
    changeStateTo(GOH3);
  }
}

void stateMachine() {
  static unsigned long startTime = 0;
  switch (state) {
    case IDLE:
      controlMachine();
      break;
    case WAIT:
      if (millis() - startTime >= 2000) {
        changeStateTo(GOH1);
        stepper1.rotate(-1);
      }
      break;
    case GOH1:
      if (H1.closed()) {
        Serial.println("H1");
        changeStateTo(IDLE);
        stepper1.stop();
      }
      break;
    case GOH2:
      if (H2.closed()) {
        Serial.println("H2");
        startTime = millis();
        changeStateTo(WAIT);
        stepper1.stop();
      }
      break;
    case GOH3:
      if (H3.closed()) {
        Serial.println("H3");
        startTime = millis();
        changeStateTo(WAIT);
        stepper1.stop();
      }
      break;
  }
}

void setStartState() {
  if (H1.closed()) {
    changeStateTo(IDLE);
  }
  if (H2.closed()) {
    changeStateTo(GOH2);
  }
  if (H3.closed()) {
    changeStateTo(GOH3);
  }
}

/******************************************************************************
  Ab hier Konstanten, Variable und Funktionen zur Simulation der Magnetkontakte
  und der manuellen Steuerung per Serial-Command
******************************************************************************/
const long h1Pos =    0;
const long h2Pos =  500;
const long h3Pos = 1000;
const long falsePos = 250;
const byte h1Sim =   11;
const byte h2Sim =   10;
const byte h3Sim =    9;

void serialControl() {
  if (Serial.available()) {
    char c = Serial.read();
    switch (c) {
      case 'v':
      case 'V':
        stepper1.rotate(1);
        break;
      case 'r':
      case 'R':
        stepper1.rotate(-1);
        break;
      case 's':
      case 'S':
        stepper1.stop();
        break;
    }
  }
  if (H1.closed()) {
    stepper1.stop();
  }
}


void initSimMagnetSwitches(byte startPos) {
  pinMode(h1Sim, OUTPUT);
  pinMode(h2Sim, OUTPUT);
  pinMode(h3Sim, OUTPUT);
  simSetSwitchesTo(HIGH, HIGH, HIGH);
  switch (startPos) {
    case 1:
      Serial.print("H1 as Start");
      stepper1.moveTo(h1Pos);
      break;
    case 2:
      Serial.print("H2 as Start");
      stepper1.moveTo(h2Pos);
      break;
    case 3:
      Serial.print("H3 as Start");
      stepper1.moveTo(h3Pos);
      break;
    default:
      Serial.print("Wrong Position as Start");
      stepper1.moveTo(falsePos);
      break;
  }
  while (stepper1.moving()) {};
  Serial.println("... reached!");
}

void simSetSwitchesTo(byte h1, byte h2, byte h3) {
  digitalWrite(h1Sim, h1);
  digitalWrite(h2Sim, h2);
  digitalWrite(h3Sim, h3);
}

void simMagnetSwitches() {
  static long prevPosition = -999999;
  long Position = stepper1.readSteps();
  if (prevPosition == Position) return;
  prevPosition = Position;
  simSetSwitchesTo(HIGH, HIGH, HIGH);
  if (Position == h1Pos) {
    simSetSwitchesTo(LOW, HIGH, HIGH);
  }
  if (Position == h2Pos) {
    simSetSwitchesTo(HIGH, LOW, HIGH);
  }
  if (Position == h3Pos) {
    simSetSwitchesTo(HIGH, HIGH, LOW);
  }
}

/*********************************************************************/