// Digital write pins to turn on the relays and readback if the Arduino pin is actually going HIGH or LOW
#define IGNITER_CMD_ON 26
#define IGNITER_CMD_ON_READBACK 27
#define IGNITER_CMD_OFF 28
#define IGNITER_CMD_OFF_READBACK 29
#define SUPPLY_FILL_CMD 30 // Not needed, changed to servo
#define SUPPLY_FILL_CMD_READBACK 31 // Not needed, changed to servo
#define SUPPLY_VENT_CMD_ON 32
#define SUPPLY_VENT_CMD_ON_READBACK 33
#define SUPPLY_VENT_CMD_OFF 34
#define SUPPLY_VENT_CMD_OFF_READBACK 35
#define RUN_VENT_CMD_ON 36
#define RUN_VENT_CMD_ON_READBACK 37
#define RUN_VENT_CMD_OFF 38
#define RUN_VENT_CMD_OFF_READBACK 39
#define SPARE1_CMD_ON 40
#define SPARE1_CMD_ON_READBACK 41
#define SPARE1_CMD_OFF 42
#define SPARE1_CMD_OFF_READBACK 43
#define SPARE2_CMD_ON 44
#define SPARE2_CMD_ON_READBACK 45
#define SPARE2_CMD_OFF 46
#define SPARE2_CMD_OFF_READBACK 47
#define SPARE3_CMD 48
#define SPARE3_CMD_READBACK 49

// Relay voltage readbacks
#define IGNITER_CONTINUITY_CHECK A0
#define IGNITER_VOLTAGE_CHECK A1
#define SUPPLY_FILL_VOLTAGE_CHECK A2  // Not needed, changed to servo
#define SUPPLY_VENT_VOLTAGE_CHECK A3
#define RUN_VENT_ON_VOLTAGE_CHECK A6
#define RUN_VENT_OFF_VOLTAGE_CHECK A12
#define SPARE1_VOLTAGE_CHECK A13
#define SPARE2_VOLTAGE_CHECK A14
#define SPARE3_VOLTAGE_CHECK A15

// Just for the BVAS control while we wait for the plumbing board and its systems to be integrated
#define BVAS_CMD_ON 30
#define BVAS_CMD_ON_READBACK 31
#define BVAS_ON_VOLTAGE_CHECK A2
#define BVAS_CMD_OFF 48
#define BVAS_CMD_OFF_READBACK 49
#define BVAS_OFF_VOLTAGE_CHECK A15

#define ENABLE_BVAS_SERVO true  // This tells the actuator box to not use the pneumatic BVAS and to instead use the servo bvas
#define ENABLE_BVAS_DELAY false  // We we want to have a delay from receiving MDOT command to actually opening the valve
#define BVAS_DELAY 7000  // Milleseconds to delay for BVAS

