#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1  // Not used with ESP32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Joystick Pins
#define JOY_X_PIN A0
#define JOY_Y_PIN A1
#define JOY_SEL_PIN 2
// Define pins for the loads
const int load1 = 10; //3.8A
const int load2 = 9; //4.2A
const int load3 = 6; //15A
const int load4 = 4; 
const int RedLed = 2;
const int GreenLed = 3;
const int BTN_Sense = 5;

String testUnits[] = {"TS166A", "TS166B", "TS158A"};
int currentSelection = 0;
String selectedUnit = "";
bool selectionMode = true;  // true = show options, false = show selected unit only

const int AC = 12;
const int Motor = 11;

// Voltage-related variables
float Vint = 24;
float Vdrop = 0.07;
float Standby = 5;
float OCP04A = 0;
float OCP15A =0;

String OCP_0_4;  // Changed variable name to remove invalid characters
String OCP_15;
String TestRes;
String status;

// Flags to control the sequence
bool stopSequence = false; // Global flag to stop the sequence
bool startTest = false;    // Flag to control whether to start the sequence

// Analog pin definitions for voltage measurements
const int voltagePin1 = A0;  // Pin for 24V measurement
const int voltagePin2 = A1;  // Pin for 5V measurement

// Resistor values for the voltage divider
float R1 = 7500;  // Resistor 1 value in ohms (7.5k)
float R2 = 1000;  // Resistor 2 value in ohms (1k)

// Function to read the 24V input voltage
float ReadVoltage24V() {
  int sensorValue = analogRead(voltagePin1);  // Read analog value
  float voltageMeasured = sensorValue * (5.0 / 1023.0);  // Convert to voltage
  float voltageInput = voltageMeasured * ((R1 + R2) / R2);  // Calculate actual input voltage
  return voltageInput;
}

// Function to read the 5V standby voltage
float ReadVoltage5V() {
  int sensorValue = analogRead(voltagePin2);  // Read analog value
  float voltageMeasured = sensorValue * (5.0 / 1023.0);  // Convert to voltage
  float standbyVoltage = voltageMeasured * ((R1 + R2) / R2);  // Calculate actual input voltage
  return standbyVoltage;
}

