// Pin Definitions
const int quickShifterPin = 3;
const int ignitionRelayPin = 6;
const int potPin = A0;
const int rpmPin = 2;
const int launchButtonPin = 9;
const int clutchPin = 5;
const int ledPin = 13;
const int modeSwitchPin = 8;
const int revLimiterSwitchPin = 7;

// Durations and thresholds
const int minCutoffDuration = 50;
const int maxCutoffDuration = 500;
const float rpmThreshold = 5000;
const unsigned long rpmCheckInterval = 1000;
const unsigned long clutchReleaseDelay = 2100;
const unsigned long blinkInterval = 10;
const unsigned long launchControlDelay = 1000;
const unsigned long buttonDebounceDelay = 100;
const unsigned long noiseFilterDelay = 1000; // Noise filtering delay (milliseconds)
const unsigned long ledBlinkInterval = 1000;
const unsigned long backfireDelay = 100;  // Durasi delay untuk efek backfire dalam milidetik
const unsigned long ignitionCutDelay = 50; // Delay before ignition cut in milliseconds

// Smart Cutoff Timings
const int cutoffDurations[] = {130, 120, 110, 90, 70, 50};
const int rpmRanges[][2] = {{1000, 4000}, {4000, 5000}, {5000, 6000}, {6000, 7000}, {7000, 8000}, {8000, 15000}};

// Variables
bool quickShifterActive = false;
unsigned long ignitionCutStart = 0;
bool ignitionCutActive = false;
unsigned long ignitionCutDuration = minCutoffDuration;
volatile unsigned long lastRpmTime = 0;
volatile unsigned long rpmInterval = 0;
volatile unsigned long pulseCount = 0;
unsigned long previousMillis = 0;
bool quickShifterTriggered = false;
bool quickShifterDebounced = false;
bool backfireActive = false;
unsigned long backfireStartTime = 0;
unsigned long quickShifterTriggerTime = 0; // Stores the time when the quick shifter is triggered
bool ignitionCutPending = false; // Flag to track if the delay is active

// Launch control variables
static volatile bool launchControlActive = false;
unsigned long clutchReleaseTime = 0;
bool clutchReleasedRecently = false;
unsigned long buttonPressTime = 0;
bool buttonPressed = false;

// Rev limiter variables
static volatile bool revLimiterActive = false;

// Button debounce and noise filter variables
unsigned long lastLaunchButtonStateChange = 0;
bool lastLaunchButtonState = HIGH;

