#include <EEPROM.h>
#include <JC_Button.h>

// Pin definitions
const int RELAY[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};  
const uint8_t MANUAL = 1;    // Define MANUAL mode as 1
const uint8_t AUTOMATIC = 2; // Define AUTOMATIC mode as 2
const int BTN_MEM_PIN[] = {17, 18, 19};
const int BTN_SET_MEM = 20;
const int up = 21; 
const int dn = 22; 
const int Tr = 23; 
const int Rtr = 24; 
const int tiltr = 25;
const int tiltl = 26;
const int backup = 27;
const int backdn = 28;
const int slide = 29;
const int slideb = 30;
const int lock = 31;
const int unlock = 32;
const int BUTTON_N = 33;
const int BUTTON_R = 34;
const int redLED = 35;
const int yellowLED = 36;
const int greenLED = 37;
const int LED_1 = 38; 
const int LED_2 = 39; 

const int HALL_SENSOR_1_PIN = A0;  // Hall sensor connected to A0
const int HALL_SENSOR_2_PIN = A1;  // Hall sensor connected to A1
const int HALL_SENSOR_3_PIN = A2;  // Hall sensor connected to A2

long targetPosition[6] = {0};  // Array to store target positions
long oldPosition[6] = {-999, -999, -999, -999, -999, -999};  // Array to store previous positions
const int EEPROM_BASE_ADDR_LOOP1 = 0;
const int EEPROM_BASE_ADDR_LOOP2 = 3 * sizeof(long[3]);
#define ACCURACY 40           
#define DEBOUNCE_MS 10        
#define PULLUP false          
#define INVERT false    
uint8_t MODE = MANUAL; // Default mode is MANUAL      