void ts166b_sequence() {
  stopSequence = false;
  bool passTest = true;  // Flag to determine if all tests pass

  while (!stopSequence) {
    digitalWrite(AC, HIGH); //AC ON
    Serial.println("AC ON");
    delay(300);
    Serial.println("On Going Test");
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
  

    // Step 2: Load 1 ON, measure voltage drop
    digitalWrite(load1, HIGH);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
    
    delay(3000);  // Wait for 3 seconds

    // Step 3: Load 4 ON, measure standby voltage
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, HIGH);
    delay(3000);  // Wait for 3 seconds

    // Step 4: Load 1 and 2 ON
    digitalWrite(load1, HIGH);
    digitalWrite(load2, HIGH);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
    
    delay(5000);  // Wait for 5 seconds

    // Check OCP 0.4A criteria
    if (OCP04A <= 0.1) {
      OCP_0_4 = "OK";
    } else {
      OCP_0_4 = "NOT OK";
    }

    // Step 5: Load 3 ON
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, HIGH);
    digitalWrite(load4, LOW);
    
    delay(2000);  // Wait for 2 seconds

    // Check OCP 15A criteria
    if (OCP15A <= 0.1) {
      OCP_15 = "OK";
    } else {
      OCP_15 = "NOT OK";
    }

    Serial.println("Discharging");
    digitalWrite(AC, HIGH);
    digitalWrite(load1, HIGH);
    digitalWrite(load4, HIGH);
    delay(2000);
    digitalWrite(load1, LOW);
    digitalWrite(load4, LOW);
    delay(500);
    Serial.println("DONE");

    // Check all conditions to determine PASS/FAIL
    if ((Vint > 23.7 && Vint < 24.5) && (Standby > 4.8 && Standby < 5.2) && (Vdrop < 0.1) && (OCP_0_4=="OK") && (OCP_15 == "OK")){
      TestRes = "PASS";
    } else {
      TestRes = "FAIL";
    }

    // Output results
    Serial.print("24V_Output: ");
    Serial.print(Vint);
    Serial.print(", Vdrop: ");
    Serial.print(Vdrop);
    Serial.print(", 5V_Output: ");
    Serial.print(Standby);
    Serial.print(", OCP_0.4A: ");
    Serial.print(OCP_0_4);
    Serial.print(", OCP_15A: ");
    Serial.print(OCP_15);
    Serial.print(", Test Result: ");
    Serial.println(TestRes);

    stop_sequence();  // Stop after one full cycle
  }
}
void ts166a_sequence() {
  stopSequence = false;
  bool passTest = true;  // Flag to determine if all tests pass

  while (!stopSequence) {
    digitalWrite(AC, HIGH); //AC ON
    Serial.println("AC ON");
    digitalWrite(GreenLed, LOW);
    delay(1000);
    Serial.println("On Going Test");
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
    if (Vint > 23.7 && Vint < 24.5){
      stopSequence = false;
     }else{
        stop_sequence(); 
        TestRes ="FAIL";
        status = "24V ERROR";
        digitalWrite(RedLed, HIGH);
        Serial.println("24V ERROR");
        digitalWrite(AC, LOW);
        break;
     }
  

    // Step 2: Load 1 ON, measure voltage drop
    digitalWrite(load1, HIGH);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
    if(Vdrop < 0.1){
      stopSequence = false;
    }
    else{
      stop_sequence(); 
      TestRes ="FAIL";
      status = "VDROP ERROR";
      digitalWrite(RedLed, HIGH);
      Serial.println("VDROP ERROR");
      digitalWrite(AC, LOW);
      digitalWrite(load1, LOW);
      break;
    }
    delay(3000);  // Wait for 3 seconds

    // Step 3: Load 4 ON, measure standby voltage
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, LOW);
    digitalWrite(load4, HIGH);
    if(Standby > 4.8 && Standby < 5.2){
       stopSequence = false;
    }else{
      stop_sequence(); 
      TestRes ="FAIL";
      status = "5V ERROR";
      digitalWrite(RedLed, HIGH);
      Serial.println("5V ERROR");
      digitalWrite(AC, LOW);
      digitalWrite(load4, LOW);
      break;
    }
    delay(3000);  // Wait for 3 seconds

    // Step 4: Load 1 and 2 ON
    digitalWrite(load1, HIGH);
    digitalWrite(load2, HIGH);
    digitalWrite(load3, LOW);
    digitalWrite(load4, LOW);
    delay(5000);  // Wait for 5 seconds

    // Check OCP 0.4A criteria
    if (OCP04A <= 0.1) {
        OCP_0_4 = "OK";
        stopSequence = false;
      } else {
        OCP_0_4 = "NOT OK";
        stop_sequence(); 
        TestRes ="FAIL";
        status = "OCP4_2 ERROR";
        digitalWrite(RedLed, HIGH);
        Serial.println("OCP4_2 ERROR");
        digitalWrite(AC, LOW);
        digitalWrite(load1, LOW);
        digitalWrite(load2, LOW);
        break;
      }

    // Step 5: Load 3 ON
    digitalWrite(load1, LOW);
    digitalWrite(load2, LOW);
    digitalWrite(load3, HIGH);
    digitalWrite(load4, LOW);
    
    delay(2000);  // Wait for 2 seconds

    // Check OCP 15A criteria
    if (OCP15A <= 0.1) {
      OCP_15 = "OK";
      stopSequence = false;
    } else {
      OCP_15 = "NOT OK";
      stop_sequence(); 
      TestRes ="FAIL";
      status = "OCP15 ERROR";
      digitalWrite(RedLed, HIGH);
      Serial.println("OCP15 ERROR");
      digitalWrite(AC, LOW);
      digitalWrite(load3, LOW);
      break;
    }
    digitalWrite(load3, LOW);
    digitalWrite(Motor, HIGH);
    Serial.println("Motor ON");
    delay(5000);
    digitalWrite(Motor, LOW);
    Serial.println("Motor OFF");
    delay(500);

    Serial.println("Discharging");
    digitalWrite(AC, LOW);
    digitalWrite(load1, HIGH);
    digitalWrite(load4, HIGH);
    delay(2000);
    digitalWrite(load1, LOW);
    digitalWrite(load4, LOW);
    delay(500);
    Serial.println("DONE");

    TestRes ="PASS";
    digitalWrite(GreenLed, HIGH);

    // Output results
    Serial.print("24V_Output: ");
    Serial.print(Vint);
    Serial.print(", Vdrop: ");
    Serial.print(Vdrop);
    Serial.print(", 5V_Output: ");
    Serial.print(Standby);
    Serial.print(", OCP_0.4A: ");
    Serial.print(OCP_0_4);
    Serial.print(", OCP_15A: ");
    Serial.print(OCP_15);
    Serial.print(", Test Result: ");
    Serial.println(TestRes);

    stop_sequence();  // Stop after one full cycle
  }
}

