// https://wokwi.com/projects/405237709378934785
// https://forum.arduino.cc/t/arduino-uno-configured-to-syren-50/1287803

# include <ezButton.h>

const byte pins[5] = {2, 3, 4, 5, 6};
enum {bFWD = 0, bSTOP, bREV, bFWDLIMIT, bREVLIMIT};

const byte speedPin = A0;

bool forked;
const byte forkedLED = 9;

// when we add toggle/deadman
const byte modePin = A1;

// reset after limit switch
const byte resetPin = 8;

ezButton button[5] = {2, 3, 4, 5, 6};

# define PRESST 0

void setup() {
  Serial.begin(115200);
  Serial.println("\nsimple motor controller\n");

  for (byte ii = 0; ii < 5; ii++)
    button[ii].setDebounceTime(25);
  
  pinMode(resetPin, INPUT_PULLUP);
  pinMode(modePin, INPUT_PULLUP);

  pinMode(forkedLED, OUTPUT);
}

enum {STOPPED, FORWARD, REVERSE};
char *tags[] = {"Stopped", "Forward", "Reverse"};
byte motorState = STOPPED;

void loop() {
  for (byte ii = 0; ii < 5; ii++)
    button[ii].loop();

  bool toggleMode = digitalRead(modePin) == LOW;

  {
    static bool lastPrintedMode;
    if (lastPrintedMode != toggleMode) {
      Serial.println(toggleMode ? "toggle mode" : "press and hold!");
      delay(20);
    }
    lastPrintedMode = toggleMode;
  }

  printMotorState(10);

  if (!toggleMode) {  // state orientated
    motorState = STOPPED;
    if (button[bREV].getState() == PRESST) motorState = REVERSE;
    if (button[bFWD].getState() == PRESST) motorState = FORWARD;
    if (button[bSTOP].getState() == PRESST) motorState = STOPPED;
  }
  else {  //toggle orientated
    if (button[bREV].isPressed()) motorState = REVERSE;
    if (button[bFWD].isPressed()) motorState = FORWARD;
    if (button[bSTOP].isPressed()) motorState = STOPPED;
  }

  if ((button[bFWDLIMIT].getState() == PRESST || button[bREVLIMIT].getState() == PRESST) && !forked)
    forked = true;

  if (forked)
    motorState = STOPPED;

  int potSpeed = getSpeed();

  switch (motorState) {
  case FORWARD :
  case REVERSE :
    
    break;

  case STOPPED :
    potSpeed = 127;

    break;
  }

  if (!digitalRead(resetPin)) forked = false;
  if (forked) potSpeed = 127;

  digitalWrite(forkedLED, forked ? HIGH : LOW);

  static int lastPrintedSent = -1;
  if (lastPrintedSent != potSpeed) {
    Serial.print(" sending ");
    Serial.println(potSpeed);
    lastPrintedSent = potSpeed;
  }
}

void printMotorState()
{
  printMotorState(0);
} 

void printMotorState(int nTag)
{
//  return; //or don't. OMG what an error?

  static int lastPrintedState = -1;
  if (motorState != lastPrintedState) {
    Serial.print("  : ");
    Serial.print(tags[motorState]);
    Serial.print("  ");
    if (nTag) Serial.print(nTag);
    Serial.println(forked ? " forked!" : "");
    lastPrintedState = motorState;
  }
}
// convert pot to 0 ... 255 speed control
// hard coded deadband

int getSpeed()
{
  int speed = analogRead(speedPin);

//  Serial.print(" reading ");
//  Serial.println(speed);

  switch (speed) {
  case 0 ... 127 :
    speed = 127;

    break;

  case 128 ... 1023 :
    if (motorState == FORWARD) {
      speed = map(speed, 128, 1023, 128, 256);
      if (speed >= 256) speed = 255;  // map grrr!
    }
    else if (motorState == REVERSE) {
      speed = map(speed, 128, 1023, 126, -1);
      if (speed <= -1) speed = 0;
    }
    else speed = 127;

    break;
  }

  return speed;
}

int getSpeed0()
{
  int speed = analogRead(speedPin);

  
  switch (speed) {
  case 0 ... 480 :
    speed = map(speed, 0, 480, 0, 127);
    if (speed >= 127) speed = 126;
    break;

  case 544 ... 1023 :
    speed = map(speed, 544, 1023, 128, 256);
    if (speed >= 256) speed = 255;
    break;

  default :
    speed = 127;    // moto stopped
  }

  return speed;
}

/*
//  if (button[STOP].isPressed()) Serial.println("I see that STOP!");
//  if (button[bFWD].getState() == PRESST) Serial.println("pressing forward!");

  if (1) {
    Serial.println(getSpeed());
    delay(250);
  }



    forked = true;
    Serial.println("   WTF?");
    printMotorState(12);
    motorState = STOPPED;
    printMotorState(13);


// if (1) { delay(100);
  if (lastPrintedSent != potSpeed) {
//    Serial.print(" sending ");
//    Serial.println(potSpeed);


*/