Button btnExtend(up, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRetract(dn, PULLUP, INVERT, DEBOUNCE_MS);
Button btnExtend1(Tr, PULLUP, INVERT, DEBOUNCE_MS); 
Button btnRetract1(Rtr, PULLUP, INVERT, DEBOUNCE_MS);
Button btnExtend2(tiltr, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRetract2(tiltl, PULLUP, INVERT, DEBOUNCE_MS);
Button btnExtend3(backup, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRetract3(backdn, PULLUP, INVERT, DEBOUNCE_MS);
Button btnExtend4(slide, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRetract4(slideb, PULLUP, INVERT, DEBOUNCE_MS);
Button btnExtend5(lock, PULLUP, INVERT, DEBOUNCE_MS);
Button btnRetract5(unlock, PULLUP, INVERT, DEBOUNCE_MS);

Button btnSetPos(BTN_SET_MEM, PULLUP, INVERT, DEBOUNCE_MS);
Button btnPos1(BTN_MEM_PIN[0], PULLUP, INVERT, DEBOUNCE_MS);
Button btnPos2(BTN_MEM_PIN[1], PULLUP, INVERT, DEBOUNCE_MS);
Button btnPos3(BTN_MEM_PIN[2], PULLUP, INVERT, DEBOUNCE_MS);

// Debounce settings for BUTTON_N and BUTTON_R
const unsigned long DEBOUNCE_DELAY = 50;
unsigned long lastDebounceTimeN = 0;
unsigned long lastDebounceTimeR = 0;
bool buttonNState = HIGH;
bool lastButtonNState = HIGH;
bool buttonRState = HIGH;
bool lastButtonRState = HIGH;

bool loop1Active = true;
bool lastResetButtonState = HIGH;  // Last state of reset button
bool resetModeActive = false;      // Flag to indicate reset mode

bool isResetButtonPressed() {
  return (digitalRead(up) == LOW && digitalRead(dn) == LOW);
}

const int analogInput = A4;  
float vout = 0.0, vin = 0.0;
const float R1 = 100000.0, R2 = 10000.0;
int value = 0;

void checkAndUpdatePositions(int loopIndex);
void performResetSequence();

void setup() {
  Serial.begin(9600); // Initialize serial communication for debugging

  pinMode(BUTTON_N, INPUT_PULLUP);
  pinMode(BUTTON_R, INPUT_PULLUP);

  pinMode(redLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);

  for (int i = 0; i < 12; i++) {
    pinMode(RELAY[i], OUTPUT);
    digitalWrite(RELAY[i], LOW); 
  }

  digitalWrite(redLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(greenLED, LOW);
  digitalWrite(LED_1, LOW);
  digitalWrite(LED_2, LOW);

  pinMode(up, INPUT_PULLUP);
  pinMode(dn, INPUT_PULLUP);
  pinMode(Tr, INPUT_PULLUP);
  pinMode(Rtr, INPUT_PULLUP);
  pinMode(tiltr, INPUT_PULLUP);
  pinMode(tiltl, INPUT_PULLUP);
  pinMode(backup, INPUT_PULLUP);
  pinMode(backdn, INPUT_PULLUP);
  pinMode(slide, INPUT_PULLUP);
  pinMode(slideb, INPUT_PULLUP);
  pinMode(lock, INPUT_PULLUP);
  pinMode(unlock, INPUT_PULLUP);
  pinMode(BTN_SET_MEM, INPUT_PULLUP);
  pinMode(BTN_MEM_PIN[0], INPUT_PULLUP);
  pinMode(BTN_MEM_PIN[1], INPUT_PULLUP);
  pinMode(BTN_MEM_PIN[2], INPUT_PULLUP);

  pinMode(HALL_SENSOR_1_PIN, INPUT_PULLUP);
  pinMode(HALL_SENSOR_2_PIN, INPUT_PULLUP);
  pinMode(HALL_SENSOR_3_PIN, INPUT_PULLUP);

  // Turn off all LEDs initially
  digitalWrite(redLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(greenLED, LOW);
}

void loop1() {
    digitalWrite(LED_1, HIGH);
    digitalWrite(LED_2, LOW);

    readButtons();
    controlRelaysLoop1();
    setModeAutomaticIfPositionButtonReleased();
    checkAndUpdatePositions(0); // Replace 0 with the appropriate loopIndex value

    // Handle BTN_SET_MEM + BTN_MEM_PIN operations for storing positions
    if (btnSetPos.isPressed()) {
        if (btnPos1.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP1, 0); // Store positions for memory 1 in loop1
        }
        if (btnPos2.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP1 + sizeof(long[3]), 0); // Store positions for memory 2 in loop1
        }
        if (btnPos3.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP1 + 2 * sizeof(long[3]), 0); // Store positions for memory 3 in loop1
        }
    }

    // Handle BTN_MEM_PIN operations for retrieving and comparing positions
    if (btnPos1.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP1, 0); // Retrieve and compare positions for memory 1 in loop1
    }
    if (btnPos2.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP1 + sizeof(long[3]), 0); // Retrieve and compare positions for memory 2 in loop1
    }
    if (btnPos3.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP1 + 2 * sizeof(long[3]), 0); // Retrieve and compare positions for memory 3 in loop1
    }
}
void loop2() {
    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, HIGH);

    readButtons();
    controlRelaysLoop2();

    // Handle BTN_SET_MEM + BTN_MEM_PIN operations for storing positions
    if (btnSetPos.isPressed()) {
        if (btnPos1.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP2, 0); // Store positions for memory 1 in loop2
        }
        if (btnPos2.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP2 + sizeof(long[3]), 0); // Store positions for memory 2 in loop2
        }
        if (btnPos3.isPressed()) {
            storePositionsToEEPROM(EEPROM_BASE_ADDR_LOOP2 + 2 * sizeof(long[3]), 0); // Store positions for memory 3 in loop2
        }
    }

    // Handle BTN_MEM_PIN operations for retrieving and comparing positions
    if (btnPos1.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP2, 0); // Retrieve and compare positions for memory 1 in loop2
    }
    if (btnPos2.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP2 + sizeof(long[3]), 0); // Retrieve and compare positions for memory 2 in loop2
    }
    if (btnPos3.wasReleased()) {
        retrieveAndComparePositions(EEPROM_BASE_ADDR_LOOP2 + 2 * sizeof(long[3]), 0); // Retrieve and compare positions for memory 3 in loop2
    }
}