void stop_sequence() {
  stopSequence = true; // Call this function to stop the sequence
}

void btnSense(){
  int buttonState = digitalRead(BTN_Sense);
  if(buttonState == LOW){
    if(selectedUnit == "TS166A"){
      ts166a_sequence();
    }
    else if(selectedUnit == "TS166B"){
       ts166b_sequence();
    }
  }
  else{
    Serial.println("Put the DUT");
  }
}
void setup() {
  Serial.begin(9600);  // Start serial communication
  Serial.print("TEST TOOL");
  pinMode(load1, OUTPUT);  // Set load pins as output
  pinMode(load2, OUTPUT);
  pinMode(load3, OUTPUT);
  pinMode(load4, OUTPUT);
  pinMode(AC, OUTPUT);
  pinMode(Motor, OUTPUT);
  pinMode(RedLed, OUTPUT);
  pinMode(GreenLed, OUTPUT);
  pinMode(BTN_Sense, INPUT_PULLUP);
  pinMode(JOY_SEL_PIN, INPUT_PULLUP);

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  
  displayOptions();
}
void loop() {
  btnSense();
  JoystickControl();
}
void JoystickControl(){
  int xValue = analogRead(JOY_X_PIN);
  int yValue = analogRead(JOY_Y_PIN);
  int buttonState = digitalRead(JOY_SEL_PIN);

  // Joystick navigation when in selection mode
  if (selectionMode) {
    if (yValue < 1000) {  // Move up
      currentSelection = (currentSelection - 1 + 3) % 3;
      displayOptions();
      delay(200);
    } else if (yValue > 3000) {  // Move down
      currentSelection = (currentSelection + 1) % 3;
      displayOptions();
      delay(200);
    }

    // Select option
    if (buttonState == LOW) {  // Button pressed
      selectedUnit = testUnits[currentSelection];
      selectionMode = false;  // Switch to display-only mode
      displaySelectedUnit();
      delay(200);  // Debounce delay
    }
  } else {
    // In display-only mode, check if joystick is moved up and button is pressed to re-enter selection mode
    if (buttonState == LOW) {  // Button pressed again to toggle back to selection mode
      selectionMode = true;
      displayOptions();
      delay(200);  // Debounce delay
    }
  }
}
void displaySelectedUnit() {
  display.clearDisplay();
   display.setTextColor(SSD1306_WHITE);  // Set text to white with no background
  display.setCursor(0, 0);
  display.println(selectedUnit);
  display.display();
}

// Display available options with the current selection highlighted
void displayOptions() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Select Test Unit:");

  for (int i = 0; i < 3; i++) {
    if (i == currentSelection) {
      display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);  // Highlight selection
    } else {
      display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
    }
    display.setCursor(0, 16 + i * 12);
    display.println(testUnits[i]);
  }

  display.display();
}