///LIBRARIES & SPECIFIC PARAMETERS
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <U8g2lib.h>

Adafruit_PWMServoDriver servo = Adafruit_PWMServoDriver(0x40);  //ServoMotor Driver Controller I2C Address
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); //OLED Constructor

//- - - - VARIABLES & METHODS - - - -//

#define jky 13              //Joystick button pin
#define emgPin A0           //EMG signal pin
#define emgThreshold 500    //EMG signal detection threshold   

/*----OLED Display & Driver----*/
void u8g2_prepare(void) {
  u8g2.setFontMode(1);              //Enable Transparent Font
  u8g2.setFont(u8g2_font_6x13_tf);  //Set font Width: 6px / Height: 13px
  u8g2.setFontRefHeightExtendedText();    
  u8g2.setFontPosTop();             //Origin Position of Text
  u8g2.setFontDirection(0);         //Font direction 0º
}
//Custom Drawn Symbols
static const unsigned char back[] U8X8_PROGMEM  = {
  0x04, 0x06, 0x7F, 0x86, 0x84, 0x60, };

/*---- Servo Driver Variables ----*/
#define Freq (uint8_t) 50     //Servo working frequency

#define posMin 102
#define posMax 512

//Finger Number Assignation for Servo Control
#define thumb (uint8_t) 0
#define index (uint8_t) 1
#define middle (uint8_t) 2 
#define ring (uint8_t) 3
#define pinky (uint8_t) 4
#define wrist (uint8_t) 5

//Method to map servo position from pulses to degrees
void setServo(uint8_t n_servo, uint8_t angulo){
  int duty;
  duty=map(angulo, 0, 180, posMin, posMax);
  servo.setPWM(n_servo, 0, duty);
}

//Method for EMG signal detection and actuation
void emg(void (*pointer)()){
  if((analogRead(A0))>emgThreshold){
    pointer();    
  }
  else{
    openHand();
  }
}

/*---- Hand Movements Methods ----*/
void closeHand(void){
    setServo(thumb,170);
    setServo(index,170);
    setServo(middle,175);
    setServo(ring,151);
    setServo(pinky,161);
    setServo(wrist,180);
}
void openHand(void){
    setServo(thumb,13);
    setServo(index,10 );
    setServo(middle,13);
    setServo(ring,13);
    setServo(pinky,13);
    setServo(wrist,180);
}
void rock_servo(void){
    setServo(thumb,170);
    setServo(index,13);
    setServo(middle,175);
    setServo(ring,151);
    setServo(pinky,13);
    setServo(wrist,180);
}
void ronaldinho_servo(){
    setServo(thumb,13);
    setServo(index,156);
    setServo(middle,145);
    setServo(ring,151);
    setServo(pinky,13);
    while(analogRead(A0)>emgThreshold){
      setServo(wrist,150);
      delay(400);
      setServo(wrist,180);
      delay(500);
    }
}
void point_servo(void){
    setServo(thumb,170);
    setServo(index,13);
    setServo(middle,145);
    setServo(ring,151);
    setServo(pinky,161);
    setServo(wrist,180);
}
void thumbsUp_servo(void){
    setServo(thumb,13);
    setServo(index,156);
    setServo(middle,145);
    setServo(ring,151);
    setServo(pinky,161);
    setServo(wrist,180);
}

/*---- Joystick Button----*/
#define jkyX A1
#define jkyY A2

uint8_t swbtn_state=0;    // Memory Storage Byte
bool btn_val=0;           // Current state of the button
bool btn_oldVal=1;        // Previous state of the button  

//Joystick Button Method
void btn_Jky(uint8_t x){
  btn_val=digitalRead(jky);   // read pin button
  if (btn_val == LOW && btn_oldVal == HIGH){
      swbtn_state=x;    
      delay(10);   
  }
  btn_oldVal=btn_val;
}

/*- *- *- *- - MENU SYSTEM - -* -* -* -*/
uint8_t M1 = 0; //Memory variable to determine menu state
uint8_t F1 = 0; //Memory variable to determine menu state
uint8_t C1 = 0; //Memory variable to determine menu state

//Mode_options
#define openClose (uint8_t) 1
#define figures (uint8_t)   2
#define custom  (uint8_t)   3

