//Parametro Piso: 0 = Distancia a posicion de reposo de bandeja
//Parametro Piso: 1 = Distancia a posicion de bandeja en piso 1.
//Parametro Piso: 2 = Distancia a posicion de bandeja en piso 2.
//Parametro Piso: 3 = Distancia a posicion de bandeja en piso 3.
//Parametro Piso: 4 = Distancia a posicion de bandeja en piso 4.
//Parametro Piso: 5 = Distancia a posicion de bandeja en piso 5.
//Parametro Piso: 6 = Distancia a posicion de bandeja en piso 6.
//Parametro Piso: 7 = Distancia a posicion de bandeja en piso 7.
//Parametro Piso: 8 = Distancia a posicion de bandeja en piso 8.
//Parametro Piso: 9 = Distancia a posicion de bandeja en piso 9.
//Parametro Piso: 10 = Distancia a posicion de bandeja en piso 10.
//Parametro Piso: 11 = Distancia a posicion de bandeja en piso 11.
//Parametro Piso: 12 = Distancia a posicion de bandeja en piso 12.
//Parametro Piso: 13 = Distancia a posicion de bandeja en piso 13.
//Parametro Piso: 14 = Distancia a posicion de bandeja en piso 14.
//Parametro Piso: 15 = Distancia a posicion de bandeja en piso 15.
//Parametro Piso: 16 = Distancia a posicion de bandeja en piso 16.
//Parametro Piso: 17 = Distancia a posicion de bandeja en piso 17.
//Parametro Piso: 18 = Distancia a posicion de bandeja en piso 18.
//Parametro Piso: 19 = Distancia a posicion de bandeja en piso 19.
//Parametro Piso: 20 = Distancia a posicion de bandeja en piso 20.

//Parametro Piso: 21 = Constante de movimiento. 748. Ver relaciones de transmision.
//Parametro Piso: 22 = Constante de velocidad alta en movimientos.
//Parametro Piso: 23 = Constante de velocidad baja en movimientos.


#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Fonts/FreeSerif9pt7b.h>  
#include <Fonts/FreeSans9pt7b.h>
#include <EEPROM.h>


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

#define PUL 4   //define Pulse pin      
#define DIR 16  //define Direccion Pin     
#define ENA 17  //define Enable pin

#define BEEP 15 //define Enable pin  
#define LED 02  //define Enable pin  

#define PULS_SUBE_A_PISO  05 //define 
#define PULS_BAJA_A_PISO 18 //define 
#define PULS_MENU 19 //define
#define FIN_CARRERA 26 //define

#define ENC_SW 32 // PIN FOR SW
#define ENCODER_A 34 // PIN FOR ENCODER A / CLK
#define ENCODER_B 35 // PIN FOR ENCODER B / DT
volatile int encoder_value = 0;

bool direccion=1;
int ciclo=0;
int menu=0;
int posicion;
unsigned int valor_eeprom1;
unsigned int valor_eeprom2;
unsigned int valor_eeprom;

unsigned int distancia_nuevo_piso;
unsigned int distancia_actual_piso;

unsigned int cte_velocidad=500;

unsigned int cte_velocidad_alta=200;
unsigned int cte_velocidad_baja=300;


unsigned long target;
unsigned long contador=0;

const int timeThreshold = 40;
long startTime = 0;

const int tiempo_actualiza_display = 200;
long tiempo_ini_display = 0;

unsigned int  constante_pulsos = 748;

int piso=0;
int posicion_actual_piso=0;
int posicion_nuevo_piso=0;


bool FLAG_PARADA=0;

int acel;

unsigned long previousMillis = 0;  // will store last time LED was updated
const long interval = 300;  // interval at which to blink (milliseconds)
int ledState = LOW;

void encoder_isr() {  // Declaracion de la funcion de interrupcion. Atiende encoder

  if (millis() - startTime > 0) //timeThreshold)
  {
    int A = digitalRead(ENCODER_A);
    int B = digitalRead(ENCODER_B);
    if ((A == HIGH) != (B == LOW)) {
      encoder_value--;
      if(encoder_value<0)encoder_value=0;
    } 
    else {
      
      encoder_value++;
      if(encoder_value>200000)encoder_value=200000;
    }

  startTime = millis();  
  }
}



