#include <TimerOne.h>

// Pin Definitions
constexpr int analogInPin = A0;  // Spindle speed signal from PLC 
constexpr int pwmOutPin = 10;    // Output to laser for duty cycle
constexpr int relayPin1 = 9;      // Pin connected to the 24v cutoff relay
constexpr int relayPin2 = 8;     // Pin connected to the laser power relay

// Voltage thresholds for the 24v cutoff relay
constexpr float voltageHighThreshold1 = 1.5;   // Upper threshold to turn off the 24v cutoff relay
constexpr float voltageLowThreshold1 = 0.5;    // Lower threshold to turn on the 24v cutoff relay

// Voltage thresholds for the laser power relay
constexpr float voltageHighThreshold2 = 1.5;   // Upper threshold to turn on the laser power relay
constexpr float voltageLowThreshold2 = 0.5;    // Lower threshold to turn off the laser power relay

bool relayState1 = false;      // Current state of the 24v cutoff relay
bool relayState2 = false;      // Current state of the laser power relay

// Filter Variables for input
constexpr int inputFilterWindowSize = 10;  // Adjust to smooth fluctuations in the analog input
float inputFilterWindow[inputFilterWindowSize] = {0}; 
int inputFilterIndex = 0; 
float inputFilterSum = 0; 

// Filter Variables for output
constexpr int outputFilterWindowSize = 10; // Adjust to smooth fluctuations in the PWM signal
float outputFilterWindow[outputFilterWindowSize] = {0}; 
int outputFilterIndex = 0; 
float outputFilterSum = 0; 

// Function to calculate the SMA filtered input
float filterInput(float inputValue) {
  inputFilterSum -= inputFilterWindow[inputFilterIndex]; 
  inputFilterWindow[inputFilterIndex] = inputValue; 
  inputFilterSum += inputValue; 
  inputFilterIndex = (inputFilterIndex + 1) % inputFilterWindowSize; 
  return inputFilterSum / inputFilterWindowSize; 
}

// Function to calculate the SMA filtered output
float filterOutput(float outputValue) {
  outputFilterSum -= outputFilterWindow[outputFilterIndex]; 
  outputFilterWindow[outputFilterIndex] = outputValue; 
  outputFilterSum += outputValue; 
  outputFilterIndex = (outputFilterIndex + 1) % outputFilterWindowSize; 
  return outputFilterSum / outputFilterWindowSize; 
}

// Constants
constexpr float analogVoltageMin = 2.5;       // Minimum analog voltage
constexpr float analogVoltageMax = 5.0;        // Maximum analog voltage
constexpr int maxDutyCycle = 100;              // Maximum duty cycle
constexpr float fineTuneOffset = 0;            // Adjust to fine-tune the PWM output
constexpr int loopDelay = 100;                 // Adjust to change delay between loop cycles/serial monitor updates
constexpr int pwmIncrements = 1;               // Increment for PWM output

constexpr bool isVoltageOutOfRange = false;    // true/false to enable/disable voltage range checking
constexpr bool isSerialPrintEnabled = false;   // true/false to enable/disable the serial monitor
constexpr bool isDelayEnabled = true;          // true/false to enable/disable the delay

void setup() {
  // Set up the relay pins as outputs
  pinMode(relayPin1, OUTPUT);     
  pinMode(relayPin2, OUTPUT);     
  
  // Set up the PWM output pin
  pinMode(pwmOutPin, OUTPUT);
  Timer1.initialize(200);                  // Set the period in microseconds (5kHz)
  Timer1.pwm(pwmOutPin, 0);                // Initialize the PWM output

  // Set up the analog input pin
  pinMode(analogInPin, INPUT);    
  analogReference(DEFAULT);                // Set reference voltage to default (5V)

  Serial.begin(9600);   // Start the Serial communication for debugging
}

void loop() {
  // Read the voltage from the analog input pin
  int analogValue = analogRead(analogInPin);
  float analogVoltage = (float)analogValue / 1023.0 * 5.0;   // Convert analog value to analog voltage

  // Check if the analog voltage is out of the expected range
  if (isVoltageOutOfRange && (analogVoltage < voltageLowThreshold1 || analogVoltage > voltageHighThreshold2)) {
    if (isSerialPrintEnabled) {
      Serial.println("Error: Analog voltage out of range");  // Print error message if serial printing is enabled
    }
    return;
  }

  // Calculate and apply the SMA filter to the input voltage
  float filteredInputVoltage = filterInput(analogVoltage);

  // Calculate PWM duty cycle
  float pwmPercentage = ((filteredInputVoltage - analogVoltageMin) / (analogVoltageMax - analogVoltageMin)) * maxDutyCycle;
  int pwmDuty = round(pwmPercentage * 1023.0 / 100.0);  // convert percentage to duty value
  pwmDuty = constrain(pwmDuty, 0, 1023);                // ensure duty value is within valid range

  // Calculate and apply the SMA filter to the PWM duty cycle
  pwmDuty = (int)filterOutput((float)pwmDuty);

  // Set the PWM duty cycle
  Timer1.setPwmDuty(pwmOutPin, pwmDuty);

  // Check the voltage to determine the relay states
  if (relayState1) {
    // 24v cutoff relay is currently on, check if voltage exceeds the upper threshold
    if (filteredInputVoltage > voltageHighThreshold1) {
      digitalWrite(relayPin1, LOW);  // Turn off the 24v cutoff relay
      relayState1 = false;           // Update the 24v cutoff relay state
    }
  } else {
    // 24v cutoff relay is currently off, check if voltage drops below the lower threshold
    if (filteredInputVoltage < voltageLowThreshold1) {
      digitalWrite(relayPin1, HIGH); // Turn on the 24v cutoff relay
      relayState1 = true;            // Update the 24v cutoff relay state
    }
  }

  if (relayState2) {
    // laser power relay is currently on, check if voltage drops below the lower threshold
    if (filteredInputVoltage < voltageLowThreshold2) {
      digitalWrite(relayPin2, LOW);  // Turn off the laser power relay
      relayState2 = false;           // Update the laser power relay state
    }
  } else {
    // laser power relay is currently off, check if voltage exceeds the upper threshold
    if (filteredInputVoltage > voltageHighThreshold2) {
      digitalWrite(relayPin2, HIGH); // Turn on the laser power relay
      relayState2 = true;            // Update the laser power relay state
    }
  }

  // Print to Serial Monitor
  if (isSerialPrintEnabled) {
    Serial.print("Analog Value: ");
    Serial.print(analogVoltage);
    Serial.print("  Filtered Voltage: ");
    Serial.print(filteredInputVoltage);
    Serial.print("  PWM Percentage: ");
    Serial.print(pwmPercentage);
    Serial.print("  PWM Duty: ");
    Serial.println(pwmDuty);
  }

  // Delay if delay is enabled
  if (isDelayEnabled) {
    delay(loopDelay);
  }
}