#include <Servo.h>
#include <TM1637.h>
#include <Stepper.h>

const int servo = 5;
const int passosVolta = 100;
const int incrementMoviment = 10;

const int pinTecles[]={9, 10, 11, 12, 14, 15, 16, 17};
const int pinemer = 2;

const int NoPlanta = 99;

boolean mostraEstat = false;

TM1637 disp(13, 3);

Servo portes;
Stepper motor(passosVolta, 4, 6, 7, 8);

int plantaDest=NoPlanta;
int estatAsc = 0; // 0->repos 1-> pujant 2->baixant
boolean portaOberta; //false-> tancades true->obertes
int plantaAsc;

int tempsEspera = 0;
int tempsMoviment = 0;
int volta = 0;

void setup() {
  // put your setup code here, to run once:
  for (int i = 0; i<8;i++){
    pinMode(pinTecles[i],INPUT_PULLUP);
  }
  pinMode(pinemer,INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pinemer), emergencia, FALLING);

  disp.set(BRIGHT_TYPICAL);
  disp.init();

  portes.attach(servo);
  mouPortes(1);

  motor.setSpeed(100);

  Serial.begin(9600);
  escriuSerial("Inici");

  escriuSerial("Portes obertes i planta 0");
  plantaAsc = 0;
  portaOberta = true;
  escriuLed(NoPlanta,NoPlanta,NoPlanta,plantaAsc);
  mouPortes(1);
}

void emergencia(){
  escriuSerial("Emergencia: Para i obre portes");
  mouPortes(1);
  volta= 2*passosVolta;

}

int llegeixTecles(){
  int lectura=NoPlanta;
  for (int i=0;i<8;i++){
    if(digitalRead(pinTecles[i])==LOW){
      lectura = i;
    }
  }
  if (lectura != NoPlanta){
    if(lectura>3){
      lectura=lectura-4;
    }
    escriuSerial("Premuda planta "+ String(lectura));
  }
  return lectura;
}

void mouPortes(int val){
  //0 tanca portes; 1 obre portes
  if (val==0){
    portes.write(0);
    escriuSerial("Portes tancades");
  }
  else if (val==1){
    portes.write(180);
    escriuSerial("Portes obertes");
  }
  delay(10);
}

void escriuSerial(String missatge){
  Serial.print(millis()/1000);
  Serial.print(" : ");
  Serial.println(missatge);
}

void escriuLed(int A, int B, int C, int D){
  if (A!=NoPlanta)
    disp.display(0,A);
  if (B!=NoPlanta)
    disp.display(1,B);
  if (C!=NoPlanta)
    disp.display(2,C);
  if (D!=NoPlanta)
    disp.display(3,D);
}


void loop() {
 if(estatAsc == 0){
   // Està en repos; Tanca les portes si es necessari; busca lectura polsadors.
   if(mostraEstat == false){
     escriuSerial("Estat 0");
     mostraEstat = true;
     tempsEspera=0;
     plantaDest=NoPlanta;
   }
   plantaDest = llegeixTecles();
   if(plantaDest != NoPlanta){
     escriuSerial("Planta actual "+String(plantaAsc));
     estatAsc=1;
     mostraEstat= false;
   }
   else if(portaOberta){
     if (tempsEspera==0){
       tempsEspera = millis();
     }
     if (millis()-tempsEspera>=5000){
       escriuSerial("Espera superada");
       mouPortes(0);
       portaOberta = false;
     }
   }
 }
 else if(estatAsc == 1){
   // Tanca les portes i determina si puja o baixa.
   if(mostraEstat == false){
     escriuSerial("Estat 1");
     mostraEstat = true;
   }
   if(plantaAsc == plantaDest){
     escriuSerial("Valors iguals");
     if(portaOberta == false){
       escriuSerial("Les portes estaven tancades");
       mouPortes(1);
       portaOberta = true;
     }
     else{
       escriuSerial("Les portes ja estaven obertes");
     }
     estatAsc = 0;
     mostraEstat = false;
   }
   else{
     if(portaOberta == true){
       escriuSerial("Les portes estaven obertes");
       mouPortes(0);
       portaOberta = false;
     }
     else{
       escriuSerial("Les portes ja estaven tancades");
     }     
     if(plantaAsc < plantaDest){
       escriuSerial("Puja");
       estatAsc = 2;
       mostraEstat = false;
     }
     else{
       escriuSerial("Baixa");
       estatAsc = 3;
       mostraEstat = false;
     }
   }
 }
 else if(estatAsc == 2){
   //Es mou de pujada
    if(mostraEstat == false){
     escriuSerial("Estat 2");
     mostraEstat = true;
   }
   volta = 0;
   while (plantaAsc!=plantaDest){
     if(tempsMoviment==0){
       tempsMoviment=millis();
     }
     if(millis()-tempsMoviment>=incrementMoviment){
       volta++;
       motor.step(+1);
       tempsMoviment=0;
     }
     if(volta==2*passosVolta){
       volta = 0;
       plantaAsc++;
       escriuSerial("Planta "+String(plantaAsc));
       escriuLed(NoPlanta,NoPlanta,NoPlanta,plantaAsc);
     }
   }
   estatAsc = 4;
   mostraEstat = false;
 }
 else if(estatAsc == 3){
   // Es mou de baixada
    if(mostraEstat == false){
     escriuSerial("Estat 3");
     mostraEstat = true;
   }
   volta = 0;
   while (plantaAsc!=plantaDest){
     if(tempsMoviment==0){
       tempsMoviment=millis();
     }
     if(millis()-tempsMoviment>=incrementMoviment){
       volta++;
       motor.step(-1);
       tempsMoviment=0;
     }
     if(volta==2*passosVolta){
       volta = 0;
       plantaAsc--;
       escriuSerial("Planta "+String(plantaAsc));
       escriuLed(NoPlanta,NoPlanta,NoPlanta,plantaAsc);
     }
   }

   estatAsc = 4;
   mostraEstat = false;   
 }
 else if(estatAsc == 4){
   // Obre les portes i passa a repòs.
   if(mostraEstat == false){
     escriuSerial("Estat 4");
     mostraEstat = true;
   }
   mouPortes(1);
   portaOberta = true;
   mostraEstat=false;
   estatAsc=0;
 }
}