#define MOTOR_A     7
#define MOTOR_B     8
#define MOTOR_C     9
#define PIN_SMAX    2
#define PIN_SMIN    3
//const byte pinMotor[] = { 5, 6, 7 };

struct ComboAct {
    bool cmd;
    uint8_t idx;
    uint32_t timer0;
    const byte pinMotor[3] = { MOTOR_A, MOTOR_B, MOTOR_C };
    
};

ComboAct ca;
void nComboAct(ComboAct &ca, bool tf) {
    if (!ca.timer0) {
        ca.cmd = tf;
        Serial.println(ca.idx);
        digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
        ca.timer0 = millis();
        ca.idx++;
    } else if (millis() - ca.timer0 > 5000) {
        Serial.println(ca.idx);
        digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
        ca.timer0 = millis();
        ca.idx++;
    }
}

void comboAct(ComboAct &ca, bool tf) {
    if (ca.cmd != tf) {
        ca.cmd = tf;
    }
    if (ca.cmd) {
        if (!ca.idx) {
            Serial.println(ca.idx);
            digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
            ca.timer0 = millis();
            ca.idx++;
        }
        if (ca.idx < 3) {
            if (millis() - ca.timer0 > 1000) {
                digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
                Serial.println(ca.idx);
                ca.timer0 = millis();
                ca.idx++;
                
            }
        }
    } else {
        if (ca.idx == 3) {
            ca.idx--;
            Serial.println(ca.idx);
            digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
            ca.timer0 = millis();
        }
        if (ca.idx > 0) {
            if (millis() - ca.timer0 > 1000) {
                
                ca.timer0 = millis();
                ca.idx--;
                Serial.println(ca.idx);
                digitalWrite(ca.pinMotor[ca.idx], ca.cmd);
                
            }

        }
    }
}

#if(0)
struct Action {
    bool actState; 
    
    uint8_t stateMachine;
    uint32_t timer;
    byte pinIndex;
    const byte pinMotor[3] = { MOTOR_A, MOTOR_B, MOTOR_C };
};

Action azione;

void azionaMotori(struct Action &act, bool tf, bool abort) {
  // tf sta per true o false

  if (abort) {
      for (byte i=0; i<3; i++) {
          digitalWrite(act.pinMotor[i], LOW);
      }
      act.actState = false;
      act.timer = 0;
      act.pinIndex = 0;

      act.stateMachine = 3;
  }
  switch (act.stateMachine) {
    case 0:
      
      if (act.actState != tf) {
          act.actState = tf;
          digitalWrite(act.pinMotor[act.pinIndex], act.actState);
          act.timer = millis();
          
          act.stateMachine = 2;   // CHANGE STATE
          
      }
      break;
    
    case 1:
      
      if (millis() - act.timer >= 1000) {
          digitalWrite(act.pinMotor[act.pinIndex], act.actState);
          act.timer = millis();
          if (act.pinIndex == 0 || act.pinIndex == 2) {
              act.stateMachine = 0;  // CHANGE STATE
          } else {
              act.stateMachine = 2;  // CHANGE STATE
          }
      }
      break;

    case 2:
      //Serial.println("case 2");
      if (act.actState == true)
          act.pinIndex++;
      else
          act.pinIndex--;

      act.stateMachine = 1; // CHANGE STATE
      break;
    case 3: //abort   
      break;
  }
}
#endif

uint32_t g_timer;
byte g_counter;
bool g_command;

bool silosState;
bool oldSilosState = true;

void setup() {
    Serial.begin(115200);  // Start the Serial monitor with speed of 9600 Bauds
    pinMode(LED_BUILTIN, OUTPUT);
    pinMode(MOTOR_A, OUTPUT);
    pinMode(MOTOR_B, OUTPUT);
    pinMode(MOTOR_C, OUTPUT);

    pinMode(PIN_SMIN, INPUT_PULLUP);
    pinMode(PIN_SMAX, INPUT_PULLUP);
    delay(100);

    //g_command = true;   // inizia con l'azione composta di accensione
    
}



void loop() {
    bool abort = false;
    if (millis() - g_timer >= 500) {
        digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
        g_timer = millis();
        g_counter++;
        if (g_counter == 10) {
            g_counter = 0;
            //abort = true;
        }   
        /*if (g_counter == 10) { // sono trascorsi 5 secondi
            g_command = !g_command; // inverte il comando
            g_counter = 0; 
        }*/
    }
    if (digitalRead(PIN_SMIN) && digitalRead(PIN_SMAX)) {
        g_command = true;
    } else if (!digitalRead(PIN_SMIN) && !digitalRead(PIN_SMAX)) {
        g_command = false;
    }
        
    
    // SMAX indica full mentre SMIN indica empty
    // forse uno dei due sensori e guasto.
    if (!digitalRead(PIN_SMIN) && digitalRead(PIN_SMAX)) {
        Serial.println("anomalia sensori");
        Serial.print("SMIN is LOW and SMAX is HIGH.");
        while(1);
    }
    //Serial.println(g_command);
        
    //azionaMotori(azione, g_command, abort);
    comboAct(ca, g_command);
  
}