#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>
LiquidCrystal_I2C lcd(0x27,20,4); //LiquidCrystal_I2C lcd(0x20,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display or 20 chars and 4 line display

#define right digitalRead(4)
#define enter digitalRead(3)
#define left digitalRead(2)
#define pwmPin A0
#define reflin analogRead(A1)
#define arusIn A2
#define tempIn A3
#define vcc A4
#define led1 5
#define led2 6
#define led3 7
#define led4 8
#define led5 11

//menu variable
int pCounter = 0;
int mode = 1;
int subCounter = 0;
int menuCounter = 0;
int buttonState = 0;
int enterState = 0;

//temp variable
int tVal1, tVal2, tVal3;
int t1 = 70, t2 = 70, t3 = 70;
int tV1, tV2, tV3;
int tCount1, tCount11, tCount2, tCount22, tCount3, tCount33;

//arus variable
int aVal1, aVal2, aVal3;
int a1 = 10, a2 = 10, a3 = 10;
int aV1, aV2, aV3;
int aCount1, aCount11, aCount2, aCount22, aCount3, aCount33;

//power variable
const int pinOut = 9;
byte PWM;
float pwrIn;
int pwmMon;
int pwr = 0;
float pwrVal= 0.0;
float pVal = 0.0;
byte pOut;
byte b_pOut[2];
float refl = 0.0;
int reflect = 0;
bool counting = true;
float limVal = 10.0;

//frequency variable
int freq1, freq2, freq3, freq4;
float freq;
float f_freq;
byte *ptr_val0;
float value0;
String frekuensi;
String batasFrekuensi[14] = 
{ "87.7", 
  "87.9", 
  "88.1", 
  "88.3", 
  "88.5", 
  "88.7", 
  "88.9", 
  "106.7", 
  "106.9", 
  "107.1", 
  "107.3", 
  "107.5", 
  "107.7", 
  "107.9"};

//symblo variable
byte w_delta[8] =
{
  B11000,
  B10100,
  B10010,
  B10001,
  B10001,
  B10010,
  B10100,
  B11000
};
byte b_delta[8] =
{
  B11000,
  B11100,
  B11110,
  B11111,
  B11111,
  B11110,
  B11100,
  B11000
};

byte circle[8] =
{
  B01110,
  B10001,
  B10001,
  B01110,
  B00000,
  B00000,
  B00000,
  B00000
};

byte val_ptr[8] =
{
  B00100,
  B01010,
  B10001,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000
};


void setup() {
 lcd.init();   
 lcd.backlight();
 lcd.createChar(0, w_delta);
 lcd.createChar(1, b_delta);
 lcd.createChar(2, circle);
 lcd.createChar(3, val_ptr);
 pinMode(2, INPUT_PULLUP);
 pinMode(3, INPUT_PULLUP);
 pinMode(4, INPUT_PULLUP);
 pinMode(5, OUTPUT);
 pinMode(6, OUTPUT);
 pinMode(7, OUTPUT);
 pinMode(8, OUTPUT);
 pinMode(11, OUTPUT);
 pinMode(A0, INPUT);
 pinMode(A1, INPUT);
 pinMode(A2, INPUT);
 pinMode(A3, INPUT);
 pinMode(A4, INPUT);
 pinMode(A1, OUTPUT);
}

//Proses
void bgProses(){
  pwr = pCounter;
  pVal = (float)pCounter;
  pVal = float(pVal/1000) * 1023.0;
  pOut = floor(pVal/4);
  analogWrite(pinOut, (byte)pOut);

  //PWM Monitor
  PWM = getPwm(pwmPin);
  pwrIn = float(PWM)/1023.0;
  pwrIn = ceil(pwrIn * 1000.0);
  pwrIn = pwrIn * 10;
  pwmMon = (int)PWM;

  //reflected
  refl = (5.0 * reflin) / 1023.0;
  refl = (refl * pwrIn * 10.0) / 1000.0;
  limVal = (2.0 * reflin) / 1023.0;

  //temp
  tV1 = (tCount1 * 10) + tCount11;
  tV2 = (tCount2 * 10) + tCount22;
  tV3 = (tCount3 * 10) + tCount33;

  //arus
  aV1 = (aCount1 * 10) + aCount11;
  aV2 = (aCount2 * 10) + aCount22;
  aV3 = (aCount3 * 10) + aCount33;

  //frequency
  freq = (float)(freq1*100 + freq2*10 + freq3 + freq4*0.1);
  f_freq = freq;
}

