// Switches necesian estar seguidos
#define inSwitchStart 2
#define inSwitchEnd 7

#define outGreenLight 9
#define outRedLight 13
#define STARTER 8
#define DIRPIN 10
#define STEPPIN 11
#define ENEPIN 12

// Tiempo en el que se espera que todos los switches se hayan prendido por lo menos una vez.
#define timeActionWaitInMilliseconds 20000

const int inSwitchLength = inSwitchEnd - inSwitchStart + 1;

bool done = true;

int steps;
boolean Inicio = 0;
unsigned long startMillis;
int pressedSwitchCount = 0;
bool started = false;
bool inSwitchStates[inSwitchEnd - inSwitchStart + 1];

unsigned long GetPressedSwitchCount()
{
  unsigned long count = 0;
  for (int inSwitch = inSwitchStart; inSwitch <= inSwitchEnd; inSwitch++)
  {
    if (GetSwitchState(inSwitch))
    {
      count++;
    }
  }

  return count;
}

void ResetInSwitchStates()
{
  for (int inSwitch = inSwitchStart; inSwitch <= inSwitchEnd; inSwitch++)
  {
    SetSwitchState(inSwitch, false);
  }
}

void SetSwitchState(int inSwitch, bool state)
{
  // No cambiar numeros furera del rango
  if (inSwitch >=  inSwitchStart && inSwitch <= inSwitchEnd)
  {
    int inSwitchStateIndex = inSwitch - inSwitchStart;
    //Serial.print(inSwitchStateIndex);
    inSwitchStates[inSwitchStateIndex] = state;
  }
}

bool GetSwitchState(int inSwitch)
{
  // No accesar numeros furera del rango
  if (inSwitch >=  inSwitchStart && inSwitch <= inSwitchEnd)
  {
    int inSwitchStateIndex = inSwitch - inSwitchStart;
    return inSwitchStates[inSwitchStateIndex];
  }

  return false;
}

bool UpdateInSwitchState(int inSwitch)
{
  int inSwitchState = digitalRead(inSwitch);
  if (inSwitchState == LOW)
  {
    SetSwitchState(inSwitch, true);
    return true;
  }

  return false;
}

void setup() {
  Serial.begin(9600);
  for (int inSwitch = inSwitchStart; inSwitch <= inSwitchEnd; inSwitch++)
  {
    pinMode(inSwitch, INPUT); 
    SetSwitchState(inSwitch, false);
  }

  pinMode(outGreenLight, OUTPUT);
  pinMode(outRedLight,OUTPUT);
  pinMode(STARTER, INPUT);
  pinMode(DIRPIN, OUTPUT);
  pinMode(STEPPIN, OUTPUT);
  pinMode(ENEPIN, OUTPUT);
  //delayMicroseconds(200);
  digitalWrite(outGreenLight, HIGH);
}


void loop() {

  delay(100);
  //digitalWrite(outGreenLight, HIGH);
  //digitalWrite(outRedLight,HIGH);
  int buttonState = digitalRead(STARTER); 
  //digitalWrite(STARTER, HIGH);

  if (buttonState == LOW && !done)
  {
    forward(1600);
  }
  else if (buttonState == LOW && done)
  {
    done = false;
  }
 
  unsigned long elapsed = 0;
  // Ya paso mas del tiempo de tolerancia
  if (started)
  {
    unsigned long currentMillis = millis();
    elapsed = currentMillis - startMillis;
    if (elapsed > timeActionWaitInMilliseconds)
    {
      ResetInSwitchStates();
      started = false;
    }
  }

  bool anyActive = false;
  for (int inSwitch = inSwitchStart; inSwitch <= inSwitchEnd; inSwitch++)
  {
    anyActive = UpdateInSwitchState(inSwitch) || anyActive;
  }

  if (anyActive && !started)
  {
    startMillis = millis();
    digitalWrite(outGreenLight, HIGH);
  }

  int newPressedSwitchCount = GetPressedSwitchCount();

  if (pressedSwitchCount != newPressedSwitchCount)
  {
    //Serial.print("Millis: ");
    Serial.print(newPressedSwitchCount);
    //Serial.print(" pressed count: ");
    //Serial.print(newPressedSwitchCount);
  }

  pressedSwitchCount = newPressedSwitchCount;
  
  // All s
  if (pressedSwitchCount == inSwitchLength && !done)
  {
    Serial.print("!!");
    forward(1600);
    delayMicroseconds(200);
    done = true;
    started = false;
    ResetInSwitchStates();
  }



/*
digitalWrite(ENEPIN,HIGH);

  int inSwitch1State = digitalRead(inSwitch1); 
  digitalWrite(inSwitch1, LOW);//digitalWrite(inSwitch1, HIGH);

  int inSwitch2State = digitalRead(inSwitch2);
  digitalWrite(inSwitch2, LOW);//digitalWrite(inSwitch2, HIGH);

  int inSwitch3State = digitalRead(inSwitch3);// renglon nuevo HIGH
  digitalWrite(inSwitch3,LOW);// renglon nuevo
  
    int inSwitch4State = digitalRead(inSwitch4);// renglon nuevo HIGH
  digitalWrite(inSwitch4,LOW);// renglon nuevo

    int inSwitch5State = digitalRead(inSwitch5);// renglon nuevo
  digitalWrite(inSwitch5,LOW);// renglon nuevo HIGH

    int inSwitch6State = digitalRead(inSwitch6);// renglon nuevo
  digitalWrite(inSwitch7,LOW);// renglon nuevo HIGH

    int inSwitch7State = digitalRead(inSwitch7);// renglon nuevo
  digitalWrite(inSwitch6,LOW);// renglon nuevo HIGH


  int inSwitch1State+inSwitch2State+inSwitch3State+inSwitch4State+inSwitch5State+inSwitch6State+inSwitch7State = digitalRead(inSwitch1,inSwitch2,inSwitch3,inSwitch4,inSwitch5,inSwitch6,inSwitch7);
  digitalWrite(inSwitch1, LOW);
  digitalWrite(inSwitch2,LOW);
  digitalWrite(inSwitch3,LOW);
  digitalWrite(inSwitch4,LOW);
  digitalWrite(inSwitch5,LOW);
  digitalWrite(inSwitch6,LOW);
  digitalWrite(inSwitch7,LOW);

  if (inSwitch1State,inSwitch2State,inSwitch3State,inSwitch4State,inSwitch5State,inSwitch6State,inSwitch7State== LOW && !done)//if (inSwitch1State,inSwitch2State== HIGH && !done)
  
  {
    
      forward(1600); 
      delayMicroseconds(200);
      

  } else if (inSwitch1State,inSwitch2State,inSwitch3State,inSwitch4State,inSwitch5State,inSwitch6State,inSwitch7State == HIGH && done)//else if (inSwitch1State+inSwitch2State == HIGH && done)
  {
    done = false;
    
  }
  */


}
  void forward(int steps){
    int i;
    digitalWrite(ENEPIN,LOW);
    digitalWrite(DIRPIN,HIGH);
    for(i=0;i<steps;i++){
      digitalWrite(STEPPIN, !digitalRead(STEPPIN));
      delayMicroseconds(800);
    }
    digitalWrite(ENEPIN,HIGH);
    
    delay(10);
    
    digitalWrite(outGreenLight, LOW);
    digitalWrite(outRedLight,LOW);
    delay(100);
    digitalWrite(outGreenLight, HIGH);
    digitalWrite(outRedLight,HIGH);
    
   Inicio = 0;
    
}