// Used for latching solenoid control
#include "LatchingSolenoid.h"

#ifndef NonLatchingRelay_h
#include "NonLatchingRelay.h"
#endif

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

LatchingSolenoid::LatchingSolenoid(String name, unsigned int onpin, unsigned int onpin_readback, unsigned int offpin, unsigned int offpin_readback, unsigned int on_voltage_read_pin, unsigned int off_voltage_read_pin) : on_relay(onpin, onpin_readback, on_voltage_read_pin), off_relay(offpin, offpin_readback, off_voltage_read_pin) {
    this->name = name;
    this->onpin = onpin;
    this->onpin_readback = onpin_readback;
    this->offpin = offpin;
    this->offpin_readback = offpin_readback;
    this->on_voltage_read_pin = on_voltage_read_pin;
    this->off_voltage_read_pin = off_voltage_read_pin;
}

LatchingSolenoid::LatchingSolenoid(unsigned int onpin, unsigned int onpin_readback, unsigned int offpin, unsigned int offpin_readback, unsigned int on_voltage_read_pin, unsigned int off_voltage_read_pin) : on_relay(onpin, onpin_readback, on_voltage_read_pin), off_relay(offpin, offpin_readback, off_voltage_read_pin) {
    this->onpin = onpin;
    this->onpin_readback = onpin_readback;
    this->offpin = offpin;
    this->offpin_readback = offpin_readback;
    this->on_voltage_read_pin = on_voltage_read_pin;
    this->off_voltage_read_pin = off_voltage_read_pin;
}

void LatchingSolenoid::set_desired_solenoid_state(unsigned char new_desired_state) {
    if (new_desired_state == 0) {
        this->desired_state = false;  // Closed
    } else if (new_desired_state == 1) {
        this->desired_state = true;  // Open
    } else if (new_desired_state == 3) {  // Keep last desired state
        this->desired_state = desired_state;
    }
}

void LatchingSolenoid::update_first_voltage_times() {
    this->on_relay_first_voltage_time = this->on_relay.get_relay_first_voltage_time();
    this->off_relay_first_voltage_time = this->off_relay.get_relay_first_voltage_time();
}

void LatchingSolenoid::update_relay_actual_states() {
    this->on_relay_actual_state = this->on_relay.get_actual_state();
    this->off_relay_actual_state = this->off_relay.get_actual_state();
}

void LatchingSolenoid::update_if_pins_working() {
    this->onpin_working = this->on_relay.get_pin_working();
    this->offpin_working = this->off_relay.get_pin_working();
}

void LatchingSolenoid::update_if_relays_working() {
    this->on_relay_working = this->on_relay.get_relay_working();
    this->off_relay_working = this->off_relay.get_relay_working();
}

void LatchingSolenoid::update_solenoid_expected_state() {
    // We only expect the solenoid to have changed positions after a duration of voltage across the on/off relay was seen
    if (on_relay_actual_state == true) {  // If the on relay is on
        if (millis() - on_relay_first_voltage_time >= set_duration_minimum) {  // If the relay has been on for long enough
            expected_state = true;  // We now would expect the solenoid to be on
        }
    }
    if (off_relay_actual_state == true) {  // If the off relay is on
        if (millis() - off_relay_first_voltage_time >= set_duration_minimum) {  // If the relay has been on for long enough
            expected_state = false;  // We now would expect the solenoid to be off
        }
    }
}

void LatchingSolenoid::turn_off_relays_after_duration() {
    // Since the relays are meant to be pulsed, we will constantly check if it is time to turn them off
    if (on_relay_actual_state == true) {  // If the on relay is on
        if ((millis() - on_relay_first_voltage_time >= set_duration) && expected_state == true) {  // If the relay has been on for long enough
            on_relay.set_desired_state(0);  // Tell it we want it to be off
            on_relay.control_relay();  // Control the relay
        }
    }
    if (off_relay_actual_state == true) {  // If the off relay is on
        if ((millis() - off_relay_first_voltage_time >= set_duration) && expected_state == false) {  // If the relay has been on for long enough
            off_relay.set_desired_state(0);  // Tell it we want it to be off
            off_relay.control_relay();  // Control the relay
        }
    }
}

void LatchingSolenoid::turn_on_solenoid() {
    if (expected_state == false) {  // Make sure the solenoid is currently expected to be off
        off_relay.set_desired_state(0);  // Tell it we want it to be off in case it is currently on
        off_relay.control_relay();  // Control the relay

        on_relay.set_desired_state(1);  // Tell it we want it to be on
        on_relay.control_relay();  // Control the relay
    }
}

void LatchingSolenoid::turn_off_solenoid() {
    if (expected_state == true) {  // Make sure the solenoid is currently expected to be on
        on_relay.set_desired_state(0);  // Tell it we want it to be off in case it is currently on
        on_relay.control_relay();  // Control the relay

        off_relay.set_desired_state(1);  // Tell it we want it to be on
        off_relay.control_relay();  // Control the relay
    }
}

void LatchingSolenoid::turn_on_solenoid_startup() {
    off_relay.set_desired_state(0);  // Tell it we want it to be off in case it is currently on
    off_relay.control_relay();  // Control the relay

    on_relay.set_desired_state(1);  // Tell it we want it to be on
    on_relay.control_relay();  // Control the relay
}

void LatchingSolenoid::turn_off_solenoid_startup() {
    on_relay.set_desired_state(0);  // Tell it we want it to be off in case it is currently on
    on_relay.control_relay();  // Control the relay

    off_relay.set_desired_state(1);  // Tell it we want it to be on
    off_relay.control_relay();  // Control the relay
}

void LatchingSolenoid::control_solenoid() {
    // This handles all of the control.
    // Includes startups, initial turning on relays, stopping the relays,
    // 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 solenoid on
            turn_on_solenoid_startup();
        } else {  // We want the solenoid off
            turn_off_solenoid_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 solenoid on
                turn_on_solenoid();
            } else {  // We want the solenoid off
                turn_off_solenoid();
            }
        }
    }
    // Now that we have begun the sequence of flipping the relays on/off,
    // we need to do the pin error checking, and relay error/state checking,
    // and grabbing the necessary values from the on/off relays
    on_relay.control_relay();
    off_relay.control_relay();

    update_relay_actual_states();
    update_first_voltage_times();
    update_solenoid_expected_state();
    update_if_pins_working();
    update_if_relays_working();
    turn_off_relays_after_duration();
}