// Used for latching relay control
#include "LatchingRelay.h"

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

LatchingRelay::LatchingRelay(String name, unsigned int setpin, unsigned int setpin_readback, unsigned int resetpin, unsigned int resetpin_readback, unsigned int voltage_read_pin) {
    this->name = name;
    this->setpin = setpin;
    this->setpin_readback = setpin_readback;
    this->resetpin = resetpin;
    this->resetpin_readback = resetpin_readback;
    this->voltage_read_pin = voltage_read_pin;

    pinMode(setpin, OUTPUT);
    pinMode(setpin_readback, INPUT);
    pinMode(resetpin, OUTPUT);
    pinMode(resetpin_readback, INPUT);
    pinMode(voltage_read_pin, INPUT);
}

LatchingRelay::LatchingRelay(unsigned int setpin, unsigned int setpin_readback, unsigned int resetpin, unsigned int resetpin_readback, unsigned int voltage_read_pin) {
    this->setpin = setpin;
    this->setpin_readback = setpin_readback;
    this->resetpin = resetpin;
    this->resetpin_readback = resetpin_readback;
    this->voltage_read_pin = voltage_read_pin;

    pinMode(setpin, OUTPUT);
    pinMode(setpin_readback, INPUT);
    pinMode(resetpin, OUTPUT);
    pinMode(resetpin_readback, INPUT);
    pinMode(voltage_read_pin, INPUT);
}

void LatchingRelay::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 LatchingRelay::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 LatchingRelay::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 LatchingRelay::update_if_pins_working() {
    // This function is mainly used to check if the writing pin for the latching remains working while in operation
    setpin_actual_state = digitalRead(setpin_readback);
    if (setpin_actual_state != setpin_desired_state) {  // If the readback pin is not the state at which we are telling the pin to be at
        setpin_working = false;
    } else {
        setpin_working = true;
    }
    resetpin_actual_state = digitalRead(resetpin_readback);
    if (resetpin_actual_state != resetpin_desired_state) {  // If the readback pin is not the state at which we are telling the pin to be at
        resetpin_working = false;
    } else {
        resetpin_working = true;
    }
}

void LatchingRelay::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_off_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 LatchingRelay::turn_off_pins_after_duration() {
    // Since the coils are meant to be pulsed, we will constantly check if it is time to turn them off
    if (setpin_desired_state) {  // If the setpin is meant to be on
        if ((millis() - relay_first_voltage_time >= extra_pin_duration) && actual_state == true) {  // We have seen a voltage AND kept the coil on for a small buffer time
            digitalWrite(setpin, LOW);
            setpin_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
            setpin_actual_state = digitalRead(setpin_readback);
            if (setpin_actual_state != setpin_desired_state) {  // If the setpin did not go HIGH
                setpin_working = false;
                return;
            }
            // The set pin is working
            setpin_working = true;
        }
    }
    if (resetpin_desired_state) {  // If the resetpin is meant to be on
        if ((millis() - relay_first_no_voltage_time >= extra_pin_duration) && actual_state == false) {  // We have seen no voltage AND kept the coil on for a small buffer time
            digitalWrite(resetpin, LOW);
            resetpin_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
            resetpin_actual_state = digitalRead(resetpin_readback);
            if (resetpin_actual_state != setpin_desired_state) {  // If the setpin did not go HIGH
                resetpin_working = false;
                return;
            }
            // The reset pin is working
            resetpin_working = true;
        }
    }
}

void LatchingRelay::turn_on_relay() {
    if (expected_state == false && setpin_desired_state == false) {  // If the relay is expected to be in the off position and we have not already tried turning it on
        digitalWrite(setpin, HIGH);
        setpin_desired_state = true;
        digitalWrite(resetpin, LOW);
        resetpin_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
        setpin_actual_state = digitalRead(setpin_readback);
        resetpin_actual_state = digitalRead(resetpin_readback);

        resetpin_working = resetpin_actual_state == resetpin_desired_state ? true : false;
        if (setpin_actual_state != setpin_desired_state) {  // If the setpin did not go HIGH
            setpin_working = false;
            return;
        }
        // The set pin is working
        setpin_turned_on_time = millis();
        setpin_working = true;
        relay_turned_on_time = setpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        expected_state = true;
    }
}

