// Define input and output pins
#define RPM_PIN 7 // rpm signal input
#define IGNITION_PIN 4 // coil driver output
#define RPM_THRESHOLD 6500 // maximum RPM limiter
#define IGNITION_CUT_TIME 50 // in milliseconds
#define INJECTOR_PIN 6 // injector driver output
#define INJECTION_INTERVAL 5000 // in microseconds

// Define variables for RPM and ignition timing
int rpm = 0;
unsigned long lastRpmTime = 0;
unsigned long lastIgnitionTime = 0;
unsigned long last_ignition_cut_time = 0;

// Define constants for ignition timing and duration
const int IGNITION_ADVANCE_DEGREES = 20;
const int IGNITION_DURATION_MS = 1000;

unsigned long pulseIn(int pin, int state) {
  // Map pin number to timer and input capture register
  uint8_t timer = digitalPinToTimer(pin);
  volatile uint8_t *icr = &ICR1;
  switch (timer) {
    case TIMER0A: icr = &ICR1; break;
    case TIMER1A: icr = &ICR1; break;
    case TIMER1B: icr = &ICR1; break;
    case TIMER2A: icr = &ICR2; break;
    case TIMER2B: icr = &ICR2; break;
  }

  // Configure timer for input capture mode
  TCCR1B = 0;
  TCCR2B = 0;
  switch (timer) {
    case TIMER0A: TCCR0A = 0; TCCR0B = _BV(CS01); break;
    case TIMER1A: TCCR1A = 0; TCCR1B = _BV(ICES1) | _BV(CS11); break;
    case TIMER1B: TCCR1A = 0; TCCR1B = _BV(ICES1) | _BV(CS11); break;
    case TIMER2A: TCCR2A = 0; TCCR2B = _BV(CS21); break;
    case TIMER2B: TCCR2A = 0; TCCR2B = _BV(CS21); break;
  }
  *icr = 0;
  TIMSK1 = _BV(ICIE1);

  // Wait for state change
  while (digitalRead(pin) != state);

  // Measure pulse duration
  unsigned long startTime = micros();
  while (digitalRead(pin) == state);
  unsigned long endTime = micros();

  // Disable timer interrupt and return pulse duration
  TIMSK1 = 0;
  return endTime - startTime;
}

// Interrupt handler for input capture
ISR(TIMER1_CAPT_vect) {
  // Do nothing
}

void setup() {
  pinMode(RPM_PIN, INPUT);
  pinMode(IGNITION_PIN, OUTPUT);
  Serial.begin(9600);

  // Configure Timer0 for PWM output on pin D6
  TCCR0A = _BV(COM0A1) | _BV(WGM01) | _BV(WGM00); // Fast PWM, non-inverted output on OC0A
  TCCR0B = _BV(CS01); // Prescaler = 8
  OCR0A = 0; // Initial duty cycle = 0%
  pinMode(INJECTOR_PIN, OUTPUT);
}

void loop() {
  // Measure RPM
  unsigned long now = micros();
  int rpmCount = pulseIn(RPM_PIN, HIGH);
  if (rpmCount > 0) {
    rpm = 60000000 / (rpmCount * 2);
    lastRpmTime = now;
  } else if (now - lastRpmTime > 1000000) {
    rpm = 0;
  }

  // RPM limiting by cutting ignition
  if (rpm > RPM_THRESHOLD) {
    unsigned long current_time = millis();
    if (current_time - last_ignition_cut_time > IGNITION_CUT_TIME) {
      digitalWrite(IGNITION_PIN, LOW); // disable ignition output
      delay(10); // wait for ignition to stop
      digitalWrite(IGNITION_PIN, HIGH); // re-enable ignition output
      last_ignition_cut_time = current_time;
    }
  }

  // Calculate ignition timing
  int ignitionAngle = (360 * now / 1000000 * rpm / 60 + IGNITION_ADVANCE_DEGREES) % 360;
  if (ignitionAngle < 0) ignitionAngle += 360;
  unsigned long ignitionTime = now + (1000000 * (360 - ignitionAngle) / rpm / 2);

  // Trigger ignition if it is time
  if (now - lastIgnitionTime > IGNITION_DURATION_MS * 1000) {
    lastIgnitionTime = now;
  } else if (now >= ignitionTime) {
    digitalWrite(IGNITION_PIN, HIGH);
    delayMicroseconds(IGNITION_DURATION_MS * 1000);
    digitalWrite(IGNITION_PIN, LOW);
    lastIgnitionTime = now;
  }

  // Calculate duty cycle based on engine load or other factors
  int duty_cycle = map(load, 0, 100, 0, 255);
  analogWrite(INJECTOR_PIN, duty_cycle);
  
  delayMicroseconds(INJECTION_INTERVAL); // wait for injection interval

  // Print RPM and ignition timing
  Serial.print("RPM: ");
  Serial.println(rpm);
  Serial.print("Ignition angle: ");
  Serial.println(ignitionAngle);
}