void aviso_sonoro() {     //Funcion Beep inicial
  for (int j=0;j<10;j++) {    
    digitalWrite(BEEP,HIGH);
   digitalWrite(LED,HIGH);
   delay(100);
   digitalWrite(BEEP,LOW);
   digitalWrite(LED,LOW);
   delay(100);
   }
  Serial.println("aviso_sonoro...");
  delay(500);
}

void aviso_movimiento() {     //Funcion Beep inicial
  for (int j=0;j<5;j++) {    
    digitalWrite(BEEP,HIGH);
   digitalWrite(LED,HIGH);
   delay(80);
   digitalWrite(BEEP,LOW);
   digitalWrite(LED,LOW);
   delay(80);
   }
  Serial.println("aviso_movimiento...");
  delay(500);
}

void poneaceroeeprom(){                 //Funcion Borra Memoria eeprom
  Serial.print("Borrando memoria...");
  

  for (int j=0;j<=80;j=j+4) {    //
    EEPROM.writeUInt(j , 10+j); //
  }
  EEPROM.writeUInt(84 , 164);     // Constante de movimiento W21
  EEPROM.writeUInt(88 , 80);       // Constante de ajuste de velocidad W22
  EEPROM.writeUInt(92 , 150);       // Constante de ajuste de velocidad W22

  boolean ok = EEPROM.commit();
  delay(100); 
  Serial.println("Memoria borrada...");
  delay(500);
}

void referencia(){                   //Funcion baja a fin de carrera de referencia a baja velocidad
    Serial.print("Referenciando...");
   digitalWrite(DIR,!direccion);
   digitalWrite(LED,!direccion);

  cte_velocidad=500;
  while((digitalRead(FIN_CARRERA)) &  !FLAG_PARADA ){
    cte_velocidad=cte_velocidad-1;
    if(cte_velocidad<=cte_velocidad_baja){cte_velocidad=cte_velocidad_baja;}

    digitalWrite(PUL,HIGH);
    delayMicroseconds(cte_velocidad);
    digitalWrite(PUL,LOW);
    delayMicroseconds(cte_velocidad);
  }

  FLAG_PARADA=0;
  posicion_actual_piso=-1;
  Serial.println("Referenciado...P-1");
    Serial.println(cte_velocidad);
  delay(500);
}

void actualiza_display() {                  //Actualiza display
  display.setFont(&FreeSans9pt7b);
  display.setTextSize(1);             
  display.setTextColor(WHITE);   
  display.clearDisplay();
  display.setCursor(0,12);             
  display.println("ELEVADOR 1.0");
  display.setCursor(0,29);
  display.print("Piso: ");
  display.print(piso);
  display.setCursor(65,29);
  display.print(" Di:");
  display.println(distancia_nuevo_piso);
  display.setCursor(0,44);             
  display.print("Actl= ");
  display.print(posicion_actual_piso);
  display.setCursor(65,44);
  display.print(" Di:");
  display.println(distancia_actual_piso);
  display.setCursor(0,63);             
  display.print("NuevoVal=");
  display.setCursor(90,63);
  display.print(encoder_value);
  display.display();

}

