#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Adafruit_NeoPixel.h>

// ==========================================
// ⚙️ USER CONFIGURATION (MODULAR)
// ==========================================
#define DEVICE_NAME         "BlazeC3_Pod_01"

// --- Feature Toggles (Comment to disable) ---
// #define FEATURE_PIEZO
#define FEATURE_BUTTON
// #define FEATURE_ACCEL

// --- Pin Assignments (ESP32-C3 Super Mini) ---
#define LED_PIN             3

#ifdef FEATURE_PIEZO
  #define PIEZO_PIN         4
  #define PIEZO_THRESHOLD   1500
#endif

#ifdef FEATURE_BUTTON
  #define BUTTON_PIN        5
  #define BUTTON_THRESHOLD  100    // Only used if analog read
#endif

#ifdef FEATURE_ACCEL
  #define ACCEL_X_PIN       6
  #define ACCEL_Y_PIN       7
  #define ACCEL_Z_PIN       0
  #define ACCEL_THRESHOLD   150
#endif

#define NUM_PIXELS          12
#define BRIGHTNESS          50
#define BREATHE_MAX         40
// ==========================================

// --- BLE UUIDs ---
#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

// --- Pod States ---
enum PodState {
  STATE_BOOT,
  STATE_WAITING,
  STATE_CONNECTED,
  STATE_ACTIVE,
  STATE_SUCCESS
};

// --- Color Structure ---
struct LEDColor {
  uint8_t r, g, b;
};

// --- Default Colors ---
LEDColor colorWait      = {0, 0, 255};      // Blue (breathing)
LEDColor colorConn      = {0, 255, 255};    // Cyan
LEDColor colorActive    = {255, 0, 0};      // Red
LEDColor colorSuccess   = {0, 255, 0};      // Green

// --- Globals ---
Adafruit_NeoPixel strip(NUM_PIXELS, LED_PIN, NEO_GRB + NEO_KHZ800);
BLEServer *pServer = NULL;
BLECharacteristic *pCharacteristic = NULL;
bool deviceConnected = false;
bool oldDeviceConnected = false;

PodState currentState = STATE_BOOT;
unsigned long stateStartTime = 0;
unsigned long lastAnimTime = 0;
int animStep = 0;

unsigned long startTime = 0;
unsigned long reactionTime = 0;
bool gameActive = false;
bool isTapped = false;

int baseX = 0, baseY = 0, baseZ = 0;
bool accelCalibrated = false;

// --- Helper: Create Color ---
uint32_t makeColor(LEDColor c) {
  return strip.Color(c.r, c.g, c.b);
}

// --- Helper: Set All LEDs ---
void fillStrip(uint32_t color) {
  for (int i = 0; i < NUM_PIXELS; i++) {
    strip.setPixelColor(i, color);
  }
  strip.show();
}

// --- Helper: Parse Color from String "R,G,B" ---
LEDColor parseColor(String str) {
  LEDColor c = {0, 0, 0};
  int comma1 = str.indexOf(',');
  int comma2 = str.lastIndexOf(',');
  if (comma1 > 0 && comma2 > comma1) {
    c.r = str.substring(0, comma1).toInt();
    c.g = str.substring(comma1 + 1, comma2).toInt();
    c.b = str.substring(comma2 + 1).toInt();
  }
  return c;
}

// --- Helper: Format Color to String ---
String formatColor(LEDColor c) {
  return String(c.r) + "," + String(c.g) + "," + String(c.b);
}