//Figures_options
#define rock (uint8_t)       1
#define ronaldinho (uint8_t) 2
#define point (uint8_t)      3
#define thumbUp (uint8_t)    4
#define goBackF1 (uint8_t)   5

//Custom_options
#define thumb (uint8_t)    1
#define index (uint8_t)    2
#define middle (uint8_t)   3
#define ring (uint8_t)     4
#define pinky (uint8_t)    5
#define wrist (uint8_t)    6
#define goBackC1 (uint8_t) 7

/*---- 1.MODES Menu Methods / All Menu Systems ----*/
/*-- Read Joystic M1 Method --*/
void jkyM1(void){
  if (analogRead(jkyX)<=341 && analogRead(jkyY)>=1010){
    M1 = openClose;
  }
  else if (analogRead(jkyX)>=342 && analogRead(jkyX)<=682 && analogRead(jkyY)>=990){
    M1 = figures;
  }
  else if (analogRead(jkyX)>=683 && analogRead(jkyX)<=1023 && analogRead(jkyY)>=980){
    M1 = custom;
  } 
}

/*---- Modes Screen Methods ----*/
void M1_openClose(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(32, 45, "OPEN/CLOSE");
  
  u8g2.drawBox(8,0,31,31);   //Selected box
  u8g2.drawStr(21,9, "1");   //Selected string
  
  u8g2.drawFrame(47,0,31,31);
  u8g2.drawStr(60,9, "2");
  
  u8g2.drawFrame(86,0,31,31);
  u8g2.drawStr(99,9, "3");
  
  u8g2.sendBuffer();
}
void M1_openClose_in(){
  emg(closeHand); 
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(32, 16, "Squeeze to");
  u8g2.drawStr(32, 32, "CLOSE HAND");
  
  u8g2.sendBuffer();
}
void M1_figures(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(42, 45, "FIGURES");
  
  u8g2.drawFrame(8,0,31,31);   
  u8g2.drawStr(21,9, "1");
  
  u8g2.drawBox(47,0,31,31);   //Selected box
  u8g2.drawStr(60,9, "2");    //Selected string
  
  u8g2.drawFrame(86,0,31,31);
  u8g2.drawStr(99,9, "3");
  
  u8g2.sendBuffer();
}
void M1_custom(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(19, 45, "CUSTOM MOVEMENT");
  
  u8g2.drawFrame(8,0,31,31);   
  u8g2.drawStr(21,9, "1");
  
  u8g2.drawFrame(47,0,31,31);   
  u8g2.drawStr(60,9, "2");
  
  u8g2.drawBox(86,0,31,31);   //Selected box
  u8g2.drawStr(99,9, "3");    //Selected string
  
  u8g2.sendBuffer();
}

//MAIN MENU SYSTEM(MODES) METHOD
void modes_M1(){  
  switch (M1){
    case openClose:
      if(swbtn_state==1){
        M1_openClose_in();
        btn_Jky(0);
      }
      else{
        jkyM1();
        openHand();
        M1_openClose();
        btn_Jky(1);
      }
      break;

    case figures:
      if(swbtn_state==2 || swbtn_state==3){
        figures_F1();
      }
      else{
        jkyM1();
        openHand();
        M1_figures();
        btn_Jky(2);
      }
      break;

    case custom:
      if(swbtn_state==4 || swbtn_state==5){
        custom_C1();
      }
      else{
        jkyM1();
        openHand();
        M1_custom();
        btn_Jky(4);
      }
      break;
  }
}

/*---- 2.FIGURES Menu Methods ----*/
/*-- Read Joystic F1 Method --*/
void jkyF1(void){
  if (analogRead(jkyX)<=150 && analogRead(jkyY)>=1022){
    F1 = rock;
  }
  else if (analogRead(jkyX)>=151 && analogRead(jkyX)<=540 && analogRead(jkyY)>=1022){
    F1 = ronaldinho;
  }
  else if (analogRead(jkyX)>=541 && analogRead(jkyX)<=767 && analogRead(jkyY)>=1022){
    F1 = point;
  }
  else if (analogRead(jkyX)>=768 && analogRead(jkyX)<=1023 && analogRead(jkyY)>=1022){
    F1 = thumbUp;
  }
  else if (analogRead(jkyX)>=400 && analogRead(jkyX)<=1023 && analogRead(jkyY)<=400){
    F1 = goBackF1;
  } 
}

