//To do:
//1. Fix up the latching switch function done
//2. allow the user to choose which button uses latch/toggle switch. done
//3. add some safety implementation e.g. only one button can be pressed at a time.
//4. there is a main on/off switch. The switches only operate if the main switch is on. Need to implement this. done
//5. 3 more leds used to indicate latching.
#define masterSwitch 0
#define T40MS_DIV 3
byte t40msCnt = T40MS_DIV;

#define btnSize 7
byte btnPin[btnSize]={8,7,6,5,4,3,2};

#define relaySize 7
byte relayPin[relaySize]={A1,A0,13,12,11,10,9};

bool keyPressed[btnSize];
bool keyState[btnSize];
bool lastKey[btnSize];

bool btnMode[btnSize] = {true, false, false, false, false, true, true}; //true = latching function, false = direct
// bool btnMode[btnSize]={true,true,true,true,true,true,true}; //true = latching function, false = direct
// bool btnMode[btnSize]={false,false,false,false,false,false,false}; //true = latching function, false = direct


void resetRelay() { //reset all relay and keypressed state (not including master switch)
  for (int k = 0; k < btnSize; k++) {
    if (k != masterSwitch) {
      digitalWrite(relayPin[k], LOW);
      keyPressed[k] = false;
    }
  }
}

void setup() {
  Serial.begin(115200); //Serial begin
  // btnPin[0] = 8;
  // btnPin[1] = 7;
  // btnPin[2] = 6;
  // btnPin[3] = 5;
  // btnPin[4] = 4;
  // btnPin[5] = 3;
  // btnPin[6] = 2;

  // relayPin[0] = A1;
  // relayPin[1] = A0;
  // relayPin[2] = 13;
  // relayPin[3] = 12;
  // relayPin[4] = 11;
  // relayPin[5] = 10;
  // relayPin[6] = 9;

  for (int i = 0; i < btnSize; i++) {
    keyPressed[i] = false;
    lastKey[i] = true;
    keyState[i] = true;
    pinMode(btnPin[i], INPUT_PULLUP);
    pinMode(relayPin[i], OUTPUT);
  }
  resetRelay(); //Set all function relays except master switch to LOW at the start
  digitalWrite(relayPin[masterSwitch], LOW); //set master switch to LOW at the start

  // initialize timer2(8 bits)
  TCCR2A = 0;   // set entire TCCR2A register to 0
  TCCR2B = 0;   // set entire TCCR2B register to 0
  TCNT2 = 0;    //initialize counter value to 0
  OCR2A = 155; // compare match register 16MHz/1024/100 - 1=155~=0.01Sec=10ms (must be <256)
  TCCR2A |= (1 << WGM21); // CTC mode
  TCCR2B = (1 << CS22) | (1 << CS21) | (1 << CS20); //1024 prescalar CS22 | CS21 | CS20 (clock = 16.625Khz = 64us)
  TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt
  interrupts(); // enable all interrupts
  // digitalWrite(relayPin[masterSwitch], 1);
}


//************************************************************************
//Interupt program: set flag when button is pressed
//************************************************************************
void baseInt() {
  bool stateBtn;
  for (int i = 0; i < btnSize; i++) {
    stateBtn = digitalRead(btnPin[i]);
    if (btnMode[i]) { //btnMode: true = latching function, false = direct
      if (!stateBtn && lastKey[i]) {  // falling edge: press the button
        keyPressed[i] = true;
        keyState[i] = LOW;
      }
    } else {
      if (stateBtn != lastKey[i]) {
        keyPressed[i] = true;
        keyState[i] = stateBtn;
      }
    }
    lastKey[i] = stateBtn;
  }
}

//********************************************************************************
//                              INTERUPT
//********************************************************************************
ISR(TIMER2_COMPA_vect) {    //call interrupt (10ms between each interrupt)
  if (t40msCnt) {
    t40msCnt--;
  } else {
    t40msCnt = T40MS_DIV;
    baseInt();      // every 40ms
  }
}//end of interrupt

bool checkIdle() { //function used to check if a key has been pressed (not including master switch)
  bool idle = true; //TRUE: no key pressed  False: key pressed
  for (int k = 0; k < btnSize; k++) {
    if (k != masterSwitch) {
      if (digitalRead(relayPin[k]))
        idle = false;
    }
  }
  return idle;
}

void loop() {
  bool idle;
  if (keyPressed[masterSwitch]) { // master key pressed
    if (btnMode[masterSwitch]) {
      digitalWrite(relayPin[masterSwitch], !digitalRead(relayPin[masterSwitch]));
    } else {
      digitalWrite(relayPin[masterSwitch], keyState[masterSwitch]);
    }
    keyPressed[masterSwitch] = false;
    resetRelay();
  } else {    // none master key pressed
    idle = checkIdle(); //no key has been pressed (not including master switch)
    if (digitalRead(relayPin[masterSwitch])) {
      for (int i = 0; i < btnSize; i++) {
        if (keyPressed[i]) {
          if (idle) {
            if (btnMode[i]) {
              digitalWrite(relayPin[i], !digitalRead(relayPin[i]));
            } else {
              digitalWrite(relayPin[i], !keyState[i]);
            }
            keyPressed[i] = false;
            Serial.println("idle");
          } else {
            resetRelay();
            Serial.println("safety off");
          }
        }
      }
    }
  }
}