// Switch inputs
#define EMERGENCY_SW 13
#define SUPPLY_FILL_SW 11
#define AUTO_FILL_SW 10
#define IGNITER_ARMING_SW 9 // On the designated PCB port it is 10. Using the continuity-check port right now
#define RUN_VENT_SW 7
#define BVAS_ARMING_SW 6
#define BVAS_SW 10 // On the designated PCB port it is 12. Using the auto-fill port right now
#define IGNITER_SW 8
#define SUPPLY_VENT_SW 3
#define FLUID_ARMING_SW 2

// LED Outputs
#define ENGINE_NODE_COMMS_LED 29
#define AB_COMMS_LED 30
#define AUTO_FILL_LED 32
#define SUPPLY_VENT_LED 37
#define SUPPLY_FILL_RELAY_LED 38
#define SUPPLY_FILL_LED 39
#define SUPPLY_VENT_RELAY_LED 40
#define IGNITER_RELAY_LED 41
#define RUN_VENT_LED 42
#define IGNITER_LED 43
#define IGNITER_CONTINUITY_LED 45
#define RUN_VENT_RELAY_LED 44
#define BVAS_RELAY_LED 46
#define BVAS_LED 47
// Arming LEDs
#define FLUID_ARMING_LED 17
#define IGNITER_ARMING_LED 16
#define BVAS_ARMING_LED 15