void loop() {

  bool readingN = digitalRead(BUTTON_N);
  bool readingR = digitalRead(BUTTON_R);
  bool resetButtonPressed = isResetButtonPressed();

  if (readingN != lastButtonNState) {
    lastDebounceTimeN = millis();
  }
  if (readingR != lastButtonRState) {
    lastDebounceTimeR = millis();
  }

  if ((millis() - lastDebounceTimeN) > DEBOUNCE_DELAY) {
    if (readingN != buttonNState) {
      buttonNState = readingN;
      if (buttonNState == LOW) {
        loop1Active = true;
      }
    }
  }

  if ((millis() - lastDebounceTimeR) > DEBOUNCE_DELAY) {
    if (readingR != buttonRState) {
      buttonRState = readingR;
      if (buttonRState == LOW) {
        loop1Active = false;
      }
    }
  }

  if (resetButtonPressed && !lastResetButtonState) {
    // Enter reset mode
    performResetSequence();
  }

  lastResetButtonState = resetButtonPressed;
  lastButtonNState = readingN;
  lastButtonRState = readingR;

  if (loop1Active) {
    loop1();
  } else {
    loop2();
  }

  // Read the hall sensor states
  bool switch1State = digitalRead(HALL_SENSOR_1_PIN);
  bool switch2State = digitalRead(HALL_SENSOR_2_PIN);
  bool switch3State = digitalRead(HALL_SENSOR_3_PIN);
}

void readButtons() {
  btnExtend.read();
  btnRetract.read();
  btnExtend1.read();
  btnRetract1.read();
  btnExtend2.read();
  btnRetract2.read();
  btnExtend3.read();
  btnRetract3.read();
  btnExtend4.read();
  btnRetract4.read();
  btnExtend5.read();
  btnRetract5.read();

  btnSetPos.read();
  btnPos1.read();
  btnPos2.read();
  btnPos3.read();
}

void controlRelaysLoop1() {
  // Toggle relays based on button states
  digitalWrite(RELAY[0], btnExtend.isPressed());
  digitalWrite(RELAY[1], btnRetract.isPressed());
  digitalWrite(RELAY[2], btnExtend1.isPressed());
  digitalWrite(RELAY[3], !resetModeActive && btnRetract1.isPressed());
  // Relays 4 to 8 do not respond in reset mode
  digitalWrite(RELAY[4],  btnExtend2.isPressed());
  digitalWrite(RELAY[5], !resetModeActive && btnRetract2.isPressed());

  digitalWrite(RELAY[6],  btnExtend3.isPressed());
  digitalWrite(RELAY[7],  !resetModeActive && btnRetract3.isPressed());
   digitalWrite(RELAY[8],  btnExtend4.isPressed());
  digitalWrite(RELAY[9],  btnRetract4.isPressed());
 
  digitalWrite(RELAY[10],  btnExtend5.isPressed());
  digitalWrite(RELAY[11],  btnRetract5.isPressed());

  MODE = MANUAL;  // Set mode to MANUAL
}

void controlRelaysLoop2() {
  // Toggle relays based on button states
  digitalWrite(RELAY[0], btnExtend.isPressed());
  digitalWrite(RELAY[1], btnRetract.isPressed());
  digitalWrite(RELAY[2], btnRetract1.isPressed());
  digitalWrite(RELAY[3], btnExtend1.isPressed());
  digitalWrite(RELAY[4], btnRetract2.isPressed());
  digitalWrite(RELAY[5], btnExtend2.isPressed());
  digitalWrite(RELAY[6], btnRetract3.isPressed());
  digitalWrite(RELAY[7], btnExtend3.isPressed());
  digitalWrite(RELAY[8], btnRetract4.isPressed());
  digitalWrite(RELAY[9], btnExtend4.isPressed());
  digitalWrite(RELAY[10], btnExtend5.isPressed());
  digitalWrite(RELAY[11], btnRetract5.isPressed());

  MODE = MANUAL;  // Set mode to MANUAL
}

void setModeAutomaticIfPositionButtonReleased() {
  // Check if any position button is released to switch to automatic mode
  if (btnPos1.wasReleased() || btnPos2.wasReleased() || btnPos3.wasReleased()) {
    MODE = 1;  // Set mode to automatic
  }
}


void storePositionsToEEPROM(int baseAddr, int loopIndex) {
    // Read Hall sensor values
    int position1 = digitalRead(HALL_SENSOR_1_PIN);
    int position2 = digitalRead(HALL_SENSOR_2_PIN);
    int position3 = digitalRead(HALL_SENSOR_3_PIN);

    // Store positions to targetPosition array
    targetPosition[loopIndex] = position1;
    targetPosition[loopIndex + 1] = position2;
    targetPosition[loopIndex + 2] = position3;

    // Store positions to EEPROM
    EEPROM.put(baseAddr, targetPosition + loopIndex);
}

