//#define DEBUG_PRINT

#define SIGNAL_PIN 8    // (Speed) Beliebigen Pin hier setzen (z. B. 8, 9, A0, etc.)
#define CADENCE_PIN 3   // This is fixed for IRQ
#define BRAKE_PIN 5     // Connection to ebike-Controller
#define LED_PIN  13     // ON: Motor allowed

#include <avr/io.h>
#include <avr/interrupt.h>

// Hilfs-Makros zur Auflösung von Arduino-Pins in Register-Zugehörigkeiten
#if (SIGNAL_PIN >= 0 && SIGNAL_PIN <= 7)   // Digital 0–7 = PORTD
  #define PCINT_VECTOR    PCINT2_vect
  #define PCICR_BIT       PCIE2
  #define PCMSK_REG       PCMSK2
  #define PIN_REG         PIND
  #define PIN_BIT         SIGNAL_PIN
  #define PCINT_BIT       SIGNAL_PIN
#elif (SIGNAL_PIN >= 8 && SIGNAL_PIN <= 13) // Digital 8–13 = PORTB
  #define PCINT_VECTOR    PCINT0_vect
  #define PCICR_BIT       PCIE0
  #define PCMSK_REG       PCMSK0
  #define PIN_REG         PINB
  #define PIN_BIT         (SIGNAL_PIN - 8)
  #define PCINT_BIT       (SIGNAL_PIN - 8)
#elif (SIGNAL_PIN >= A0 && SIGNAL_PIN <= A5) // Analog Pins A0–A5 = PORTC
  #define PCINT_VECTOR    PCINT1_vect
  #define PCICR_BIT       PCIE1
  #define PCMSK_REG       PCMSK1
  #define PIN_REG         PINC
  #define PIN_BIT         (SIGNAL_PIN - A0)
  #define PCINT_BIT       (SIGNAL_PIN - A0)
#else
  #error "SIGNAL_PIN ist nicht gültig für PCINT"
#endif

// Parameter to controll the Pedelec
#define TIMEOUT_CADENCE 1000  // after 1s no signal we are inactive  
#define TIMEOUT_MS 3000   // 8s = 2.2m * 60*60s/1000m) = 7.92 s  
#define DIAMETER   2.224  // distance a wheel makes with 1 turn
#define SPEED_MIN  6      // 6km/h below no cadence required
#define SPEED_MAX  27.5   // above that no support by motor (25km/h+10%)



void setup() {
  pinMode(SIGNAL_PIN, INPUT_PULLUP);
  pinMode(CADENCE_PIN, INPUT_PULLUP);
  pinMode(BRAKE_PIN, OUTPUT);
  digitalWrite(BRAKE_PIN, LOW);        // brake - motor not active
  digitalWrite(LED_PIN, HIGH);         // LED on: Motor on / Brake not active

  // Pin Change Interrupt für ausgewählten Pin aktivieren
  PCICR |= (1 << PCICR_BIT);
  PCMSK_REG |= (1 << PCINT_BIT);

  attachInterrupt(digitalPinToInterrupt(CADENCE_PIN), cadenceISR, RISING);

  sei(); // Globale Interrupts aktivieren
  Serial.begin(115200);
}


volatile uint32_t lastMicros = 0;
volatile bool readyToSend = false;
volatile uint32_t lastEventMillis = 0;
volatile uint32_t speed_time_us = 0;

ISR(PCINT_VECTOR) {
  static uint8_t lastState = 1;   // is only 1 at first call - as static!
  static uint32_t lastMicros = 0; // is only 0 at first call - as static!
  static uint32_t last_LH_Micros = 0;  // static 
  
  bool currentState = PIN_REG & (1 << PIN_BIT);
 
  if (currentState && !lastState) { // Ignore RISING Edge
    lastState = currentState;
    return;           
  }

  uint32_t now = micros();            // FALLING Edge
  uint32_t delta = now - lastMicros;  // delta to last FALLING
  lastMicros = now;
  if (delta < 150000)                 // check if this the right FALLING Edge   
    return;                           // debounce

  // Now we have a FALLING EDGE and H was stable for 150ms
  speed_time_us = now - last_LH_Micros;
  last_LH_Micros = now;

  lastEventMillis = millis();     // for set speed to "0" after timeout
  readyToSend = true;
} 


unsigned long  lastCadenceMillis = 0;   // global variable, output of ISR

// === ISR Kadenz (INT1) ===
void cadenceISR() {
  bool currentLevel = digitalRead(CADENCE_PIN);
  uint32_t now = millis();
  uint32_t delta = now - lastCadenceMillis;

  if (delta < 100 || currentLevel != HIGH ) // 100ms debounce, Rising should give HIGH level
    return;

//  Serial.println("Cadence");
  lastCadenceMillis = now;  // copy to global variable
}



float speed_now = 0;
float speed_filtered = 0;
uint32_t lastEventDiff;
float speed_old=0;

void loop() {
//    delay(200);
//    return;
    
#ifdef DEBUG_PRINT
//  Serial.print("Level at D8 :\t");
//  Serial.println(digitalRead(8));
//  delay(200);
//  return;
#endif
  
  bool cadenceActive = false;
  uint32_t now = millis();
  if (now - lastCadenceMillis < TIMEOUT_CADENCE)
    cadenceActive = true; 
#ifdef DEBUG_PRINT
  Serial.print("C_diff:\t");
  Serial.print(now - lastCadenceMillis);
  Serial.print("\tcadenceActive: ");
  Serial.print(cadenceActive ? "H   " : "L   ");
#endif  
  
  lastEventDiff = now - lastEventMillis;   // update auch wenn keine IRQs mehr kommen
  if (readyToSend == true || lastEventDiff > TIMEOUT_MS) {
    if ( readyToSend == false ) {
 //     Serial.print("\tTimeout\t");
      speed_now = 0;
      speed_filtered = 0;
      lastEventMillis = millis();
    }
    else {
      speed_now = DIAMETER * 3.6 * 1e6 / speed_time_us;
#ifdef DEBUG_PRINT
      Serial.print(speed_time_us/1e3);
      Serial.print("  ms / ");
      Serial.print(speed_now);
      Serial.print(" km/h");
#endif
      readyToSend = false;
    }

    if ( (speed_old < 2) && (speed_now > 4))
      speed_now = 2;
    speed_old = speed_now;
    
    speed_filtered = 1.0 * speed_now + 0.0 * speed_filtered;
#ifdef DEBUG_PRINT
//  Serial.print("speed: \t");
//  Serial.print(speed_now,1);
//  Serial.print(" km/h");
#endif
  }

  // set the Brake-Signal (Low = Brake, High = Run)  
  if ( (speed_now < SPEED_MIN) || ( speed_now <= SPEED_MAX && cadenceActive )) {
//  if ( true ) {
    digitalWrite(BRAKE_PIN, HIGH);       // motor active
    digitalWrite(LED_PIN, HIGH);         // LED on: Motor on / Brake not active
#ifdef DEBUG_PRINT
    Serial.print(" -> RUN\n");
#endif
  } else {
    digitalWrite(BRAKE_PIN, LOW);        // brake active
    digitalWrite(LED_PIN, LOW);         // LED off: Motor off / Brake active
#ifdef DEBUG_PRINT
    Serial.print(" -> BRAKE\n");
#endif
  }

  delay(200);
}