void sube_a_piso(int nuevo_piso, int actual_piso){  //Funcion sube a piso
  aviso_movimiento();
  if(nuevo_piso > actual_piso ){

    contador=0;
    digitalWrite(DIR,direccion);
    digitalWrite(LED,direccion);

    valor_eeprom1 = EEPROM.readUInt(nuevo_piso*4);
    Serial.println(valor_eeprom1);
    valor_eeprom2 = EEPROM.readUInt(actual_piso*4);
    Serial.println(valor_eeprom2);
    valor_eeprom = valor_eeprom1 - valor_eeprom2;
    Serial.println(valor_eeprom);
    target=valor_eeprom*constante_pulsos;

   posicion_nuevo_piso=nuevo_piso;
   //distancia_nuevo_piso=valor_eeprom1;

    Serial.print("Subiendo a piso...");
    Serial.print(posicion_nuevo_piso);
    Serial.print("  /  Distancia=");
    Serial.println(valor_eeprom1);
  
    Serial.print("Pulsos= ");  
    Serial.println(target);
    cte_velocidad=500;
    while(contador<target & !FLAG_PARADA ){
      if(contador <= 500 ){cte_velocidad=cte_velocidad-1;}
      if(contador>=(target-500)){cte_velocidad=cte_velocidad+1;}
      if(cte_velocidad<=cte_velocidad_alta){cte_velocidad=cte_velocidad_alta;}
      digitalWrite(PUL,HIGH);
      delayMicroseconds(cte_velocidad);
      digitalWrite(PUL,LOW);
      delayMicroseconds(cte_velocidad);
      contador=contador+1;  
    }
    FLAG_PARADA=0;
    posicion_actual_piso=posicion_nuevo_piso;
    distancia_actual_piso=valor_eeprom1;

    Serial.print("Posicion actual...");
    Serial.println(posicion_actual_piso);



  }
}
 
void baja_a_piso(int nuevo_piso, int actual_piso){  //Funcion baja a piso 
  aviso_movimiento();
  if(nuevo_piso < actual_piso ){

    contador=0;
    digitalWrite(DIR,!direccion);
    digitalWrite(LED,!direccion);
   
    valor_eeprom1 = EEPROM.readUInt(nuevo_piso*4);
    Serial.println(valor_eeprom1);
    valor_eeprom2 = EEPROM.readUInt(actual_piso*4);
    Serial.println(valor_eeprom2);
    valor_eeprom = valor_eeprom2 - valor_eeprom1;
    Serial.println(valor_eeprom);
    target=valor_eeprom*constante_pulsos;
    Serial.print("Pulsos= ");  
    Serial.println(target);  

    posicion_nuevo_piso=nuevo_piso;
  //  distancia_nuevo_piso=valor_eeprom1;


    Serial.print("Bajando a piso...");
    Serial.print(posicion_nuevo_piso);
    Serial.print("  /  Distancia=");
    Serial.println(distancia_nuevo_piso);

    cte_velocidad=500;
    while(contador<target &  !FLAG_PARADA ){
      if(contador <= 500 ){cte_velocidad=cte_velocidad-1;}
      if(contador>=(target-500)){cte_velocidad=cte_velocidad+1;}
      if(cte_velocidad<=cte_velocidad_alta){cte_velocidad=cte_velocidad_alta;}

      digitalWrite(PUL,HIGH);
      delayMicroseconds(cte_velocidad);
      digitalWrite(PUL,LOW);
      delayMicroseconds(cte_velocidad);
      contador=contador+1; 
    }
    FLAG_PARADA=0;

    posicion_actual_piso=posicion_nuevo_piso;
    distancia_actual_piso=valor_eeprom1;

    Serial.print("Posicion actual...");
    Serial.println(posicion_actual_piso);



  }
}




void sube_a_piso_descanso(){                   //Funcion sube a piso de descanso
  
  if(posicion_actual_piso==-1){
    Serial.print("Subiendo a posicion de descanso...");
    contador=0;
    digitalWrite(DIR,direccion);
    digitalWrite(LED,direccion);


    valor_eeprom = EEPROM.readUInt(0);
    Serial.print("Distancia=");
    Serial.println(valor_eeprom);
    target=valor_eeprom*constante_pulsos;
    Serial.print("Pulsos= ");  
    Serial.println(target);  


cte_velocidad=500;
    while(contador<target & !FLAG_PARADA ){
      if(contador <= 500 ){cte_velocidad=cte_velocidad-1;}
      if(contador>=(target-500)){cte_velocidad=cte_velocidad+1;}
      if(cte_velocidad<=cte_velocidad_alta){cte_velocidad=cte_velocidad_alta;}

      digitalWrite(PUL,HIGH);
      delayMicroseconds(cte_velocidad);
      digitalWrite(PUL,LOW);
      delayMicroseconds(cte_velocidad);
      contador=contador+1;  
    }
    FLAG_PARADA=0;
    posicion_actual_piso=0;


  Serial.println("En posicion de descanso...P0");
  }


}





