#include <Wire.h>
#include <U8glib.h>
#include <Adafruit_SSD1306.h>
//#include <Adafruit_SH1106.h>

//U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);  
  U8GLIB_SH1106_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);

#define LOGO_WIDTH 64
#define LOGO_HEIGHT 8  

#define VBUS_PIN 2
#define D_MINUS_PIN 3
#define D_PLUS_PIN 4
#define GND_PIN 5

#define LED_VBUS_PIN 7
#define LED_D_MINUS_PIN 8
#define LED_D_PLUS_PIN 9
#define LED_GND_PIN 10

#define BUTTON_PIN 6

const int BUZZER_PIN = 11;

const int logoScrollDelay = 0.1; // Time delay for logo scrolling (in milliseconds)

enum TestState {
  PRESS_BUTTON_TO_TEST,
  VBUS_TEST,
  D_MINUS_TEST,
  D_PLUS_TEST,
  GND_TEST,
  SHOW_RESULTS,
  TEST_COMPLETE
};

TestState currentState = PRESS_BUTTON_TO_TEST;
bool runTest = false;

 void drawCheckmark(uint8_t x, uint8_t y) {
  // Draws a checkmark symbol
  u8g.drawLine(x, y + 6, x + 3, y + 9);
  u8g.drawLine(x + 3, y + 9, x + 8, y + 4);
}

  // Function to blink an LED
void blinkLED(int pin, int times, int interval) {
  for (int i = 0; i < times; i++) {
    digitalWrite(pin, HIGH);
    delay(interval);
    digitalWrite(pin, LOW);
    delay(interval);
  }
}

  void displayLogo() {
  int logoY = 0; // Starting position of the logo (off-screen to the top)
  int stopPosition = (u8g.getHeight() - LOGO_HEIGHT) / 2;

  while (logoY <= stopPosition) {
    u8g.firstPage();
    do {
      u8g.setFont(u8g_font_fub20);
      u8g.setPrintPos(0, logoY + 2);
      u8g.print(" INOEL");

      u8g.setFont(u8g_font_6x12);
      u8g.setPrintPos(85, logoY + 2);
      u8g.print(" @2024");

      // Draw "USB CABLE TESTER" in a smaller font
      u8g.setFont(u8g_font_unifont);
      u8g.setPrintPos(0, logoY + 18);
      u8g.print(" TES KABEL USB");
     
    } while (u8g.nextPage());

    // Reduce the logoScrollDelay value to increase scrolling speed
    delay(1); // Adjust this value as needed for the desired speed

    // Move the logo position down
    logoY++;
  }
}

void setup() {

  if (u8g.getMode() == U8G_MODE_R3G3B2)
    u8g.setColorIndex(255);
  else if (u8g.getMode() == U8G_MODE_GRAY2BIT)
    u8g.setColorIndex(3);
  else if (u8g.getMode() == U8G_MODE_BW)
    u8g.setColorIndex(1);
  else if (u8g.getMode() == U8G_MODE_HICOLOR)
    u8g.setHiColorByRGB(255, 255, 255);

  pinMode(D_PLUS_PIN, INPUT_PULLUP);
  pinMode(D_MINUS_PIN, INPUT_PULLUP);
  pinMode(VBUS_PIN, INPUT_PULLUP);
  pinMode(GND_PIN, INPUT_PULLUP);
  pinMode(LED_VBUS_PIN, OUTPUT);
  pinMode(LED_D_PLUS_PIN, OUTPUT);
  pinMode(LED_D_MINUS_PIN, OUTPUT);
  pinMode(LED_GND_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);

  displayLogo();

    tone(BUZZER_PIN, 1000, 200);
    delay(70);
    tone(BUZZER_PIN, 2000, 200);

  // Run the LEDs back and forth sequentially once
  int delayTime = 100; // Adjust this value to control the LED sequence speed

  for (int i = 0; i < 1; i++) {
    digitalWrite(LED_VBUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_D_MINUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_D_PLUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_GND_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_VBUS_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_D_MINUS_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_D_PLUS_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_GND_PIN, LOW);
    delay(delayTime);
  }

  // Run back from GND to VBUS
  for (int i = 0; i < 1; i++) {
    digitalWrite(LED_GND_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_D_PLUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_D_MINUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_VBUS_PIN, HIGH);
    delay(delayTime);
    digitalWrite(LED_GND_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_D_PLUS_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_D_MINUS_PIN, LOW);
    delay(delayTime);
    digitalWrite(LED_VBUS_PIN, LOW);
    delay(delayTime);
  }

  delay(2000);

  // After the LED sequence, proceed to the main test
  currentState = PRESS_BUTTON_TO_TEST;
}

