#include <Wire.h>
//#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>
#include "ACS712.h"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define BATTERY_PIN A0
#define BATTERY_PIN2 A2
#define BATTERY_PIN3 A6

ACS712  ACS(A1, 5.0, 1023, 100);
ACS712  ACS2(A3, 5.0, 1023, 100);
ACS712  ACS3(A7, 5.0, 1023, 100);

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
static const unsigned char PROGMEM image_ButtonRight_4x7_bits[] = {0x80,0xc0,0xe0,0xf0,0xe0,0xc0,0x80};
//static const unsigned char PROGMEM image_ButtonLeft_4x7_bits[] = {0x10,0x30,0x70,0xf0,0x70,0x30,0x10};

int previousBatteryLevel = 0;
int previousBatteryLevel2 = 0;
int previousBatteryLevel3 = 0;

int batteryLevel;
int batteryLevel2;
int batteryLevel3;

int batteryPercentage; 
int batteryPercentage2;
int batteryPercentage3; 

float Am;
float Am2;
float Am3;

float mA;
float mA2;
float mA3;

float maxv;
float minv;
float minvc;

float batchg;
float batchg2;
float batchg3;
int maxvanalog;
int minvanalog;
float voltage1;
float voltage2;
float voltage3;

float maxc;

const int blinkInterval = 1000;
bool isBlinkVisible = true;
unsigned long lastBlinkMillis = 0;

const int blinkInterval2 = 1000;
bool isBlinkVisible2 = true;
unsigned long lastBlinkMillis2 = 0;

const int blinkInterval3 = 1000;
bool isBlinkVisible3 = true;
unsigned long lastBlinkMillis3 = 0;
int intervalepp;
int interval;
bool issensorVisible = true;
unsigned long lastsensorMillis = 0;

int intervaleppA;
int intervalA;
bool issensorVisibleA = true;
unsigned long lastsensorMillisA = 0;



const byte dnKey = 11; 
const byte upKey = 12; 
const byte selKey = 10; // pins for buttons up, dn, back, select
  byte page1 = 0;   //Main Menu page switch case  // Menu Options i.e Reverse, Borders, Elevon, Servo Trim, Battery & Factory Reset Transmitter
  byte page2 = 0;   //Reverse menu switch case  // Channel Reverse function To Opposite movement of servo 
  byte page3 = 0;   //Boarders menu switch case  // set the PWM maximun & minimum // Servo max-min movement
  byte page4 = 0;   //Elevon menu switch case // Elevon menu is for Mixing Roll & Pitch for deltaWings
   //Max Amp menus status. // 0 = not selected & 1 = selected.
  bool menuStatus = 0; 
  bool borderStatus = 0; 
  bool settingStatus = 0; 
  bool offsetStatus = 0; 
  bool offsetStatus1 = 0; 
  bool offsetStatus2 = 0;
  bool offsetStatus3 = 0;
  bool offsetStatus4 = 0;
  bool resetStatus = 0; //Max Amp menus/sub menu status for entering concern menu while loop      
   // Channels Border Set.
  
  //bool settingMix = false;       // Elevon mode status. //True = Activated,
  float setv;         // Roll+Pitch mix offset in %, default 50%, 
  float setv2;
  float seta;
  int sett;
  int settA;
  bool latchc = 0;
  bool latchc2 = 0;
  bool latchc3 = 0;
  //float calibration=0.1039;

