#include <Ultrasonic.h>
#include <Servo.h>
#include <EEPROM.h>
#define TRIGGER_PIN  12
#define ECHO_PIN     13

Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

//Servo base;
//Servo garra;
//Servo elevador;
//Servo encolhedor;

float ultrassonic_mensure();
void alerta_pessoa();
int push_button();
void EEPROM_ESCRITA(int a, int adress);
int EEPROM_LEITURA(int adress);
int SALVAR_COMANDO();
void clear_EEPROM();

int _leitura = 0;
int cont = 0;
int adress_escrita = 0;
int logic;
int leitura_X;
int leitura_Y;
int leitura_EEPROM;
int pot_X;
int pot_Y;
int pot_X2;
int counter = 0;
int pot_Y2;
int timer_presente;
int timer_passado = 0;
int aux_leitura_Y;
int timer;
int timer_pass = 0;
int mode;
int salvar;
int button;
int aux;
int aux_salvar;
int aux_salvar2;
int aux_salvar3;
int aux_salvar4;
unsigned long timer_future;


void setup() {

  Serial.begin(9600);
  pinMode(2, INPUT);
  pinMode(8, OUTPUT);
  pinMode(5, INPUT);

  //base.attach(9);
  //garra.attach(10);
  //elevador.attach(11);
  //encolhedor.attach(6);


}

void loop() {

  float mensure =  ultrassonic_mensure();

  if (mensure >= 20 && mode != 1){

    leitura_Y = analogRead(0);
    leitura_X = analogRead(1);
    pot_X = map(leitura_X, 0, 1023, 0, 180);
    pot_Y = map(leitura_Y,0,1023,0,180);

    timer_presente = millis();

    if ((timer_presente - timer_passado) > 100 && pot_Y != 89){

       Serial.print(pot_Y);
       Serial.println(" Eixo Y. ");
       //base.write(pot_Y);

    }

    if((timer_presente - timer_passado) > 100 && pot_X != 91){

       Serial.print(pot_X);
       Serial.println(" Eixo X. ");
       //garra.write(pot_X);
    }

    /*
       if((timer_presente - timer_passado) > 100 && pot_X2 != 91){

       Serial.print(pot_X2);
       Serial.println(" Eixo X2. ");
       //elevador.write(pot_X2);
    }

       if((timer_presente - timer_passado) > 100 && pot_Y2 != 91){

       Serial.print(pot_Y2);
       Serial.println(" Eixo Y2. ");
       //encolhedor.write(pot_Y2);
    }
     */

}

timer_passado = millis();

   if (mensure < 20){
      Serial.println("Alerta Pessoa no Perimetro");
      alerta_pessoa();
   }

  salvar = SALVAR_COMANDO();
  if (adress_escrita == EEPROM.length())
          Serial.println("Aperte o botao X para limpar a memoria e gravar novos movimentos");

   if (mensure >= 20 && salvar == 1 && adress_escrita != EEPROM.length()){

    aux_salvar = pot_X;
    EEPROM_ESCRITA(pot_X, adress_escrita);
    adress_escrita = adress_escrita +1;
    aux_salvar2 = pot_Y;
    EEPROM_ESCRITA(pot_Y, adress_escrita);
    adress_escrita = adress_escrita +1;
    EEPROM_ESCRITA(pot_Y2, adress_escrita);
    adress_escrita = adress_escrita +1;
    EEPROM_ESCRITA(pot_X2, adress_escrita);
    adress_escrita = adress_escrita +1;
   }

   _leitura = 0;

   if (mensure >= 20 && mode == 1 ){

      leitura_EEPROM = EEPROM_LEITURA(_leitura);
      logic = ((_leitura)-(4*counter));
      counter = counter + 1;
      _leitura = _leitura + 1;

      if (leitura_EEPROM == 0){

        cont ++;

        if (cont > 4)
          _leitura = 0;
      }

      if (logic == 1)
        //base.write(leitura_EEPROM);
      if (logic == 2)
        //garra.write(leitura_EEPROM);
      if (logic == 3)
        //elevador.write(leitura_EEPROM);
      if (logic == 4)
        //encolhedor.write(leitura_EEPROM);

      delay (500);

      if (_leitura == EEPROM.length())
          _leitura = 0;

   }

   /* if (button X == 1)
        clear_EEPROM();
    */



 /* int mode = push_button();

   if(mode == 1){
    Serial.println("---- Modo automatico Inicializado ---");
   }
    else{
     Serial.println("--- Modo Manual Inicializado ---");
    }
*/
}


float ultrassonic_mensure(){

  float cmMsec, inMsec;
  long microsec = ultrasonic.timing();
  cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);
  delay(500);
  return cmMsec;
}

void alerta_pessoa(){

  digitalWrite(8, HIGH);
  delay(400);
  digitalWrite(8, LOW);
  delay(400);

}

int push_button(){


  unsigned long timer_present = millis();

  if(timer_present - timer_future > 150){

        button = digitalRead(2);
        timer_future = millis();

        if(button == 1){

          if(aux == 1)
            aux = 0;
          else
            aux = 1;
        }

        if(aux == 1)
          return 1;

        if(aux == 0)
          return 0;

  }
}

void EEPROM_ESCRITA(int a, int adress){

      EEPROM.write(adress, a);
      delay (100);

}


int EEPROM_LEITURA(int adress){

  int leitura = EEPROM.read(adress);

  if (adress == EEPROM.length())
    adress = 0;

  return leitura;

}

int SALVAR_COMANDO(){

  int val = digitalRead(5);

    if (val == 1)
      return 1;

      else
        return 0;

}

void clear_EEPROM(){

  for (int i = 0 ; i < EEPROM.length() ; i++) {
    EEPROM.write(i, 0);
  }

  Serial.println("--- Memoria Limpa ---");
}