#include <Keypad.h> 
#include <ESP32Servo.h>
#define led0 22
#define led1 23
#define servo_delta_var 10
#define base_minimo 0
#define base_maximo 180
#define angulo_base_inicial 90
#define cotovelo_minimo 0
#define cotovelo_maximo 180
#define angulo_cotovelo_inicial 90
#define ombro_minimo 0
#define ombro_maximo 180
#define angulo_ombro_inicial 90
#define garra_minimo 0
#define garra_maximo 180
#define angulo_garra_inicial 90
#define servo_base_pin 12
#define servo_cotovelo_pin 13
#define servo_ombro_pin 14
#define servo_garra_pin 27

const byte ROWS = 4; /* four rows */ 
const byte COLS = 4; /* four columns */ 

/* define the symbols on the buttons of the keypads */ 
char hexaKeys[ROWS][COLS]={ {'1','2','3','A'}, 
                            {'4','5','6','B'}, 
                            {'7','8','9','C'}, 
                            {'*','0','#','D'} }; 

/* connect to the row pinouts of the keypad */ 
byte rowPins[ROWS] = {2,0,4,16};
/* connect to the column pinouts of the keypad */ 
byte colPins[COLS] = {17,5,18,19};

/* initialize an instance of class NewKeypad */ 
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); 

signed int atraso;

Servo servo_base;
Servo servo_cotovelo;
Servo servo_ombro;
Servo servo_garra;

//int servo_base_pin=12;
signed int angulo_base;
signed int angulo_cotovelo;
signed int angulo_ombro;
signed int angulo_garra;
unsigned char modo_execucao;
int programacao[4][255];
int programacao_linha, programacao_linha_fim;
int execucao_linha;

void programacao_inicial();
void setModoLeds(unsigned char mode);
void execucaoServo( int base, int ombro, int cotovelo, int garra);

void setup(){ 
    Serial.begin(9600);
    pinMode(led0, OUTPUT);
    pinMode(led1, OUTPUT);
    modo_execucao = 0;
    setModoLeds(modo_execucao);
    programacao_linha = 0;
    programacao_linha_fim = programacao_linha;
    servo_base.attach(servo_base_pin);
    servo_cotovelo.attach(servo_cotovelo_pin);
    servo_ombro.attach(servo_ombro_pin);
    servo_garra.attach(servo_garra_pin);
    angulo_base = angulo_base_inicial;
    angulo_cotovelo = angulo_cotovelo_inicial;
    angulo_ombro = angulo_ombro_inicial;
    angulo_garra = angulo_garra_inicial;
    atraso = 50;
} 