void retrieveAndComparePositions(int baseAddr, int loopIndex) {
    long targetPosition[3]; // Assuming this is defined correctly somewhere in your code

    // Retrieve positions from EEPROM
    EEPROM.get(baseAddr, targetPosition); // Assuming targetPosition is an array

    // Read current positions
    int currentPosition1 = digitalRead(HALL_SENSOR_1_PIN);
    int currentPosition2 = digitalRead(HALL_SENSOR_2_PIN);
    int currentPosition3 = digitalRead(HALL_SENSOR_3_PIN);

    // Compare and control actuators based on differences
    if (currentPosition1 != targetPosition[0]) {
        if (currentPosition1 < targetPosition[0]) {
            digitalWrite(RELAY[2], HIGH);  // Extend actuator 1
            digitalWrite(RELAY[3], LOW);   // Ensure retract is off
        } else {
            digitalWrite(RELAY[3], HIGH);  // Retract actuator 1
            digitalWrite(RELAY[2], LOW);   // Ensure extend is off
        }
    }

    // Repeat for other actuators (positions 2 and 3)
    if (currentPosition2 != targetPosition[1]) {
        if (currentPosition2 < targetPosition[1]) {
            digitalWrite(RELAY[4], HIGH);  // Extend actuator 2
            digitalWrite(RELAY[5], LOW);   // Ensure retract is off
        } else {
            digitalWrite(RELAY[5], HIGH);  // Retract actuator 2
            digitalWrite(RELAY[4], LOW);   // Ensure extend is off
        }
    }

    if (currentPosition3 != targetPosition[2]) {
        if (currentPosition3 < targetPosition[2]) {
            digitalWrite(RELAY[6], HIGH);  // Extend actuator 3
            digitalWrite(RELAY[7], LOW);   // Ensure retract is off
        } else {
            digitalWrite(RELAY[7], HIGH);  // Retract actuator 3
            digitalWrite(RELAY[6], LOW);   // Ensure extend is off
        }
    }
}



void checkAndUpdatePositions(int loopIndex) {
  // Read Hall sensor values
  int position1 = analogRead(HALL_SENSOR_1_PIN);
  int position2 = analogRead(HALL_SENSOR_2_PIN);
  int position3 = analogRead(HALL_SENSOR_3_PIN);

  // Update targetPosition array based on sensor readings
  targetPosition[loopIndex] = position1;  // Update according to your mapping and scaling if needed
  targetPosition[loopIndex + 1] = position2;
  targetPosition[loopIndex + 2] = position3;

  // Optionally, perform accuracy checks or other logic here
}

void storePositionToEEPROM() {
  if (btnSetPos.wasReleased()) {
    // Save target positions to EEPROM
    EEPROM.put(0, targetPosition);
  }
}

void performResetSequence() {
  digitalWrite(RELAY[3], HIGH);  // Retract relay 4
  digitalWrite(RELAY[5], HIGH);  // Retract relay 5
  digitalWrite(RELAY[7], HIGH);  // Retract relay 6
 
  // Update EEPROM with default positions or values
  resetEEPROM();
}

void resetEEPROM() {
  // Example: Set default positions in EEPROM
  long defaultPosition[3] = {0, 0, 0};
  EEPROM.put(0, defaultPosition);
  EEPROM.put(sizeof(defaultPosition), defaultPosition);
  EEPROM.put(2 * sizeof(defaultPosition), defaultPosition);
}

void readVoltage() {
  value = analogRead(analogInput);  // Read the analog input
  vout = (value * 5.0) / 1024.0;  // Calculate the output voltage from the ADC value

  // Calculate the input voltage using the voltage divider formula
  vin = vout / (R2 / (R1 + R2));
  Serial.print("Input Voltage = ");
  Serial.println(vin, 2);  // Print the input voltage with 2 decimal places

  // Control the LEDs based on the input voltage
  if (vin <= 22.0 && vin > 20.0) {
    digitalWrite(redLED, HIGH);  // Turn on red LED for 20.0V - 22.0V
  } else {
    digitalWrite(redLED, LOW);
  }

  if (vin <= 24.0 && vin > 22.0) {
    digitalWrite(yellowLED, HIGH);  // Turn on yellow LED for 22.0V - 24.0V
  } else {
    digitalWrite(yellowLED, LOW);
  }

  if (vin <= 25.0 && vin > 24.0) {
    digitalWrite(greenLED, HIGH);  // Turn on green LED for 24.0V - 25.0V
  } else {
    digitalWrite(greenLED, LOW);
  }

  delay(500);  // Delay for 0.5 seconds
}