#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     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define BTN A0
#define BTN_SW A1
#define RELAY A2
#define CHN A3

bool first = true;
uint8_t USB[8];
uint8_t USB_prev[8];
float USB2[6] ={44.1, 48, 88.2, 96, 176.4, 192};
bool change_state = true;
bool _pushed = false; //initial state of the button is not pressed
bool _pushedopt = false; //for the relay switching function
bool coax = true; //Initially turn on the coaxial input
uint8_t channel = 2;  //switch board channel in use, can be 2-4. It must be set according the usage.
uint8_t prevChannel;

//int state = HIGH;     // the current state of the output pin
int reading;          // the current reading from the input pin
int previous = HIGH;   // the previous reading from the input pin
// the follow variables are long's because the time, measured in miliseconds,
// will quickly become a bigger number than can be stored in an int.
unsigned long time = 0;           // the last time the output pin was toggled
unsigned long debounce = 50UL;   // the debounce time, increase if the output flickers
unsigned long time2;  //for the relay switching function
unsigned long time3 = 5000UL;  //for the relay switching function

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

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
 
 //The display needs to be clear, because it displays the last output for sec if it doesn't made
  display.clearDisplay();
  display.display();

  pinMode(2, INPUT); //44,1kHZ
  pinMode(3, INPUT); //48kHZ
  pinMode(4, INPUT); //88,2kHZ
  pinMode(5, INPUT); //96kHZ
  pinMode(6, INPUT); //176,4kHZ
  pinMode(7, INPUT); //192kHZ
  pinMode(8, INPUT); //Host Active
  pinMode(9, INPUT); //Audio Streaming
  pinMode(10, INPUT); //I2S switch channel A
  pinMode(11, INPUT); //I2S switch channel B
  pinMode(12, INPUT); //I2S switch channel C
  pinMode(13, INPUT); //I2S switch channel D
  pinMode(BTN, INPUT);  //Push button
  pinMode(BTN_SW, OUTPUT);  //output to I2s Board button input
  digitalWrite(BTN_SW, HIGH);
  pinMode(RELAY, OUTPUT); //output to relay  for coax / opt changing
  pinMode(CHN, INPUT);  //channel number setting
}

void loop(){
  float freq = 0;
  uint8_t HA_state = 0;
  uint8_t AS_state = 0;

//Check the output frequency of the USB board
  for(int i = 2; i <=7; i++){
  USB[i-2] = digitalRead(i);
    if(USB[i-2]) freq = USB2[i-2];
    if(USB_prev[i-2] != USB[i-2]){
      USB_prev[i-2] = USB[i-2];
      change_state = true;
    }
  }
  
//Check the Host Active state
  USB[6] = digitalRead(8);
  if(USB[6]) HA_state = 1;
  if(USB_prev[6] != USB[6]){
    USB_prev[6] = USB[6];
    change_state = true;
  }

//Check the Audio Stream state
  USB[7] = digitalRead(9);
  if(USB[7]) AS_state = 1;
  if(USB_prev[7] != USB[7]){
    USB_prev[7] = USB[7];
    change_state = true;
  }

  //Check if the input switch button is pressed, then debouncing it.
  reading = digitalRead(BTN);
  delay(30);  //It is the real debounce
  if(reading == LOW && previous == HIGH && millis() - time > debounce){
    _pushed = true;
    change_state = true;
    Serial.print("pushed   ");
    time = millis();
  }
  previous = reading;

  //Checks if the optical output is really setted or just run through it,
  //and only use the relay if it is necessary
  if(_pushedopt && !coax && millis() - time2 > time3){
    digitalWrite(RELAY, HIGH); //Turn on the relay to switch to optical input
    Serial.println(millis() - time2);
  }

  if(coax){
    _pushedopt = false;
    digitalWrite(RELAY, LOW); //Turn off the relay to switch to coaxial input
    }

  if(change_state) screen_refresh(freq, HA_state, AS_state);
}

//Check the initial state of the IIS switch board
uint8_t init_check(){
  uint8_t BoardInput;
  delay(1000);  //Give 1 second to the I2S board to start
  for(int i = 10; i < 14; i++ ){
    if(digitalRead(i) == LOW) BoardInput = i-9;   //The board use common anode (+) for the leds, so LOW mins on
  }
  first = false;
  return BoardInput;
}

