#define WAIT  100

int sensor1 = true;
int sensor1_old = true;
int sensor2 = true;
int sensor2_old = true;
int sensor3 = true;
int sensor3_old = true;   
int sensor4 = true;
int sensor4_old = true;   
int sensor5 = true;
int sensor5_old = true;   
int adelante,atras,sensor_abajo,sensor_centro,sensor_arriba,sensor_alto,sensor_bajo;
int start = true;
int arriba = false;
int abajo = false;
int franco_arriba;
int franco_abajo;
int recibido[20] = {
  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

char serInString[128];

int sms_position = 1;

long time;

int s5 = 8;   // sensor elevador arriba 
int s4 = 9;   // sensor elevador abajo
int s3 = 10;  // sensor palanca centro
int s2 = 11;  // sensor palanca abajo
int s1 = 12;  // sensor palanca arriba

int motor_plus = 4;
int motor_minus = 3;
int beepPin = 5;

int led1 = 2;
int led2 = 13;


void beep(void) {

  digitalWrite(beepPin, HIGH);
  delay(50);
  digitalWrite(beepPin, LOW);

}

void palanca_adelante(void) {

  digitalWrite(motor_plus, HIGH);
  delay(50);
  digitalWrite(motor_minus, LOW);

}

void palanca_atras(void) {

  digitalWrite(motor_plus, LOW);
  delay(50);
  digitalWrite(motor_minus, HIGH);

}

void palanca_paro(void) {

  digitalWrite(motor_plus, LOW);
  digitalWrite(motor_minus, LOW);

}

void leer_sensores(void) {

  sensor1_old = sensor1;
  sensor2_old = sensor2;
  sensor3_old = sensor3;
  sensor4_old = sensor4;
  sensor5_old = sensor5;

  sensor1 = digitalRead(s1);
  sensor2 = digitalRead(s2);
  sensor3 = digitalRead(s3);
  sensor4 = digitalRead(s4);
  sensor5 = digitalRead(s5);  
}

void subir_plataforma(void){

  palanca_atras();
  leer_sensores();

  while(sensor1 == sensor1_old){ // espera a llegar al tope del brazo para subir (palanca atras)
    leer_sensores();
  }

  //Serial.println("Palanca atras"); 
  palanca_paro();
  leer_sensores();
  time = 0;

  while ((sensor5 == sensor5_old) && (time<40000)){  // espera a llegar a la posicion superior de la plataforma
    leer_sensores();
    delay(1);
    time++;
  }

  //Serial.println("Posicion alta de plataforma"); 
  palanca_adelante();
  leer_sensores();

  while (sensor3 == sensor3_old){  // espera a llegar al centro palanca
    leer_sensores();
  }

  //Serial.println("Palanca  centro");
  palanca_paro();

  //}
}

void bajar_plataforma(void){

  palanca_adelante();
  leer_sensores();
  time = 0;
  
  while ( (sensor2 == sensor2_old) && (time < 30000) ) { // espera a llegar al tope del brazo para bajar (palanca adelante)
    leer_sensores();
    delay(1);
    time++;    // seguridad
  }

  // Serial.println("Palanca adelante"); 
  palanca_paro();
  leer_sensores();


  while (sensor4 == sensor4_old){  // espera a llegar a la posicion inferior de la plataforma
    leer_sensores();
  }

  // Serial.println("Posicion baja de plataforma"); 
  palanca_atras();
  leer_sensores();

  while (sensor3 == sensor3_old){  // espera a llegar al centro palanca
    leer_sensores();
  }

  //Serial.println("Palanca  centro");
  palanca_paro();

}

void inicio(void) {

  digitalWrite(led2, HIGH);
  digitalWrite(led1, HIGH);

  //printString("AT+CPIN=5053\r\n");  // PIN SELECT
  //delay(3000);
  printString("AT+CNMI=2,1,0,0,1\r\n"); // CONFIG SMS MODE
  delay(3000);
  printString("AT+CMGF=1\r\n"); // TEXT MODE
  delay(3000);

  digitalWrite(led2, LOW);
  digitalWrite(led1, LOW);

  //Serial.print("AT\r\n");   // test
  //AT+CMGL="ALL" // read all sms's
  //AT+CMGD=?     // delete sms
  //AT+CSQ        // signal quality

  // RUTINA DE INICIALIZACION
  // 1. Buscar sensor alto bajando la plataforma un poco
  // 2. Si detecta sensor5 entonces bajar_plataforma
  // 3. Si no detecta sensor5 entonces aplicar pequeña subida y bajar_plataforma

  
  palanca_atras();
  //printString("palanca atras");
  delay(3500);
  palanca_paro();
  //printString("palanca paro");
  delay(2500);    // AJUSTAR TIEMPOS PARA CALIBRACION DE INICIALIZACION
  //printString("bajar");
  bajar_plataforma();
  
  franco_arriba = 0;
  franco_abajo = 1;

}

void test_sms(char *strArray, int i) {

  if(((strArray[i] == 'p') || (strArray[i] == 'P')) && ((strArray[i-1] == 'u') || (strArray[i-1] == 'U'))) {
    arriba = true;
    abajo = false;
  }
  else if(((strArray[i] == 'n') || (strArray[i] == 'N')) && ((strArray[i-1] == 'w') || (strArray[i-1] == 'W')) && ((strArray[i-2] == 'o') || (strArray[i-2] == 'O')) && ((strArray[i-3] == 'd') || (strArray[i-3] == 'D'))) {
    arriba = false;
    abajo = true;         
  }
  else if(((strArray[i] == 'a') || (strArray[i] == 'A')) && ((strArray[i-1] == 'b') || (strArray[i-1] == 'B')) && ((strArray[i-2] == 'i') || (strArray[i-2] == 'I')) && ((strArray[i-3] == 'r') || (strArray[i-3] == 'R')) && ((strArray[i-4] == 'r') || (strArray[i-4] == 'R')) && ((strArray[i-5] == 'a') || (strArray[i-5] == 'A'))  ) {
    arriba = true;
    abajo = false;
  }
  else if(((strArray[i] == 'o') || (strArray[i] == 'O')) && ((strArray[i-1] == 'j') || (strArray[i-1] == 'J')) && ((strArray[i-2] == 'a') || (strArray[i-2] == 'A')) && ((strArray[i-3] == 'b') || (strArray[i-3] == 'B')) && ((strArray[i-4] == 'a') || (strArray[i-4] == 'A'))  ) {
    arriba = false;
    abajo = true;
  }
  else if(((strArray[i] == 't') || (strArray[i] == 'T')) && ((strArray[i-1] == 'n') || (strArray[i-1] == 'N')) && ((strArray[i-2] == 'u') || (strArray[i-2] == 'U')) && ((strArray[i-3] == 'm') || (strArray[i-3] == 'M')) && ((strArray[i-4] == 'a') || (strArray[i-4] == 'A'))  ) {
    arriba = true;
    abajo = false;
  }
  else if(((strArray[i] == 'l') || (strArray[i] == 'L')) && ((strArray[i-1] == 'l') || (strArray[i-1] == 'L')) && ((strArray[i-2] == 'a') || (strArray[i-2] == 'A')) && ((strArray[i-3] == 'v') || (strArray[i-3] == 'V')) && ((strArray[i-4] == 'a') || (strArray[i-4] == 'A'))  ) {
    arriba = false;
    abajo = true;
  }

}


void readSerialString (char *strArray) {

  int i = 0;
  while (serialAvailable()){         
    digitalWrite(led1, HIGH);   
    strArray[i] = serialRead();     
    //printByte(strArray[i]);
    //printByte(10);
    test_sms(strArray, i);     
    i++;
  }  
  digitalWrite(led1, LOW);
}


void setup() {

  beginSerial(9600);

  pinMode(motor_plus, OUTPUT);  // declare LED as output
  pinMode(motor_minus, OUTPUT);  // declare LED as output
  pinMode(beepPin, OUTPUT);  // declare LED as output
  pinMode(led1, OUTPUT);  // declare LED as output
  pinMode(led2, OUTPUT);  // declare LED as output

  pinMode(s1, INPUT);    // declare pushbutton as input
  pinMode(s2, INPUT);    // declare pushbutton as input
  pinMode(s3, INPUT);    // declare pushbutton as input
  pinMode(s4, INPUT);    // declare pushbutton as input
  pinMode(s5, INPUT);    // declare pushbutton as input

}

void loop(){

  if(start) {    // Inicializacion de la maquina, SOLO EJECUTA 1 VEZ
    inicio();
    start = 0 ;
  }

  digitalWrite(led2, HIGH);
  digitalWrite(led1, LOW);

  if(sms_position < 25){  // search in the first 20 sms postitons
    sms_position++; 
  }
  else{
    sms_position = 1;  
  }

  printString("AT+CMGR=");  // read sms
  printInteger(sms_position);
  printString("\r\n");

  digitalWrite(led2, LOW);
  
  delay(WAIT);    // time betwen every sms read, waiting...
  readSerialString(serInString);
  delay(WAIT);
  
  if(arriba) { // mensaje detectado de subida
    digitalWrite(led1, HIGH);
    arriba = 0;
    abajo = 0;
    beep();  // 1 beep() -> up
    //delay(500);
    recibido[sms_position] = 1;
    //digitalWrite(led1, LOW);
    //delay(10);
    //printString("up");
    //Serial.println();
    //printInteger(franco_abajo);
    if( franco_abajo == 1){
      subir_plataforma(); 
      franco_arriba = 1;
      franco_abajo = 0;
    }
    //else
    //  plataforma_arriba = 1;

    digitalWrite(led1, LOW);
  }
  else if(abajo) { // mensaje detectado de bajada
    digitalWrite(led1, HIGH);
    abajo = 0;
    arriba = 0;
    beep();
    delay(50);
    beep();

    recibido[sms_position]=1;
    //beep();      // 2 beep -> down
    //digitalWrite(led2, LOW);
    //delay(10);
    //printString("down");
    //Serial.println();
    //printInteger(plataforma_abajo);
    if(franco_arriba == 1){
      bajar_plataforma();
      franco_abajo = 1;
      franco_arriba = 0;
    }
    //else
    //  plataforma_abajo = 1;

    digitalWrite(led1, LOW);

  }  
  else
    recibido[sms_position] = 0;

  //if(recibido[sms_position])
  //{
    //digitalWrite(led1, HIGH);
    //delay(50);
    //digitalWrite(led1, LOW);
    //delay(50);
    //digitalWrite(led1, HIGH);
    //delay(50);
    //digitalWrite(led1, LOW);
    //printString("borrado:");
    //printInteger(sms_position);
    //printByte(10);
    printString("AT+CMGD=");     // delete sms
    printInteger(sms_position);
    printString("\r\n"); 
    
    delay(WAIT);    // time betwen every sms read, waiting...
    readSerialString(serInString);
    delay(WAIT);
  //}

}  // loop