void loop() {
  
  if (currentState == PRESS_BUTTON_TO_TEST) {
    displayPressButtonMessage();
  } else {
    if (digitalRead(BUTTON_PIN) == LOW) {
      
    tone(BUZZER_PIN, 2000, 50);
    
      runTest = true;
      delay(200); // Debounce delay
    }

    if (runTest) {
      switch (currentState) {
        case VBUS_TEST:
          testVBUS();
          break;
        case D_MINUS_TEST:
          testDMinus();
          break;
        case D_PLUS_TEST:
          testDPlus();
          break;
        case GND_TEST:
          testGND();
          break;
        case SHOW_RESULTS:
          showResults();
          break;
        case TEST_COMPLETE:
          turnOffLEDs();
          showCompleteMessage();
          break;
      }
    }
  }
}

void displayPressButtonMessage() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "  Tekan Tombol");
    u8g.drawStr(0, 40, "untuk Mulai Tes");
  } while (u8g.nextPage());

  if (digitalRead(BUTTON_PIN) == LOW) {
    delay(200); // Debounce delay
    runTest = true;
    currentState = VBUS_TEST;
  }
}

void testVBUS() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "Testing VBUS...");

    int vbusStatus = digitalRead(VBUS_PIN);
    if (vbusStatus == LOW) {
      digitalWrite(LED_VBUS_PIN, HIGH);
      u8g.drawStr(0, 40, "VBUS Connected");
    } else {
      digitalWrite(LED_VBUS_PIN, LOW);
      u8g.drawStr(0, 40, "VBUS Not Connected");
    }
  } while (u8g.nextPage());

  delay(1000); // Wait for 1 second before the next test
  currentState = D_MINUS_TEST;
}

void testDMinus() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "Testing D-...");

    int dMinusStatus = digitalRead(D_MINUS_PIN);
    if (dMinusStatus == LOW) {
      digitalWrite(LED_D_MINUS_PIN, HIGH);
      u8g.drawStr(0, 40, "D- Connected");
    } else {
      digitalWrite(LED_D_MINUS_PIN, LOW);
      u8g.drawStr(0, 40, "D- Not Connected");
    }
  } while (u8g.nextPage());

  delay(1000); // Wait for 1 second before the next test
  currentState = D_PLUS_TEST;
}

void testDPlus() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "Testing D+...");

    int dPlusStatus = digitalRead(D_PLUS_PIN);
    if (dPlusStatus == LOW) {
      digitalWrite(LED_D_PLUS_PIN, HIGH);
      u8g.drawStr(0, 40, "D+ Connected");
    } else {
      digitalWrite(LED_D_PLUS_PIN, LOW);
      u8g.drawStr(0, 40, "D+ Not Connected");
    }
  } while (u8g.nextPage());

  delay(1000); // Wait for 1 second before the next test
  currentState = GND_TEST;
}

