#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;

float afrTarget = 14.7;
float currentPW = 0;

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

bool injectorState = false;

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

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

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

  Serial.println("=================================");
  Serial.println("FEROZA ECU V2 - LOAD MODEL");
  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                         */
/* ============================= */

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

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;

  float accel = 1.0;

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

  accelFuel = accel;

  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;

  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;
}

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

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 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();
  }
}