void setup() {
  //Serial.begin(115200);
  pinMode(BATTERY_PIN, INPUT);
  pinMode(BATTERY_PIN2, INPUT);
  pinMode(BATTERY_PIN3, INPUT);
  //pinMode(13, INPUT_PULLUP);  //select li-ion/nimh
  pinMode(2, OUTPUT); 
  pinMode(3, OUTPUT); 
  pinMode(4, OUTPUT);       
  pinMode(5, OUTPUT);     
  pinMode(6, OUTPUT); 
  pinMode(7, OUTPUT);       
  pinMode(8, OUTPUT);      
  pinMode(9, OUTPUT); 
  pinMode(13, OUTPUT);       
  pinMode(dnKey, INPUT_PULLUP);
  pinMode(upKey, INPUT_PULLUP);
  //pinMode(bacKey, INPUT_PULLUP);
  pinMode(selKey, INPUT_PULLUP) ; 
  //analogReference(INTERNAL);        
  ACS.autoMidPoint();
  ACS2.autoMidPoint();
  ACS3.autoMidPoint();

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  delay(2000);
  display.clearDisplay();  // Clear the display on startup
  display.display();       // Show the cleared display
}
void loop() {
    sensor();
    itungan();
    mainPage();
    checkSettingStatus();
    while(settingStatus==1)
    {                 //sub menu Elevon Page //menuPage/settingPage
    sensor();
    itungan();
     settingCheck();
     updateSetting();
     checkSettingStatus();
     checkOffsetStatus();
     settingkembali();
     while(offsetStatus==1){             //menuPage/eleronPage/offsetPage         
      selecPage();
      checkOffsetStatus();
     }
     while(offsetStatus1==1){              //menuPage/eleronPage/offsetPage         
      selecPage1();
      checkOffsetStatus();
     }
     while(offsetStatus2==1){              //menuPage/eleronPage/offsetPage         
      selecPage2();
      checkOffsetStatus();
     }
     while(offsetStatus3==1){              //menuPage/eleronPage/offsetPage         
      selecPage3();
      checkOffsetStatus();
     }
     while(offsetStatus4==1){              //menuPage/eleronPage/offsetPage         
      selecPage4();
      checkOffsetStatus();
     }
    }
}
void sensor(){
  const int sensorInterval = interval;
  if (millis() - lastsensorMillis >= sensorInterval)
      {
        batteryLevel = analogRead(BATTERY_PIN)-183.1;
        batteryLevel2 = analogRead(BATTERY_PIN2)-183.1;
        batteryLevel3 = analogRead(BATTERY_PIN3)-183.1;
        lastsensorMillis=millis();
      }

  const int sensorIntervalA = intervalA;
  if (millis() - lastsensorMillisA >= sensorIntervalA)
      {
        Am = ACS.mA_DC();
         if (Am<0){
            mA=((Am/1000)*(-1));
             }
        else{
            mA=Am/1000;
           }
        Am2 = ACS2.mA_DC();
         if (Am2<0){
            mA2=((Am2/1000)*(-1));
           }
        else{
           mA2=Am2/1000;
         }
        Am3 = ACS3.mA_DC();
         if (Am3<0){
            mA3=((Am3/1000)*(-1));
             }
        else{
            mA3=Am3/1000;
           }
         
        lastsensorMillisA=millis();
      }
      
}
void itungan(){
  EEPROM.get(1, maxv); 
  EEPROM.get(10, minvc);
  EEPROM.get(20, intervalepp);
  EEPROM.get(30, maxc);
  EEPROM.get(40, intervaleppA);

  setv=maxv;
  setv2=minvc;
  interval=intervalepp*1000;
  sett=intervalepp;
  intervalA=intervaleppA*1000;
  settA=intervaleppA;
  seta=maxc;
  minv=minvc;
  
  batchg=0.075;
  maxvanalog= ((1023/5)*maxv);
  minvanalog= ((1023/5)*minv);
  //batery 1
  //batteryLevel = analogRead(BATTERY_PIN);
  batteryPercentage = map(batteryLevel, minvanalog, maxvanalog, 0, 100);
  voltage1 = batteryLevel/204.6;
  //mA = ACS.mA_DC()/1000;
    
  if (mA>=maxc){
    latchc=1;
  }
  if (latchc==0)
  {
  if ((voltage1 >=minvc) && (voltage1<= maxv))
    {
    digitalWrite(9, HIGH);
    digitalWrite(13, LOW);
    if (mA>=batchg)
    {
    //digitalWrite(8, HIGH);
       if (millis() - lastBlinkMillis >= blinkInterval)
      {
        isBlinkVisible = !isBlinkVisible;
        lastBlinkMillis = millis();
      }

         if (isBlinkVisible)
      {
        digitalWrite(8, HIGH);
      }
         else
      {
        digitalWrite(8, LOW);
      }
    }
    else
    {
     digitalWrite(8, LOW);
     }
    }
  else
    {
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    digitalWrite(13, HIGH);
    }
  }
  else{
    digitalWrite(8, HIGH);
    digitalWrite(9, LOW);
    digitalWrite(13, HIGH);
  }
    //batery 2
    //batteryLevel2 = analogRead(BATTERY_PIN2);
    batteryPercentage2 = map(batteryLevel2, minvanalog, maxvanalog, 0, 100);
    voltage2 = batteryLevel2/204.6;
   // mA2 = ACS2.mA_DC()/1000;
    
   if (mA2>=maxc){
    
    latchc2=1;
  }
  if (latchc2==0){
  if ((voltage2 >=minvc) && (voltage2<= maxv))
    {
    digitalWrite(6, HIGH);
    digitalWrite(7, LOW);
    if (mA2>=batchg)
    {
    if (millis() - lastBlinkMillis2 >= blinkInterval2)
      {
        isBlinkVisible2 = !isBlinkVisible2;
        lastBlinkMillis2 = millis();
      }

      if (isBlinkVisible2)
      {
        digitalWrite(5, HIGH);
      }
      else
      {
        digitalWrite(5, LOW);
      }
    }
  else
  {
     digitalWrite(5, LOW);
  }
    }
  else
    {
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    }
  }
  else{
    digitalWrite(5, HIGH);
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
  }
  //batteryLevel3 = analogRead(BATTERY_PIN3);
  batteryPercentage3 = map(batteryLevel3, minvanalog, maxvanalog, 0, 100);
  voltage3 = batteryLevel3/204.6;
  //mA3 = ACS3.mA_DC()/1000;
  
  if (mA3>=maxc){
    latchc3=1;
  }
  if (latchc3==0){

    if ((voltage3 >=minvc) && (voltage3<= maxv))
      {
      digitalWrite(3, HIGH);
      digitalWrite(4, LOW);
      if (mA3>=batchg)
        {
          digitalWrite(2, HIGH);
          if (millis() - lastBlinkMillis3 >= blinkInterval3)
          {
          isBlinkVisible3 = !isBlinkVisible3;
          lastBlinkMillis3 = millis();
          }

            if (isBlinkVisible3)
            {
            digitalWrite(2, HIGH);
            }
           else
           {
            digitalWrite(2, LOW);
            }
         }
     else
      {
     digitalWrite(2, LOW);
      }
     }
  else
    {
    digitalWrite(2, LOW);
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH);
    }
  }
  else
    {
    digitalWrite(2, HIGH);
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH);
    }

}
void mainPage(){

  display.clearDisplay();  // Clear the display before updating
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.drawRect(0, 0, 128, 64, SSD1306_WHITE);
  display.drawLine(43, 0, 43, 64, SSD1306_WHITE);
  display.drawLine(85, 0, 85, 64, SSD1306_WHITE);
 
  //batery 1
  if (voltage1<=0){
  display.setCursor(4, 45);
  display.setTextSize(.5);
  //display.print(voltage1);
  display.print("0.00 V");
  }
  else{
  display.setCursor(4, 45);
  display.setTextSize(.5);
  display.print(voltage1);
  display.print(" V");
  }

  display.setCursor(4, 55);
  display.print(mA);
  display.print(" A");
  if (latchc==1){
     display.setCursor(10, 10);
     display.print("OCR!");
     display.setCursor(8, 20);
     display.print("Need");
     display.setCursor(5, 30);
     display.print("Reset!");
     }
  else{
  int batteryHeight = map(batteryPercentage, 0, 100, 0, 18);
  int batteryX = SCREEN_WIDTH - 113;
  int batteryY = 20;
  int batteryWidth = 13;
  int batteryTopHeight = 2;
  int batteryBottomHeight = 20;

  display.drawRect(batteryX, batteryY + batteryTopHeight, batteryWidth, batteryBottomHeight, SSD1306_WHITE);
  int batteryFillHeight = map(batteryPercentage, 0, 100, 0, batteryBottomHeight);
  batteryFillHeight = min(batteryFillHeight, batteryBottomHeight);
  display.fillRect(batteryX + 1, batteryY + batteryTopHeight + (batteryBottomHeight - batteryFillHeight), batteryWidth - 2, batteryFillHeight, SSD1306_WHITE);
  
  
  if (batteryPercentage>=1 && batteryPercentage<=100){
    display.setCursor(13, 13);
    display.print(batteryPercentage);
    display.print("%");
    }
  else{
    if (batteryPercentage>=101){
    display.setCursor(13, 13);
    display.print("100%");
    }
    if (batteryPercentage<=0){
    display.setCursor(13, 13);
    display.print("0%");
    }
  }
  }
  
  //batery 2
  if (voltage2<=0){
  display.setCursor(48, 45);
  //display.print(voltage2);
  display.print("0.00 V");
  }
  else{
  display.setCursor(48, 45);
  display.print(voltage2);
  display.print(" V");
  }

  display.setCursor(48, 55);
  display.print(mA2);
  display.print(" A");

  if (latchc2==1){
     display.setCursor(54, 10);
     display.print("OCR!");
     display.setCursor(52, 20);
     display.print("Need");
     display.setCursor(49, 30);
     display.print("Reset!");
     }

  else{
  int batteryHeight2 = map(batteryPercentage2, 0, 100, 0, 18);
  int batteryX2 = SCREEN_WIDTH - 70;
  int batteryY2 = 20;
  int batteryWidth2 = 13;
  int batteryTopHeight2 = 2;
  int batteryBottomHeight2 = 20;

  display.drawRect(batteryX2, batteryY2 + batteryTopHeight2, batteryWidth2, batteryBottomHeight2, SSD1306_WHITE);
  int batteryFillHeight2 = map(batteryPercentage2, 0, 100, 0, batteryBottomHeight2);
  batteryFillHeight2 = min(batteryFillHeight2, batteryBottomHeight2);
  display.fillRect(batteryX2 + 1, batteryY2 + batteryTopHeight2 + (batteryBottomHeight2 - batteryFillHeight2), batteryWidth2 - 2, batteryFillHeight2, SSD1306_WHITE);
  
  if (batteryPercentage2>=1 && batteryPercentage2<=100){
    display.setCursor(58, 13);
    display.print(batteryPercentage2);
    display.print("%");
    }
  else{
    if (batteryPercentage2>=101){
    display.setCursor(58, 13);
    display.print("100%");
    }
    if (batteryPercentage2<=0){
    display.setCursor(58, 13);
    display.print("0%");
    }
  }
  }

  if (voltage3<=0){
  display.setCursor(90, 45);
  //display.print(voltage3);
  display.print("0.00 V");
  }
  else{
  display.setCursor(90, 45);
  display.print(voltage3);
  display.print(" V");
  }
  display.setCursor(90, 55);
  display.print(mA3);
  display.print(" A");
  
  if (latchc3==1){
     display.setCursor(96, 10);
     display.print("OCR!");
     display.setCursor(94, 20);
     display.print("Need");
     display.setCursor(91, 30);
     display.print("Reset!");
     }
  else{
  int batteryHeight3 = map(batteryPercentage3, 0, 100, 0, 18);
  int batteryX3 = SCREEN_WIDTH - 27;
  int batteryY3 = 20;
  int batteryWidth3 = 13;
  int batteryTopHeight3 = 2;
  int batteryBottomHeight3 = 20;

  display.drawRect(batteryX3, batteryY3 + batteryTopHeight3, batteryWidth3, batteryBottomHeight3, SSD1306_WHITE);
  int batteryFillHeight3 = map(batteryPercentage3, 0, 100, 0, batteryBottomHeight3);
  batteryFillHeight3 = min(batteryFillHeight3, batteryBottomHeight3);
  display.fillRect(batteryX3 + 1, batteryY3 + batteryTopHeight3 + (batteryBottomHeight3 - batteryFillHeight3), batteryWidth3 - 2, batteryFillHeight3, SSD1306_WHITE);

   if (batteryPercentage3>=1 && batteryPercentage3<=100){
    display.setCursor(100, 13);
    display.print(batteryPercentage3);
    display.print("%");
    }
  else{
    if (batteryPercentage3>=101){
    display.setCursor(100, 13);
    display.print("100%");
    }
    if (batteryPercentage3<=0){
    display.setCursor(100, 13);
    display.print("0%");
    }
  }
  }
  display.display();  // Show the updated display
  previousBatteryLevel = batteryLevel;
  //delay(100);
}
void settingCheck(){
 if (!digitalRead(upKey)){       //When Button is Pressed change screen page 
    page4--;
    while (!digitalRead(upKey));
  }
  if (!digitalRead(dnKey)){       //When Button is Pressed change screen page 
    page4++;
    while (!digitalRead(dnKey));
  }
}
void updateSetting(){
 switch(page4){
    case 0:
    page4=1;
      break;
    case 1:
    settingPage();
      break;
    case 2:
    offsetPage1();
      break;
    case 3:
    offsetPage2();
      break;
    case 4:
    offsetPage3();
      break;
    case 5:
    offsetPage4();
      break;
    case 6:
    offsetPage5();
      break;
    case 7:
    settingPage();
    page4=1;
      break;
 }
}
void checkSettingStatus(){
  if (!digitalRead(selKey) && settingStatus==0){
      settingPage();
      //settingPage2();
      settingStatus=1;
      while (!digitalRead(selKey));
      }
  
} 
void checkOffsetStatus(){
 if(page4==1){
  if (!digitalRead(selKey) && settingStatus==1){
      //offsetPage();
      offsetStatus=1;
      while (!digitalRead(selKey));
      }
    }
 if(page4==2){
  if (!digitalRead(selKey) && settingStatus==1){
      //offsetPage();
      offsetStatus1=1;
      while (!digitalRead(selKey));
      }
    }
  if(page4==3){
  if (!digitalRead(selKey) && settingStatus==1){
      //offsetPage();
      offsetStatus2=1;
      while (!digitalRead(selKey));
      }
    }
  if(page4==4){
  if (!digitalRead(selKey) && settingStatus==1){
      //offsetPage();
      offsetStatus3=1;
      while (!digitalRead(selKey));
      }
    }
  if(page4==5){
  if (!digitalRead(selKey) && settingStatus==1){
      //offsetPage();
      offsetStatus4=1;
      while (!digitalRead(selKey));
      }
    }
}
void settingPage(){
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");
 display.setCursor(80, 8);
 display.print(setv);
 display.setCursor(110, 8);
 display.print("V");
 display.setCursor(80, 29);
 display.print(setv2);
 display.setCursor(110, 29);
 display.print("V"); 
 display.setCursor(80, 50);
 display.print(sett); 
 display.setCursor(110, 50);
 display.print("S"); 
 display.drawBitmap(4, 6, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void offsetPage1(){
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");
 display.setCursor(80, 8);
 display.print(setv);
 display.setCursor(110, 8);
 display.print("V");
 display.setCursor(80, 29);
 display.print(setv2);
 display.setCursor(110, 29);
 display.print("V"); 
 display.setCursor(80, 50);
 display.print(sett); 
 display.setCursor(110, 50);
 display.print("S"); 
 display.drawBitmap(4, 27, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void offsetPage2(){
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");
 display.setCursor(80, 8);
 display.print(setv);
 display.setCursor(110, 8);
 display.print("V");
 display.setCursor(80, 29);
 display.print(setv2);
 display.setCursor(110, 29);
 display.print("V"); 
 display.setCursor(80, 50);
 display.print(sett); 
 display.setCursor(110, 50);
 display.print("S"); 
 display.drawBitmap(4, 48, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void offsetPage3(){
display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 108, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.setCursor(15, 8);
 display.print("Max Amp");
 display.setCursor(15, 29);
 display.print("Time Amp"); 
 display.setCursor(15, 50);
 display.print("Save & Exit"); 
 display.setCursor(80, 8);
 display.print(seta);
 display.setCursor(110, 8);
 display.print("A");
 display.setCursor(80, 29);
 display.print(settA);
 display.setCursor(110, 29);
 display.print("S"); 
 display.drawBitmap(4, 6, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void offsetPage4(){
display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 108, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.setCursor(15, 8);
 display.print("Max Amp");
 display.setCursor(15, 29);
 display.print("Time Amp"); 
 display.setCursor(15, 50);
 display.print("Save & Exit"); 
 display.setCursor(80, 8);
 display.print(seta);
 display.setCursor(110, 8);
 display.print("A");
 display.setCursor(80, 29);
 display.print(settA);
 display.setCursor(110, 29);
 display.print("S"); 
 display.drawBitmap(4, 27, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void offsetPage5(){
display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 108, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.setCursor(15, 8);
 display.print("Max Amp");
 display.setCursor(15, 29);
 display.print("Time Amp"); 
 display.setCursor(15, 50);
 display.print("Save & Exit"); 
 display.setCursor(80, 8);
 display.print(seta);
 display.setCursor(110, 8);
 display.print("A");
 display.setCursor(80, 29);
 display.print(settA);
 display.setCursor(110, 29);
 display.print("S"); 
 display.drawBitmap(4, 48, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.display();
}
void selecPage(){
  if (isnan(setv))
  {
    setv=1;
    EEPROM.put(1, setv);
  }

 else{
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");; 
 display.drawBitmap(4, 6, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.setCursor(80, 29);
 display.print(setv2);
 display.setCursor(110, 29);
 display.print("V");
 display.setCursor(80, 50);
 display.print(sett); 
 display.setCursor(110, 50);
 display.print("S"); 
 setv = constrain(setv,1,20);
 display.setCursor(80, 8);
 display.setTextWrap(false);
 if(!digitalRead(upKey)) {
   setv = setv+0.05;
 display.print(setv);
 while (!digitalRead(upKey));
  }else if(!digitalRead(dnKey)){
   setv = setv-0.05;
   display.print(setv);
 while (!digitalRead(upKey));
  }else{
   display.print(setv);
  }
 
 display.setCursor(110, 8);
 display.setTextWrap(false);
 display.print("V");
 display.display();
 if (!digitalRead(selKey) && offsetStatus==1){
      //settingPage();
      page4=1;
      //settingPage2();
      EEPROM.put(1, setv);
      //EEPROM.commit();
      offsetStatus=0;
      while (!digitalRead(selKey));
      }
 }
}
void selecPage1(){
  if (isnan(setv2))
  {
    setv2=1;
    EEPROM.put(10, setv2);
  }

 else{
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");
 display.drawBitmap(4, 27, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.setCursor(80, 8);
 display.print(setv);
 display.setCursor(110, 29);
 display.print("V");
 display.setCursor(80, 50);
 display.print(sett); 
 display.setCursor(110, 50);
 display.print("S"); 
 setv2 = constrain(setv2,0.05,20);
 display.setCursor(80, 29);
 display.setTextWrap(false);
 if(!digitalRead(upKey)) {
   setv2 = setv2+0.05;
 display.print(setv2);
 while (!digitalRead(upKey));
  }else if(!digitalRead(dnKey)){
   setv2 = setv2-0.05;
   display.print(setv2);
 while (!digitalRead(upKey));
  }else{
   display.print(setv2);
  }
 
 display.setCursor(110, 8);
 display.setTextWrap(false);
 display.print("V");
 display.display();
 if (!digitalRead(selKey) && offsetStatus1==1){
      //settingPage();
      page4=2;
      //settingPage2();
      EEPROM.put(10, setv2);
      //EEPROM.commit();
      offsetStatus1=0;
      while (!digitalRead(selKey));
      }
 }
}
void selecPage2(){
  if (isnan(sett))
  {
    sett=1;
    EEPROM.put(20, sett);
  }

 else{
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 60, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.drawRect(75, 45, 45, 17, 1);
 display.setTextColor(1);
 display.setTextSize(1);
 display.setCursor(15, 8);
 display.print("Max Volt");
 display.setCursor(15, 29);
 display.print("Min Volt"); 
 display.setCursor(15, 50);
 display.print("Time Volt");; 
 display.drawBitmap(4, 48, image_ButtonRight_4x7_bits, 4, 7, 1);
 display.setCursor(80, 29);
 display.print(setv2);
 display.setCursor(110, 29);
 display.print("V");
 display.setCursor(80, 8);
 display.print(setv); 
 display.setCursor(110, 8);
 display.print("V"); 
 sett = constrain(sett,0,20);
 display.setCursor(80, 50);
 display.setTextWrap(false);
 if(!digitalRead(upKey)) {
   sett = sett+1;
 display.print(sett);
 while (!digitalRead(upKey));
  }else if(!digitalRead(dnKey)){
   sett = sett-1;
   display.print(sett);
 while (!digitalRead(upKey));
  }else{
   display.print(sett);
  }
 
 display.setCursor(110, 50);
 display.setTextWrap(false);
 display.print("S");
 display.display();
 if (!digitalRead(selKey) && offsetStatus2==1){
      //settingPage();
      page4=3;
      //settingPage2();
      EEPROM.put(20, sett);
      //EEPROM.commit();
      offsetStatus2=0;
      while (!digitalRead(selKey));
      }
 }
}
void selecPage3(){
 if (isnan(seta))
  {
    seta=0.5;
    EEPROM.put(30, seta);
  }

 else{
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 108, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.setCursor(15, 8);
 display.print("Max Amp");
 display.setCursor(15, 29);
 display.print("Time Amp"); 
 display.setCursor(15, 50);
 display.print("Save & Exit"); 
 //display.setCursor(80, 8);
 //display.print(seta);
 //display.setCursor(110, 8);
 //display.print("A");
 display.setCursor(80, 29);
 display.print(settA);
 display.setCursor(110, 29);
 display.print("S"); 
 display.drawBitmap(4, 6, image_ButtonRight_4x7_bits, 4, 7, 1);
 seta = constrain(seta,0.1,50);
 display.setCursor(80, 8);
 display.setTextWrap(false);
 if(!digitalRead(upKey)) {
   seta = seta+0.05;
 display.print(seta);
 while (!digitalRead(upKey));
  }else if(!digitalRead(dnKey)){
   seta = seta-0.05;
   display.print(seta);
 while (!digitalRead(upKey));
  }else{
   display.print(seta);
  }
 display.setCursor(110, 8);
 display.setTextWrap(false);
 display.print("A");
 display.display();
 if (!digitalRead(selKey) && offsetStatus3==1){
      //settingPage();
      page4=4;
      //settingPage2();
      offsetStatus3=0;
      EEPROM.put(30, seta);
      //EEPROM.commit();
      while (!digitalRead(selKey));
      }
 }
}
void selecPage4(){
 if (isnan(settA))
  {
    settA=1;
    EEPROM.put(40, settA);
  }

 else{
 display.clearDisplay();
 display.setTextColor(1);
 display.setTextSize(1);
 display.drawRect(12, 3, 60, 17, 1);
 display.drawRect(12, 24, 60, 17, 1);
 display.drawRect(12, 45, 108, 17, 1);
 display.drawRect(75, 3, 45, 17, 1);
 display.drawRect(75, 24, 45, 17, 1);
 display.setCursor(15, 8);
 display.print("Max Amp");
 display.setCursor(15, 29);
 display.print("Time Amp"); 
 display.setCursor(15, 50);
 display.print("Save & Exit"); 
 display.setCursor(80, 8);
 display.print(seta);
 display.setCursor(110, 8);
 display.print("A");
 //display.setCursor(80, 29);
 //display.print(settA);
 //display.setCursor(110, 29);
 //display.print("S"); 
 display.drawBitmap(4, 27, image_ButtonRight_4x7_bits, 4, 7, 1);
 settA = constrain(settA,0,20);
 display.setCursor(80, 29);
 display.setTextWrap(false);
 if(!digitalRead(upKey)) {
   settA = settA+1;
 display.print(settA);
 while (!digitalRead(upKey));
  }else if(!digitalRead(dnKey)){
   settA = settA-1;
   display.print(settA);
 while (!digitalRead(upKey));
  }else{
   display.print(settA);
  }
 display.setCursor(110, 29);
 display.setTextWrap(false);
 display.print("S");
 display.display();
 if (!digitalRead(selKey) && offsetStatus4==1){
      //settingPage();
      page4=5;
      //settingPage2();
      offsetStatus4=0;
      EEPROM.put(40, settA);
      //EEPROM.commit();
      while (!digitalRead(selKey));
      }
 }
}
void settingkembali(){
 if(page4==6)
 {
  if (!digitalRead(selKey))
  {
    mainPage();
    settingStatus=0;
    latchc=0;
    latchc2=0;
    latchc3=0;
    page4=1;
   // page1=1;
    while (!digitalRead(selKey));
  }
 }
}