#include <math.h>

/* ============================= */
/* PIN CONFIG                    */
/* ============================= */
#define RPM_DIGITAL_PIN   33  // Input digital dari pulsa CDI (Trigger Utama)
#define MAP_PIN           34
#define TRIM_PIN          35
#define TEMP_PIN          32
#define INJECTOR_PIN      25

/* ============================= */
/* VARIABLES                     */
/* ============================= */
float rpm = 0;
float rpmFiltered = 0;

// Variabel Interrupt RPM & Trigger
volatile unsigned long lastPulseTime = 0;
volatile unsigned long pulseInterval = 0;
volatile bool newPulseDetected = false;
volatile bool triggerInjectorNow = false; // Flag pemicu injektor synchronous

float mapVoltage = 0;
float mapFiltered = 0;

float fuelTrim = 1.0;

float coolantTemp = 25.0;
float warmupFactor = 1.0;

float accelFuel = 1.0;
float lastMapVoltage = 0;

float afrTarget = 14.7;
float currentPW = 0; // Durasi semprotan (milidetik)

/* Kontrol injector */
bool injectorState = false;
unsigned long injectorStart = 0;

unsigned long accelTimer = 0;
unsigned long lastTrimRead = 0;

/* ============================= */
/* INTERRUPT SERVICE ROUTINE     */
/* ============================= */
void IRAM_ATTR rpmPulseISR() {
  unsigned long now = micros();
  pulseInterval = now - lastPulseTime;
  lastPulseTime = now;
  newPulseDetected = true;

  // SYSTEM SYNCHRONOUS: Langsung set trigger aktif tiap ada pulsa CDI
  triggerInjectorNow = true; 
}

/* ============================= */
/* SETUP                         */
/* ============================= */
void setup()
{
  Serial.begin(115200);
  delay(1000);

  Serial.println("=================================");
  Serial.println("FEROZA ECU V3 - CRANKING FIX");
  Serial.println("=================================");

  pinMode(INJECTOR_PIN, OUTPUT);
  digitalWrite(INJECTOR_PIN, LOW);

  pinMode(RPM_DIGITAL_PIN, INPUT); 
  attachInterrupt(digitalPinToInterrupt(RPM_DIGITAL_PIN), rpmPulseISR, RISING);

  analogReadResolution(12);
}

/* ============================= */
/* MAIN LOOP                     */
/* ============================= */
void loop()
{
  readRPM();
  readMAP();
  readTemperature();
  calculateWarmup();
  accelerationEnrichment();

  if (millis() - lastTrimRead > 100)
  {
    readFuelTrim();
    lastTrimRead = millis();
  }

  // REVISI: Selama RPM di atas 50 (kondisi mesin sedang di-starter), PW akan dihitung dan aktif
  if (rpm > 50)
    currentPW = calculatePulseWidth();
  else
    currentPW = 0; // Benar-benar mati hanya jika roda gila berhenti total

  injectorTask();
  serialMonitor();
}

/* ============================= */
/* RPM (DIGITAL PULSE)           */
/* ============================= */
void readRPM()
{
  float newRPM = 0;
  unsigned long currentMicros = micros();

  noInterrupts();
  unsigned long interval = pulseInterval;
  bool pulseSeen = newPulseDetected;
  newPulseDetected = false;
  interrupts();

  // Proteksi mesin mati total: jika tidak ada pulsa masuk lebih dari 1 detik (1000000 µs)
  if (currentMicros - lastPulseTime > 1000000) {
    newRPM = 0;
  } 
  else if (pulseSeen && interval > 0) {
    newRPM = 30000000.0 / interval; 
  }
  else {
    return; 
  }

  if (newRPM > 8000) {
    newRPM = rpm; 
  }

  rpmFiltered = (rpmFiltered * 0.80) + (newRPM * 0.20);
  rpm = rpmFiltered;
}

/* ============================= */
/* INJECTOR TASK (SYNCHRONOUS)   */
/* ============================= */
void injectorTask()
{
  // REVISI: Pengaman diturunkan ke 50 RPM agar bensin tetap keluar saat starter awal
  if (rpm < 50)
  {
    digitalWrite(INJECTOR_PIN, LOW);
    injectorState = false;
    triggerInjectorNow = false;
    return;
  }

  unsigned long now = micros();

  // 1. TAHAP MODULASI ON
  if (triggerInjectorNow && !injectorState)
  {
    digitalWrite(INJECTOR_PIN, HIGH); 
    injectorStart = now;
    injectorState = true;
    triggerInjectorNow = false; 
  }

  // 2. TAHAP MODULASI OFF
  if (injectorState)
  {
    if ((now - injectorStart) >= (currentPW * 1000))
    {
      digitalWrite(INJECTOR_PIN, LOW); 
      injectorState = false;
    }
  }
}

