#define System_Armed_LED 7
const int System_Activated_LED = 10;
#define Oil_Sprayer_Engaged_LED 11
#define RELAY_IC_S 21             // Relay used to control the intercool sprayer solenoid
#define RELAY_OC_S 20            // Relay used to control to oil cooler sprayer solenoid
#define RELAY_WATERPUMP 19        // Relay used to control the system waterpump
#define Paddle_Button_1 13           // Subaru steering wheel paddle button control - left hand side
#define Paddle_Button_2 12          // Subaru steering wheel paddle button contol - right hand side
const int POWER_BTN = 6;             // System power button used to turn the system on/off
//#define Potentiometer_Pin_Boost = A0;
//#define Potentiometer_Pin_RPM = A1;
//#define Potentiometer_Pin_Speed = A2;

int VEHICLE_BOOST = 0;
int VEHICLE_SPEED = 0;
int VEHICLE_RPM = 0;
int VEHICLE_INTAKE_PRESSURE = 0;
int VEHICLE_BAROMETRIC_PRESSURE = 0;
int VEHICLE_COOLANT_TEMP = 0;
int BOOST_KPA = 0;
int RPM_SHIFT_POINT = 6500;
int TARGET_SPEED = 30;
int TARGET_RPM_START = 2500;
int TARGET_RPM_STOP = 6500;
int TARGET_BOOST_START = 10;
int TARGET_BOOST_STOP = 24;
int TARGET_COOLANT_TEMP = 195;
int SPRAY_INTERVAL = 2;
int SPRAY_DURATION = 3;
int POWER_BTN_State= 1;
int SYSTEM_STATUS = 0;
int POWER_BTN_LastState = 1;

bool SYSTEM_ARMED = false;
bool SYSTEM_ACTIVATED = false;
bool ENABLE_OIL_IC = false;
bool Paddle_Button_1_Pressed = false;
bool Paddle_Button_2_Pressed = false;

unsigned long Paddle_Button_1_PressTime = 0;
unsigned long Paddle_Button_2_PressTime = 0;

const int Potentiometer_Pin_Boost = A0;
const int Potentiometer_Pin_RPM = A1;
const int Potentiometer_Pin_Speed = A2;
const unsigned long holdTime = 3000; 

int Current_Boost_Value = 0;
int Boost_PSI_Start = 12;
int Boost_PSI_Stop = 24;
int Current_RPM_Value = 0;
int RPM_Start = 2500;
int RPM_Stop = 7000;
int Coolant_Temp_Target = 195;
int Current_Coolant_Temp = 0;
int RPM_Shift_Point = 6500;
int Current_SPD_Value = 0;
int MinStartSPD = 30;
int lastPowerBtnState = 0;

#include <TM1637.h>
#include <Wire.h>

TM1637 TM1, TM2, TM3, TM4;       // Each display needs its own object



void setup() {
  // put your setup code here, to run once:
  
  pinMode(Potentiometer_Pin_Boost, INPUT);
  pinMode(Potentiometer_Pin_RPM, INPUT_PULLUP);
  pinMode(Potentiometer_Pin_Speed, INPUT_PULLUP);
  pinMode(System_Armed_LED, OUTPUT);
  pinMode(System_Activated_LED, OUTPUT);
  pinMode(Oil_Sprayer_Engaged_LED, OUTPUT);
  pinMode(Paddle_Button_1, INPUT);
  pinMode(Paddle_Button_2, INPUT);
  pinMode(POWER_BTN, INPUT);
  pinMode(RELAY_WATERPUMP, OUTPUT);
  pinMode(RELAY_IC_S, OUTPUT);
  pinMode(RELAY_OC_S, OUTPUT);
Serial.begin(112500);

  // -----------------------------------------
  // First display
  TM1.begin(8, 9, 4);       //  clockpin, datapin, #digits
  TM1.displayClear();
  TM1.setBrightness(7);     // full brightness, default is 3
  // -----------------------------------------
  // Second display
  TM2.begin(3, 2, 4);       //  clockpin, datapin, #digits
  TM2.displayClear();
  TM2.setBrightness(7);     // full brightness, default is 3
  // -----------------------------------------
  // Third display
  TM3.begin(4, 5, 4);       //  clockpin, datapin, #digits
  TM3.displayClear();
  TM3.setBrightness(7);     // full brightness, default is 3
  // -----------------------------------------
  // Third display
  TM4.begin(14,15,4);
  TM4.displayClear();
  TM4.setBrightness(7);
  
  // Initialize relays to be off
  digitalWrite(RELAY_WATERPUMP, LOW);
  digitalWrite(RELAY_IC_S, LOW);
  digitalWrite(RELAY_OC_S, LOW);
  

}