/*--------------------------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------ETHERNET COMMS----------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
#define ENABLE_ETHERNET_COMMS false  // Whether or not we are using ethernet comms at all or not

#if (ENABLE_ETHERNET_COMMS == true)
    #define ETHERNET_CS_PIN 25  // CS pin on PCB to the ethernet module

    #ifndef EthernetUDPWrapper_h
    #include "EthernetUDPWrapper.h"
    #endif

    unsigned int port = 5000;
    IPAddress ip(192, 168, 0, 115);  // IP address of the Actuator Box. Set the router to bind to this IP address. Also update DAQ GUI code to match. IP address can change to whatever is needed

    EthernetUDPWrapper UDPWrapper(ETHERNET_CS_PIN, port, ip);
#endif


/*--------------------------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------RELAY CONTROL-----------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
// Used for latching relay control
#ifndef LatchingRelay_h
#include "LatchingRelay.h"
#endif

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

// Used for solenoids that are latching (run-vent, BVAS)
#ifndef LatchingSolenoid_h
#include "LatchingSolenoid.h"
#endif

// Defining relay objects 
LatchingRelay igniter_relay("Igniter Relay", IGNITER_CMD_ON, IGNITER_CMD_ON_READBACK, IGNITER_CMD_OFF, IGNITER_CMD_OFF_READBACK, IGNITER_VOLTAGE_CHECK);
LatchingRelay supply_vent_relay("Supply Vent Relay", SUPPLY_VENT_CMD_ON, SUPPLY_VENT_CMD_ON_READBACK, SUPPLY_VENT_CMD_OFF, SUPPLY_VENT_CMD_OFF_READBACK, SUPPLY_VENT_VOLTAGE_CHECK);
LatchingRelay spare1_relay(SPARE1_CMD_ON, SPARE1_CMD_ON_READBACK, SPARE1_CMD_OFF, SPARE1_CMD_OFF_READBACK, SPARE1_VOLTAGE_CHECK);
LatchingRelay spare2_relay(SPARE2_CMD_ON, SPARE2_CMD_ON_READBACK, SPARE2_CMD_OFF, SPARE2_CMD_OFF_READBACK, SPARE2_VOLTAGE_CHECK);

LatchingSolenoid run_vent_solenoid("Run Vent Solenoid", RUN_VENT_CMD_ON, RUN_VENT_CMD_ON_READBACK, RUN_VENT_CMD_OFF, RUN_VENT_CMD_OFF_READBACK, RUN_VENT_ON_VOLTAGE_CHECK, RUN_VENT_OFF_VOLTAGE_CHECK);

#if (ENABLE_BVAS_SERVO == false)
    LatchingSolenoid bvas_solenoid("BVAS Solenoid", BVAS_CMD_ON, BVAS_CMD_ON_READBACK, BVAS_CMD_OFF, BVAS_CMD_OFF_READBACK, BVAS_ON_VOLTAGE_CHECK, BVAS_OFF_VOLTAGE_CHECK);
#endif

bool igniter_continuity_check(unsigned int pin) {
    unsigned int raw_read = analogRead(pin);  // maps (0V, 5V) to (0, 1023)
    float voltage = raw_read*(5.0/1024);  // Voltage at the analog pin

   if (voltage < 0.75) {  // Igniter is connected
       return true;
   } else {
      return false;
   }
}


/*--------------------------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------SERVO-------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

// Servo stuff
#ifndef Servo_h
#include <Servo.h>
#endif
#define SUPPLY_FILL_SERVO_PIN 11 // Use the unused connectors
Servo supply_fill_servo; // 90 is closed, 0 is open
bool supply_fill_servo_state = false;  // False for closed, true for open

#if (ENABLE_BVAS_SERVO == true)
    // Below is everything for the servo main oxidizer valve
    #define BVAS_SERVO_PIN 9  // Use the unused connectors
    #define BVAS_SERVO_FULL_CLOSED 0  // Angle to write when full closed
    #define BVAS_SERVO_FULL_OPEN 90  // Angle to write when full open. Currently 120 because of 48:32 gear reduction

    Servo bvas_servo;
    int bvas_servo_angle;  // 0 is closed, 90 is full open
    bool bvas_servo_desired_state = false;  // False for closed, true for open any angle
    bool bvas_servo_actual_state = false;  // False for closed, true for open any angle

    const unsigned int amplitude = 20;  // 20 degree amplitude
    const double angular_frequency = 2*PI / 3000.0;  // Angular frequency such that period = 3000ms
    const unsigned int vertical_offset = 35;  // Vertical offset to sin function
    const unsigned int phase_shift = PI;  // How much to shift the sin function

    unsigned int calc_bvas_servo_angle(unsigned long time_since_mdot) {
        return amplitude * cos(angular_frequency * time_since_mdot - phase_shift) + vertical_offset;
    }

    void bvas_servo_set_desired_state(unsigned char new_desired_state) {
        if (new_desired_state == 0) {  // Should be closed
            bvas_servo_desired_state = false;
        } else if (new_desired_state == 3) {  // Keep last desired state
            bvas_servo_desired_state = bvas_servo_desired_state;
        } else if (new_desired_state == 1) {  // Open
            bvas_servo_desired_state = true;
        }
    }
    
    void control_bvas_servo() {
        static unsigned long mdot_time;
        if (bvas_servo_desired_state == false) {
            bvas_servo.write(BVAS_SERVO_FULL_CLOSED);  // Close
            bvas_servo_actual_state = false;
        } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == false) {  // This is the first time we are seeing the "open" command which means begin mdot
            mdot_time = millis();
            bvas_servo_actual_state = true;

            unsigned int desired_degrees_open = calc_bvas_servo_angle(0);
            bvas_servo_angle = map(desired_degrees_open, 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
            bvas_servo.write(bvas_servo_angle);
        } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == true) {  // Already begun mdot, let's continue mdotting
            unsigned long time_since_mdot = millis() - mdot_time;
            unsigned int desired_degrees_open = calc_bvas_servo_angle(time_since_mdot);
            bvas_servo_angle = map(desired_degrees_open, 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
            bvas_servo.write(bvas_servo_angle);
            bvas_servo_actual_state = true;
        }
    }

    // #define NUM_BVAS_SERVO_ANGLE_VALUES 5  // Tells how many angle values we will provide for the bvas servo
    // /*2D array with the 1st row being the times after mdot in milliseconds 
    // and the 2nd row being the desired angle openings of the servo
    // ***please note, this is using 0 as full closed and 90 as full open. Do not do any conversion here if the
    // servo is oriented opposite. That conversion is taken care of if you set the above definitions correctly. Thanks. - Judson*/
    // const unsigned int bvas_servo_times_with_desired_angles[2][NUM_BVAS_SERVO_ANGLE_VALUES] = {
    //     /*Times in (ms)*/       {0,      4000,    7000,   8000,   9000,},
    //     /*Angle in degrees*/    {0,        10,      90,     70,     90,}
    // };

    // void bvas_servo_set_desired_state(unsigned char new_desired_state) {
    //     if (new_desired_state == 0) {  // Should be closed
    //         bvas_servo_desired_state = false;
    //     } else if (new_desired_state == 3) {  // Keep last desired state
    //         bvas_servo_desired_state = bvas_servo_desired_state;
    //     } else if (new_desired_state == 1) {  // Open
    //         bvas_servo_desired_state = true;
    //     }
    // }

    // void control_bvas_servo() {
    //     static unsigned long mdot_time;
    //     if (bvas_servo_desired_state == false) {
    //         bvas_servo.write(BVAS_SERVO_FULL_CLOSED);  // Close
    //         bvas_servo_actual_state = false;
    //     } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == false) {  // This is the first time we are seeing the "open" command which means begin mdot
    //         mdot_time = millis();
    //         bvas_servo_actual_state = true;
    //         bvas_servo_angle = map(bvas_servo_times_with_desired_angles[1][0], 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
    //         //bvas_servo_angle = bvas_servo_times_with_desired_angles[1][0];
    //         bvas_servo.write(bvas_servo_angle);
    //     } else if (bvas_servo_desired_state == true && bvas_servo_actual_state == true) {  // Already begun mdot, let's continue mdotting
    //         unsigned int time_index_to_use = 0;
    //         for (unsigned int i = 0; i < NUM_BVAS_SERVO_ANGLE_VALUES; i++) {
    //             unsigned long time_since_mdot = millis() - mdot_time;
    //             if (time_since_mdot >= bvas_servo_times_with_desired_angles[0][i]) {  // We have at least gotten to this time range
    //                 time_index_to_use = i;
    //             } else {  // We have not gotten to this new time value so continue using the last index
    //                 break;
    //             }
    //         }
    //         bvas_servo_angle = map(bvas_servo_times_with_desired_angles[1][time_index_to_use], 0, 90, BVAS_SERVO_FULL_CLOSED, BVAS_SERVO_FULL_OPEN);
    //         //bvas_servo_angle = bvas_servo_times_with_desired_angles[1][time_index_to_use];
    //         bvas_servo.write(bvas_servo_angle);
    //         bvas_servo_actual_state = true;
    //     }
    // }