byte getPwm(byte pin){
  unsigned long highTime = pulseIn(pin, HIGH, 50000);
  unsigned long lowTime = pulseIn(pin, LOW, 50000);

  if(highTime == 0 || lowTime == 0){
    return analogRead(pin) ? 100 : 0;
  }
  return (100*highTime)/(highTime + lowTime);
}

// MENU 1
void title1(){
  lcd.setCursor(3,0);
  lcd.print("RF Power Status");
  lcd.setCursor(19,1);
  lcd.print("W");
  lcd.setCursor(19,2);
  lcd.print("W");
  lcd.setCursor(19,3);
  lcd.print("W");
}

void setUp(){
  lcd.setCursor(2,1);
  lcd.print("Set Power : ");
  lcd.setCursor(13,1);
  lcd.print(pCounter);
}

void power(){
  lcd.setCursor(2,2);
  lcd.print("Power     : ");
  lcd.setCursor(13,2);
  lcd.print(pwrIn, 1);
}

void reflected(){
  lcd.setCursor(2,3);
  lcd.print("Refl      : ");
  lcd.setCursor(13,3);
  lcd.print(refl, 1);
}

void pwmMonitor(){
  lcd.clear();
  lcd.setCursor(2,0);
  lcd.print("PWM Monitor: ");
  lcd.setCursor(15,0);
  lcd.print(pwmMon);
  lcd.setCursor(2,2);
  lcd.print("Enter Setup");
  lcd.setCursor(2,3);
  lcd.print("Exit");
  delay(150);
}

void menu1(){
  lcd.clear();
  bgProses();
  setUp();
  title1();
  power();
  reflected();
  if(refl > limVal && counting && mode == 0){
    digitalWrite(11, HIGH);
    pCounter -= 15;
    if(pCounter < 0){
      pCounter = 0;
    }
  }
  else{
    digitalWrite(11, LOW);
  }
  delay(150);
}

//MENU 2

void title2(){
  lcd.setCursor(5,0);
  lcd.print("Temp Status");
  lcd.setCursor(18,1);
  lcd.write(2);
  lcd.setCursor(19,1);
  lcd.print("C");
  lcd.setCursor(18,2);
  lcd.write(2);
  lcd.setCursor(19,2);
  lcd.print("C");
  lcd.setCursor(18,3);
  lcd.write(2);
  lcd.setCursor(19,3);
  lcd.print("C");
}

void temp1(){
  int tArray1[] = {t1, tV1};
  lcd.setCursor(2,1);
  lcd.print("Temp 1 : ");
  lcd.setCursor(11,1);
  lcd.print(tArray1[tVal1]);
}

void temp2(){
  int tArray2[] = {t2, tV2};
  lcd.setCursor(2,2);
  lcd.print("Temp 2 : ");
  lcd.setCursor(11,2);
  lcd.print(tArray2[tVal2]);
}

void temp3(){
  int tArray3[] = {t3, tV3};
  lcd.setCursor(2,3);
  lcd.print("Temp 3 : ");
  lcd.setCursor(11,3);
  lcd.print(tArray3[tVal3]);
}

void menu2(){
  lcd.clear();
  title2();
  temp1();
  temp2();
  temp3();
  delay(150);
}

//FUNCTION TO LIMIT TEMP

void tLim1(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT TEMP 1 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(tCount1);
  lcd.setCursor(9,2);
  lcd.print(tCount11);
  lcd.setCursor(10,2);
  lcd.write(2);
  lcd.setCursor(11,2);
  lcd.print("C");
  delay(150);
}

void tLim2(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT TEMP 2 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(tCount2);
  lcd.setCursor(9,2);
  lcd.print(tCount22);
  lcd.setCursor(10,2);
  lcd.write(2);
  lcd.setCursor(11,2);
  lcd.print("C");
  delay(150);
}

void tLim3(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT TEMP 3 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(tCount3);
  lcd.setCursor(9,2);
  lcd.print(tCount33);
  lcd.setCursor(10,2);
  lcd.write(2);
  lcd.setCursor(11,2);
  lcd.print("C");
  delay(150);
}

//MENU 3

void title3(){
  lcd.setCursor(5,0);
  lcd.print("Arus Status");
  lcd.setCursor(19,1);
  lcd.print("A");
  lcd.setCursor(19,2);
  lcd.print("A");
  lcd.setCursor(19,3);
  lcd.print("A");
}