/*--------------------------------------------------------------------------------------------------------------------*/
/*-------------------------------------------SWITCH INPUTS------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

struct switch_inputs_struct {
    bool igniter_arming;
    bool igniter;
    bool fluid_arming;
    bool supply_vent;
    bool supply_fill;
    bool run_vent;
    bool bvas_arming;
    bool bvas;
    bool abort;
};

void read_switch_states(struct switch_inputs_struct *return_struct) {
    // Igniter
    return_struct->igniter_arming = digitalRead(IGNITER_ARMING_SW);
    return_struct->igniter = return_struct->igniter_arming && digitalRead(IGNITER_SW);

    //  Fluid panel
    return_struct->fluid_arming = digitalRead(FLUID_ARMING_SW);
    return_struct->supply_vent = return_struct->fluid_arming && digitalRead(SUPPLY_VENT_SW);
    return_struct->supply_fill = return_struct->fluid_arming && digitalRead(SUPPLY_FILL_SW);
    return_struct->run_vent = return_struct->fluid_arming && digitalRead(RUN_VENT_SW);

    // BVAS
    return_struct->bvas_arming = digitalRead(BVAS_ARMING_SW);
    return_struct->bvas = return_struct->bvas_arming && digitalRead(BVAS_SW);

    // Abort
    return_struct->abort = digitalRead(EMERGENCY_SW);
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*----------------------------------------------CAN BUS---------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
// For the CAN Bus module
#include <SPI.h>
#include <mcp2515.h>

MCP2515 mcp2515(49);

bool comms_with_ab = false;
unsigned long last_ab_can_message_sent;
unsigned long last_ab_can_message_received;
#define AB_CAN_INTERVAL 250  // In ms, how often we should send a CAN message
#define TIME_TO_INIT_PAUSE_STATE 2000  // If this many ms has gone by without a CAN message from CB, go into pause state
unsigned long pause_state_entered_time;
#define TIME_TO_INIT_ABORT_STATE 600000  // 10 minutes. If this many ms has gone by without a CAN message from CB, go into abort state
unsigned long abort_state_entered_time;


void update_can_command_message(struct can_frame *message, const struct switch_inputs_struct *switch_inputs) {
    message->can_id = 50;
    message->can_dlc = 8;

    message->data[0] = switch_inputs->supply_fill;
    message->data[1] = switch_inputs->supply_vent;
    message->data[2] = switch_inputs->run_vent;
    message->data[3] = switch_inputs->bvas;
    message->data[4] = switch_inputs->igniter;
    message->data[5] = 0;
    message->data[6] = 0;
    message->data[7] = 0;
}

bool read_from_ab(struct can_frame *ab_response) {
    struct can_frame read_msg;
    if (mcp2515.readMessage(&read_msg) == MCP2515::ERROR_OK) {  // We received a CAN message
        //Serial.println(read_msg.can_id);
        if (read_msg.can_id == 50) {  // AB is acknowledging our command message
            last_ab_can_message_received = millis();
            ab_response->can_id = read_msg.can_id;
            ab_response->can_dlc = read_msg.can_dlc;
            for (int i = 0; i < read_msg.can_dlc; i++) {
                ab_response->data[i] = read_msg.data[i];
                ///Serial.print(ab_response->data[i]); ///Serial.print(" ");
            }
            for (int i = read_msg.can_dlc; i < 8; i++) {  // Bytes after what the cb received
                ab_response->data[i] = 0;
            }
            ///Serial.println();
        }
        return true;
    } else {
        return false;
    }

}

/*--------------------------------------------------------------------------------------------------------------------*/
/*------------------------------------------------DISPLAY BOARD-------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

// LCD Stuff
// For the I2C LCD on the display board
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd_lib(0x27, 20, 4); // This is the object from the library that we use to write to the LCD

// I will create my own LCD class so that we can store more information and functions
class LCD {
    private:
        String current_text[4] = {"", "", "", ""};  // An array of strings. Each element represents what is currently being shown on the row
        String desired_text[4] = {"", "", "", ""};  // What we will want to update the LCD with
        bool blank_rows[4] = {true, true, true, true};  // Keep track of any blank lines

    public:
        LCD() {  // Construct the object
        }

        void clear_lcd() {
            // Printing everything as a space is faster than the built in function
            for(unsigned int i = 0; i < 4; i++) {
                lcd_lib.setCursor(0, i);
                lcd_lib.print("                    ");  // 20 Spaces
                current_text[i] = "";
                blank_rows[i] = true;
            }
            lcd_lib.setCursor(0, 0);
        }

        void clear_lcd_line(int row) {
            // Printing everything as a space is faster than the built in function
            // If row = 0, that is the first row on the lcd
            lcd_lib.setCursor(0, row);
            lcd_lib.print("                    ");  // 20 Spaces
            current_text[row] = "                    ";
            lcd_lib.setCursor(0, row);
            blank_rows[row] = true;
        }

        void set_lcd_to_desired_text() {
            // Go through each row and if the current_text string differs from the desired then we will write over it
            for (unsigned int i = 0; i < 4; i++) {
                if (current_text[i] != desired_text[i]) {
                    String extra_spaces = "";
                    for (int i = desired_text[i].length(); i < 20; i++) {
                        extra_spaces += " ";
                    }
                    lcd_lib.setCursor(0, i);
                    //Serial.println("Putting: [" + desired_text[i] + extra_spaces + "] to the LCD on row " + i);
                    lcd_lib.print(desired_text[i] + extra_spaces);
                    current_text[i] = desired_text[i];
                }
                String trimmed = current_text[i];
                trimmed.trim();
                if (trimmed == "") {  // Row is blank
                    blank_rows[i] = true;
                } else {  // Row is not blank
                    blank_rows[i] = false;
                }
            }
            lcd_lib.setCursor(0, 0);
        }

        int add_string_to_desired_text(String to_add, int row = -1) {
            // Inputs a string that wants to be displayed and puts it on the first blank line available
            // Returns the row that it was added to
            if (to_add == "") {
                return -1;
            }
            int already_on_line = check_if_already_in_desired_text(to_add);
            if (already_on_line != -1) {  // Already in the desired text
                return already_on_line;  // Return the line it is on already
            }
            if (row == -1) {  // No row specification
                for (unsigned int i = 0; i < 4; i++) {  // Go through each available line and see if it is blank
                    if (blank_rows[i] == true) {  // If the line is blank let's use it
                        desired_text[i] = to_add;
                        for (unsigned int j = desired_text[i].length(); j < 19; j++) {
                            desired_text[i] += " ";
                        }
                        blank_rows[i] = false;  // No longer blank
                        return i;  // Return the line it is now on
                    }
                }
            } else {
                desired_text[row] = to_add;
                for (unsigned int j = desired_text[row].length(); j < 19; j++) {
                    desired_text[row] += " ";
                }
                blank_rows[row] = false;  // No longer blank
                return row;
            }
            return -1;
        }

        int check_if_already_in_desired_text(String to_add) {
            // Returns the row that the text is already at
            for (unsigned int i = 0; i < 4; i++) {
                if (to_add == desired_text[i]) {
                    return i;
                }
            }
            return -1;
        }

        void clear_desired_text(int row = -1) {
            // Resets the desired text to be empty
            if (row == -1) {
                for (unsigned int i = 0; i < 4; i++) {
                    desired_text[i] = "";
                }
            } else {
                desired_text[row] = "";
            }
            
        }
};

LCD status_lcd;  // The LCD that is on the display board

// Output LEDs
// We will control all of the outputs for the relays using classes. 
// This is helpful because it is essentially setup where each relay has 3 LEDs and all use the same error formatting depending on the type of control class used: non-latching, latching, latching solenoid
// Because each type of solenoid setup has its own possible errors, we need to account for them differently. Therefore this will also do error message parsing
class NonLatchingRelayDisplays {
    private: 
        unsigned int relay_led_pin;  // Represents if the AB saw the on signal and set the pin HIGH
        unsigned int state_led_pin;  // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
        String name;  // So our LCD messages have meaning
        String pin_error_message = "";  // If we come across a pin error state, update this string so the LCD can find it and print it
        String relay_error_message = "";  // If we come across a relay error state, update this string so the LCD can find it and print it

    public:
        int pin_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD
        int relay_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD

        NonLatchingRelayDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
            this->relay_led_pin = relay_led_pin;
            this->state_led_pin = state_led_pin;
            this->name = name;

            pinMode(relay_led_pin, OUTPUT);
            pinMode(state_led_pin, OUTPUT);
            digitalWrite(relay_led_pin, LOW);
            digitalWrite(state_led_pin, LOW);
        }

        String get_pin_error_message() {
            // Returns what pin error message string we currently have
            return pin_error_message;
        }
        String get_relay_error_message() {
            // Returns what relay error message string we currently have
            return relay_error_message;
        }

        void control_leds(unsigned int status) {
            // The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
            unsigned int ones_place = status % 10;
            unsigned int tens_place = (status % 100) / 10;

            // Controlling the state_led (ones_place)
            if (ones_place == 1 || ones_place == 4) {  // 1 is on and no relay error. 4 is on and relay error
                digitalWrite(state_led_pin, HIGH);
            } else if (ones_place == 0 || ones_place == 3) {  // 0 is off and no relay error. 3 is off and relay error
                digitalWrite(state_led_pin, LOW);
            }

            // Controlling the relay_led
            if (tens_place == 1 || tens_place == 4) {  // 1 is relay should be on and no pin error. 4 is relay should be on and pin error
                digitalWrite(relay_led_pin, HIGH);
            } else if (tens_place == 0 || tens_place == 3) {  // 0 is relay should be off and no pin error. 3 is relay should be off and pin error
                digitalWrite(relay_led_pin, LOW);
            }

            // Setting error messages for the LCD
            // Relay errors
            if (ones_place == 5 || ones_place == 4) {  // The voltage across the relay is not what it should be
                relay_error_message = name + "relay FAIL";
            } else if (ones_place == 0 || ones_place == 1) {  // No relay failure
                relay_error_message = "";
            }
            // Pin errors
            if (tens_place == 3 || tens_place == 4) {  // There was a pin failure
                pin_error_message = name + " pin FAIL";
            } else if (tens_place == 0 || tens_place == 1) {  // No error
                pin_error_message = "";
            }
        }
};

class LatchingRelayDisplays {
    private: 
        unsigned int relay_led_pin;  // Represents if the AB saw the on signal and set the pin HIGH
        unsigned int state_led_pin;  // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
        String name;  // So our LCD messages have meaning
        String pin_error_message = "";  // If we come across a pin error state, update this string so the LCD can find it and print it
        String relay_error_message = "";  // If we come across a relay error state, update this string so the LCD can find it and print it
    
    public:
        int pin_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD
        int relay_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD

        LatchingRelayDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
            this->relay_led_pin = relay_led_pin;
            this->state_led_pin = state_led_pin;
            this->name = name;

            pinMode(relay_led_pin, OUTPUT);
            pinMode(state_led_pin, OUTPUT);
            digitalWrite(relay_led_pin, LOW);
            digitalWrite(state_led_pin, LOW);
        }

        String get_pin_error_message() {
            // Returns what pin error message string we currently have
            return pin_error_message;
        }
        String get_relay_error_message() {
            // Returns what relay error message string we currently have
            return relay_error_message;
        }

        void control_leds(unsigned int status) {
            // The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
            unsigned int ones_place = status % 10;
            unsigned int tens_place = (status % 100) / 10;

            // Controlling the state_led (ones_place)
            if (ones_place == 1 || ones_place == 4) {  // 1 is on and no relay error. 4 is on and relay error
                digitalWrite(state_led_pin, HIGH);
            } else if (ones_place == 0 || ones_place == 3) {  // 0 is off and no relay error. 3 is off and relay error
                digitalWrite(state_led_pin, LOW);
            }

            // Controlling the relay_led
            if (tens_place == 1 || tens_place == 4 || tens_place == 6 || tens_place == 9) {  // 1 is relay should be on and no pin error. 4 is relay should be on and setpin error. 6 is relay should be on and resetpin error. 9 is relay should be on and both pin errors
                digitalWrite(relay_led_pin, HIGH);
            } else if (tens_place == 0 || tens_place == 3 || tens_place == 5 || tens_place == 8) {  // 0 is relay should be off and no pin error. 3 is relay should be off and setpin error. 5 is relay should be off and resetpin error. 8 is relay should be off and both pin errors
                digitalWrite(relay_led_pin, LOW);
            }

            // Setting error messages for the LCD
            // Relay errors
            if (ones_place == 3 || ones_place == 4) {  // The voltage across the relay is not what it should be
                relay_error_message = name + " relay FAIL";
            } else if (ones_place == 0 || ones_place == 1) {  // No relay failure
                relay_error_message = "";
            }
            // Pin failure messages
            if (tens_place == 3 || tens_place == 4) {  // There was only a setpin failure
                pin_error_message = name + " R pin FAIL";
            } else if (tens_place == 5 || tens_place == 6) {  // There was only a resetpin failure
                pin_error_message = name + " R pin FAIL";
            } else if (tens_place == 8 || tens_place == 9) {  // Both pins failed
                pin_error_message = name + " R&S pin FAIL";
            } else if (tens_place == 0 || tens_place == 1) {  // No error
                pin_error_message = "";
            }
        }
};

class LatchingSolenoidDisplays {
    private:
        unsigned int relay_led_pin;  // Represents if the AB saw the on signal and set the pin HIGH
        unsigned int state_led_pin;  // Represents if the AB actually saw a voltage across the relay and thinks the valve was opened
        String name;  // So our LCD messages have meaning
        String pin_error_message = "";  // If we come across a pin error state, update this string so the LCD can find it and print it
        String relay_error_message = "";  // If we come across a relay error state, update this string so the LCD can find it and print it

    public:
        int pin_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD
        int relay_error_message_row = -1;  // The row that the error message is on the LCD. -1 means it is not on the LCD
        LatchingSolenoidDisplays (unsigned int relay_led_pin, unsigned int state_led_pin, String name) {
            this->relay_led_pin = relay_led_pin;
            this->state_led_pin = state_led_pin;
            this->name = name;

            pinMode(relay_led_pin, OUTPUT);
            pinMode(state_led_pin, OUTPUT);
            digitalWrite(relay_led_pin, LOW);
            digitalWrite(state_led_pin, LOW);
        }

        String get_pin_error_message() {
            // Returns what pin error message string we currently have
            return pin_error_message;
        }
        String get_relay_error_message() {
            // Returns what relay error message string we currently have
            return relay_error_message;
        }

        void control_leds(unsigned int on_relay_status, unsigned int off_relay_status) {
            // The AB sends back a 3 digit integer encoded with the possible errors/states of the relays
            unsigned int on_ones_place = on_relay_status % 10;
            unsigned int on_tens_place = (on_relay_status % 100) / 10;
            unsigned int on_hundreds_place = (on_relay_status % 1000) / 100;
            // This is redundant to the on_ones_place. unsigned int off_ones_place = off_relay_status % 10;
            unsigned int off_tens_place = (off_relay_status % 100) / 10;
            unsigned int off_hundreds_place = (off_relay_status % 1000) / 100;

            // Controlling the state_led (ones_place)
            if (on_ones_place == 1 || on_ones_place == 4) {  // 1 is on and no relay error. 4 is on and relay error
                digitalWrite(state_led_pin, HIGH);
            } else if (on_ones_place == 0 || on_ones_place == 3) {  // 0 is off and no relay error. 3 is off and relay error
                digitalWrite(state_led_pin, LOW);
            }
            // Controlling the relay_led
            if (on_tens_place == 1 || on_tens_place == 4) {  // 1 is desired to be off and no pin error. 4 is on and relay error
                digitalWrite(relay_led_pin, HIGH);
            } else if (on_tens_place == 0 || on_tens_place == 3) {  // 0 is desired to be off and no pin error. 3 is off and relay error
                digitalWrite(relay_led_pin, LOW);
            }
            // Pin failure message
            bool on_pin_fail = on_tens_place == 3 || on_tens_place == 4;
            bool off_pin_fail = off_tens_place == 3 || off_tens_place == 4;
            if (on_pin_fail && off_pin_fail) {  // There was a pin failure for both the ON and OFF relays
                pin_error_message = name + " both pin FAIL";
            } else if (on_pin_fail) {  // Only the ON pin failed
                pin_error_message = name + " ON pin FAIL";
            } else if (off_pin_fail) {  // Only the OFF pin failed
                pin_error_message = name + " OFF pin FAIL";
            } else {  // Neither pin failed
                pin_error_message = "";
            }
            // Relay failure message
            bool on_relay_fail = on_hundreds_place == 1 || on_hundreds_place == 2;
            bool off_relay_fail = off_hundreds_place == 1 || off_hundreds_place == 2;
            if (on_relay_fail && off_relay_fail) {  // There was a relay failure for both the ON and OFF relays
                relay_error_message = name + " relays FAIL";
            } else if (on_relay_fail) {  // Only the ON relay failed
                relay_error_message = name + " ON relay FAIL";
            } else if (off_relay_fail) {  // Only the OFF relay failed
                relay_error_message = name + " OFF relay FAIL";
            } else {  // Neither relay failed
                relay_error_message = "";
            }
        }

};

NonLatchingRelayDisplays supply_fill_leds(SUPPLY_FILL_RELAY_LED, SUPPLY_FILL_LED, "Sup Fill");  // Even though it is a servo, it is compatible with this class
LatchingRelayDisplays supply_vent_leds(SUPPLY_VENT_RELAY_LED, SUPPLY_VENT_LED, "Sup Vnt");
LatchingRelayDisplays igniter_leds(IGNITER_RELAY_LED, IGNITER_LED, "Igniter");
LatchingSolenoidDisplays bvas_leds(BVAS_RELAY_LED, BVAS_LED, "BVAS");
LatchingSolenoidDisplays run_vent_leds(RUN_VENT_RELAY_LED, RUN_VENT_LED, "Rn Vnt");

void control_all_leds(const struct can_frame *ab_response, const struct switch_inputs_struct *switch_inputs) {
    // Control the arming LEDS
    digitalWrite(FLUID_ARMING_LED, switch_inputs->fluid_arming);
    digitalWrite(BVAS_ARMING_LED, switch_inputs->bvas_arming);
    digitalWrite(IGNITER_ARMING_LED, switch_inputs->igniter_arming);

    // COMMS LEDs
    digitalWrite(AB_COMMS_LED, comms_with_ab);

    // Controlling all LEDs associated with the actuator box
    supply_fill_leds.control_leds(ab_response->data[0]);
    supply_vent_leds.control_leds(ab_response->data[1]);
    run_vent_leds.control_leds(ab_response->data[2], ab_response->data[3]);
    bvas_leds.control_leds(ab_response->data[4], ab_response->data[5]);
    igniter_leds.control_leds(ab_response->data[6]);


    unsigned int igniter_hundreds_place = (ab_response->data[6] % 1000) / 100;
    // Specifically the igniter continuity LED
    digitalWrite(IGNITER_CONTINUITY_LED, igniter_hundreds_place);
}

void control_ab_related_leds(const struct can_frame *ab_response) {
    // Controlling all LEDs associated with the actuator box
    supply_fill_leds.control_leds(ab_response->data[0]);
    supply_vent_leds.control_leds(ab_response->data[1]);
    run_vent_leds.control_leds(ab_response->data[2], ab_response->data[3]);
    bvas_leds.control_leds(ab_response->data[4], ab_response->data[5]);
    igniter_leds.control_leds(ab_response->data[6]);


    unsigned int igniter_hundreds_place = (ab_response->data[6] % 1000) / 100;
    // Specifically the igniter continuity LED
    digitalWrite(IGNITER_CONTINUITY_LED, igniter_hundreds_place);
}

void control_cb_related_leds(const struct switch_inputs_struct *switch_inputs) {
    // Control the arming LEDS
    digitalWrite(FLUID_ARMING_LED, switch_inputs->fluid_arming);
    digitalWrite(BVAS_ARMING_LED, switch_inputs->bvas_arming);
    digitalWrite(IGNITER_ARMING_LED, switch_inputs->igniter_arming);

    // COMMS LEDs
    digitalWrite(AB_COMMS_LED, comms_with_ab);
}

void write_errors_to_lcd() {
    // Loop through all relay "pin_error_message" & "relay_error_message" and write as much as we can.

    // Supply fill
    if (supply_fill_leds.get_pin_error_message() != "") {  // If there actually is an error message
        supply_fill_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(supply_fill_leds.get_pin_error_message());
    }
    if (supply_fill_leds.get_relay_error_message() != "") {  // If there actually is an error message
        supply_fill_leds.relay_error_message_row = status_lcd.add_string_to_desired_text(supply_fill_leds.get_relay_error_message());
    }

    // Supply vent
    if (supply_vent_leds.get_pin_error_message() != "") {  // If there actually is an error message
        supply_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(supply_vent_leds.get_pin_error_message());
    }
    if (supply_vent_leds.get_relay_error_message() != "") {  // If there actually is an error message
        supply_vent_leds.relay_error_message_row = status_lcd.add_string_to_desired_text(supply_vent_leds.get_relay_error_message());
    }

    // Igniter
    if (igniter_leds.get_pin_error_message() != "") {  // If there actually is an error message
        igniter_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(igniter_leds.get_pin_error_message());
    }
    if (igniter_leds.get_relay_error_message() != "") {  // If there actually is an error message
        igniter_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(igniter_leds.get_relay_error_message());
    }

    // BVAS
    if (bvas_leds.get_pin_error_message() != "") {  // If there actually is an error message
        bvas_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(bvas_leds.get_pin_error_message());
    }
    if (bvas_leds.get_relay_error_message() != "") {  // If there actually is an error message
        bvas_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(bvas_leds.get_relay_error_message());
    }

    // Run vent
    if (run_vent_leds.get_pin_error_message() != "") {  // If there actually is an error message
        run_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(run_vent_leds.get_pin_error_message());
    }
    if (run_vent_leds.get_relay_error_message() != "") {  // If there actually is an error message
        run_vent_leds.pin_error_message_row = status_lcd.add_string_to_desired_text(run_vent_leds.get_relay_error_message());
    }

    status_lcd.set_lcd_to_desired_text();  // Actually update the screen
}

void write_igniter_countup() {
    // TODO. In the bottom right of the LCD, write 3 places (00.0) of time for a timer after ignition
}

bool establish_can_with_ab() {
    // Establishes CAN with the Actuator Box.

    comms_with_ab = false;
    struct can_frame ab_command;
    ab_command.can_id  = 5;
    ab_command.can_dlc = 8;
    ab_command.data[0] = 1;
    ab_command.data[1] = 1;
    ab_command.data[2] = 1;
    ab_command.data[3] = 1;
    ab_command.data[4] = 1;
    ab_command.data[5] = 1;
    ab_command.data[6] = 1;
    ab_command.data[7] = 1;

    mcp2515.sendMessage(&ab_command);
    unsigned long last_ping_sent = millis();

    while(comms_with_ab == false) {
        struct can_frame ab_response;
        if (mcp2515.readMessage(&ab_response) == MCP2515::ERROR_OK) {  // We received a CAN message
            ///Serial.println("Received CAN from AB");
            if (ab_response.can_id == 5 && ab_response.data[0] == 2) {  // AB is acknowledging our ping
                ///Serial.println("AB was responding to our ping");
                comms_with_ab = true;
                last_ab_can_message_received = millis();
                ab_command.data[0] = 3;
                ab_command.data[1] = 3;
                ab_command.data[2] = 3;
                ab_command.data[3] = 3;
                ab_command.data[4] = 3;
                ab_command.data[5] = 3;
                ab_command.data[6] = 3;
                ab_command.data[7] = 3;
                delay(15);  // I like waiting for the line to stabilize and give time
                ///Serial.println("Telling AB we heard it");
                ///Serial.println("2-Way CAN established");
                mcp2515.sendMessage(&ab_command);  // Tell AB we heard it
            }
        } else {
            if (millis() - last_ping_sent >= AB_CAN_INTERVAL) {  // Wait some time before pinging again
                ///Serial.println("Sent ping to AB");
                mcp2515.sendMessage(&ab_command);
                last_ping_sent = millis();
            }
      
        }
    }
    return true;
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*-------------------------------------EMERGENCY PROCEDURE/STATE MANAGEMENT-------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

// Global variables to keep track of what the actuator box has told us is the state of the system
struct ab_states_struct {
    bool igniter = false;
    bool bvas = false;
    bool supply_fill = false;
    bool supply_vent = false;
    bool run_vent = false;
};

ab_states_struct ab_states;

void update_system_states(const struct can_frame *ab_response) {
    unsigned int ones_place; // Used for parsing the data

    // Supply fill
    ones_place = (ab_response->data[0] % 10);
    if (ones_place == 1) {  // Supply fill is open
        ab_states.supply_fill = true;
    } else {
        ab_states.supply_fill = false;
    }
    // Supply vent
    ones_place = (ab_response->data[1] % 10);
    if (ones_place == 1 || ones_place == 4) {  // Supply fill is open
        ab_states.supply_vent = true;
    } else {
        ab_states.supply_vent = false;
    }
    // Run tank vent
    ones_place = (ab_response->data[2] % 10);
    if (ones_place == 1 || ones_place == 4) {  // Run tank vent is open
        ab_states.run_vent = true;
    } else {
        ab_states.run_vent = false;
    }
    // BVAS
    ones_place = (ab_response->data[4] % 10);
    if (ones_place == 1 || ones_place == 4) {  // BVAS is open
        ab_states.bvas = true;
    } else {
        ab_states.bvas = false;
    }
    // Igniter
    ones_place = (ab_response->data[6] % 10);
    if (ones_place == 1 || ones_place == 4) {  // Supply fill is open
        ab_states.igniter = true;
    } else {
        ab_states.igniter = false;
    }
}

void abort_loop() {
    // When the control presses the abort switch.
    // Igniter off, close supply fill, open run vent, open supply vent, open bvas
    const unsigned long abort_initiated_time = millis();  // This is the time we first turned on the abort button. Needed to make sure we give supply fill adequate time to close
    switch_inputs_struct switch_inputs;

    can_frame command_message;
    command_message.can_id = 5;
    command_message.can_dlc = 8;
    command_message.data[0] = 0;  // Close the supply fill
    command_message.data[4] = 0;  // Turn off the igniter
    command_message.data[5] = 9;  // Tell the AB we are aborting
    command_message.data[6] = 9;  // Tell the AB we are aborting
    command_message.data[7] = 9;  // Tell the AB we are aborting

    can_frame ab_response;

    // Loop until the abort switch is turned off
    bool supply_fill_open = ab_states.supply_fill;  // Wanted so we actually wait for supply fill to close
    while (1) {
        if (supply_fill_open == true) {  // If the supply fill is open, we should close it before opening the vents
            command_message.data[1] = 3;  // Tell AB to keep the supply vent state
            command_message.data[2] = 3;  // Tell AB to keep the run vent state
            command_message.data[3] = 3;  // Tell AB to keep the BVAS state
            if (millis() - abort_initiated_time >= 1000) {  // Has been enough time for the supply fill to close and for us to vent the lines
                supply_fill_open = false;  // It has now been closed
            }
        } else {  // Supply fill valve is no longer open
            command_message.data[1] = 1;  // Open the supply vent
            command_message.data[2] = 1;  // Open the run tank vent
            command_message.data[3] = 3;  // Tell AB to keep the BVAS state
        }

        // We will send our message now
        if (millis() - last_ab_can_message_sent >= (AB_CAN_INTERVAL)) {  // Only send at most every whatever time specified
            print_can_frame(command_message);
            mcp2515.sendMessage(&command_message);
            last_ab_can_message_sent = millis();
        }

        if (read_from_ab(&ab_response)) {  // Get the AB response if there is one. If not, the we keep the same response from previous
            update_system_states(&ab_response);  // Update the control box's tracking of states based upon the AB's response
            control_ab_related_leds(&ab_response);
        }   
    
    
        // Control all of the LEDs on the display board
        // control_all_leds(&ab_response, &switch_inputs);
        read_switch_states(&switch_inputs);
        control_cb_related_leds(&switch_inputs);

        comms_with_ab = (millis() - last_ab_can_message_received >= TIME_TO_INIT_PAUSE_STATE) ? false : true;

        // Control the LCD on the display board
        write_errors_to_lcd();  // FIXME?

        if (switch_inputs.abort == false) {  // Only leave if the emergency switch is turned off
            break;
        }
    }
}

bool ab_entered_pause_state() {
    // Returns true if enough time has passed that we believe the AB would have entered its pause state
    if (millis() - last_ab_can_message_received > TIME_TO_INIT_PAUSE_STATE) {
        return true;
    } else {
        return false;
    }
}

bool ab_entered_abort_state() {
    // Returns true if enough time has passed that we believe the AB would have entered its abort state
    if (millis() - last_ab_can_message_received > TIME_TO_INIT_ABORT_STATE) {
        return true;
    } else {
        return false;
    }
}

bool reestablish_can_with_ab() {
    // Reestablishes CAN with the Actuator Box.

    comms_with_ab = false;
    struct can_frame ab_command;
    ab_command.can_id  = 5;
    ab_command.can_dlc = 8;
    ab_command.data[0] = 1;
    ab_command.data[1] = 1;
    ab_command.data[2] = 1;
    ab_command.data[3] = 1;
    ab_command.data[4] = 1;
    ab_command.data[5] = 1;
    ab_command.data[6] = 1;
    ab_command.data[7] = 1;

    ///Serial.println("Sent ping to AB");
    mcp2515.sendMessage(&ab_command);
    unsigned long last_ping_sent = millis();

    struct can_frame ab_response;

    while (millis() - last_ping_sent < AB_CAN_INTERVAL) {
        if (mcp2515.readMessage(&ab_response) == MCP2515::ERROR_OK) {  // We received a CAN message
            //Serial.println("Received ackowledgement from AB");
        if (ab_response.can_id == 5 && ab_response.data[0] == 2) {  // AB is acknowledging our ping
            comms_with_ab = true;
            last_ab_can_message_received = millis();
            ab_command.data[0] = 3;
            ab_command.data[1] = 3;
            ab_command.data[2] = 3;
            ab_command.data[3] = 3;
            ab_command.data[4] = 3;
            ab_command.data[5] = 3;
            ab_command.data[6] = 3;
            ab_command.data[7] = 3;
            delay(50);  // I like waiting for the line to stabilize and give time
            //Serial.print("Sent AB message with ID: "); ///Serial.print(ab_command.can_id); ///Serial.print(" And data[0]: "); ///Serial.println(ab_command.data[0]);
            mcp2515.sendMessage(&ab_command);  // Tell AB we heard it
            return true;
            }
        }
    }
    return false;  // No comms yet
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*----------------------------------------------------SETUP-----------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

void setup() {
    Serial.begin(115200);
    pinMode(EMERGENCY_SW, INPUT);
    pinMode(SUPPLY_FILL_SW, INPUT);
    pinMode(AUTO_FILL_SW, INPUT);
    pinMode(IGNITER_ARMING_SW, INPUT);
    pinMode(RUN_VENT_SW, INPUT);
    pinMode(BVAS_ARMING_SW, INPUT);
    pinMode(BVAS_SW, INPUT);
    pinMode(IGNITER_SW, INPUT);
    pinMode(SUPPLY_VENT_SW, INPUT);
    pinMode(FLUID_ARMING_SW, INPUT);

    pinMode(FLUID_ARMING_LED, OUTPUT);
    pinMode(IGNITER_ARMING_LED, OUTPUT);
    pinMode(BVAS_ARMING_LED, OUTPUT);
    pinMode(ENGINE_NODE_COMMS_LED, OUTPUT);
    pinMode(AB_COMMS_LED, OUTPUT);
    pinMode(AUTO_FILL_LED, OUTPUT);
    pinMode(IGNITER_CONTINUITY_LED, OUTPUT);

    lcd_lib.init();
    lcd_lib.backlight();

    status_lcd.clear_desired_text();
    status_lcd.clear_lcd();
    status_lcd.add_string_to_desired_text("Connecting to AB...", 0);
    status_lcd.set_lcd_to_desired_text();

    mcp2515.reset();
    mcp2515.setBitrate(CAN_125KBPS);
    mcp2515.setNormalMode();

    // Initializes and establishes CAN comms to AB
    ///Serial.println("Connecting to AB");
    establish_can_with_ab();
    status_lcd.clear_lcd();
    status_lcd.clear_desired_text();
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*-----------------------------------------------------LOOP-----------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

void loop() {
    static switch_inputs_struct switch_inputs;
    static can_frame command_message;
    static can_frame ab_response;

    // Check for any lost comms and try to predict what is going on with the actuator box
    if (ab_entered_pause_state() == true) {  // We think the AB went into a pause state because we haven't seen comms in a bit
        ///Serial.println("AB entered pause state");
        comms_with_ab = false;
        pause_state_entered_time = millis();
        digitalWrite(AB_COMMS_LED, LOW);
        status_lcd.clear_lcd();
        status_lcd.clear_desired_text();
        status_lcd.clear_desired_text(0);
        status_lcd.add_string_to_desired_text("Bad Comms|AB PAUSED", 0);
        status_lcd.clear_desired_text(1);
        status_lcd.add_string_to_desired_text("Supply fill, vent,", 1);
        status_lcd.clear_desired_text(2);
        status_lcd.add_string_to_desired_text("Run vent closed", 2);
        status_lcd.set_lcd_to_desired_text();  // Actually update the screen
        unsigned long last_updated_time_for_pause_lcd = 0;

        while (ab_entered_abort_state() == false) {  // Loop for as long as we can until we think the AB went into the abort state
            if (reestablish_can_with_ab()) {  // If we managed to reestablish comms
                ///Serial.println("Reestablished CAN with AB");
                digitalWrite(AB_COMMS_LED, HIGH);
                break;  // We reestablished so resume normal comms
            }
            if (millis() - last_updated_time_for_pause_lcd >= 250) {
                // LCD message counting time since lost comms
                String foo = String((TIME_TO_INIT_ABORT_STATE - (millis() - pause_state_entered_time)) / 1000) + "." + String(((TIME_TO_INIT_ABORT_STATE - (millis() - pause_state_entered_time)) / 100)%10) + "s until ABORT";
                status_lcd.clear_desired_text(3);
                status_lcd.add_string_to_desired_text(foo, 3);
                status_lcd.set_lcd_to_desired_text();  // Actually update the screen
                last_updated_time_for_pause_lcd = millis();
            }
            read_switch_states(&switch_inputs);
            control_cb_related_leds(&switch_inputs);
        }
        // At this point we the AB either aborted or we reestablished comms
        if (ab_entered_abort_state() == true) {
            //Serial.println("AB ABORTED");
            status_lcd.clear_lcd();
            status_lcd.clear_desired_text(0);
            status_lcd.add_string_to_desired_text("AB ABORTED", 0);
            status_lcd.set_lcd_to_desired_text();  // Actually update the screen
            while (1) {
                read_switch_states(&switch_inputs);
                control_cb_related_leds(&switch_inputs);
            }
        }
        // If we are at this point then we reestablished comms
        status_lcd.clear_lcd();
        status_lcd.clear_desired_text();
    }

    // Lets get the most current state of the switches
    read_switch_states(&switch_inputs);

    // Check to see if we need to be in the abort priority loop because of the switch
    if (switch_inputs.abort == true) {
        status_lcd.clear_desired_text();  // Reset all to blank
        status_lcd.clear_lcd();
        status_lcd.clear_desired_text(0);
        status_lcd.add_string_to_desired_text("ABORT INITIATED", 0);
        status_lcd.set_lcd_to_desired_text();  // Actually update the screen
        ///Serial.println("ABORT LOOP Entered");

        abort_loop();  // Auto-loops itself until the abort switch is turned off

        ///Serial.println("Left abort loop");
        status_lcd.clear_lcd();
        status_lcd.clear_desired_text();

        // Let's get the most current state of the switches in case they were changed while we were aborting
        read_switch_states(&switch_inputs);
    }

    // Update the command CAN message and send it
    update_can_command_message(&command_message, &switch_inputs);
    if (millis() - last_ab_can_message_sent >= AB_CAN_INTERVAL) {  // Only send at most every whatever time specified
        mcp2515.sendMessage(&command_message);
        last_ab_can_message_sent = millis();
    }

    if (read_from_ab(&ab_response)) {  // Get the AB response if there is one. If not, the we keep the same response from previous
        update_system_states(&ab_response);  // Update the control box's tracking of states based upon the AB's response
        control_ab_related_leds(&ab_response);
    }   
    
    
    // Control all of the LEDs on the display board
    // control_all_leds(&ab_response, &switch_inputs);
    control_cb_related_leds(&switch_inputs);

    // Control the LCD on the display board
    write_errors_to_lcd();
}


void print_can_frame(struct can_frame& can_message) {
    Serial.println("CAN FRAME:");
    Serial.print("CAN ID"); Serial.println(can_message.can_id);
    Serial.print("CAN DLC"); Serial.println(can_message.can_dlc);

    Serial.print("DATA 0 - Supply Fill: "); Serial.println(can_message.data[0]);
    Serial.print("DATA 1 - Supply Vent: "); Serial.println(can_message.data[1]);
    Serial.print("DATA 2 - Run Tank Vent: "); Serial.println(can_message.data[2]);
    Serial.print("DATA 3 - BVAS: "); Serial.println(can_message.data[3]);
    Serial.print("DATA 4 - Igniter: "); Serial.println(can_message.data[4]);
    Serial.print("DATA 5: "); Serial.println(can_message.data[5]);
    Serial.print("DATA 6: "); Serial.println(can_message.data[6]);
    Serial.print("DATA 7: "); Serial.println(can_message.data[7]);

    Serial.println("-------------------------------------------------\n");
}