void loop(){ 
  char valorChave = customKeypad.getKey(); 
  switch (valorChave){
    case '4' : if (angulo_base<base_maximo) { 
                  angulo_base += servo_delta_var; 
                  Serial.print(valorChave);
                  Serial.print(" : ");
                  Serial.println(angulo_base);
               }
               break;
    case '6' : if (angulo_base>base_minimo) { 
                angulo_base -= servo_delta_var; 
                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(angulo_base);
               }
               break;
    case '1' : if (angulo_ombro<ombro_maximo) { 
                  angulo_ombro += servo_delta_var; 
                  Serial.print(valorChave);
                  Serial.print(" : ");
                  Serial.println(angulo_ombro);
               }
               break;
    case '7' : if (angulo_ombro>ombro_minimo) { 
                angulo_ombro -= servo_delta_var; 
                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(angulo_ombro);
               }
               break;
    case '2' : if (angulo_cotovelo < cotovelo_maximo) { 
                  angulo_cotovelo += servo_delta_var; 
                  Serial.print(valorChave);
                  Serial.print(" : ");
                  Serial.println(angulo_cotovelo);
               }
               break;
    case '8' : if (angulo_cotovelo > cotovelo_minimo) { 
                angulo_cotovelo -= servo_delta_var; 
                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(angulo_cotovelo);
               }
               break;
    case '3' : if (angulo_garra < garra_maximo) { 
                  angulo_garra += servo_delta_var; 
                  Serial.print(valorChave);
                  Serial.print(" : ");
                  Serial.println(angulo_garra);
               }
               break;
    case '9' : if (angulo_garra > garra_minimo) { 
                angulo_garra -= servo_delta_var; 
                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(angulo_garra);
               }
               break;
    case '5' : switch(modo_execucao){
               case 1: programacao[0][programacao_linha] = angulo_base;
                       programacao[1][programacao_linha] = angulo_ombro;
                       programacao[2][programacao_linha] = angulo_cotovelo;
                       programacao[3][programacao_linha] = angulo_garra;
                       Serial.print(programacao_linha);
                       Serial.print(" : base[");
                       Serial.print(angulo_base);
                       Serial.print("] : ombro[");
                       Serial.print(angulo_ombro);
                       Serial.print("] : cotovelo[");
                       Serial.print(angulo_cotovelo);
                       Serial.print("] : garra[");
                       Serial.print(angulo_garra);
                       Serial.println("]");
                       programacao_linha++;break;
               case 3: Serial.print(execucao_linha);
                       Serial.print(" : base[");
                       Serial.print(programacao[0][execucao_linha]);
                       Serial.print("] : ombro[");
                       Serial.print(programacao[1][execucao_linha]);
                       Serial.print("] : cotovelo[");
                       Serial.print(programacao[2][execucao_linha]);
                       Serial.print("] : garra[");
                       Serial.print(programacao[3][execucao_linha]);
                       Serial.println("]");
                       execucao_linha++;break;
               }
               break; // Marca Ponto / Executa Passo
    case '*' : break; // Apagar Ultimo
    case '#' : modo_execucao++; // 0-Modo Manual / 1- Gravacao / 2- Modo Auto Repetitivo / 3- Modo Auto Passo a Passo 
               if (modo_execucao > 3){
                modo_execucao = 0;
               }
               setModoLeds( modo_execucao );
               break; 
    case 'A' : switch(modo_execucao){
               case 1: programacao_linha = 0;break;
               case 2: case 3: execucao_linha = 0;break;
               }
               break; // Reinicia
    case 'B' : programacao_linha_fim = programacao_linha-1;
               break; // Finaliza
    case 'C' : switch(modo_execucao){
               case 1: programacao_inicial();
                       programacao_linha = 0;
                       programacao_linha_fim = 0;
                       break;
               case 2: case 3: execucao_linha = 0;break;
               }
               break; // Parada
    case 'D' : posicaoHome();
               execucaoServo( angulo_base, angulo_ombro, angulo_cotovelo, angulo_garra);
               break; // Apagar Tudo
  }
  switch ( modo_execucao){
    case 0: execucaoServo( angulo_base, angulo_ombro, angulo_cotovelo, angulo_garra);
            break;
    case 1: execucaoServo( angulo_base, angulo_ombro, angulo_cotovelo, angulo_garra);
            break;
    case 2: execucaoServo( programacao[0][execucao_linha],programacao[1][execucao_linha],programacao[2][execucao_linha],programacao[3][execucao_linha]);
            if (execucao_linha < programacao_linha_fim){
              execucao_linha++;
            }else{
              execucao_linha=0;
            }
            break;
    case 3: execucaoServo( programacao[0][execucao_linha],programacao[1][execucao_linha],programacao[2][execucao_linha],programacao[3][execucao_linha]);
            if (execucao_linha >= programacao_linha_fim){
              execucao_linha=0;
            }
            break;
            break;
  }
  delay(atraso);
}

void programacao_inicial(){
  for (int p; p<255;p++){
    programacao[0][p] = angulo_base;
    programacao[1][p] = angulo_ombro;
    programacao[2][p] = angulo_cotovelo;
    programacao[3][p] = angulo_garra;
  }
}

void setModoLeds(unsigned char mode){
  switch(mode){
     case 0: digitalWrite(led0, 0);digitalWrite(led1, 0);break;
     case 1: digitalWrite(led0, 1);digitalWrite(led1, 0);break;
     case 2: digitalWrite(led0, 0);digitalWrite(led1, 1);break;
     case 3: digitalWrite(led0, 1);digitalWrite(led1, 1);break;
  }
}

void posicaoHome(){
  angulo_base = angulo_base_inicial;
  angulo_cotovelo = angulo_cotovelo_inicial;
  angulo_ombro = angulo_ombro_inicial;
  angulo_garra = angulo_garra_inicial;
}

void execucaoServo( int base, int ombro, int cotovelo, int garra){
  servo_base.write(base);
  servo_cotovelo.write(cotovelo);
  servo_ombro.write(ombro);
  servo_garra.write(garra);
}