#endif



/*--------------------------------------------------------------------------------------------------------------------*/
/*------------------------------------------------CAN BUS-------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

// CAN Stuff
#ifndef SPI_h
#include <SPI.h>
#endif

#ifndef mcp2515_h
#include <mcp2515.h>
#endif

//#include "CANComms.h"

#define CAN_CS 53
MCP2515 mcp2515(CAN_CS);  // CAN CS pin is 53
struct can_frame received_msg;  // The struct we will constantly be using to receive from the CB
struct can_frame acknowledge_msg;  // The struct we will constantly be updating so we can send it as acknowledgement to the CB
bool comms_with_cb = false;
unsigned long last_cb_message_time;
#define TIME_BETWEEN_MESSAGES 100  // The ms between CAN reads/writes

#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  // 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;
unsigned long time_for_supply_fill_to_close = 1500;  // Give time for the supply fill to close

bool initialize_can_with_CB() {
    // Used to initialize CAN bus wth the CB. Only returns true once it has succesfully received, acknowledged, and received an acknowledgment from CB
    // Returning false can't really happen because it loops until 2-way communication is established

    unsigned long last_read_time = millis();

    while (true) {
        if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
            last_read_time = millis();
            if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {  // We have an incoming message
                //Serial.println("CAN message received");
                if (received_msg.can_id == 5) {  // CB is trying to ping the AB
                    unsigned long received_ping_time = millis();
                    if (received_msg.data[0] == 1) { // CB is doing its first ping to the AB
                        //Serial.println("Received ping from CB");
                        last_cb_message_time = millis();
                        acknowledge_msg.can_id = 5;
                        acknowledge_msg.can_dlc = 8;
                        acknowledge_msg.data[0] = 2; acknowledge_msg.data[1] = 2; acknowledge_msg.data[2] = 2; acknowledge_msg.data[3] = 2; acknowledge_msg.data[4] = 2; acknowledge_msg.data[5] = 2; acknowledge_msg.data[6] = 2; acknowledge_msg.data[7] = 2;
                        delay(100);
                        //Serial.println("Sending acknowledgement");
                        mcp2515.sendMessage(&acknowledge_msg);  // Send acknowledgement to CB
                        while (millis() - received_ping_time < 1000) {  // Now we wait for the message back from the CB to confirm 2-way communication
                            if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
                                last_read_time = millis();
                                if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {  // We have an incoming message
                                    //Serial.println("Received another CAN message");
                                    if (received_msg.can_id == 5 && received_msg.data[0] == 3) { // CB is acknowledging the acknowledgement from AB
                                        //Serial.println("2-Way CAN established");
                                        last_cb_message_time = millis();
                                        comms_with_cb = true;
                                        return true;
                                    } else {
                                        break;
                                    }
                                } else {
                                    //Serial.println("No second CAN message received");
                                }
                            }
                            
                        }
                    }
                }
            } else {
                //Serial.println("No CAN message received");
            }
        }
    }
}

bool reinitialize_can_with_CB() {
    // Used to initialize CAN bus wth the CB. Only returns true once it has succesfully received, acknowledged, and received an acknowledgment from CB
    // Returning false can't really happen because it loops until 2-way communication is established

    static unsigned long last_read_time = millis();

        if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
            last_read_time = millis();
            if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {  // We have an incoming message
                Serial.println("CAN message received");
                if (received_msg.can_id == 5) {  // CB is trying to ping the AB
                    unsigned long received_ping_time = millis();
                    if (received_msg.data[0] == 1) { // CB is doing its first ping to the AB
                        Serial.println("Received ping from CB");
                        last_cb_message_time = millis();
                        acknowledge_msg.can_id = 5;
                        acknowledge_msg.can_dlc = 8;
                        acknowledge_msg.data[0] = 2; acknowledge_msg.data[1] = 2; acknowledge_msg.data[2] = 2; acknowledge_msg.data[3] = 2; acknowledge_msg.data[4] = 2; acknowledge_msg.data[5] = 2; acknowledge_msg.data[6] = 2; acknowledge_msg.data[7] = 2;
                        delay(100);
                        Serial.println("Sending acknowledgement");
                        mcp2515.sendMessage(&acknowledge_msg);  // Send acknowledgement to CB
                        while (millis() - received_ping_time < 1000) {  // Now we wait for the message back from the CB to confirm 2-way communication
                            if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
                                last_read_time = millis();
                                if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {  // We have an incoming message
                                    Serial.println("Received another CAN message");
                                    if (received_msg.can_id == 5 && received_msg.data[0] == 3) { // CB is acknowledging the acknowledgement from AB
                                        Serial.println("2-Way CAN established");
                                        last_cb_message_time = millis();
                                        comms_with_cb = true;
                                        return true;
                                    }
                                } else {
                                    Serial.println("No CAN message received");
                                }
                            }
                            
                        }
                    }
                }
            } else {
                Serial.println("No CAN message received");
            }
        }
        return false;
}

bool need_to_enter_pause_state() {
    // Returns true if enough time has passed that we believe we have lost comms with the CB and need to pause
    if (millis() - last_cb_message_time > TIME_TO_INIT_PAUSE_STATE) {
        return true;
    } else {
        return false;
    }
}

bool need_to_enter_abort_state() {
    // Returns true if enough time has passed that we believe we have lost comms with the CB and need to abort
    if (millis() - last_cb_message_time > TIME_TO_INIT_ABORT_STATE) {
        return true;
    } else {
        return false;
    }
}

void lost_comms_procedural_loop() {
    // Calling this function immediately tells us we need to pause the system
    // We need 2 different pauses:
    //  1. Before we open the BVAS for mdot: igniter off, bvas closed, close supply fill, close run vent, close supply vent
    //  2. During / after mdot: igniter off, close supply fill,  close run vent, close supply vent, leave bvas open

    Serial.println("Entered pause state");
    igniter_relay.set_desired_state(false);  // Make sure the igniter is off
    supply_fill_servo.write(90);  // Close the supply fill valve
    supply_fill_servo_state = false;
    supply_vent_relay.set_desired_state(false);  // Close the supply vent
    run_vent_solenoid.set_desired_solenoid_state(false);  // Close the run vent
    // BVAS is kept in the same position
    
    // The above is putting us in the "pause" state

    while (comms_with_cb == false) {  // While no comms, this is the "pause state"
        if (reinitialize_can_with_CB() == false) {  // Comms not regained
            if (need_to_enter_abort_state()) {  // It has been enough time to where we need to abort due to not getting comms back
                Serial.println("Entered abort state");
                abort_state_entered_time = millis();

                while (1) {  // We need to abort. No need to do anything else anymore
                    run_vent_solenoid.set_desired_solenoid_state(true);  // Open the run vent
                    supply_vent_relay.set_desired_state(true);  // Open the supply vent
                    // Dylan Mahoney said to leave BVAS as is
                    while (1) {
                        control_relays_and_servos();
                    }
                }
            }
        // Message is available over ethernet (probably the daq requesting for data)
        #if (ENABLE_ETHERNET_COMMS == true)
            if (UDPWrapper.is_message_available()) {
                char* incoming_msg = UDPWrapper.get_incoming_packet();

                if (strcmp("daq_request", incoming_msg) == 0) {
                    char* outgoing_msg = format_daq_response();
            
                    UDPWrapper.send_response(outgoing_msg, 9);
                    delete outgoing_msg;
                }
            }
        #endif

        } else {  // We did get our comms back
            comms_with_cb = true;
            //Serial.println("Re initialized CAN with CB");
            return;  // We got comms back, are in the "pause" state, and can go back to being controlled by the control box
        }
        control_relays_and_servos();
    }
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*------------------------------------------------SETUP AND LOOP------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/

void setup() {
    #if (ENABLE_ETHERNET_COMMS == false)
        pinMode(25, OUTPUT);  // Ethernet SPI module CS pin
        digitalWrite(25, HIGH);  // Disable Ethernet SPI module
    #endif

    Serial.begin(115200);

    supply_vent_relay.set_desired_state(0);
    run_vent_solenoid.set_desired_solenoid_state(0);
    igniter_relay.set_desired_state(0);

    #if (ENABLE_BVAS_SERVO == false)
        bvas_solenoid.set_desired_solenoid_state(0);
    #else
        bvas_servo.attach(BVAS_SERVO_PIN);
        bvas_servo.write(BVAS_SERVO_FULL_CLOSED);  // Close the valve
    #endif

    supply_fill_servo.attach(SUPPLY_FILL_SERVO_PIN);
    supply_fill_servo.write(90); // Close the valve
    unsigned long start_time = millis();
    while (millis() - start_time < 2500) {  // Give time to make sure relays are off and whatnot
        control_relays_and_servos();
    }

    //Ethernet stuff
    #if (ENABLE_ETHERNET_COMMS == true)
        if (UDPWrapper.begin() == false) {
            //Serial.println("Failed to start UDPWrapper");
        }
    #endif

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

    //Serial.println("Intitializing CAN with CB");
    initialize_can_with_CB();
    // CAN is now established with the Control Box
    //Serial.println("CAN Initialized with CB");
}

/*--------------------------------------------------------------------------------------------------------------------*/
/*------------------------------------------------LOOP FUNCTIONS------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------*/
void set_desired_relay_states() {
    // Uses the received CAN message to update the desired states of the valves/relays

    // Supply Fill is conveniently absent because it is controlled in the control_relays_and_servos() loop
    supply_vent_relay.set_desired_state(received_msg.data[1]);  // 0 for closed, 1 for open, 3 for keep last desired state
    run_vent_solenoid.set_desired_solenoid_state(received_msg.data[2]);  // 0 for closed, 1 for open, 3 for keep last desired state
    igniter_relay.set_desired_state(received_msg.data[4]);  // 0 for closed, 1 for open, 3 for keep last desired state

    #if (ENABLE_BVAS_SERVO == false)
        #if (ENABLE_BVAS_DELAY == true)
            static bool first_mdot_command_received = false;
            static unsigned long first_mdot_time;
            if (received_msg.data[3] == 1U) {  // MDOT command is being sent
                if (first_mdot_command_received == false) {
                    first_mdot_time = millis();
                    first_mdot_command_received = true;
                }
                if (millis() - first_mdot_time >= BVAS_DELAY) {
                    bvas_solenoid.set_desired_solenoid_state(1U);  // 0 for closed, 1 for open, 3 for keep last desired state
                } else {
                    bvas_solenoid.set_desired_solenoid_state(0U);  // 0 for closed, 1 for open, 3 for keep last desired state
                }
            } else if (received_msg.data[3] == 0U) {  // We definitely want it closed
                bvas_solenoid.set_desired_solenoid_state(0U);  // 0 for closed, 1 for open, 3 for keep last desired state
                first_mdot_command_received = false;
            }
        #else
            bvas_solenoid.set_desired_solenoid_state(received_msg.data[3]);  // 0 for closed, 1 for open, 3 for keep last desired state
        #endif
    #else
        #if (ENABLE_BVAS_DELAY == true)
            bvas_servo_set_desired_state(received_msg.data[3]);  // 0 for closed, 1 for continue mdot process, 3 for keep last desired state
        #else
            bvas_servo_set_desired_state(received_msg.data[3]);  // 0 for closed, 1 for continue mdot process, 3 for keep last desired state
        #endif
    #endif
    
}