void testGND() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "Testing GND...");

    int gndStatus = digitalRead(GND_PIN);
    if (gndStatus == LOW) {
      digitalWrite(LED_GND_PIN, HIGH);
      u8g.drawStr(0, 40, "GND Connected");
    } else {
      digitalWrite(LED_GND_PIN, LOW);
      u8g.drawStr(0, 40, "GND Not Connected");
    }
  } while (u8g.nextPage());

  delay(1000); // Wait for 1 second before displaying results
  currentState = SHOW_RESULTS;
}

void showResults() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_9x15);
    u8g.drawStr(0, 10, "Hasil Tes:");

    int vbusStatus = digitalRead(VBUS_PIN);
    u8g.setFont(u8g_font_6x12);
    u8g.setFontPosTop();
    u8g.setPrintPos(0, 20);
    u8g.print("VBUS: ");
    if (vbusStatus == LOW) {
      digitalWrite(LED_VBUS_PIN, HIGH);
      u8g.println("Connected");
      drawCheckmark(97, 20);
    } else {
      u8g.println("Not Connected");
      u8g.println(" X");
    }

    int dMinusStatus = digitalRead(D_MINUS_PIN);
    u8g.setPrintPos(0, 30);
    u8g.print("D-: ");
    if (dMinusStatus == LOW) {
      digitalWrite(LED_D_MINUS_PIN, HIGH);
      u8g.println("  Connected");
      drawCheckmark(97, 30);
    } else {
      u8g.println("  Not Connected");
      u8g.println(" X");
    }

    int dPlusStatus = digitalRead(D_PLUS_PIN);
    u8g.setPrintPos(0, 40);
    u8g.print("D+: ");
    if (dPlusStatus == LOW) {
      digitalWrite(LED_D_PLUS_PIN, HIGH);
      u8g.println("  Connected");
      drawCheckmark(97, 40);
    } else {
      u8g.println("  Not Connected");
      u8g.println(" X");
    }

    int gndStatus = digitalRead(GND_PIN);
    u8g.setPrintPos(0, 50);
    u8g.print("GND: ");
    if (gndStatus == LOW) {
      digitalWrite(LED_GND_PIN, HIGH);
      u8g.println(" Connected");
      drawCheckmark(97, 50);
    } else {
      u8g.println(" Not Connected");
      u8g.println(" X");
    }
  } while (u8g.nextPage());

  // Check for disconnected pins and blink the specific LED accordingly
  if (digitalRead(VBUS_PIN) == HIGH) {
    tone(BUZZER_PIN, 2000, 200);
    blinkLED(LED_VBUS_PIN, 6, 250);
  }
  if (digitalRead(D_MINUS_PIN) == HIGH) {
    tone(BUZZER_PIN, 2000, 200);
    blinkLED(LED_D_MINUS_PIN, 6, 250);
  }
  if (digitalRead(D_PLUS_PIN) == HIGH) {
    tone(BUZZER_PIN, 2000, 200);
    blinkLED(LED_D_PLUS_PIN, 6, 250);
  }
  if (digitalRead(GND_PIN) == HIGH) {
    tone(BUZZER_PIN, 2000, 200);
    blinkLED(LED_GND_PIN, 6, 250);
  }

  delay(8000); // Wait for 8 seconds before showing "Test Complete"
  currentState = TEST_COMPLETE;
}

  void turnOffLEDs() {
  digitalWrite(LED_VBUS_PIN, LOW);
  digitalWrite(LED_D_MINUS_PIN, LOW);
  digitalWrite(LED_D_PLUS_PIN, LOW);
  digitalWrite(LED_GND_PIN, LOW);
}

void showCompleteMessage() {
  u8g.firstPage();
  do {
    u8g.setFont(u8g_font_unifont);
    u8g.drawStr(0, 20, "  Tes Selesai!");
    u8g.drawStr(0, 40, "  Tekan Tombol");
    u8g.drawStr(0, 60, " Untuk Tes Lagi");
  } while (u8g.nextPage());

  // Turn off all LEDs before starting a new test
  turnOffLEDs();

  runTest = false;
  currentState = VBUS_TEST;
}