void loop() {

  checkSystemPower();
  
      // TEMPORARY TEST CODE
      int pot_PPB_Value = analogRead(Potentiometer_Pin_Boost);
      int Current_Boost_Value = map(pot_PPB_Value, 0, 1023, 0, 35);
      TM1.displayInt(Current_Boost_Value);
      
      int pot_PPR_Value = analogRead(Potentiometer_Pin_Boost);
      int Current_RPM_Value = map(pot_PPR_Value, 0, 1023, 0, 9000); 
      TM2.displayInt(Current_RPM_Value);
      
      int pot_PPS_Value = analogRead(Potentiometer_Pin_Speed);
      int Current_SPD_Value = map(pot_PPS_Value, 0, 1023, 0, 150); 
      TM3.displayInt(Current_SPD_Value);

      int pot_COT_Value = analogRead(Potentiometer_Pin_RPM);
      int Current_Coolant_Temp = map(pot_COT_Value, 0, 1023, 0, 300); 
      TM4.displayInt(Current_Coolant_Temp);
      // TEMPORARY TEST CODE
      
if (SYSTEM_ARMED == true){

    checkPaddleButtons();

    if (SYSTEM_ARMED == true && SYSTEM_ACTIVATED == true){
      if (VEHICLE_SPEED >= TARGET_SPEED && VEHICLE_BOOST >= TARGET_BOOST_START && VEHICLE_BOOST <= TARGET_BOOST_STOP && VEHICLE_RPM >= TARGET_RPM_START && VEHICLE_RPM <= TARGET_RPM_STOP){
        // TURN ON THE IC SPRAYER SYSTEM
        digitalWrite(RELAY_WATERPUMP, HIGH);
        digitalWrite(RELAY_IC_S,HIGH);
        digitalWrite(System_Activated_LED, HIGH);
        if (VEHICLE_COOLANT_TEMP >= TARGET_COOLANT_TEMP){
          digitalWrite(RELAY_OC_S,HIGH);
          digitalWrite(Oil_Sprayer_Engaged_LED, HIGH);
        }else {
          digitalWrite(RELAY_OC_S,LOW);
          digitalWrite(Oil_Sprayer_Engaged_LED, LOW);
        }
      

    }else {
        // TURN OFF THE IC SPRAY SYSTEM
        digitalWrite(RELAY_WATERPUMP, LOW);
        digitalWrite(RELAY_IC_S,LOW);
    }


  }
}
}

// FUNCTION CHECK SYSTEM ARMED BUTTON STATUS

void checkSystemPower() {
  POWER_BTN_State = digitalRead(POWER_BTN);// current state of button
//Serial.println(buttonState);
 //Serial.println(POWER_BTN_LastState);
  if (POWER_BTN_State!= POWER_BTN_LastState) { 
    
    if (POWER_BTN_State== HIGH) { // if the button is pushed
      if (SYSTEM_STATUS == 0) { // and we are not already in "on" mode
        digitalWrite(System_Armed_LED, HIGH);
        SYSTEM_STATUS = 1; // turn us into "on" mode
        SYSTEM_ARMED = true;
        Serial.println("SYSTEM ARMED");
      }
      else if (SYSTEM_STATUS == 1) {
        digitalWrite(System_Armed_LED, LOW);
        SYSTEM_STATUS = 0; // take us out
        SYSTEM_ARMED = false;
        Serial.println("SYSTEM OFF");
      }
    }
    else { // button is not pressed but was in last loop
    }
    delay(50); // debounce thing
  }
  POWER_BTN_LastState = POWER_BTN_State; // update last state
}

// FUNCTION TO CHECK THE PADDLE BUTTON STATUS
void checkPaddleButtons() {
  if (SYSTEM_ARMED == true){
  // Check if Paddle_Button_1 is pressed
  if (digitalRead(Paddle_Button_1) == HIGH) {
    // If it's pressed and hasn't been pressed before
    if (Paddle_Button_1_PressTime == 0) {
      Paddle_Button_1_PressTime = millis(); // Record the current time
      Paddle_Button_1_Pressed = true;       // Mark button as pressed
    }
  } else {
    // If the button is released and it was pressed
    if (Paddle_Button_1_Pressed) {
      // If the press was shorter than the hold time, it's a momentary press
      if (millis() - Paddle_Button_1_PressTime < holdTime) {
        Serial.write("Paddle Button 1 Pressed\n");
      }
      // Reset button 1 state
      Paddle_Button_1_PressTime = 0;
      Paddle_Button_1_Pressed = false;
    }
  }
  }
  // Check if Paddle_Button_2 is pressed
  if (digitalRead(Paddle_Button_2) == HIGH) {
    if (Paddle_Button_2_PressTime == 0) {
      Paddle_Button_2_PressTime = millis();  // Record the current time
      Paddle_Button_2_Pressed = true;        // Mark button as pressed
      
    }
  } else {
    if (Paddle_Button_2_Pressed) {
      if (millis() - Paddle_Button_2_PressTime < holdTime) {
        Serial.write("Paddle Button 2 Pressed\n");
      }
      Paddle_Button_2_PressTime = 0;
      Paddle_Button_2_Pressed = false;
    }
  }

  // If both buttons have been held for the required time
  if (Paddle_Button_1_PressTime != 0 && Paddle_Button_2_PressTime != 0) {
    if (millis() - Paddle_Button_1_PressTime >= holdTime && millis() - Paddle_Button_2_PressTime >= holdTime) {
      // Send a signal to the Serial when both buttons are held for 3 seconds
      Serial.write("Activated\n");

      // Reset the press times to prevent repeated activation
      Paddle_Button_1_PressTime = 0;
      Paddle_Button_2_PressTime = 0;
      Paddle_Button_1_Pressed = false;
      Paddle_Button_2_Pressed = false;
    }
  }
}