//Make the required number of switching corresponding to the used channels count
void button_switch(uint8_t nos){
  //The other controller operates with ground as a switchig pulse
  for(nos; nos >= 1; nos--){
    digitalWrite(BTN_SW, LOW);
    delay(50);
    digitalWrite(BTN_SW, HIGH);
    delay(50);
  }
  //if(!coax) digitalWrite(RELAY, HIGH); //Turn on the relay to switch to optical input
  //else digitalWrite(RELAY, LOW); //Turn off the relay to switch to coaxial input
  _pushed = false;
}

void screen_refresh(float freq, uint8_t HA_state, uint8_t AS_state) {
  uint8_t NumberOfSwitch = 0;
  uint8_t actChannel;
  display.clearDisplay();
  display.setTextColor(WHITE);
  prevChannel = actChannel;

  if(first) actChannel = init_check();
 
  switch (actChannel) {
  case 1: // USB
    //If button was pressed, set the button_switch flag to call the function,
    //send the needed amount of switching for it, and set the next actual channel
    if(_pushed){
      NumberOfSwitch = 1;
      actChannel = 2;
      Serial.print("USB0  ");
    }

    display.setTextSize(4);
    display.setCursor(30,10);
    display.print("USB");

    if(HA_state == 1){
      display.setTextSize(1);             
      display.setCursor(5,50);
      display.print("*");
    }

    if(freq != 0){
      if(freq > 100) display.setCursor(17,50);
      else display.setCursor(25,50);
      display.setTextSize(2);
      display.print(freq,1);
      display.print("kHz");
      
    }

    if(AS_state == 1){
      display.setTextSize(1);             
      display.setCursor(120,50);
      display.print("*");
    }

    Serial.print("USB  ");
    break;

  case 2: // COAX1 and OPT1
    display.setTextSize(4);
    //If button was pressed then make the necessary changes
    if(_pushed){
      if(coax){
        NumberOfSwitch = 0;
        actChannel = 2;
        coax = false;
        _pushedopt = true;
        time2 = millis();
      }
      else{
        if(channel == 2){
          NumberOfSwitch = 3;
          actChannel = 1;
        }
        else{
          NumberOfSwitch = 1;
          actChannel = 3;
        }
        coax = true;
      }
    }
    //Coaxial part
    if(coax && actChannel == 2){
      if(channel == 2){
        display.setCursor(19,10);
        display.print("COAX");
      }
      else{
        display.setCursor(8,10);
        display.print("COAX1");
      }
      Serial.print("COAX  ");
    }
    //optical part
    else{
      if(channel == 2){
        Serial.print("OPT21  ");
        display.setCursor(30,10);
        display.print("OPT");
      }
      else{
        Serial.print("OPT22  ");
        display.setCursor(19,10);
        display.print("OPT1");
      }
      Serial.print("OPT23  ");
    }
    break;
  case 3: // COAX2 and OPT2
    display.setTextSize(4);
    //If button was pressed then make the necessary changes
    if(_pushed){
      if(coax){
        NumberOfSwitch = 0;
        actChannel = 3;
        coax = false;
      }
      else{
        if(channel == 3){
          NumberOfSwitch = 2;
          actChannel = 1;
        }
        else{
          NumberOfSwitch = 1;
          actChannel = 4;
        }
        coax = true;
      }
    }
    //Coaxial part
    if(coax && actChannel == 3){
      display.setCursor(8,10);
      display.print("COAX2");
      Serial.print("COAX2  ");
    }
    //optical part
    else{
      display.setCursor(19,10);
      display.print("OPT2");
      Serial.print("OPT33  ");
    }
    break;
  case 4: // BLUETOOTH
    display.setTextSize(4);
    if(_pushed){
      NumberOfSwitch = 1;
      actChannel = 1;
    }
    display.setCursor(42,10);
    display.print("BT");
    break;
  default:
    display.setTextSize(4);
    display.setCursor(42,1);
    display.print("NO");
    display.setCursor(5,35);
    display.print("INPUT");
    actChannel = 1; //Just for test gets first input
    break;
  }
  
  //display.display();
  if(actChannel == prevChannel) change_state = false;
  
  if(_pushed){
    Serial.print("callButton   ");
    button_switch(NumberOfSwitch);
  }

  display.display();

  Serial.println("END");
  //delay(1000);
}