void arus1(){
  int aArray1[] = {a1, aV1};
  lcd.setCursor(2,1);
  lcd.print("Arus 1 : ");
  lcd.setCursor(11,1);
  lcd.print(aArray1[aVal1]);
}

void arus2(){
  int aArray2[] = {a2, aV2};
  lcd.setCursor(2,2);
  lcd.print("Arus 2 : ");
  lcd.setCursor(11,2);
  lcd.print(aArray2[aVal2]);
}

void arus3(){
  int aArray3[] = {a3, aV3};
  lcd.setCursor(2,3);
  lcd.print("Arus 3 : ");
  lcd.setCursor(11,3);
  lcd.print(aArray3[aVal3]);
}

void menu3(){
  lcd.clear();
  title3();
  arus1();
  arus2();
  arus3();
  delay(150);
}

//FUNCTION TO LIMIT ARUS

void aLim1(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT ARUS 1 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(aCount1);
  lcd.setCursor(9,2);
  lcd.print(aCount11);
  lcd.setCursor(11,2);
  lcd.print("A");
  delay(150);
}

void aLim2(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT ARUS 2 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(aCount2);
  lcd.setCursor(9,2);
  lcd.print(aCount22);
  lcd.setCursor(11,2);
  lcd.print("A");
  delay(150);
}

void aLim3(){
  lcd.clear();
  lcd.setCursor(1,1);
  lcd.print("SET LIMIT ARUS 3 : ");
  lcd.setCursor(6,2);
  lcd.write(1);
  lcd.setCursor(8,2);
  lcd.print(aCount3);
  lcd.setCursor(9,2);
  lcd.print(aCount33);
  lcd.setCursor(11,2);
  lcd.print("A");
  delay(150);
}

//MENU 4

void menu4(){
  lcd.clear();
  lcd.setCursor(4,0);
  lcd.print("Frequency : ");
  lcd.setCursor(5,2);
  ptr_val0 = (byte *)&f_freq;
  EEPROM.write(1, *ptr_val0++);
  EEPROM.write(2, *ptr_val0++);
  EEPROM.write(3, *ptr_val0++);
  EEPROM.write(4, *ptr_val0);

  ptr_val0 = (byte *)&value0;
  *ptr_val0++ = EEPROM.read(1);
  *ptr_val0++ = EEPROM.read(2);
  *ptr_val0++ = EEPROM.read(3);
  *ptr_val0 = EEPROM.read(4);

  lcd.print(value0);
  frekuensi = String(value0, 1);
  lcd.setCursor(12,2);
  lcd.print("MHZ");
  delay(150);

  if(frekuensi == batasFrekuensi[0]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[1]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[2]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[3]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[4]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[5]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[6]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, LOW);
  }
  else if(frekuensi == batasFrekuensi[7]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[8]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[9]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, LOW);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[10]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, LOW);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[11]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[12]){
    digitalWrite(led1, HIGH);
    digitalWrite(led2, LOW);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, HIGH);
  }
  else if(frekuensi == batasFrekuensi[13]){
    digitalWrite(led1, LOW);
    digitalWrite(led2, HIGH);
    digitalWrite(led3, HIGH);
    digitalWrite(led4, HIGH);
  }
  else{
    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    digitalWrite(led4, LOW);
  }
}

//MENU 4 SETUP
void freqSetup(){
  lcd.clear();
  lcd.setCursor(3,0);
  lcd.print("Set Frequency : ");
  lcd.setCursor(4,2);
  lcd.write(1);
  lcd.setCursor(6,2);
  lcd.print(freq1);
  lcd.setCursor(7,2);
  lcd.print(freq2);
  lcd.setCursor(8,2);
  lcd.print(freq3);
  lcd.setCursor(9,2);
  lcd.print(".");
  lcd.setCursor(10,2);
  lcd.print(freq4);
  lcd.setCursor(13,2);
  lcd.print("MHZ");
  delay(150);
}

void (*menuSelection[])(void) = {
  menu1,
  menu2,
  menu3,
  menu4
};