/*---- Figures Screen Methods ----*/
void F1_rock(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(32, 32, "ROCK'N ROLL");
  
  u8g2.drawBox(11,0,17,17);   //Selected box
  u8g2.drawStr(17,2, "1");    //Selected string 
  
  u8g2.drawFrame(39,0,17,17);
  u8g2.drawStr(45,2, "2");
  
  u8g2.drawFrame(67,0,17,17);
  u8g2.drawStr(73,2, "3");
  
  u8g2.drawFrame(95,0,17,17);
  u8g2.drawStr(101,2, "4");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();

}
void F1_rock_in(void){
  emg(rock_servo);
  btn_Jky(2);
  u8g2.clearBuffer();
  
  u8g2.drawStr(32, 16, "Squeeze to");
  u8g2.drawStr(32, 32, "ROCK'N ROLL");
  
  u8g2.sendBuffer();
}
void F1_ronaldinho(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(34, 32, "RONALDINHO");
  
  u8g2.drawFrame(11,0,17,17);
  u8g2.drawStr(17,2, "1");
  
  u8g2.drawBox(39,0,17,17);   //Selected box
  u8g2.drawStr(45,2, "2");    //Selected string
  
  u8g2.drawFrame(67,0,17,17);
  u8g2.drawStr(73,2, "3");
  
  u8g2.drawFrame(95,0,17,17);
  u8g2.drawStr(101,2, "4");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}
void F1_ronaldinho_in(void){
  emg(ronaldinho_servo);
  btn_Jky(2);
  u8g2.clearBuffer();
  
  u8g2.drawStr(32, 32, "RONALDINHO!");
  
  u8g2.sendBuffer();
}
void F1_point(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(49, 32, "POINT");
  
  u8g2.drawFrame(11,0,17,17);
  u8g2.drawStr(17,2, "1");
  
  u8g2.drawFrame(39,0,17,17);     
  u8g2.drawStr(45,2, "2");
  
  u8g2.drawBox(67,0,17,17);   //Selected box
  u8g2.drawStr(73,2, "3");    //Selected string
  
  u8g2.drawFrame(95,0,17,17);
  u8g2.drawStr(101,2, "4");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}
void F1_point_in(void){
  emg(point_servo);
  btn_Jky(2);
  u8g2.clearBuffer();
  
  u8g2.drawStr(32, 16, "Squeeze to");
  u8g2.drawStr(49, 32, "POINT");
  
  u8g2.sendBuffer();
}
void F1_thumbUp(){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(39, 32, "THUMBS UP");
  
  u8g2.drawFrame(11,0,17,17);
  u8g2.drawStr(17,2, "1");
  
  u8g2.drawFrame(39,0,17,17);     
  u8g2.drawStr(45,2, "2");
  
  u8g2.drawFrame(67,0,17,17);     
  u8g2.drawStr(73,2, "3");
  
  u8g2.drawBox(95,0,17,17);     //Selected box
  u8g2.drawStr(101,2, "4");     //Selected string
  
  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}