// --- BLE Callbacks ---
class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
      currentState = STATE_CONNECTED;
      stateStartTime = millis();
      Serial.println("Device Connected");
    }
    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
      currentState = STATE_WAITING;
      stateStartTime = millis();
      Serial.println("Device Disconnected");
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      String value = pCharacteristic->getValue();
      value.trim();
      
      Serial.print("Received: [");
      Serial.print(value);
      Serial.println("]");

      // --- Color Commands ---
      if (value.startsWith("COLOR_WAIT:")) {
        colorWait = parseColor(value.substring(11));
        pCharacteristic->setValue("OK:COLOR_WAIT:" + formatColor(colorWait));
        pCharacteristic->notify();
      }
      else if (value.startsWith("COLOR_CONN:")) {
        colorConn = parseColor(value.substring(11));
        pCharacteristic->setValue("OK:COLOR_CONN:" + formatColor(colorConn));
        pCharacteristic->notify();
      }
      else if (value.startsWith("COLOR_ACTIVE:")) {
        colorActive = parseColor(value.substring(13));
        pCharacteristic->setValue("OK:COLOR_ACTIVE:" + formatColor(colorActive));
        pCharacteristic->notify();
      }
      else if (value.startsWith("COLOR_SUCCESS:")) {
        colorSuccess = parseColor(value.substring(14));
        pCharacteristic->setValue("OK:COLOR_SUCCESS:" + formatColor(colorSuccess));
        pCharacteristic->notify();
      }
      else if (value == "GET_COLORS") {
        String response = "COLORS|WAIT:" + formatColor(colorWait) + 
                         "|CONN:" + formatColor(colorConn) +
                         "|ACTIVE:" + formatColor(colorActive) +
                         "|SUCCESS:" + formatColor(colorSuccess);
        pCharacteristic->setValue(response);
        pCharacteristic->notify();
      }
      else if (value == "RESET_COLORS") {
        colorWait = {0, 0, 255};
        colorConn = {0, 255, 255};
        colorActive = {255, 0, 0};
        colorSuccess = {0, 255, 0};
        pCharacteristic->setValue("OK:COLORS_RESET");
        pCharacteristic->notify();
      }
      // --- Game Commands ---
      else if (value == "START") {
        currentState = STATE_ACTIVE;
        fillStrip(makeColor(colorActive));
        startTime = micros();  // Microsecond precision
        gameActive = true;
        isTapped = false;
        accelCalibrated = false;
        pCharacteristic->setValue("OK:STARTED");
        pCharacteristic->notify();
        Serial.println("Game Started");
      }
      else if (value == "RESET") {
        currentState = STATE_CONNECTED;
        fillStrip(makeColor(colorConn));
        gameActive = false;
        pCharacteristic->setValue("OK:RESET");
        pCharacteristic->notify();
        Serial.println("Game Reset");
      }
      else {
        pCharacteristic->setValue("ERROR:UNKNOWN_COMMAND");
        pCharacteristic->notify();
      }
    }
};

// --- Calibrate Accelerometer ---
void calibrateAccel() {
  #ifdef FEATURE_ACCEL
    if (!accelCalibrated) {
      baseX = analogRead(ACCEL_X_PIN);
      baseY = analogRead(ACCEL_Y_PIN);
      baseZ = analogRead(ACCEL_Z_PIN);
      accelCalibrated = true;
      Serial.print("Accel Calibrated: ");
      Serial.print(baseX); Serial.print(", ");
      Serial.print(baseY); Serial.print(", ");
      Serial.println(baseZ);
    }
  #endif
}

// --- Check All Enabled Triggers ---
void checkTriggers() {
  if (!gameActive || !deviceConnected || isTapped) return;

  bool triggered = false;

  // 1. Piezo Check (Analog)
  #ifdef FEATURE_PIEZO
    int piezoValue = analogRead(PIEZO_PIN);
    if (piezoValue > PIEZO_THRESHOLD) {
      triggered = true;
      Serial.println("Trigger: Piezo");
    }
  #endif

  // 2. Button Check (Digital - More Reliable)
  #ifdef FEATURE_BUTTON
    #if !defined(FEATURE_PIEZO) || (BUTTON_PIN != PIEZO_PIN)
      // Use digital read for button (more reliable than analog)
      if (digitalRead(BUTTON_PIN) == LOW) {
        triggered = true;
        Serial.println("Trigger: Button");
      }
    #endif
  #endif

  // 3. Accelerometer Check (Analog)
  #ifdef FEATURE_ACCEL
    if (accelCalibrated && !triggered) {
      int curX = analogRead(ACCEL_X_PIN);
      int curY = analogRead(ACCEL_Y_PIN);
      int curZ = analogRead(ACCEL_Z_PIN);
      
      int diffX = abs(curX - baseX);
      int diffY = abs(curY - baseY);
      int diffZ = abs(curZ - baseZ);
      
      if (diffX > ACCEL_THRESHOLD || diffY > ACCEL_THRESHOLD || diffZ > ACCEL_THRESHOLD) {
        triggered = true;
        Serial.println("Trigger: Accelerometer");
      }
    }
  #endif

  // Process Trigger
  if (triggered) {
    isTapped = true;
    unsigned long endTime = micros();  // Microsecond precision
    reactionTime = (endTime - startTime) / 1000;  // Convert to milliseconds
    
    // Send result with millisecond accuracy
    String result = "RESULT:" + String(reactionTime) + "ms";
    pCharacteristic->setValue(result);
    pCharacteristic->notify();
    
    currentState = STATE_SUCCESS;
    stateStartTime = millis();
    fillStrip(makeColor(colorSuccess));
    
    gameActive = false;
    Serial.print("Reaction Time: ");
    Serial.print(reactionTime);
    Serial.println("ms");
  }
}