/* ============================= */
/* MAP                            */
/* ============================= */
void readMAP()
{
  int adc = analogRead(MAP_PIN);
  float rawVoltage = adc * (3.3 / 4095.0);
  mapFiltered = (mapFiltered * 0.8) + (rawVoltage * 0.2);
  mapVoltage = mapFiltered;
}

/* ============================= */
/* TEMPERATURE                   */
/* ============================= */
void readTemperature()
{
  int adc = analogRead(TEMP_PIN);
  float voltage = adc * (3.3 / 4095.0);
  if (voltage < 0.05) voltage = 0.05;

  float resistance = (10000.0 * voltage) / (3.3 - voltage);
  float tempK = 1.0 / ((1.0 / 298.15) + (1.0 / 3950.0) * log(resistance / 10000.0));
  coolantTemp = tempK - 273.15;
}

/* ============================= */
/* WARMUP                        */
/* ============================= */
void calculateWarmup()
{
  if (coolantTemp < 10)       warmupFactor = 1.60;
  else if (coolantTemp < 20)  warmupFactor = 1.45;
  else if (coolantTemp < 30)  warmupFactor = 1.30;
  else if (coolantTemp < 40)  warmupFactor = 1.20;
  else if (coolantTemp < 60)  warmupFactor = 1.10;
  else                        warmupFactor = 1.00;
}

/* ============================= */
/* FUEL TRIM                     */
/* ============================= */
void readFuelTrim()
{
  int adc = analogRead(TRIM_PIN);
  fuelTrim = 0.85 + ((float)adc / 4095.0) * 0.30;
}

/* ============================= */
/* ACCEL ENRICHMENT             */
/* ============================= */
void accelerationEnrichment()
{
  float deltaMap = mapVoltage - lastMapVoltage;
  lastMapVoltage = mapVoltage;

  if (deltaMap > 0.30)       accelFuel = 1.35;
  else if (deltaMap > 0.15)  accelFuel = 1.20;
  else if (deltaMap > 0.05)  accelFuel = 1.10;

  if (millis() - accelTimer > 250)
    accelFuel = 1.00;
  else
    accelTimer = millis();
}

/* ============================= */
/* ENGINE LOAD                   */
/* ============================= */
float getEngineLoad()
{
  float mapNorm = mapVoltage / 3.3;
  float rpmNorm = rpm / 6000.0;
  float load = mapNorm * (0.6 + rpmNorm);

  if (load < 0.2) load = 0.2;
  if (load > 1.5) load = 1.5;
  return load;
}

/* ============================= */
/* AFR TARGET                    */
/* ============================= */
float getAfrTarget()
{
  if (rpm < 1000)       return 13.8;
  else if (rpm < 2500)  return 14.7;
  else if (rpm < 4000)  return 13.5;
  else                  return 12.8;
}

/* ============================= */
/* PULSE WIDTH                   */
/* ============================= */
float calculatePulseWidth()
{
  float basePW;

  // REVISI: Menambahkan pengondisian khusus saat RPM di bawah 600 (Kondisi mesin di-starter / Cranking)
  // Saat starter, mesin butuh bensin lebih banyak (Base PW dibuat tinggi, misal 4.5ms) agar mesin mudah hidup.
  if (rpm < 600)        basePW = 4.5; 
  else if (rpm < 1000)  basePW = 1.8;
  else if (rpm < 2000)  basePW = 2.5;
  else if (rpm < 3000)  basePW = 3.5;
  else if (rpm < 4000)  basePW = 5.0;
  else                  basePW = 6.5;

  float load = getEngineLoad();
  afrTarget = getAfrTarget();

  float fuel = basePW * load;
  float afrCorrection = 14.7 / afrTarget;
  float pw = fuel * afrCorrection;

  pw *= fuelTrim;
  pw *= accelFuel;
  pw *= warmupFactor;

  if (mapVoltage > 2.80)                  pw *= 1.15;
  if (rpm < 1500 && mapVoltage > 2.5)     pw *= 1.25;

  return pw;
}

/* ============================= */
/* SERIAL MONITOR                */
/* ============================= */
void serialMonitor()
{
  static unsigned long lastPrint = 0;

  if (millis() - lastPrint > 500)
  {
    Serial.print("RPM=");     Serial.print(rpm, 0);
    Serial.print(" MAP=");     Serial.print(mapVoltage, 2);
    Serial.print(" TEMP=");    Serial.print(coolantTemp, 1);
    Serial.print(" WARM=");    Serial.print(warmupFactor, 2);
    Serial.print(" LOAD=");    Serial.print(getEngineLoad(), 2);
    Serial.print(" AFR_T=");   Serial.print(afrTarget, 1);
    Serial.print(" TRIM=");    Serial.print(fuelTrim, 2);
    Serial.print(" ACC=");     Serial.print(accelFuel, 2);
    Serial.print(" PW=");      Serial.println(currentPW, 2);

    lastPrint = millis();
  }
}