#include "esp_system.h"
#include "rom/ets_sys.h"
#include "MultiMap.h" // Library by Rob Tillaart
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels

#define DIST_TRIGGER 18
#define COIL_PULSE 19
#define MAP_SENSOR 36
#define FUEL_PUMP 23

hw_timer_t *timer = NULL;
hw_timer_t *timer2 = NULL;

// create an OLED display object connected to I2C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//variables to keep track of the timing of recent interrupts
const int pulsePerRev = 2; // 4 cylinder engine
unsigned long period = 1;
float timing_advance;
unsigned long ign_delay;
unsigned int rpm = 0;
float kpa;
int pulseDelay = 3;

float array[9][9] = {
  {0, 600, 750, 1400, 2100, 2800, 3500, 4200, 6000},
  {25, 13,  10, 22.5, 33.6, 44.6, 47.8, 47.8, 47.8},
  {36, 13,  10, 21.9, 32.7, 43.4, 46.5, 46.5, 46.5},
  {47, 13,  10, 21.3, 31.8, 42.2, 45.2, 45.2, 45.2},
  {58, 13,  10, 20.7, 30.9, 41,   43.9, 43.9, 43.9},
  {68, 13,  10, 20.1, 30,   39.9, 42.7, 42.7, 42.7},
  {79, 13,  10, 19.5, 29.1, 38.7, 41.5, 41.5, 41.5},
  {90, 13,  10, 18.9, 28.2, 37.5, 40.2, 40.2, 40.2},
  {101, 5,  10, 18.3, 27.3, 36.3, 38.9, 38.9, 38.9}
};


void ARDUINO_ISR_ATTR Ign_Pulse() {
	//Fires ignition Coil
  //ets_printf("\n entry @ %u\n",entry);
	timerWrite(timer2, 0);  //reset timer to call Dwell start
	timerAlarm(timer2, 3000, false, 0);  //set time in us until next dwell start
	digitalWrite(COIL_PULSE, LOW);  

}

void ARDUINO_ISR_ATTR Dwell_Start() {
	// sets coil high to start dwell for next pulse
	digitalWrite(COIL_PULSE, HIGH);  
}

void IRAM_ATTR isr() {
	period = timerReadMicros(timer); // how long sice the last trigger pulse
	timerWrite(timer, 0);  //reset Ignition pulse timer
  timerAlarm(timer, ign_delay, false, 0);  //set time in us until next ign pulse
}

float advance(float rpmx, float kpax){
  //Serial.printf("%.1f, %.1f\n",rpmx,kpax);
  int row1;
  float adv;

  if (kpax<= array[1][0]) {kpax = array[1][0]+.01;}
  if (kpax>= array[8][0]) {kpax = array[8][0]-.01;}

  for(int i =1; i<9;i++){
    if(array[i][0] < kpax) {
      row1 = i;
      //Serial.println(array[i][0]);
    }
  }
  
  float adv1 = interp(rpmx,row1);
  float adv2 = interp(rpmx,row1+1);
  float kpa1 = array[row1][0];
  float kpa2 = array[row1+1][0];
  adv = adv1+(kpax-kpa1)*(adv2-adv1)/(kpa2-kpa1);

  //Serial.printf("%.1f, %.1f, %.1f\n",kpa1,kpax,kpa2); 
  //Serial.printf("%.1f, %.1f, %.1f\n",adv1,adv,adv2); 
  return adv;
}


float interp(int val,int row)
{
  //  out[] holds the advance values for the given row
  //  in[] holds the rpm values for the columns
  float dist;
  float out[9];
  float in[9];
  for (int i =0;i<8;i++){
    out[i] = array [row][i+1];
    in[i] = array[0][i+1];
 }
  //for (int i = 0; i < 8; i++) Serial.printf("%.1f ",in[i]);
  //Serial.println();
  //for (int i = 0; i < 8; i++) Serial.printf("%.1f ",out[i]);
  //Serial.println();
  if (val <= in[0]) {dist = out[0];}
  else if  (val >= in[7]) {dist = out[7];}
  else {dist = multiMap<float>(val, in, out, 8);}
  return dist;
}


void setup() {
	Serial.begin(115200);
	pinMode(DIST_TRIGGER, INPUT_PULLUP);
	pinMode(COIL_PULSE, OUTPUT);
	pinMode(FUEL_PUMP, OUTPUT);
	pinMode(MAP_SENSOR, INPUT);
	digitalWrite(COIL_PULSE, HIGH);
	digitalWrite(FUEL_PUMP, HIGH);
	attachInterrupt(DIST_TRIGGER, isr, FALLING);
  timer = timerBegin(1000000);                     //timer 1Mhz resolution
  timer2 = timerBegin(1000000);                     //timer 1Mhz resolution
  timerAttachInterrupt(timer, &Ign_Pulse);       //attach callback
  timerAttachInterrupt(timer2, &Dwell_Start);       //attach callback

  // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }

  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(2);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("RPM"); // set text
  oled.println("KPA"); // set text
  oled.println("Advance"); // set text
  oled.display();              // display on OLED


	Serial.println("Setup complete");
}


void update_status_display() {
  oled.clearDisplay(); // clear display
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.printf("RPM = %u\n", rpm); // set text
  oled.printf("KPA = %.1f\n", kpa); // set text
  oled.printf("Adv = %.1f\n", timing_advance); // set text
  oled.display();              // display on OLED
}

void fuel_pump_control() {
  if (rpm >100 | millis() < 5000){
  	digitalWrite(FUEL_PUMP, HIGH);
  }
  else{	digitalWrite(FUEL_PUMP, LOW);
  }
  //Serial.println(millis());
}

void loop() {
	kpa = map(analogRead(MAP_SENSOR),0,4095,10,100);
  
	if (period < 1){
		rpm = 0;
	} else{
		rpm = 60000000/(period*pulsePerRev);
	}
	float usec_per_deg = 1000000.*60./360./rpm;
	timing_advance = advance(rpm,kpa);
	unsigned long adv_usec = timing_advance * usec_per_deg;
	ign_delay = period - adv_usec; // determine time after input pulse to fire coil

  fuel_pump_control();

  update_status_display();
	
  
  // Diagnostic print statemetns
  //Serial.printf("Period is %u usec, RPM is %u \n", period,rpm);
	//Serial.printf("Kpa is %.1f, RPM is %u, Advance is %.1f\n", kpa,rpm,timing_advance);
	//Serial.printf("usec/deg is %.1f \n", usec_per_deg);
	//Serial.printf("adv usec is %u \n", adv_usec);
	//Serial.printf("ign delay %u \n", ign_delay);

}