#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#include <Key.h>
#include <Keypad.h>
const byte ROWS = 4; // Four rows
const byte COLS = 4; // 4 columns
// Define the Keymap
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
byte rowPins[ROWS] = { 12, 11, 10, 9 };
// Connect keypad COL0, COL1 and COL2 to these Arduino pins.
byte colPins[COLS] = { 8, 7, 6, 5 }; 

// Create the Keypad
Keypad kpd = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );

//decoders

#define enc1Clk 2
#define enc2Clk 3
#define enc1Da A3
#define enc2Da A0
#define enc1Sw A1
#define enc2Sw A2
int counter1=0;
int counter2=0;
int counter1oud=0;
int counter2oud=0;

int GPS=0;// 0=G500; 1=GTN750; 2=G1000

void setup() {
  Serial.begin(115200);   

  pinMode(enc1Clk, INPUT);
  pinMode(enc2Clk, INPUT);
  pinMode(enc1Da, INPUT);
  pinMode(enc2Da, INPUT);
  pinMode(enc1Sw, INPUT_PULLUP);
  pinMode(enc2Sw, INPUT_PULLUP);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.cp437(true);         // Use full 256 char 'Code Page 437' font
  display.display();
  G500txt();      // Draw characters G500

  attachInterrupt(digitalPinToInterrupt(enc1Clk), readEncoder1, FALLING);
  attachInterrupt(digitalPinToInterrupt(enc2Clk), readEncoder2, FALLING);
}

void readEncoder1() {
  int dtValue = digitalRead(enc1Da);
  if (dtValue == HIGH) {counter1++;} // Clockwise
  if (dtValue == LOW) {counter1--; }// Counterclockwise
}
void readEncoder2() {
  int dtValue = digitalRead(enc2Da);
  if (dtValue == HIGH) {counter2++;} // Clockwise
  if (dtValue == LOW) {counter2--; }// Counterclockwise
}


void loop() {
  if(abs(counter1)>10)counter1=0,counter1oud=0;
  if(abs(counter2)>10)counter2=0,counter2oud=0; 

  switch (GPS){
    case 0:
      if(counter1>counter1oud) Serial.println("G20"),Serial.println("@410/$"), counter1oud=counter1;//group_up
      if(counter1<counter1oud) Serial.println("G21"),Serial.println("@409/$"), counter1oud=counter1;//group_dn
      if(counter2>counter2oud) Serial.println("G22"),Serial.println("@418/$"), counter2oud=counter2;//page_up
      if(counter2<counter2oud) Serial.println("G23"),Serial.println("@417/$"), counter2oud=counter2;//page_dn
      GPS500key();
      if(!digitalRead(enc1Sw)){Serial.println("G19");Serial.println("@405/$");delay(500);} 
      if(!digitalRead(enc2Sw)){display.clearDisplay(); display.display();GPS++; GTN750txt();delay(500);}
      break;
    case 1:
      display.fillRect(0, 0, 64, 64, SSD1306_BLACK);
      display.setCursor(2, 2); display.println(counter1);    
      display.setCursor(2, 24); display.println(counter2);    
      display.display();
      GTN750key();
      if(!digitalRead(enc1Sw)){Serial.println("@10000/=L:GTN750_PANEL_DirectToPush;1$");}
      if(!digitalRead(enc2Sw)){display.clearDisplay(); display.display();GPS++; G1000txt(); delay(500);}
      break;
    case 2:
      G1000key();
      if(!digitalRead(enc2Sw)){display.clearDisplay();display.display();GPS=0; G500txt();delay(500);}
      break;
  }
}
  

void G500txt(void) {
  display.clearDisplay();
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(2, 2); display.println(F("NRST"));
  display.setCursor(2, 18); display.println(F("OBS"));
  display.setCursor(2, 34); display.println(F("MSG"));
  display.setCursor(2, 50); display.println(F("FPL"));
  display.setCursor(34, 2); display.println(F("TERR"));
  display.setCursor(34, 18); display.println(F("PROC"));
  display.setCursor(34, 34); display.println(F("ENT"));
  display.setCursor(34, 50); display.println(F("CLR"));
  display.setCursor(66, 2); display.println(F("MENU"));
  display.setCursor(66, 18); display.println(F("D->"));
  display.setCursor(98, 2); display.println(F("R_UP"));
  display.setCursor(98, 18); display.println(F("R_DN"));
  /*
  display.setCursor(66, 34); display.println(F("OR_U"));
  display.setCursor(66, 50); display.println(F("IR_U"));
  display.setCursor(98, 34); display.println(F("OR_D"));
  display.setCursor(98, 50); display.println(F("IR_D"));
  */
  display.setTextSize(2); 
  display.setCursor(70,40); display.println(F("G500"));  
  display.display();
}

