// Used for non-latching relay control
#ifndef NonLatchingRelay_h
#include "NonLatchingRelay.h"
#endif

#ifndef Arduino_H
#include <Arduino.h>
#endif

NonLatchingRelay::NonLatchingRelay(String name, unsigned int pin, unsigned int pin_readback, unsigned int voltage_read_pin) {
    this->name = name;
    this->pin = pin;
    this->pin_readback = pin_readback;
    this->voltage_read_pin = voltage_read_pin;

    pinMode(pin, OUTPUT);
    pinMode(pin_readback, INPUT);
    pinMode(voltage_read_pin, INPUT);
}

NonLatchingRelay::NonLatchingRelay(unsigned int pin, unsigned int pin_readback, unsigned int voltage_read_pin) {
    this->pin = pin;
    this->pin_readback = pin_readback;
    this->voltage_read_pin = voltage_read_pin;

    pinMode(pin, OUTPUT);
    pinMode(pin_readback, INPUT);
    pinMode(voltage_read_pin, INPUT);
}

void NonLatchingRelay::set_desired_state(unsigned char new_desired_state) {
    if (new_desired_state == 0) {
        desired_state = false;  // Closed
    } else if (new_desired_state == 1) {
        desired_state = true;  // Open
    } else if (new_desired_state == 3) {  // Keep last desired state
        desired_state = desired_state;
    }
}

float NonLatchingRelay::read_relay_voltage() {
    // Read the voltage across the relay
    unsigned int raw_read = analogRead(voltage_read_pin);  // maps (0V, 5V) to (0, 1023)
    float voltage = raw_read*(5.0/1024);  // Voltage at the analog pin
    voltage = voltage * (1/0.29078);  // Accounts for the voltage divider

    return voltage;
}

void NonLatchingRelay::update_actual_state() {
    // Returns true if the voltage is detected
    // Returns false if the voltage is NOT detected
    // Let's also use a sample size of 3 and average
    float voltage_sum = 0;
    for (unsigned int i = 0; i < num_voltage_reads; i++) {
        voltage_sum += read_relay_voltage();
    }
    float voltage = voltage_sum / (num_voltage_reads * 1.0);

    if (voltage >= 7.5) {  // A reading of 7.5 means at least 2/3 read 12V
        if (actual_state == false && expected_state == true) {  // If this is the first voltage we have seen since trying to turn it on
            relay_first_voltage_time = millis();
        }
        actual_state = true;
    } else {
        if (actual_state == true && expected_state == false) {  // If this is the first no voltage we have seen since trying to turn it off
            relay_first_no_voltage_time = millis();
        }
        actual_state = false;
    }
}

void NonLatchingRelay::update_if_pin_working() {
    // This function is mainly used to check if the writing pin for the non-latching remains working while in operation
    pin_actual_state = digitalRead(pin_readback);
    if (pin_actual_state != pin_desired_state) {  // If the readback pin is not the state at which we are telling the pin to be at
        pin_working = false;
    } else {
        pin_working = true;
    }
}

void NonLatchingRelay::update_if_relay_working() {
    if (expected_state) {  // Relay is expected to be on
        if (millis() - relay_turned_on_time >= relay_delay) {  // If it has been long enough for the relay to have 12V across it
            if (actual_state) {  // If there is 12V across the relay
                relay_working = true;
            } else {
                relay_working = false;
            }
        } else {  // There hasn't been enough time to let 12V go across the contacts
            relay_working = relay_working;  // Keep same state
        }
    } else {  // The relay is supposed to be off
        if (millis() - relay_turned_on_time >= relay_delay) {  // If it has been long enough for the relay to have 12V across it
            if (actual_state) {  // If there is 12V across the relay
                relay_working = false;
            } else {
                relay_working = true;
            }
        } else {  // There hasn't been enough time to let 12V go across the contacts
            relay_working = relay_working;  // Keep same state
        }
    }
}

void NonLatchingRelay::turn_on_relay() {
    if (expected_state == false && pin_desired_state == false) {  // If the relay is expected to be in the off position and we have not already tried turning it on
        digitalWrite(pin, HIGH);
        pin_desired_state = true;
        delayMicroseconds(5);  // This is to give the Arduino enough time to turn on the digital pin and for readback to be ready
        pin_actual_state = digitalRead(pin_readback);
        if (pin_actual_state != pin_desired_state) {  // If the setpin did not go HIGH
            pin_working = false;
            return;
        }
        // The pin is working
        relay_turned_on_time = millis();  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        pin_working = true;
        expected_state = true;
    }
}

void NonLatchingRelay::turn_off_relay() {
    if (expected_state == true && pin_desired_state == true) {  // If the relay is expected to be in the on position and we have not already tried turning it off
        digitalWrite(pin, LOW);
        pin_desired_state = false;
        delayMicroseconds(5);  // This is to give the Arduino enough time to turn on the digital pin and for readback to be ready
        pin_actual_state = digitalRead(pin_readback);
        if (pin_actual_state != pin_desired_state) {  // If the setpin did not go LOW
            pin_working = false;
            return;
        }
        // The pin is working
        relay_turned_off_time = millis();  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        pin_working = true;
        expected_state = false;
    }
}

void NonLatchingRelay::turn_on_relay_startup() {
    // Used on startup when the state of the relay is unkown and we don't expect it to be 
    digitalWrite(pin, HIGH);
    pin_desired_state = true;
    delayMicroseconds(5);  // This is to give the Arduino enough time to turn on the digital pin and for readback to be ready
    pin_actual_state = digitalRead(pin_readback);
    if (pin_actual_state != pin_desired_state) {  // If the setpin did not go HIGH
        pin_working = false;
        return;
    }
    // The pin is working
    relay_turned_on_time = millis();  // Useful for checking if the voltage actually goes to 12V in a reasonable time
    pin_working = true;
    expected_state = true;
}

void NonLatchingRelay::turn_off_relay_startup() {
    // Used on startup when the state of the relay is unkown and we don't expect it to be 
    digitalWrite(pin, LOW);
    pin_desired_state = false;
    delayMicroseconds(5);  // This is to give the Arduino enough time to turn on the digital pin and for readback to be ready
    pin_actual_state = digitalRead(pin_readback);
    if (pin_actual_state != pin_desired_state) {  // If the setpin did not go LOW
        pin_working = false;
        return;
    }
    // The pin is working
    relay_turned_off_time = millis();  // Useful for checking if the voltage actually goes to 12V in a reasonable time
    pin_working = true;
    expected_state = false;
}

void NonLatchingRelay::control_relay() {
    // This handles all of the control.
    // Includes startups, initial turning on coils, stopping the coils,
    // pin error checking, relay error checking, everything.
    // Call this function frequently so that the pins and error messages are kept in check
    
    if (startup) {  // If this is the first time we have done anything
        if (desired_state) {  // We want the relay on
            turn_on_relay_startup();
        } else {  // We want the relay off
            turn_off_relay_startup();
        }
        startup = false;
    } else {  // Not startup
        if (desired_state != expected_state) {  // If we have not already begun the process of setting the state
            if (desired_state) {  // We want the relay on
                turn_on_relay();
            } else {  // We want the relay off
                turn_off_relay();
            }
        }
    }
    // Now that we have begun the sequence of flipping the relay on/off,
    // we need to do the pin error checking, and relay error/state checking
    update_actual_state();
    update_if_pin_working();
    update_if_relay_working();
}