void LatchingRelay::turn_off_relay() {
    if (expected_state == true && resetpin_desired_state == false) {  // If the relay is expected to be in the on position and we have not already tried turning it off
        digitalWrite(resetpin, HIGH);
        resetpin_desired_state = true;
        digitalWrite(setpin, LOW);
        setpin_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
        resetpin_actual_state = digitalRead(resetpin_readback);
        setpin_working = setpin_actual_state == setpin_desired_state ? true : false;
        if (resetpin_actual_state != resetpin_desired_state) {  // If the setpin did not go HIGH
            resetpin_working = false;
            return;
        }
        // The reset pin is working
        resetpin_turned_on_time = millis();
        resetpin_working = true;
        relay_turned_off_time = resetpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        expected_state = false;
    }
}

void LatchingRelay::turn_on_relay_startup() {
    // Used on startup when the state of the relay is unkown and we don't expect it to be 
    digitalWrite(setpin, HIGH);
    setpin_desired_state = true;
    digitalWrite(resetpin, LOW);
    resetpin_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
    setpin_actual_state = digitalRead(setpin_readback);
    resetpin_working = resetpin_actual_state == resetpin_desired_state ? true : false;
    if (setpin_actual_state != setpin_desired_state) {  // If the setpin did not go HIGH
        setpin_working = false;
        return;
    }
    // The set pin is working
    setpin_turned_on_time = millis();
    setpin_working = true;
    relay_turned_on_time = setpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
    expected_state = true;
}

void LatchingRelay::turn_off_relay_startup() {
    // Used on startup when the state of the relay is unkown and we don't expect it to be
    digitalWrite(resetpin, HIGH);
    resetpin_desired_state = true;
    digitalWrite(setpin, LOW);
    setpin_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
    resetpin_actual_state = digitalRead(resetpin_readback);
    setpin_working = setpin_actual_state == setpin_desired_state ? true : false;
    if (resetpin_actual_state != resetpin_desired_state) {  // If the resetpin did not go HIGH
        resetpin_working = false;
        return;
    }
    // The reset pin is working
    resetpin_turned_on_time = millis();
    resetpin_working = true;
    relay_turned_off_time = resetpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
    expected_state = false;
}

void LatchingRelay::turn_on_relay_from_fail() {
    // Only called if the actual state disagrees with the expected state due to some unkown failure (vibration, transistor for some reason turning on, etc)
    if (setpin_desired_state == false) {  // We have not already tried turning it on
        digitalWrite(setpin, HIGH);
        setpin_desired_state = true;
        digitalWrite(resetpin, LOW);
        resetpin_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
        setpin_actual_state = digitalRead(setpin_readback);
        resetpin_actual_state = digitalRead(resetpin_readback);

        resetpin_working = resetpin_actual_state == resetpin_desired_state ? true : false;
        if (setpin_actual_state != setpin_desired_state) {  // If the setpin did not go HIGH
            setpin_working = false;
            return;
        }
        // The set pin is working
        setpin_turned_on_time = millis();
        setpin_working = true;
        relay_turned_on_time = setpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        expected_state = true;
    }
}

void LatchingRelay::turn_off_relay_from_fail() {
    // Only called if the actual state disagrees with the expected state due to some unkown failure (vibration, transistor for some reason turning on, etc)
    if (resetpin_desired_state == false) {  // We have not already tried turning it off
        digitalWrite(resetpin, HIGH);
        resetpin_desired_state = true;
        digitalWrite(setpin, LOW);
        setpin_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
        resetpin_actual_state = digitalRead(resetpin_readback);
        setpin_working = setpin_actual_state == setpin_desired_state ? true : false;
        if (resetpin_actual_state != resetpin_desired_state) {  // If the setpin did not go HIGH
            resetpin_working = false;
            return;
        }
        // The reset pin is working
        resetpin_turned_on_time = millis();
        resetpin_working = true;
        relay_turned_off_time = resetpin_turned_on_time;  // Useful for checking if the voltage actually goes to 12V in a reasonable time
        expected_state = false;
    }
}

void LatchingRelay::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();
            }
        } else if (desired_state == expected_state) {  // We SHOULD be at the right state.
            if (desired_state != actual_state) {  
                // The only way we can get here is if there was some failure. But, if there is a failure, we should try to reset it to the correct state
                if (desired_state) {  // We want the relay on
                    turn_on_relay_from_fail();
                } else {  // We want the relay off
                    turn_off_relay_from_fail();
                }
            }
        }
    }
    // Now that we have begun the sequence of flipping the relay on/off,
    // we need to do the pin error checking, relay error/state checking,
    // and turning off the pins at the right time
    update_actual_state();
    update_if_pins_working();
    update_if_relay_working();
    turn_off_pins_after_duration();
}