void menu(){
  menuSelection[menuCounter]();
  if(right == LOW && buttonState == 0){
    menuCounter++;
    if(menuCounter > 3){
      menuCounter = 0;
    }
  }
  if(left == LOW && buttonState == 0){
    menuCounter--;
    if(menuCounter < 0){
      menuCounter = 3;
    }
  }
  //MENU 1 SETUP
  if(mode == 1){
    if(enter == LOW && menuCounter == 0){
      enterState = 1;
      buttonState = 1;
      while(enterState == 1){
        lcd.setCursor(0,1);
        lcd.write(1);
        delay(150);
        menuSelection[0]();
        if(right == LOW && buttonState == 1){
          pCounter += 20;
          if(pCounter >= 1000){
            pCounter = 1000;
          }
        }
        if(left == LOW && buttonState == 1){
          pCounter -= 20;
          if(pCounter <= 0){
            pCounter = 0;
          }
        }
        if(enter == LOW && enterState == 1){
          enterState = 0;
          buttonState = 0;
          subCounter = 0;
          mode = 0;
          return menuSelection[0];
        }
      }
    }
    //MENU 2 SETUP
    if( enter == LOW && menuCounter == 1){
      enterState = 1;
      buttonState = 1;
      while(enterState == 1){
        menu2();
        switch(subCounter){
          case 0:
            lcd.setCursor(0,1);
            lcd.write(0);
            delay(150);
            break;
          case 1:
            lcd.setCursor(0,2);
            lcd.write(0);
            delay(150);
            break;
          case 2:
            lcd.setCursor(0,3);
            lcd.write(0);
            delay(150);
            break;
        }
        if(right == LOW && buttonState == 1){
          subCounter++;
          if(subCounter > 2){
            subCounter = 0;
          }
        }
        if(left == LOW && buttonState == 1){
          subCounter--;
          if(subCounter < 0){
            subCounter = 2;
          }
        }
        //TEMP 1
        if(subCounter == 0 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            tLim1();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              tCount1++;
              if(tCount1 > 9){
                tCount1 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              tCount1--;
              if(tCount1 < 0){
                tCount1 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                tLim1();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  tCount11++;
                  if(tCount11 > 9){
                    tCount11 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  tCount11--;
                  if(tCount11 < 0){
                    tCount11 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  tVal1 = 1;
                  return menuSelection[1];
                }
              }
            }
          }
        }
        //TEMP 2
        if(subCounter == 1 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            tLim2();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              tCount2++;
              if(tCount2 > 9){
                tCount2 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              tCount2--;
              if(tCount2 < 0){
                tCount2 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                tLim2();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  tCount22++;
                  if(tCount22 > 9){
                    tCount22 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  tCount22--;
                  if(tCount22 < 0){
                    tCount22 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  tVal2 = 1;
                  return menuSelection[1];
                }
              }
            }
          }
        }
        //TEMP 3
        if(subCounter == 2 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            tLim3();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              tCount3++;
              if(tCount3 > 9){
                tCount3 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              tCount3--;
              if(tCount3 < 0){
                tCount3 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                tLim3();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  tCount33++;
                  if(tCount33 > 9){
                    tCount33 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  tCount33--;
                  if(tCount33 < 0){
                    tCount33 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  tVal3 = 1;
                  return menuSelection[1];
                }
              }
            }
          }
        }
      }
    }
    //MENU 3 SETUP
    if( enter == LOW && menuCounter == 2){
      enterState = 1;
      buttonState = 1;
      while(enterState == 1){
        menu3();
        switch(subCounter){
          case 0:
            lcd.setCursor(0,1);
            lcd.write(0);
            delay(150);
            break;
          case 1:
            lcd.setCursor(0,2);
            lcd.write(0);
            delay(150);
            break;
          case 2:
            lcd.setCursor(0,3);
            lcd.write(0);
            delay(150);
            break;
        }
        if(right == LOW && buttonState == 1){
          subCounter++;
          if(subCounter > 2){
            subCounter = 0;
          }
        }
        if(left == LOW && buttonState == 1){
          subCounter--;
          if(subCounter < 0){
            subCounter = 2;
          }
        }
        //ARUS 1
        if(subCounter == 0 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            aLim1();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              aCount1++;
              if(aCount1 > 9){
                aCount1 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              aCount1--;
              if(aCount1 < 0){
                aCount1 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                aLim1();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  aCount11++;
                  if(aCount11 > 9){
                    aCount11 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  aCount11--;
                  if(aCount11 < 0){
                    aCount11 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  aVal1 = 1;
                  return menuSelection[2];
                }
              }
            }
          }
        }
        //ARUS 2
        if(subCounter == 1 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            aLim2();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              aCount2++;
              if(aCount2 > 9){
                aCount2 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              aCount2--;
              if(aCount2 < 0){
                aCount2 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                aLim2();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  aCount22++;
                  if(aCount22 > 9){
                    aCount22 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  aCount22--;
                  if(aCount22 < 0){
                    aCount22 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  aVal2 = 1;
                  return menuSelection[2];
                }
              }
            }
          }
        }
        //ARUS 3
        if(subCounter == 2 && enter == LOW){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            aLim3();
            lcd.setCursor(8,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              aCount3++;
              if(aCount3 > 9){
                aCount3 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              aCount3--;
              if(aCount3 < 0){
                aCount3 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                aLim3();
                lcd.setCursor(9,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  aCount33++;
                  if(aCount33 > 9){
                    aCount33 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  aCount33--;
                  if(aCount33 < 0){
                    aCount33 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 0;
                  buttonState = 0;
                  subCounter = 0;
                  aVal3 = 1;
                  return menuSelection[2];
                }
              }
            }
          }
        }
      }
    }
  }
    //MENU 4
    if(enter == LOW && menuCounter == 3){
      enterState = 1;
      buttonState = 1;
      while(enterState == 1){
        bgProses();
        freqSetup();
        lcd.setCursor(6,3);
        lcd.write(3);
        delay(150);
        if(right == LOW && buttonState == 1){
          freq1++;
          if(freq1 >= 1){
            freq1 = 1;
          }
        }
        if(left == LOW && buttonState == 1){
          freq1--;
          if(freq1 <= 0){
            freq1 = 0;
          }
        }
        if(enter == LOW && enterState == 1){
          enterState = 2;
          buttonState = 2;
          while(enterState == 2){
            bgProses();
            freqSetup();
            lcd.setCursor(7,3);
            lcd.write(3);
            delay(150);
            if(right == LOW && buttonState == 2){
              freq2++;
              if(freq2 > 9){
                freq2 = 0;
              }
            }
            if(left == LOW && buttonState == 2){
              freq2--;
              if(freq2 < 0){
                freq2 = 9;
              }
            }
            if(enter == LOW && enterState == 2){
              enterState = 3;
              buttonState = 3;
              while(enterState == 3){
                bgProses();
                freqSetup();
                lcd.setCursor(8,3);
                lcd.write(3);
                delay(150);
                if(right == LOW && buttonState == 3){
                  freq3++;
                  if(freq3 > 9){
                    freq3 = 0;
                  }
                }
                if(left == LOW && buttonState == 3){
                  freq3--;
                  if(freq3 < 0){
                    freq3 = 9;
                  }
                }
                if(enter == LOW && enterState == 3){
                  enterState = 4;
                  buttonState = 4;
                  while(enterState == 4){
                    bgProses();
                    freqSetup();
                    lcd.setCursor(10,3);
                    lcd.write(3);
                    delay(150);
                    if(right == LOW && buttonState == 4){
                      freq4++;
                      if(freq4 > 9){
                        freq4 = 0;
                      }
                    }
                    if(left == LOW && buttonState == 4){
                      freq4--;
                      if(freq4 < 0){
                        freq4 = 9;
                      }
                    }
                    if(enter == LOW && enterState == 4){
                      enterState = 0;
                      buttonState = 0;
                      return menuSelection[3];
                    }
                  }
                }
              }
            }
          }
        }
      }
    }

    // MODE 0

    if(mode == 0){
    if(enter == LOW && menuCounter == 0){
      enterState = 1;
      buttonState = 1;
      while(enterState == 1){
        pwmMonitor();
        switch(subCounter){
          case 0:
            lcd.setCursor(0,2);
            lcd.write(0);
            delay(150);
            break;
          case 1:
            lcd.setCursor(0,3);
            lcd.write(0);
            delay(150);
            break;
        }
        if(right == LOW && buttonState == 1){
          subCounter++;
          if(subCounter > 1){
            subCounter = 0;
          }
        }
        if(left == LOW && buttonState == 1){
          subCounter--;
          if(subCounter < 0){
            subCounter = 1;
          }
        }
        if(subCounter == 0 && enter == LOW){
          enterState = 0;
          buttonState = 0;
          subCounter = 0;
          mode = 1;
          return menuSelection[0];
        }
        if(subCounter == 1 && enter == LOW){
          enterState = 0;
          buttonState = 0;
          subCounter = 0;
          mode = 0;
          return menuSelection[0];
        }
      }
    }
  }
}


void loop(){
  menu();
}