void control_relays_and_servos() {
    // Controls relays and servos
    // Automatically checks for pin errors, relay voltages/errors, and timing events

    // Supply Fill
    if (received_msg.data[0] == 0) {  // Supply fill valve should be closed
        supply_fill_servo.write(90); // Close the valve
        supply_fill_servo_state = false;
    } else if (received_msg.data[0] == 1) {  // Supply fill valve should be open
        supply_fill_servo.write(0); // Open the valve
        supply_fill_servo_state = true;
    } else if (received_msg.data[0] == 3) {  // The servo needs to keep whatever its last state was
        if (supply_fill_servo_state == true) {
            supply_fill_servo.write(0); // Open the valve
            supply_fill_servo_state = true;
        } else {
            supply_fill_servo.write(90); // Close the valve
            supply_fill_servo_state = true;
        }
    }

    supply_vent_relay.control_relay();
    run_vent_solenoid.control_solenoid();
    igniter_relay.control_relay();

    #if (ENABLE_BVAS_SERVO == false)
        bvas_solenoid.control_solenoid();
    #else
        control_bvas_servo();
    #endif

    
}

void format_can_response() {
    // Format acknowledgement CAN message to show errors
    // Reset message bytes
    acknowledge_msg.can_id = 50;
    acknowledge_msg.can_dlc = 8;
    acknowledge_msg.data[0] = 0;
    acknowledge_msg.data[1] = 0;
    acknowledge_msg.data[2] = 0;
    acknowledge_msg.data[3] = 0;
    acknowledge_msg.data[4] = 0;
    acknowledge_msg.data[5] = 0;
    acknowledge_msg.data[6] = 0;
    acknowledge_msg.data[7] = 0;

    // Supply Fill. 0 = closed, 1 = open
    if (supply_fill_servo_state == false) {  // Supposed to be closed
        acknowledge_msg.data[0] = 0;
    } else {  // Supposed to be open
        acknowledge_msg.data[0] = 11;  
    }

    // Supply Vent. One's place for voltage. Ten's place for what it's telling relay to do
    if (supply_vent_relay.get_desired_state() == false) {  // Supposed to be closed
        acknowledge_msg.data[1] += 0;
    } else {  // Supposed to be open
        acknowledge_msg.data[1] += 10;  
    }
    if (supply_vent_relay.get_setpin_working() == false) {  // Setpin not working
        acknowledge_msg.data[1] += 30;  
    }
    if (supply_vent_relay.get_resetpin_working() == false) {  // Resetpin not working
        acknowledge_msg.data[1] += 50;  
    }
    if (supply_vent_relay.get_actual_state() == false) {  // There is NOT a voltage across the relay
        acknowledge_msg.data[1] += 0;
    } else {  // There is a voltage across the relay
        acknowledge_msg.data[1] += 1;
    }
    if (supply_vent_relay.get_relay_working() == false) { // Voltage disagrees with desired state
        acknowledge_msg.data[1] += 3;
    } else {
        acknowledge_msg.data[1] += 0;  // Voltage agrees with desired state
    }

    // Run Tank Vent OFF
    if (run_vent_solenoid.get_desired_state() == false) {  // Solenoid is desired to be off
        acknowledge_msg.data[2] += 0;
    } else {  // Relay is supposed to be on
        acknowledge_msg.data[2] += 10;  
    }
    if (run_vent_solenoid.get_offpin_working() == false) {  // The pin is not working
        acknowledge_msg.data[2] += 30;  
    }
    if (run_vent_solenoid.get_expected_state() == false) {  // The solenoid is thought to be off
        acknowledge_msg.data[2] += 0;
    } else {  // The solenoid is thought to be on
        acknowledge_msg.data[2] += 1;  
    }
    if (run_vent_solenoid.get_expected_state() != run_vent_solenoid.get_desired_state()) {
        acknowledge_msg.data[2] += 3;  
    }

    // Run Tank Vent ON
    if (run_vent_solenoid.get_desired_state() == false) {  // Solenoid is desired to be off
        acknowledge_msg.data[3] += 0;
    } else {  // Relay is supposed to be on
        acknowledge_msg.data[3] += 10;  
    }
    if (run_vent_solenoid.get_onpin_working() == false) {  // The pin is not working
        acknowledge_msg.data[3] += 30;  
    }
    if (run_vent_solenoid.get_expected_state() == false) {  // The solenoid is thought to be off
        acknowledge_msg.data[3] += 0;
    } else {  // The solenoid is thought to be on
        acknowledge_msg.data[3] += 1;  
    }
    if (run_vent_solenoid.get_expected_state() != run_vent_solenoid.get_desired_state()) {
        acknowledge_msg.data[3] += 3;  
    }


    #if (ENABLE_BVAS_SERVO == false)
        // BVAS OFF
        if (bvas_solenoid.get_desired_state() == false) {  // Solenoid is desired to be off
            acknowledge_msg.data[4] += 0;
        } else {  // Relay is supposed to be on
            acknowledge_msg.data[4] += 10;  
        }
        if (bvas_solenoid.get_offpin_working() == false) {  // The pin is not working
            acknowledge_msg.data[4] += 30;  
        }
        if (bvas_solenoid.get_expected_state() == false) {  // The solenoid is thought to be off
            acknowledge_msg.data[4] += 0;
        } else {  // The solenoid is thought to be on
            acknowledge_msg.data[4] += 1;  
        }
        if (bvas_solenoid.get_expected_state() != bvas_solenoid.get_desired_state()) {
            acknowledge_msg.data[4] += 3;  
        }

        // BVAS ON
        if (bvas_solenoid.get_desired_state() == false) {  // Solenoid is desired to be off
            acknowledge_msg.data[5] += 0;
        } else {  // Relay is supposed to be on
            acknowledge_msg.data[5] += 10;  
        }
        if (bvas_solenoid.get_onpin_working() == false) {  // The pin is not working
            acknowledge_msg.data[5] += 30;  
        }
        if (bvas_solenoid.get_expected_state() == false) {  // The solenoid is thought to be off
            acknowledge_msg.data[5] += 0;
        } else {  // The solenoid is thought to be on
            acknowledge_msg.data[5] += 1;  
        }
        if (bvas_solenoid.get_expected_state() != bvas_solenoid.get_desired_state()) {
            acknowledge_msg.data[5] += 3;  
        }
    #else  // Using the bvas servo
        if (bvas_servo_actual_state == false) {  // Should be closed
            acknowledge_msg.data[4] = 0;
            acknowledge_msg.data[5] = 0;  
        } else {
            acknowledge_msg.data[4] = 11;
            acknowledge_msg.data[5] = 11; 
        }
    #endif

    // Igniter
    if (igniter_relay.get_desired_state() == false) {  // Supposed to be off
        acknowledge_msg.data[6] += 0;
    } else {  // Supposed to be open
        acknowledge_msg.data[6] += 10;  
    }
    if (igniter_relay.get_setpin_working() == false) {  // Setpin not working
        acknowledge_msg.data[6] += 30;  
    }
    if (igniter_relay.get_resetpin_working() == false) {  // Resetpin not working
        acknowledge_msg.data[6] += 50;  
    }
    if (igniter_relay.get_actual_state() == false) {  // There is NOT a voltage across the relay
        acknowledge_msg.data[6] += 0;
    } else {  // There is a voltage across the relay
        acknowledge_msg.data[6] += 1;
    }
    if (igniter_relay.get_relay_working() == false) { // Voltage disagrees with desired state
        acknowledge_msg.data[6] += 3;
    } else {
        acknowledge_msg.data[6] += 0;  // Voltage agrees with desired state
    }
    if (igniter_continuity_check(IGNITER_CONTINUITY_CHECK) == false) {  // No continuity detected
        acknowledge_msg.data[6] += 0;
    } else {  // We have igniter continuity
        acknowledge_msg.data[6] += 100;
    }
}

