#include <math.h>

/* ============================= */
/* PIN CONFIG                    */
/* ============================= */

#define RPM_POT_PIN   33
#define MAP_PIN       34
#define TRIM_PIN      35
#define TEMP_PIN      32
#define INJECTOR_PIN  25

/* ============================= */
/* 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;

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

bool injectorState = false;

unsigned long injectorStart = 0;
unsigned long injectorCycleStart = 0;

float currentPW = 0;

/* ============================= */
/* SETUP                        */
/* ============================= */

void setup()
{
  Serial.begin(115200);
  delay(1000);

  Serial.println("=================================");
  Serial.println("FEROZA ECU WOKWI - RPM VARIABLE");
  Serial.println("=================================");

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

  analogReadResolution(12);
}

/* ============================= */
/* 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 (VARIABLE POT)           */
/* ============================= */

void readRPM()
{
  int adc = analogRead(RPM_POT_PIN);

  float newRPM = (adc / 4095.0) * 6000.0;

  if (newRPM < 100)
    newRPM = 0;

  rpmFiltered = (rpmFiltered * 0.85) + (newRPM * 0.15);

  rpm = rpmFiltered;
}

/* ============================= */
/* MAP SENSOR                   */
/* ============================= */

void readMAP()
{
  int adc = analogRead(MAP_PIN);

  float rawVoltage = adc * (3.3 / 4095.0);

  mapFiltered = (mapFiltered * 0.8) + (rawVoltage * 0.2);

  mapVoltage = mapFiltered;
}

/* ============================= */
/* TEMPERATURE (NTC SIM)        */
/* ============================= */

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 ENRICHMENT            */
/* ============================= */

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.05)
  {
    accelFuel = 1.20;
    accelTimer = millis();
  }

  if (deltaMap > 0.15)
  {
    accelFuel = 1.40;
    accelTimer = millis();
  }

  if (deltaMap > 0.30)
  {
    accelFuel = 1.60;
    accelTimer = millis();
  }

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

/* ============================= */
/* PULSE WIDTH CALC             */
/* ============================= */

float calculatePulseWidth()
{
  float pw;

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

  float loadFactor = mapVoltage / 3.0;

  if (loadFactor < 0.30)
    loadFactor = 0.30;

  if (loadFactor > 1.20)
    loadFactor = 1.20;

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

  if (mapVoltage > 2.80)
    pw *= 1.20;

  if (mapVoltage > 3.00)
    pw *= 1.10;

  return pw;
}

/* ============================= */
/* INJECTOR CONTROL             */
/* ============================= */

void injectorTask()
{
  if (rpm < 300)
  {
    digitalWrite(INJECTOR_PIN, LOW);
    injectorState = false;
    return;
  }

  unsigned long now = micros();

  float cycleUs = (120000.0 / rpm) * 1000.0;

  if (!injectorState)
  {
    if ((now - injectorCycleStart) >= cycleUs)
    {
      digitalWrite(INJECTOR_PIN, HIGH);

      injectorStart = now;
      injectorCycleStart = now;
      injectorState = true;
    }
  }
  else
  {
    if ((now - injectorStart) >= (currentPW * 1000))
    {
      digitalWrite(INJECTOR_PIN, LOW);
      injectorState = false;
    }
  }
}

/* ============================= */
/* SERIAL OUTPUT                */
/* ============================= */

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(" TRIM=");
    Serial.print(fuelTrim, 2);

    Serial.print(" ACC=");
    Serial.print(accelFuel, 2);

    Serial.print(" PW=");
    Serial.println(currentPW, 2);

    lastPrint = millis();
  }
}