void setup() {

  pinMode (PUL, OUTPUT);                      //Salidas motor Pulsos
  pinMode (DIR, OUTPUT);                      //Salidas motor Direccion
  pinMode (ENA, OUTPUT);                      //Salidas motor Habilitacion

  pinMode (PULS_SUBE_A_PISO, INPUT_PULLUP);   //Entradas botones Sube a piso
  pinMode (PULS_BAJA_A_PISO, INPUT_PULLUP);   //Entradas botones Baja a piso
  pinMode (PULS_MENU, INPUT_PULLUP);          //Entradas botones Menu
  pinMode (FIN_CARRERA, INPUT_PULLUP);        //Entrada fin de carrera

  pinMode(ENCODER_A, INPUT_PULLUP);           //Entrada A encoder rotativo
  pinMode(ENCODER_B, INPUT_PULLUP);           //Entrada B encoder rotativo
  pinMode (ENC_SW, INPUT_PULLUP);             //Entrada SW encoder rotativo
  pinMode (BEEP, OUTPUT);                     //Salida BEEP
  pinMode (LED, OUTPUT);                      //Salida led


 Serial.begin(9600);                                //Define velocidad comunicacion serie

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { //define comunicacion I2C con pantalla
    Serial.println("SSD1306 Fallo Display");
    for(;;);
  }
  actualiza_display();                            //Muestra info en pantalla

  attachInterrupt(digitalPinToInterrupt(ENCODER_A), encoder_isr, RISING);   //Define interrupcion para encoder rotativo

  EEPROM.begin(96);                               //Define tamano de EEPROM
  
  aviso_sonoro();

  poneaceroeeprom();                              //Pone a cero toda eeprom
  

  
  constante_pulsos=EEPROM.readUInt(84);           //Lee constante de movimiento
  cte_velocidad_alta=EEPROM.readUInt(88);            //Lee constante de velocidad
  cte_velocidad_baja=EEPROM.readUInt(92);            //Lee constante de velocidad
  valor_eeprom = EEPROM.readUInt(0);              //Lee constante 0

piso=0;
distancia_nuevo_piso = EEPROM.readUInt(piso*4);
distancia_actual_piso = EEPROM.readUInt(piso*4);

referencia();                                   //Se mueve a baja velocidad hasta el fin de carrera, descarga bandeja
  sube_a_piso_descanso();                         //Sube a piso 0. Posicion de descanso

}

void loop() {                                     //Bucle principal


  if( !digitalRead(PULS_MENU)){                   //Cambia menu de 0 a 13
    menu=menu+1;
    if(menu>23)menu=0;
    delay(200);
    piso=menu;
    distancia_nuevo_piso = EEPROM.readUInt(piso*4);
  }



  if( !digitalRead(ENC_SW)){                      //Swicht de encoder rotativo
    EEPROM.writeUInt(piso*4 , encoder_value);     //Graba en eeprom si se presiona
    boolean ok = EEPROM.commit();
    delay(200);
    distancia_nuevo_piso = EEPROM.readUInt(piso*4);
      Serial.print("Valor Guardado en EEPROM= ");
      Serial.println(distancia_nuevo_piso);
  }
  

  if(!digitalRead(PULS_SUBE_A_PISO))              //Sube a piso indicado
  sube_a_piso(piso,posicion_actual_piso);

  if(!digitalRead(PULS_BAJA_A_PISO)){             //Baja a piso 0 y descarga en -1
    baja_a_piso(0,posicion_actual_piso);
    referencia();
    sube_a_piso_descanso();
  }

  if (millis() - tiempo_ini_display > tiempo_actualiza_display){  //actualiza info en display cada 0,2seg
    actualiza_display();
    tiempo_ini_display = millis(); 
  }



}