#include "tap.h"

#ifdef FEATURE_TAP_DETECTION
#include "imu.h"
#include <bluefruit.h>

extern BLEHidAdafruit blehid;

// Tap detection setup 
// REG_TAP_THS_6D bits[4:0] = tapThreshold (1–31); 1 LSB = FS/32 = 62.5 mg at ±2g.
// REG_INT_DUR2 at ODR=416 Hz:
//  SHOCK[7:6] = 2 → 38 ms max tap duration
//  QUIET[5:4] = 2 → 19 ms refractory after tap
//  DUR[3:0]   = 6 → 115 ms max inter-tap window for double detection
void applyTapThreshold() {
  uint8_t thr = cfg.tapThreshold;
  if (thr < 1)  thr = 1;
  if (thr > 31) thr = 31;
  imuWriteReg(REG_TAP_THS_6D, thr & 0x1F);
}

void setupTapDetection() {
  imuWriteReg(REG_CTRL1_XL,    0x60);  // ODR=416 Hz, FS=±2g
  imuWriteReg(REG_TAP_CFG,     0x8E);  // TIMER_EN + LIR + TAP_Z/Y/X enabled
  applyTapThreshold();
  imuWriteReg(REG_INT_DUR2,    0x62);  // SHOCK=2(38ms), QUIET=2(19ms), DUR=6(115ms)
  imuWriteReg(REG_WAKE_UP_THS, 0x80);  // bit7=1 → single + double tap both enabled
  imuWriteReg(REG_MD1_CFG,     0x48);  // route single-tap(0x08) + double-tap(0x40) → INT1
  Serial.print("[TAP] threshold="); Serial.print(cfg.tapThreshold);
  Serial.print(" (~"); Serial.print(cfg.tapThreshold * 62.5f, 0); Serial.println(" mg)");
}

// Tap processing 
static enum { TAP_IDLE, TAP_PENDING, TAP_EXECUTING } tapState = TAP_IDLE;
static unsigned long tapPendingMs  = 0;
static uint8_t       pendingButton = 0;   // 0 = key action pending

// After DOUBLE_TAP fires we add a small settle guard before committing.
static const unsigned long TAP_CONFIRM_MS = 20;

static void fireTapAction(unsigned long now) {
  switch (cfg.tapAction) {
    case TAP_ACTION_LEFT:
      blehid.mouseButtonPress(MOUSE_BUTTON_LEFT);
      pendingButton = MOUSE_BUTTON_LEFT;
      Serial.println("[TAP] Double → LEFT click");
      statLeftClicks++;
      break;
    case TAP_ACTION_RIGHT:
      blehid.mouseButtonPress(MOUSE_BUTTON_RIGHT);
      pendingButton = MOUSE_BUTTON_RIGHT;
      Serial.println("[TAP] Double → RIGHT click");
      statRightClicks++;
      break;
    case TAP_ACTION_MIDDLE:
      blehid.mouseButtonPress(MOUSE_BUTTON_MIDDLE);
      pendingButton = MOUSE_BUTTON_MIDDLE;
      Serial.println("[TAP] Double → MIDDLE click");
      statLeftClicks++;
      break;
    case TAP_ACTION_KEY: {
      uint8_t keys[6] = {cfg.tapKey, 0, 0, 0, 0, 0};
      blehid.keyboardReport(cfg.tapMod, keys);
      pendingButton = 0;
      Serial.print("[TAP] Double → KEY 0x"); Serial.println(cfg.tapKey, HEX);
      statLeftClicks++;
      break;
    }
  }
  clickButtonDown = true; clickDownMs = now;
  tapState = TAP_EXECUTING;
}

void processTaps(unsigned long now) {
  // Release 
  if (tapState == TAP_EXECUTING) {
    if (now - clickDownMs >= CLICK_HOLD_MS) {
      if (pendingButton) {
        blehid.mouseButtonRelease();
      } else {
        // Key action: release all keys
        uint8_t noKeys[6] = {};
        blehid.keyboardReport(0, noKeys);
      }
      clickButton = 0; clickButtonDown = false;
      tapState = TAP_IDLE;
    }
    return;
  }

  // Poll TAP_SRC 
  uint8_t tapSrc = imuReadReg(REG_TAP_SRC);
  bool tapIA     = !!(tapSrc & 0x40);
  bool doubleTap = !!(tapSrc & 0x10);

  if (tapState == TAP_IDLE) {
    if (tapIA && doubleTap) {
      tapPendingMs = now;
      tapState = TAP_PENDING;
    }
    return;
  }

  if (tapState == TAP_PENDING) {
    if (now - tapPendingMs >= TAP_CONFIRM_MS) {
      fireTapAction(now);
    }
  }
}

#endif // FEATURE_TAP_DETECTION