#include <math.h>

/* ============================= */
/* PIN CONFIG                    */
/* ============================= */
#define CDI_PIN        33
#define MAP_PIN        34
#define TRIM_PIN       35
#define TEMP_PIN       32
#define INJECTOR_PIN   25

/* ============================= */
/* RPM VARIABLES                 */
/* ============================= */
volatile unsigned long lastPulseMicros = 0;
volatile unsigned long pulseInterval = 0;
volatile bool newPulse = false;

/* Variable tambahan untuk memisahkan trigger injektor agar tidak "berebutan" */
bool injectorTrigger = false; 

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

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;

/* injector cycle control */
int injectionEvent = 0;

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

bool injectorState = false;
unsigned long injectorStart = 0;

/* ============================= */
/* CDI INTERRUPT                 */
/* ============================= */
void IRAM_ATTR cdiISR()
{
    unsigned long now = micros();
    pulseInterval = now - lastPulseMicros;
    lastPulseMicros = now;
    newPulse = true;
}

/* ============================= */
/* FUNCTION PROTOTYPE            */
/* ============================= */
void readRPM();
void readMAP();
void readTemperature();
void calculateWarmup();
void readFuelTrim();
void accelerationEnrichment();

float getEngineLoad();
float getAfrTarget();
float calculatePulseWidth();

void injectorTask();
void serialMonitor();

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

    Serial.println("=================================");
    Serial.println("FEROZA ECU V3 (FIXED)");
    Serial.println("=================================");

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

    analogReadResolution(12); // 0 - 4095

    attachInterrupt(
        digitalPinToInterrupt(CDI_PIN),
        cdiISR,
        RISING
    );
}

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

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

    if (rpm > 300)
        currentPW = calculatePulseWidth();
    else
        currentPW = 0;

    injectorTask();
    serialMonitor();
}

/* ============================= */
/* RPM (CDI PULSE)               */
/* ============================= */
void readRPM()
{
    static unsigned long lastUpdate = 0;
    bool localNewPulse = false;
    unsigned long interval = 0;

    // Ambil data volatile secara aman menggunakan critical section
    noInterrupts();
    if (newPulse) {
        localNewPulse = true;
        interval = pulseInterval;
        newPulse = false; // Reset flag interrupt
    }
    interrupts();

    if (localNewPulse)
    {
        if (interval > 0)
        {
            float newRPM = 30000000.0f / interval;

            if (newRPM > 7000.0f)  newRPM = 7000.0f;
            if (newRPM < 100.0f)   newRPM = 0.0f;

            /* Low Pass Filter */
            rpmFiltered = (rpmFiltered * 0.85f) + (newRPM * 0.15f);
            rpm = rpmFiltered;
            
            // Set pemicu untuk injector task agar tahu ada pulsa baru masuk
            injectorTrigger = true; 
        }
        lastUpdate = millis();
    }

    /* Tidak ada pulsa = mesin mati */
    if (millis() - lastUpdate > 300)
    {
        rpm = 0;
        rpmFiltered = 0;
    }
}

/* ============================= */
/* READ MAP SENSOR (BARU)        */
/* ============================= */
void readMAP()
{
    int adc = analogRead(MAP_PIN);
    float voltage = (adc / 4095.0f) * 3.3f; 
    
    // Low pass filter untuk menstabilkan pembacaan MAP dari guncangan manifol
    mapFiltered = (mapFiltered * 0.80f) + (voltage * 0.20f);
    mapVoltage = mapFiltered;
}

/* ============================= */
/* READ TEMPERATURE SENSOR (BARU)*/
/* ============================= */
void readTemperature()
{
    int adc = analogRead(TEMP_PIN);
    float voltage = (adc / 4095.0f) * 3.3f;
    
    // CONTOH RUMUS NTC SENSOR SEDERHANA (Sesuaikan dengan kurva sensor Anda)
    // Untuk simulasi sementara, kita konversi voltase ke perkiraan derajat Celsius
    coolantTemp = 100.0f - (voltage * 25.0f); 
    if(coolantTemp < -10) coolantTemp = -10;
    if(coolantTemp > 120) coolantTemp = 120;
}

/* ============================= */
/* 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.85f + ((float)adc / 4095.0f) * 0.30f;
}

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

    if (deltaMap > 0.30) {
        accelFuel = 1.35;
        accelTimer = millis();
    }
    else if (deltaMap > 0.15) {
        accelFuel = 1.20;
        accelTimer = millis();
    }
    else if (deltaMap > 0.05) {
        accelFuel = 1.10;
        accelTimer = millis();
    }

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

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

    if (load < 0.2f) load = 0.2f;
    if (load > 1.5f) load = 1.5f;

    return load;
}

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

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

    if (rpm < 1000)       basePW = 1.8f;
    else if (rpm < 2000)  basePW = 2.5f;
    else if (rpm < 3000)  basePW = 3.5f;
    else if (rpm < 4000)  basePW = 5.0f;
    else                  basePW = 6.5f;

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

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

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

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

    return pw;
}

/* ============================= */
/* INJECTOR TASK                 */
/* ============================= */
void injectorTask()
{
    if (rpm < 300)
    {
        digitalWrite(INJECTOR_PIN, LOW);
        injectorState = false;
        injectionEvent = 0;
        injectorTrigger = false;
        return;
    }

    unsigned long now = micros();

    // Menggunakan injectorTrigger hasil operan dari fungsi readRPM
    if (injectorTrigger && !injectorState)
    {
        injectorTrigger = false; // Reset trigger

        digitalWrite(INJECTOR_PIN, HIGH);
        injectorStart = now;
        injectorState = true;
        injectionEvent++;

        if (injectionEvent >= 4)
            injectionEvent = 0;
    }

    if (injectorState)
    {
        if ((now - injectorStart) >= (unsigned long)(currentPW * 1000.0f))
        {
            digitalWrite(INJECTOR_PIN, LOW);
            injectorState = false;
        }
    }
}

/* ============================= */
/* 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=");     Serial.print(afrTarget, 1);
        Serial.print(" TRIM=");    Serial.print(fuelTrim, 2);
        Serial.print(" ACC=");     Serial.print(accelFuel, 2);
        Serial.print(" PW=");      Serial.print(currentPW, 2);
        Serial.print(" EVENT=");   Serial.println(injectionEvent);

        lastPrint = millis();
    }
}