void GTN750txt(void) {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE); // Draw white text 
  display.setTextSize(2);   
  display.setCursor(70, 40); display.println(F("G750"));
  /*
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setCursor(2, 2); display.println(F("HPu"));
  display.setCursor(2, 18); display.println(F("KPu"));
  display.setCursor(34, 2); display.println(F("HPuL"));
  display.setCursor(34, 18); display.println(F("KPuL"));
  display.setCursor(98, 2); display.println(F("D"));
  */
  display.display();
}

void G1000txt(void) {
  display.clearDisplay();
  //display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text 
  display.setTextSize(2);      // Normal 1:1 pixel scale
  display.setCursor(54, 40); display.println(F("G1000"));
  display.display();
}

void GPS500key(){
  char key = kpd.getKey();
  if(key){  // Check for a valid key.
    switch (key){  //keys B, 7 en * nog vrij
      case '1': //NRST
        Serial.println("G02"); Serial.println("@415/$");
        break;
      case '4': //OBS
        Serial.println("G03"); Serial.println("@416/$");
        break;
      case '7': //MSG
        Serial.println("G04"); Serial.println("@412/$");
        break;
      case '*': //FPL
        Serial.println("G07"); Serial.println("@408/$");
        break;
      case '2': //TERR
        Serial.println("G08"); Serial.println("@421/$");
        break;
      case '5': //PROC
        Serial.println("G09"); Serial.println("@420/$");
        break;
      case '8': //ENT
        Serial.println("G18"); Serial.println("@407/$");
        break;
      case '0': //CLR
        Serial.println("G15"); Serial.println("@401/$");
        break;
      case '3': //MENU
        Serial.println("G13"); Serial.println("@411/$");
        break;

      case '6': //D->
        Serial.println("G12"); Serial.println("@406/$");
        break;
      case 'A': //R_UP
        Serial.println("G11"); Serial.println("@423/$");
        break;
      case 'B': //R_DN
        Serial.println("G10"); Serial.println("@422/$");
        break;

      case 'C': //GROUP_UP
        Serial.println("G20"); Serial.println("@410/$");
        break;
      case 'D': //GROUP_DN
        Serial.println("G21"); Serial.println("@409/$");
        break;
      case '9': //PAGE_UP
        Serial.println("G22"); Serial.println("@418/$");
        break;
      case '#': //PAGE_DN
        Serial.println("G23"); Serial.println("@417/$");
        break;
     
    }
  }
}

void GTN750key(void){
char key = kpd.getKey();
/*
  if(key){  // Check for a valid key.
    switch (key){
      case'1':
        Serial.println("@10000/=L:GTN750_PANEL_HomePush;1$");
        break;
      case'2':
        Serial.println("@10000/=L:GTN750_PANEL_HomePushLong;1$");
        break;
      case'4':
        Serial.println("@10000/=L:GTN750_PANEL_KnobPush;1$");
        break;
      case'5':
        Serial.println("@10000/=L:GTN750_PANEL_KnobPushLong;1$");
        break;
      case'A':
        Serial.println("@10000/=L:GTN750_PANEL_DirectToPush;1$");
        break;
      case 'C': //GROUP_UP
        Serial.println("@10000/=L:GTN750_PANEL_KnobSmallInc;1$");
        break;
      case 'D': //GROUP_DN
        Serial.println("@10000/=L:GTN750_PANEL_KnobSmallDec;1$"); 
        break;
      case '9': //PAGE_UP
        Serial.println("@10000/=L:GTN750_PANEL_KnobLargeInc;1$"); 
        break;
      case '#': //PAGE_DN
        Serial.println("@10000/=L:GTN750_PANEL_KnobLargeDec;1$"); 
        break;
    }
  }
*/
}

void G1000key(void){

}