// --- Reset Trigger After Settle ---
void resetTrigger() {
  if (isTapped) {
    bool settled = false;
    
    #ifdef FEATURE_PIEZO
      int piezoValue = analogRead(PIEZO_PIN);
      if (piezoValue < (PIEZO_THRESHOLD / 2)) settled = true;
    #elif defined(FEATURE_BUTTON)
      if (digitalRead(BUTTON_PIN) == HIGH) settled = true;  // Button released
    #else
      if (millis() - stateStartTime > 2000) settled = true;
    #endif
    
    if (settled) {
      isTapped = false;
      accelCalibrated = false;
    }
  }
}

// --- Smart Animation Engine ---
void handleAnimations() {
  unsigned long now = millis();

  if (currentState == STATE_BOOT) {
    if (now - stateStartTime < 2000) {
      if (now - lastAnimTime > 50) {
        for (int i = 0; i < NUM_PIXELS; i++) {
          uint32_t color = strip.ColorHSV(i * 255 / NUM_PIXELS + animStep * 10, 255, BRIGHTNESS);
          strip.setPixelColor(i, color);
        }
        strip.show();
        animStep++;
        lastAnimTime = now;
      }
    } else {
      currentState = STATE_WAITING;
      stateStartTime = now;
      fillStrip(0);
    }
  }

  // SMOOTH BREATHING - Gamma corrected, 10ms updates
  else if (currentState == STATE_WAITING) {
    if (now - lastAnimTime > 10) {
      float breath = sin(now / 800.0);  // Slower, natural cycle
      float normalized = (breath + 1.0) / 2.0;  // 0 to 1
      float gammaCorrected = pow(normalized, 2.2);  // Gamma correction
      int brightness = (int)(gammaCorrected * BREATHE_MAX);
      
      uint32_t color = strip.Color(colorWait.r * brightness / BREATHE_MAX,
                                   colorWait.g * brightness / BREATHE_MAX,
                                   colorWait.b * brightness / BREATHE_MAX);
      fillStrip(color);
      lastAnimTime = now;
    }
  }

  else if (currentState == STATE_CONNECTED) {
    if (now - lastAnimTime > 500) {
      fillStrip(makeColor(colorConn));
      lastAnimTime = now;
    }
  }

  else if (currentState == STATE_ACTIVE) {
    if (now - lastAnimTime > 500) {
      fillStrip(makeColor(colorActive));
      lastAnimTime = now;
    }
  }

  else if (currentState == STATE_SUCCESS) {
    if (now - stateStartTime > 2000) {
      currentState = STATE_CONNECTED;
      fillStrip(makeColor(colorConn));
    }
  }
}

void setup() {

  Serial.begin(115200);
  while (!Serial);

  Serial.println("================================");
  Serial.println("  BlazePod C3 - Production Ready");
  Serial.println("================================");
  
  #ifdef FEATURE_PIEZO
    Serial.println("Piezo:   ON");
  #else
    Serial.println("Piezo:   OFF");
  #endif
  
  #ifdef FEATURE_BUTTON
    Serial.println("Button:  ON");
  #else
    Serial.println("Button:  OFF");
  #endif
  
  #ifdef FEATURE_ACCEL
    Serial.println("Accel:   ON");
  #else
    Serial.println("Accel:   OFF");
  #endif
  
  Serial.println("================================");

  pinMode(LED_PIN, OUTPUT);
  
  #ifdef FEATURE_PIEZO
    pinMode(PIEZO_PIN, INPUT);
  #endif
  
  #ifdef FEATURE_BUTTON
    pinMode(BUTTON_PIN, INPUT_PULLUP);
  #endif
  
  #ifdef FEATURE_ACCEL
    pinMode(ACCEL_X_PIN, INPUT);
    pinMode(ACCEL_Y_PIN, INPUT);
    pinMode(ACCEL_Z_PIN, INPUT);
  #endif

  analogReadResolution(12);

  strip.begin();
  strip.setBrightness(BRIGHTNESS);
  strip.show();
  fillStrip(0);

  currentState = STATE_BOOT;
  stateStartTime = millis();

  BLEDevice::init(DEVICE_NAME);
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService *pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE |
                      BLECharacteristic::PROPERTY_NOTIFY
                    );

  pCharacteristic->setCallbacks(new MyCallbacks());
  pCharacteristic->addDescriptor(new BLE2902());
  pService->start();

  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  BLEDevice::startAdvertising();
  
  Serial.println("System Ready - Waiting for connection...");

}

void loop() {
  if (!deviceConnected && oldDeviceConnected) {
    delay(500);
    pServer->startAdvertising();
    oldDeviceConnected = deviceConnected;
  }
  if (deviceConnected && !oldDeviceConnected) {
    oldDeviceConnected = deviceConnected;
  }

  handleAnimations();
  
  if (gameActive && !accelCalibrated) {
    calibrateAccel();
  }
  
  checkTriggers();
  resetTrigger();
  
  delay(10);
}