// Format the UDP message to be sent to the daq when it wants it
char* format_daq_response() {
    char *outgoing_daq_msg = new char(9);
    outgoing_daq_msg[0] = '0';
    outgoing_daq_msg[1] = '0';
    outgoing_daq_msg[2] = '0';
    outgoing_daq_msg[3] = '0';
    outgoing_daq_msg[4] = '0';
    outgoing_daq_msg[5] = '0';
    outgoing_daq_msg[6] = '0';
    outgoing_daq_msg[7] = '0';
    outgoing_daq_msg[8] = '0';
    outgoing_daq_msg[9] = '0';


    // Supply Fill. 0 = closed, 1 = open
    if (supply_fill_servo_state == false) {  // Supposed to be closed. '00' is closed
        outgoing_daq_msg[0] = '0';
        outgoing_daq_msg[1] = '0';
    } else {  // Supposed to be open. '90' is open
        outgoing_daq_msg[0] = '9';
        outgoing_daq_msg[1] = '0';
    }

    if (supply_vent_relay.get_actual_state() == false) {  // There is NOT a voltage across the relay
        outgoing_daq_msg[2] = '0';
        outgoing_daq_msg[3] = '0';
    } else {  // There is a voltage across the relay
        outgoing_daq_msg[2] = '9';
        outgoing_daq_msg[3] = '0';
    }

    if (run_vent_solenoid.get_expected_state() == false) {
        outgoing_daq_msg[4] = '0';
        outgoing_daq_msg[5] = '0';
    } else {  // There is a voltage across the relay
        outgoing_daq_msg[4] = '9';
        outgoing_daq_msg[5] = '0';
    }

    #if (ENABLE_BVAS_SERVO == false)
        if (bvas_solenoid.get_expected_state() == false) {  // The solenoid is thought to be off
            outgoing_daq_msg[6] = '0';
            outgoing_daq_msg[7] = '0';
        } else {  // The solenoid is thought to be on
            outgoing_daq_msg[6] = '9';
            outgoing_daq_msg[7] = '0'; 
        }
    #else  // Using the bvas servo
        if (bvas_servo_actual_state == false) {  // Should be closed
            outgoing_daq_msg[6] = '0';
            outgoing_daq_msg[7] = '0';
        } else {
            itoa(bvas_servo_angle, outgoing_daq_msg+6, 10);
        }
    #endif

    if (igniter_relay.get_actual_state() == false) {  // There is NOT a voltage across the relay
        outgoing_daq_msg[8] = '0';
    } else {  // There is a voltage across the relay
        outgoing_daq_msg[8] = '1';
    }

    return outgoing_daq_msg;
}

