#define r_pin 11                 // Define an RGB LED (Red pin)
#define g_pin 12                 // Define an RGB LED (Green pin)
#define b_pin 13                 // Define an RGB LED (Blue pin)

#define buzzer 10                // Define buzzer pin
#define freq 250                 // Define frequency for the buzzer

int lastInputState;              // The previous reading from the input door sensors
int currentInputState;           // The current reading from the input door sensors

int lastButtonState;             // The previous state of button on D14
int currentButtonState;          // The current state of button on D14

unsigned long lastOpenTime = 0;  // The last time the door changed state from close to open
unsigned long timer = 10000;     // The default timer

unsigned long interval = 500;
unsigned long previousMillis = 0;
unsigned long currentMillis;

int modeState = 0;               // 0 -> Normal Mode, 1 -> Time Mode, 2 -> Additional Time Mode

void setup() {
  Serial.begin(9600);
  DDRH &= B10001111;  // Set D7, D8, and D9 as INPUT -> (PH4, PH5, and PH6)
  PORTH |= B01110000; // Enable the pullup
  DDRE |= B00111000;  // Set D2, D3, and D5 as OUTPUT (LEDs) -> (PE4, PE5, and PE3)
  DDRB |= B11100000;  // Set D11, D12, and D13 as OUTPUT (RGB) -> (PB5, PB6, and PB7)
  DDRB |= B00010000;  // Set D10 as OUTPUT (Buzzer) -> (PB4)

  DDRG &= B11011111;
  PORTG |= B00100000;

  // DDRJ &= B11111101;  // Set D14 as INPUT -> (PJ0)
  // PORTJ |= B00000010; // Enable the pullup
  DDRJ |= B00000010;  // Set D14 as OUTPUT -> (PJ0)
  PORTJ &= B11111100; // Clear the value on PORT J
}

void loop() {
  // lastButtonState = currentButtonState;
  // currentButtonState = (PINJ >> 1 & B00000010 >> 1);

  lastButtonState = currentButtonState;
  currentButtonState = (PING >> 5 & B00100000 >> 5);

  if (PINH != B01110000) {
    doorOpen();
  } else {
    doorClose();
    if (lastButtonState == HIGH && currentButtonState == LOW) {
      // Cycle through mode states when the mode button is pressed
      // Mode 0 = Normal, 1 = Timed Mode, 2 = Additonal Timed Mode
      (modeState < 2) ? modeState++ : modeState=0;
    }
    currentInputState = B01110000;
    Serial.println(modeState);
  }

  // Serial.print("Current Input State: ");
  // Serial.println(currentInputState);

}

// Function to turn RGB LED on (RED)
void rgb_Red_On() {
  analogWrite(r_pin, 255);
  analogWrite(g_pin, 0);
  analogWrite(b_pin, 0);
}

// Function to turn RGB LED on (GREEN)
void rgb_Green_On() {
  analogWrite(r_pin, 0);
  analogWrite(g_pin, 255);
  analogWrite(b_pin, 0);
}

// Function to turn RGB LED OFF
void rgb_Off() {
  analogWrite(r_pin, 0);
  analogWrite(g_pin, 0);
  analogWrite(b_pin, 0);
}

void doorOpen() {
  // The inputs are wired to the ground with the internal pull up enabled
  // so this program treats the input as Normally Close on contact. 
  // The input will be HIGH when not pressed, and LOW when pressed. (Inputs normally HIGH)

  lastInputState = currentInputState;
  Serial.print("Last Input State: ");
  Serial.println(lastInputState);
  currentInputState = PINH;

  // Door Open
  if ((currentInputState != B01110000) && (lastInputState == B01110000)) {
    lastOpenTime = millis();
  }
  Serial.print("Current Input State: ");
  Serial.println(currentInputState);

  // Read status on PINH
  switch (currentInputState) {
    // If inputs on D8 and D9 (PH5 and PH6) are LOW then turn on RED LEDs on D5 and D3 (PE3 and PE5) and turn RGB LED off
    case (B00010000):
      PORTE = (PORTE & B00000000) | B00101000;
      rgb_Off();
      // if ((millis() - lastOpenTime) > timer*mode) {
      //   if (mode) PORTJ = (PORTJ & B00000000) | B00000001;
      //   tone(buzzer, freq);
      // }
      buzzerOn();
      break;
    // If inputs on D7 and D9 (PH4 and PH6) are LOW then turn on RED LEDs on D5 and D2 (PE3 and PE4) and turn RGB LED off
    case (B00100000):
      PORTE = (PORTE & B00000000) | B00011000;
      rgb_Off();
      buzzerOn();
      break;
    // If inputs on D7 and D8 (PH4 and PH5) are LOW then turn on RED LEDs on D2 and D3 (PE4 and PE5) and turn RGB LED off
    case (B01000000):
      PORTE = (PORTE & B00000000) | B00110000;
      rgb_Off();
      buzzerOn();
      break;
    // If an input on D9 (PH6) is LOW then turn on RED LED on D5 (PE3) and turn RGB LED off
    case (B00110000):
      PORTE = (PORTE & B00000000) | B00001000;
      rgb_Off();
      buzzerOn();
      break;
    // If an input on D7 (PH4) is LOW then turn on RED LED on D2 (PE4) and turn RGB LED off
    case (B01100000):
      PORTE = (PORTE & B00000000) | B00010000;
      rgb_Off();
      buzzerOn();
      break;
    // If an input on D8 (PH5) is LOW then turn on RED LED on D3 (PE5) and turn RGB LED off
    case (B01010000):
      PORTE = (PORTE & B00000000) | B00100000;
      rgb_Off();
      buzzerOn();
      break;  
    // If all inputs are LOW (Door fully open): All RED LED on D2, D3, and D5 are ON and RGB LED turn RED
    case (B00000000): 
      PORTE = (PORTE & B00000000) | B00111000;
      rgb_Red_On();
      buzzerOn();
      break;
  }
}

void doorClose() {
  // If all inputs are HIGH (Door fully close): All RED are OFF and RGB LED turn GREEN
  PORTE &= B00000000;
  PORTJ &= B11111100;   // Clear the value on PORT J
  rgb_Green_On();
  noTone(buzzer);       // Buzzer off
}

void buzzerOn() {
  currentMillis = millis();
  if ((currentMillis - lastOpenTime) >= timer*modeState) {
    if (modeState) PORTJ = (PORTJ & B00000000) | B00000010;
    tone(buzzer, freq);
  } else if ((currentMillis - lastOpenTime) < timer*modeState) {
    Serial.println("< timer mode");
    if (currentMillis - previousMillis >= (interval/modeState)) {
      Serial.println(">= interval");
      previousMillis = currentMillis;
      if (PINJ) {
        PORTJ &= B11111101;
        Serial.println(PORTJ);
      } else {
        PORTJ |= B00000010;
        Serial.println(PORTJ);
      }
    }
  }
}