#include <Arduino.h>

// Pin configuration
const int squareWavePin = 4; // Pin to output the square wave
const int inputPin = 34; // Pin to measure voltage peak

// Variables
unsigned long previousMillis = 0;
unsigned long duration = 1000; //  Measurement frequency in milliseconds
//byte dutyCycle = 100; // Initial duty cycle (percentage)
float dutyCycleFactor = 0.1; // Duty cycle factor for analogWrite (0-1)
bool squareWaveState = false; // Initial state of square wave
bool lastSquareWaveState = false; // Last state of square wave
bool swMode = false; // Mode flag: false for Not Connected mode, true for Square Wave mode
bool lastMode = false;
bool canMeasure = true; // Flag to ensure measurement happens once per second
bool innit = false; // Flag to read voltage on boot
float voltagePeak = 12.0;
byte state = 1;
byte lastState = 0;
byte amps = 0;
byte lastAmps = 0;
//volatile bool triggered = false; // Flag to indicate interrupt trigger

void setup() {
  pinMode(squareWavePin, OUTPUT);
//  analogWriteFreq(1000);
  pinMode(inputPin, INPUT);
  digitalWrite(squareWavePin, HIGH); // Initially set the output pin HIGH
 // attachInterrupt(digitalPinToInterrupt(squareWavePin), squareWaveInterrupt, RISING); // Attach interrupt to rising edge of square wave
  Serial.begin(9600);
}

void loop() {
  unsigned long currentMillis = millis();

  // ### ANY SERIAL PRINTS MUST BE AFTER ALL ADC/VOLTAGE READS ###

  handler();

  // Reset the flag to allow measurement once a second
  if (currentMillis - previousMillis >= duration) {
    canMeasure = true;
    Serial.println("Can Measure"); 
    previousMillis = millis();
    if (state != 5 && state != 1){
      dutyCycleFactor = map(amps, 6, 30, 0.1, 0.5);
    }
    if (swMode == true) {
      attachInterrupt(digitalPinToInterrupt(squareWavePin), squareWaveInterrupt, RISING); // Attach interrupt to rising edge of square wave
    }
  }
} 

void serialPrint() {
  if (state >= 0) { // state != lastState || !innit
    Serial.print(voltagePeak);
    Serial.print("V State ");
    Serial.print(state);
    Serial.print(". ");
    if (voltagePeak >= 11.0) {
      Serial.println("Not Connected");
    } else if (voltagePeak >= 8.0) {
      Serial.println("Connected");
    } else if (voltagePeak >= 5.0) {
      Serial.println("Charging");
    } else if (voltagePeak >= 2.0) {
      Serial.println("Ventilation Required");
    } else if (voltagePeak >= 0.0) {
      Serial.println("Error");
    }
    
  }
}

void handler() {
   // Check mode and execute corresponding logic
  if (innit == false) { // Not Connected mode
    //    digitalWrite(squareWavePin, HIGH);
    swMode = false;
    amps = 0;
    handleNotConnectedMode();
    innit = true;
    Serial.println("Innit");
    return;
    }
  if (swMode == false) { // Not Connected mode
      handleNotConnectedMode();
    } 
    else { // Square Wave mode
      handleSquareWaveMode();
    }
}

void handleNotConnectedMode() {
  digitalWrite(squareWavePin, HIGH); // Set output pin HIGH if voltage is 11V or higher
  if (canMeasure == true) {
    // Measure voltage once a second
    voltagePeak = measureVoltagePeak();
  	canMeasure = false; // Set flag to prevent further measurement until 1 second passes 
    updateState();
    Serial.println("Void NotConnectedMode");
    serialPrint();
  }
}

void handleSquareWaveMode() {
  // Generate square wave with variable duty cycle
  analogWrite(squareWavePin, 255 * dutyCycleFactor); // Convert duty cycle percentage to PWM value
}

void handlePeakMeasurement() {
  detachInterrupt(digitalPinToInterrupt(squareWavePin));
  // Measure voltage peak at the leading edge of the square wave
  voltagePeak = measureVoltagePeak();
  canMeasure = false; // Set flag to prevent further measurement until 1 second passes
  updateState();	
  Serial.println("Void PeakMeasurement");
  serialPrint();
  //	triggered = false; // Reset flag
  //  lastSquareWaveState = squareWaveState;
}

void squareWaveInterrupt() {
  handlePeakMeasurement();
  //  triggered = true; // Set flag on interrupt trigger
}

void updateState() {
  if (voltagePeak >= 10.75) {
    state = 1;
    swMode = false;
  } else {
    swMode = true; // Change to SquareWaveMode
    if (voltagePeak >= 8.0) {
    state = 2;
    //    Serial.println("Connected");
  } else if (voltagePeak >= 5.0) {
    state = 3;
    //   Serial.println("Charging");
  } else if (voltagePeak >= 2.0) {
    state = 4;
    //    Serial.println("Ventilation Required");
  } else if (voltagePeak >= 0.0) {
    error();
    //    Serial.println("Error");
  }
  if (lastMode != swMode) {
    Serial.print("swMode ");
    Serial.println(swMode);
  }
}
  lastState = state;
  lastMode = swMode;
}

void error() {
    state = 5;
    swMode = false;
    amps = 0;
    // disconnect relay
}

float measureVoltagePeak() {
  int sensorValue;
  float voltage;

  // Measure voltage peak
  sensorValue = analogRead(inputPin);
  voltage = sensorValue * (12.0 / 4095.0); // Convert ADC value to voltage
 // Serial.println(sensorValue);
 // Serial.println(voltage);
 // Serial.println(voltagePeak);
  return voltage;
}