void manual_abort_loop() {
// This loops when the CB told the AB that it is manually aborting
// Primary concern is if the CB sends a manual abort command then loses comms, we want to prevent the AB from going into a "pause state" and ruining the abort sequence
    unsigned int last_read_time = millis();
    while(1) {
        if (millis() - last_read_time > TIME_BETWEEN_MESSAGES) {
            if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {
                last_cb_message_time = millis();
                set_desired_relay_states();
                control_relays_and_servos();
    
                format_can_response(); 
                mcp2515.sendMessage(&acknowledge_msg);
                if (received_msg.can_id != 5) {  // No longer in manual abort, only BREAK condition
                    return;
                }
            }
        }
        // Still trying to update the relays every loop
        control_relays_and_servos();

        if (need_to_enter_pause_state()) {
            comms_with_cb = false;  // We lost comms with the control box
            reinitialize_can_with_CB();
        }
    }
}

void loop() {
    /*--------------------------------------------------------------------------------------------------------------------*/

    // Check for possible pause state scenarios and abort state scenarios due to lost comms
    if (need_to_enter_pause_state()) {
        //Serial.println("Lost Comms, entering PAUSE state");
        comms_with_cb = false;  // We lost comms with the control box
        pause_state_entered_time = millis();
        lost_comms_procedural_loop();  // Loops until either comms are regained or we abort
    }

    /*--------------------------------------------------------------------------------------------------------------------*/

    // Read CAN message, record time

    if (mcp2515.readMessage(&received_msg) == MCP2515::ERROR_OK) {  // We have an incoming message
        if (received_msg.can_id == 5 && received_msg.data[5] == 9) { // CB is sending ABORT command message to AB
            last_cb_message_time = millis();
            set_desired_relay_states();
            manual_abort_loop();
        } else if (received_msg.can_id == 50) {  // CB is sending normal command message to AB
            // Send CAN message to Control Box
            last_cb_message_time = millis();
            set_desired_relay_states();
            control_relays_and_servos();
            format_can_response();
            mcp2515.sendMessage(&acknowledge_msg);
        }
    }

    // Message is available over ethernet
    #if (ENABLE_ETHERNET_COMMS == true)
        if (UDPWrapper.is_message_available()) {
            char* incoming_msg = UDPWrapper.get_incoming_packet();

            if (strcmp("daq_request", incoming_msg) == 0) {
                char* outgoing_msg = format_daq_response();
        
                UDPWrapper.send_response(outgoing_msg, 9);
                delete outgoing_msg;
            }
        }
    #endif
    control_relays_and_servos();
}