void setup() {
  pinMode(quickShifterPin, INPUT_PULLUP);
  pinMode(ignitionRelayPin, OUTPUT);
  pinMode(potPin, INPUT);
  pinMode(rpmPin, INPUT);
  pinMode(launchButtonPin, INPUT_PULLUP);
  pinMode(clutchPin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(modeSwitchPin, INPUT_PULLUP);
  pinMode(revLimiterSwitchPin, INPUT_PULLUP);

  digitalWrite(ignitionRelayPin, HIGH);

  attachInterrupt(digitalPinToInterrupt(rpmPin), rpmPulse, RISING);
  attachInterrupt(digitalPinToInterrupt(quickShifterPin), quickShifterInterrupt, CHANGE);

  Serial.begin(9600);
}

void loop() {
  controlRelay();
  handleRpmControl();
  handleLaunchControl();
}

void controlRelay() {
  unsigned long currentTime = millis();

  // Handle ignition cut delay
  if (ignitionCutPending) {
    if (currentTime - quickShifterTriggerTime >= ignitionCutDelay) {
      ignitionCutPending = false;  // Clear the delay flag
      ignitionCutActive = true;    // Start the ignition cut
      ignitionCutStart = currentTime;
      ignitionCutDuration = getIgnitionCutDuration();
    }
  }

  // Ignition cut control logic
  if (quickShifterActive && ignitionCutActive) {
    if (currentTime - ignitionCutStart <= ignitionCutDuration) {
      digitalWrite(ignitionRelayPin, LOW); // Cut ignition
    } else if (!backfireActive) {
      backfireActive = true;
      backfireStartTime = currentTime; // Start backfire after ignition cut
    } else if (backfireActive && currentTime - backfireStartTime <= backfireDelay) {
      digitalWrite(ignitionRelayPin, HIGH); // Turn ignition on briefly for backfire effect
    } else {
      quickShifterActive = false;
      ignitionCutActive = false;
      quickShifterTriggered = false;
      backfireActive = false; // Reset backfire state
      digitalWrite(ignitionRelayPin, HIGH); // Restore ignition
      quickShifterDebounced = false; // Reset debounce flag
    }
  } else if (launchControlActive) {
    controlLaunchControlRelay();
  } else if (revLimiterActive) {
    if (rpmInterval > 0 && calculateRPM() > 9000) {
      digitalWrite(ignitionRelayPin, LOW); // Cut ignition on rev limiter
    } else {
      digitalWrite(ignitionRelayPin, HIGH);
    }
  } else {
    digitalWrite(ignitionRelayPin, HIGH);
  }
}

void controlLaunchControlRelay() {
  static unsigned long lastBlinkTime = 0;
  static bool relayState = HIGH;

  unsigned long currentTime = millis();
  if (rpmInterval > 0 && calculateRPM() > rpmThreshold) {
    if (currentTime - lastBlinkTime >= blinkInterval) {
      lastBlinkTime = currentTime;
      relayState = !relayState;
      digitalWrite(ignitionRelayPin, LOW);
    }
  } else {
    digitalWrite(ignitionRelayPin, HIGH);
  }
}

void handleLaunchControl() {
  static unsigned long lastLedBlinkTime = 0;
  static bool ledState = LOW;

  bool currentLaunchButtonState = digitalRead(launchButtonPin) == LOW;
  bool clutchPressed = digitalRead(clutchPin) == LOW;

  if (currentLaunchButtonState != lastLaunchButtonState && millis() - lastLaunchButtonStateChange > noiseFilterDelay) {
    lastLaunchButtonStateChange = millis();
    lastLaunchButtonState = currentLaunchButtonState;

    if (currentLaunchButtonState && !buttonPressed && millis() - buttonPressTime > buttonDebounceDelay) {
      buttonPressed = true;
      buttonPressTime = millis();
    }

    if (buttonPressed && (millis() - buttonPressTime >= launchControlDelay)) {
      launchControlActive = !launchControlActive;
      buttonPressed = false;
    }
  }

  if (!clutchPressed) {
    if (launchControlActive && !clutchReleasedRecently) {
      clutchReleaseTime = millis();
      clutchReleasedRecently = true;
    }
  } else {
    clutchReleasedRecently = false;
  }

  if (clutchReleasedRecently && (millis() - clutchReleaseTime >= clutchReleaseDelay)) {
    launchControlActive = false;
    clutchReleasedRecently = false;
  }

  if (launchControlActive) {
    unsigned long currentTime = millis();
    if (currentTime - lastLedBlinkTime >= ledBlinkInterval) {
      lastLedBlinkTime = currentTime;
      ledState = !ledState;
      digitalWrite(ledPin, ledState);
    }
  } else {
    digitalWrite(ledPin, LOW);
  }
}

void rpmPulse() {
  unsigned long currentTime = micros();
  rpmInterval = currentTime - lastRpmTime;
  lastRpmTime = currentTime;
}

unsigned long calculateRPM() {
  if (rpmInterval == 0) return 0;
  return (60000000UL / rpmInterval);
}

void handleRpmControl() {
  unsigned long rpm = calculateRPM();
  unsigned long currentMillis = millis();
  Serial.print("RPM: ");
  Serial.println(rpm);  // Debugging RPM value

  if (digitalRead(revLimiterSwitchPin) == LOW) {
    revLimiterActive = true;
  } else {
    revLimiterActive = false;
  }

  if (currentMillis - previousMillis > rpmCheckInterval) {
    previousMillis = currentMillis;
    rpmInterval = 0;
  }
}

void quickShifterInterrupt() {
  if (digitalRead(quickShifterPin) == LOW) {
    if (!quickShifterDebounced) {
      quickShifterActive = true;
      quickShifterTriggered = true;
      ignitionCutPending = true;               // Set the flag to indicate delay is active
      quickShifterTriggerTime = millis();      // Record the trigger time
      quickShifterDebounced = true;
    }
  } else {
    // Reset when the quick shifter sensor goes HIGH
    quickShifterDebounced = false;
    quickShifterActive = false;
    ignitionCutActive = false;
    ignitionCutPending = false;  // Reset pending flag
    digitalWrite(ignitionRelayPin, HIGH); // Ensure relay is turned off
  }
}

unsigned long getIgnitionCutDuration() {
  if (digitalRead(modeSwitchPin) == LOW) {
    int potValue = analogRead(potPin);
    Serial.println("Potentiometer mode active");  // Debugging
    return map(potValue, 0, 1023, minCutoffDuration, maxCutoffDuration);
  } else {
    unsigned long rpm = calculateRPM();
    for (int i = 0; i < sizeof(rpmRanges) / sizeof(rpmRanges[0]); i++) {
      if (rpm >= rpmRanges[i][0] && rpm <= rpmRanges[i][1]) {
        return cutoffDurations[i];
      }
    }
    return minCutoffDuration;
  }
}