void F1_thumbUp_in(void){
  emg(thumbsUp_servo);
  btn_Jky(2);
  u8g2.clearBuffer();
  
  u8g2.drawStr(20, 16, "Squeeze to give");
  u8g2.drawStr(32, 30, "a THUMBS UP");
  
  u8g2.sendBuffer();
}
void F1_goBack(void){
  btn_Jky(0);  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(42, 32, "GO BACK");
  
  u8g2.drawFrame(11,0,17,17);
  u8g2.drawStr(17,2, "1");
  
  u8g2.drawFrame(39,0,17,17);     
  u8g2.drawStr(45,2, "2");
  
  u8g2.drawFrame(67,0,17,17);     
  u8g2.drawStr(73,2, "3");
  
  u8g2.drawFrame(95,0,17,17);
  u8g2.drawStr(101,2, "4");    

  u8g2.drawBox(115,51,12,10);   //Selected box
  
  u8g2.setDrawColor(0);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

//MAIN FIGURES METHOD
void figures_F1(){
  switch (F1){
    case rock:
      if(swbtn_state==3){ //s'ha de canviar!!
        F1_rock_in();
      }
      else{
        btn_Jky(3);
        jkyF1();
        F1_rock();  //print on display F1_rock
      }
      break;
    
    case ronaldinho: 
      if(swbtn_state==3){ //s'ha de canviar!!
        F1_ronaldinho_in(); 
      }
      else{
        btn_Jky(3);
        jkyF1();
        F1_ronaldinho(); //print on display F1_ronaldinho
      }
      break;
    
    case point:
      if(swbtn_state==3){ //s'ha de canviar!!
        F1_point_in(); 
      }
      else{
        btn_Jky(3);
        jkyF1();
        F1_point(); //print on display F1_ronaldinho
      }
      break;

     case thumbUp:
      if(swbtn_state==3){ //s'ha de canviar!!
        F1_thumbUp_in(); 
      }
      else{
        btn_Jky(3);
        jkyF1();
        F1_thumbUp(); //print on display F1_ronaldinho
      }
      break;

     case goBackF1:
        jkyF1();
        F1_goBack(); //print on display F1_ronaldinho
      break;
  }
}

/*---- 3.CUSTOM Menu Methods ----*/
/*-- Read Joystic C1 Method --*/
void jkyC1(void){
  if (analogRead(jkyX)<=170 && analogRead(jkyY)>=1023){
    C1 = thumb;
  }
  else if (analogRead(jkyX)>=171 && analogRead(jkyX)<=341 && analogRead(jkyY)>=1023){
    C1 = index;
  }
  else if (analogRead(jkyX)>=342 && analogRead(jkyX)<=511 && analogRead(jkyY)>=1023){
    C1 = middle;
  }
  else if (analogRead(jkyX)>=512 && analogRead(jkyX)<=682 && analogRead(jkyY)>=1023){
    C1 = ring;
  }
  else if (analogRead(jkyX)>=683 && analogRead(jkyX)<=853 && analogRead(jkyY)>=1023){
    C1 = pinky;
  }
  else if (analogRead(jkyX)>=854 && analogRead(jkyX)<=1023 && analogRead(jkyY)>=1023){
    C1 = wrist;
  }
  else if (analogRead(jkyX)>=512 && analogRead(jkyX)<=1023 && analogRead(jkyY)<=450){
    C1 = goBackC1;
  }
}
/*-- Movement State for Individual Fingers Method --*/
uint8_t movement=0;
void jky_servo_movement(void){
    if(analogRead(jkyY)>=600){
        movement=1;
    }
    else if(analogRead(jkyY)<=400){
        movement=2;
    }
    else if(analogRead(jkyY)>=401 && analogRead(jkyY)<=599){
        movement=0;
    }
}

/*---- Custom Screen Methods ----*/
void C1_thumb(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(49, 32, "THUMB");
  
  u8g2.drawBox(2,0,17,17);   //Selected box
  u8g2.drawStr(8,2, "1");    //Selected string
  
  u8g2.drawFrame(23,0,17,17);
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawFrame(44,0,17,17);
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

#define thumbMin 13
#define thumbMax 170
uint8_t thumb_angle=13;
uint8_t thumb_percentage=0;

void C1_thumb_in(void){
    jky_servo_movement();
    btn_Jky(4);
    if(movement==1 && thumb_angle<thumbMax){
        thumb_angle++;
    }
    else if(movement==2 && thumb_angle>thumbMin){
        thumb_angle--;
    }
    setServo(0,thumb_angle);
    thumb_percentage=map(thumb_angle, thumbMin, thumbMax, 0, 100);
    String percentage = String(thumb_percentage);
  
    u8g2.clearBuffer();
    u8g2.setDrawColor(2);
  
    u8g2.drawStr(16, 0, "Adjust Position");

    u8g2.drawStr(12,24, "Thumb:");
    u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
    u8g2.drawStr(82,24, "%");
    
    u8g2.drawFrame(12,40,102,17);
    u8g2.drawBox(13,41,thumb_percentage,15);   //x is percentage of map
    
    u8g2.sendBuffer();
  
}
void C1_index(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(49, 32, "INDEX");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawBox(23,0,17,17);   //Selected box
  u8g2.drawStr(29,2, "2");     //Selected string
  
  u8g2.drawFrame(44,0,17,17);
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);

  u8g2.sendBuffer();
}

#define indexMin 10
#define indexMax 170
uint8_t index_angle=10;
uint8_t index_percentage=0;

void C1_index_in(){
  jky_servo_movement();
  btn_Jky(4);
  if(movement==1 && index_angle<indexMax){
      index_angle++;
  }
  else if(movement==2 && index_angle>indexMin){
      index_angle--;
  }
  setServo(1,index_angle);
  index_percentage=map(index_angle, indexMin, indexMax, 0, 100);
  String percentage = String(index_percentage);
  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(16, 0, "Adjust Position");

  u8g2.drawStr(12,24, "Index:");
  u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
  u8g2.drawStr(82,24, "%");
    
  u8g2.drawFrame(12,40,102,17);
  u8g2.drawBox(13,41,index_percentage,15);   //x is percentage of map
    
  u8g2.sendBuffer();
}
void C1_middle(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(47, 32, "MIDDLE");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawFrame(23,0,17,17);   
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawBox(44,0,17,17);   //Selected box
  u8g2.drawStr(50,2, "3");    //Selected string
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

#define middleMin 13
#define middleMax 175
uint8_t middle_angle=13;
uint8_t middle_percentage=0;

void C1_middle_in(void){
  jky_servo_movement();
  btn_Jky(4);
  if(movement==1 && middle_angle<middleMax){
      middle_angle++;
  }
  else if(movement==2 && middle_angle>middleMin){
      middle_angle--;
  }
  setServo(2,middle_angle);
  middle_percentage=map(middle_angle, middleMin, middleMax, 0, 100);
  String percentage = String(middle_percentage);
  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(16, 0, "Adjust Position");

  u8g2.drawStr(12,24, "Middle:");
  u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
  u8g2.drawStr(82,24, "%");
    
  u8g2.drawFrame(12,40,102,17);
  u8g2.drawBox(13,41,middle_percentage,15);   //x is percentage of map
    
  u8g2.sendBuffer();
}
void C1_ring(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(52, 32, "RING");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawFrame(23,0,17,17);   
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawFrame(44,0,17,17);
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawBox(65,0,17,17);   //Selected box
  u8g2.drawStr(71,2, "4");    //Selected string
  
  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

#define ringMin 13
#define ringMax 151
uint8_t ring_angle=13;
uint8_t ring_percentage=0;

void C1_ring_in(void){
  jky_servo_movement();
  btn_Jky(4);
  if(movement==1 && ring_angle<ringMax){
      ring_angle++;
  }
  else if(movement==2 && ring_angle>ringMin){
      ring_angle--;
  }
  setServo(3,ring_angle);
  ring_percentage=map(ring_angle, ringMin, ringMax, 0, 100);
  String percentage = String(ring_percentage);
  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(16, 0, "Adjust Position");

  u8g2.drawStr(12,24, "Ring:");
  u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
  u8g2.drawStr(82,24, "%");
    
  u8g2.drawFrame(12,40,102,17);
  u8g2.drawBox(13,41,ring_percentage,15);   //x is percentage of map
    
  u8g2.sendBuffer();
}
void C1_pinky(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(49, 32, "PINKY");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawFrame(23,0,17,17);   
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawFrame(44,0,17,17);   
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawBox(86,0,17,17);   //Selected box
  u8g2.drawStr(92,2, "5");    //Selected string

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

#define pinkyMin 13
#define pinkyMax 161
uint8_t pinky_angle=13;
uint8_t pinky_percentage=0;

void C1_pinky_in(void){
  jky_servo_movement();
  btn_Jky(4);
  if(movement==1 && pinky_angle<pinkyMax){
      pinky_angle++;
  }
  else if(movement==2 && pinky_angle>pinkyMin){
      pinky_angle--;
  }
  setServo(4,pinky_angle);
  pinky_percentage=map(pinky_angle, pinkyMin, pinkyMax, 0, 100);
  String percentage = String(pinky_percentage);
  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(16, 0, "Adjust Position");

  u8g2.drawStr(12,24, "Pinky:");
  u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
  u8g2.drawStr(82,24, "%");
    
  u8g2.drawFrame(12,40,102,17);
  u8g2.drawBox(13,41,middle_percentage,15);   //x is percentage of map
    
  u8g2.sendBuffer();
}
void C1_wrist(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(49, 32, "WRIST");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawFrame(23,0,17,17);   
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawFrame(44,0,17,17);   
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawBox(107,0,17,17);    //Selected box
  u8g2.drawStr(113,2, "6");     //Selected string

  u8g2.drawFrame(115,51,12,10);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  
  u8g2.sendBuffer();
}

#define wristMin 180
#define wristMax 150
uint8_t wrist_angle=180;
uint8_t wrist_percentage=0;

void C1_wrist_in(void){
  jky_servo_movement();
  btn_Jky(4);
  if(movement==1 && wrist_angle>wristMax){
      wrist_angle--;
  }
  else if(movement==2 && wrist_angle<wristMin){
      wrist_angle++;
  }
  setServo(5,wrist_angle);
  wrist_percentage=map(wrist_angle, wristMin, wristMax, 0, 100);
  String percentage = String(wrist_percentage);
  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(16, 0, "Adjust Position");

  u8g2.drawStr(12,24, "Wrist:");
  u8g2.drawStr(64,24, percentage.c_str());   //map variable for %
  u8g2.drawStr(82,24, "%");
    
  u8g2.drawFrame(12,40,102,17);
  u8g2.drawBox(13,41,wrist_percentage,15);   //x is percentage of map
    
  u8g2.sendBuffer();
}
void C1_goBack(void){  
  u8g2.clearBuffer();
  u8g2.setDrawColor(2);
  
  u8g2.drawStr(43, 32, "GO BACK");
  
  u8g2.drawFrame(2,0,17,17);   
  u8g2.drawStr(8,2, "1");
  
  u8g2.drawFrame(23,0,17,17);   
  u8g2.drawStr(29,2, "2");
  
  u8g2.drawFrame(44,0,17,17);   
  u8g2.drawStr(50,2, "3");
  
  u8g2.drawFrame(65,0,17,17);
  u8g2.drawStr(71,2, "4");

  u8g2.drawFrame(86,0,17,17);
  u8g2.drawStr(92,2, "5");

  u8g2.drawFrame(107,0,17,17);
  u8g2.drawStr(113,2, "6");
  
  u8g2.drawBox(115,51,12,10);   //Selected box
  
  u8g2.setDrawColor(0);
  u8g2.drawXBMP(117, 53, 8, 6, back);
  u8g2.sendBuffer();
}

//MAIN CUSTOM METHOD
void custom_C1(void){
  switch(C1){
    case thumb:
      if(swbtn_state==5){
        jky_servo_movement();
        C1_thumb_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_thumb();  //print on display F1_rock
      }
      break;
    
    case index:
      if(swbtn_state==5){ //s'ha de canviar!!
        C1_index_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_index();  //print on display F1_rock
      }
      break;
    
    case middle:
      if(swbtn_state==5){ //s'ha de canviar!!
        C1_middle_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_middle();  //print on display F1_rock
      }
      break;

    case ring:
      if(swbtn_state==5){ //s'ha de canviar!!
        C1_ring_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_ring();  //print on display F1_rock
      }
      break;
    
    case pinky:
      if(swbtn_state==5){ //s'ha de canviar!!
        C1_pinky_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_pinky();  //print on display F1_rock
      }
      break;

    case wrist:
      if(swbtn_state==5){ //s'ha de canviar!!
        C1_wrist_in();
      }
      else{
        btn_Jky(5);
        jkyC1();
        C1_wrist();  //print on display F1_rock
      }
      break;

    case goBackC1:
        btn_Jky(0);
        jkyC1();
        C1_goBack();  //print on display F1_rock
      break;
  }
}

void setup(){
  u8g2.begin();
  u8g2_prepare();
  servo.begin();
  servo.setPWMFreq(Freq);
  pinMode(jky, INPUT_PULLUP);
  F1=rock;
  C1=thumb;
  M1=openClose;
  swbtn_state=0;
}

void loop(){ 
  modes_M1();
}