#include <Wire.h>
#include <U8g2lib.h>
#include <math.h>

// ======================
// OLED INIT
// ======================
U8G2_SH1107_128X128_1_HW_I2C u8g2(U8G2_R0);

// ======================
// Kurbelwellensignale
// ======================
#define SIG720_MASK (1 << 6)  // D6
#define OT_MASK     (1 << 7)  // D7

volatile uint8_t lastPIND;
volatile uint32_t last720Time = 0;
volatile uint32_t period720   = 0;

volatile uint32_t lastOTTime  = 0;
volatile uint8_t newOT        = 0;

volatile uint16_t markerCount = 0;          // Marker innerhalb 360°
volatile uint16_t markerCountLastRev = 0;   // Für Display

// ======================
// Trigger-Parameter
// ======================

// Zündwinkel: positiv = vor OT, negativ = nach OT
float ZW_VOR_OT = 10.0f;

// Trigger-Pulsdauer in Millisekunden
const uint16_t TRIGGER_PULSE_MS = 90;

// KW-Offset zur Justierung des OT (in Grad)
float KW_OFFSET = 0.0f;

// Triggerstatus
bool triggerActive = false;
uint32_t triggerStartTime = 0;
float lastKW = 0.0f;

// ======================
// XOR-Zeiger
// ======================
int cx = 64, cy = 90;
int radius = 35;
int old_x = -1, old_y = -1;

// ======================
// ISR für Marker & OT
// ======================
ISR(PCINT2_vect)
{
  uint8_t current = PIND;
  uint8_t changed = current ^ lastPIND;
  lastPIND = current;

  uint32_t now = micros();

  // === 720 Marker Erkennung ===
  if (changed & SIG720_MASK)
  {
    if (!(current & SIG720_MASK))
    {
      period720 = now - last720Time;
      last720Time = now;

      markerCount++;  // 720 Marker pro 360°, 0.5° pro Marker
      if (markerCount >= 720) markerCount = 0;
    }
  }

  // === OT Erkennung ===
  if (changed & OT_MASK)
  {
    if (!(current & OT_MASK))
    {
      lastOTTime = now;
      markerCountLastRev = markerCount;
      markerCount = 0;    // OT = 0°
      newOT = 1;
    }
  }
}

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

  pinMode(6, INPUT);
  pinMode(7, INPUT);

  lastPIND = PIND;

  PCICR |= (1 << PCIE2);
  PCMSK2 |= SIG720_MASK | OT_MASK;

  pinMode(9, OUTPUT);   // Trigger-Out
  digitalWrite(9, LOW);

  // Display init
  u8g2.begin();
  u8g2.setContrast(255);

  // Statischer Text oben
  u8g2.firstPage();
  do {
    u8g2.setFont(u8g2_font_6x12_tf);
    u8g2.drawStr(0, 10, "RPM:");
    u8g2.drawStr(0, 22, "d720:");
    u8g2.drawStr(0, 34, "OT:");
    u8g2.drawStr(0, 46, "720cnt:");
  } while (u8g2.nextPage());
}

uint32_t lastTextUpdate = 0;
float rpm = 0;

// ======================
// MAIN LOOP
// ======================
void loop()
{
  uint32_t now = micros();

  // ======================
  // RPM aus period720 berechnen
  // ======================
  if (period720 > 0)
  {
    rpm = 60000000.0 / (period720 * 720.0);  
  }

  // ======================
  // KW-Winkel bestimmen
  // ======================
  uint16_t mc;

  noInterrupts();
  mc = markerCount;
  interrupts();

  float KW = mc * 0.5f;     // 0.5° pro Marker
  KW += KW_OFFSET;          // Offset hinzufügen

  // Normieren
  while (KW >= 360.0f) KW -= 360.0f;
  while (KW <    0.0f) KW += 360.0f;

  // ======================
  // Trigger-Berechnung
  // ======================
  float triggerAngle;

  if (ZW_VOR_OT >= 0)
      triggerAngle = 360.0f - ZW_VOR_OT;  // vor OT
  else
      triggerAngle = -ZW_VOR_OT;          // nach OT

  if (triggerAngle < 0) triggerAngle = 0;
  if (triggerAngle >= 360.0f) triggerAngle = 359.999f;

  // Auslösen beim Überqueren
  if (!triggerActive &&
      lastKW < triggerAngle &&
      KW     >= triggerAngle)
  {
      triggerActive = true;
      triggerStartTime = millis();
      digitalWrite(9, HIGH);
  }

  // Puls abschalten
  if (triggerActive &&
      millis() - triggerStartTime >= TRIGGER_PULSE_MS)
  {
      triggerActive = false;
      digitalWrite(9, LOW);
  }

  lastKW = KW;

  // ======================
  // Display: Text (nur 4 Hz)
  // ======================
  if (millis() - lastTextUpdate > 250)
  {
      lastTextUpdate = millis();

      u8g2.firstPage();
      do {
        u8g2.setFont(u8g2_font_6x12_tf);

        char buf[20];
        sprintf(buf, "RPM: %d   ", (int)rpm);
        u8g2.drawStr(0, 10, buf);

        sprintf(buf, "d720: %lu   ", period720);
        u8g2.drawStr(0, 22, buf);

        sprintf(buf, "OT: %lu   ", lastOTTime);
        u8g2.drawStr(0, 34, buf);

        sprintf(buf, "720cnt: %u   ", markerCountLastRev);
        u8g2.drawStr(0, 46, buf);

      } while (u8g2.nextPage());
  }

  // ======================
  // XOR-Zeiger (flackerfrei)
  // ======================
  float rad = (KW - 90) * 3.14159 / 180.0;
  int new_x = cx + radius * cos(rad);
  int new_y = cy + radius * sin(rad);

  u8g2.firstPage();
  do {
    // alten Zeiger löschen
    if (old_x >= 0)
    {
      u8g2.setDrawColor(2);
      u8g2.drawLine(cx, cy, old_x, old_y);
    }

    // neuen Zeiger zeichnen
    u8g2.setDrawColor(2);
    u8g2.drawLine(cx, cy, new_x, new_y);

    u8g2.setDrawColor(1);
    u8g2.drawCircle(cx, cy, radius, U8G2_DRAW_ALL);

  } while (u8g2.nextPage());

  old_x = new_x;
  old_y = new_y;
}