#include <NimBLEDevice.h>
#include <NimBLEHIDDevice.h>
#include <NimBLEUtils.h>
#include <NimBLEServer.h>
#include <driver/adc.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// --- Manual Key Definitions (Workaround for include issues) ---
// This block makes the code self-contained and removes the need
// to find the correct, version-specific header file.
#define KEY_LEFT_CTRL   0x01
#define KEY_LEFT_SHIFT  0x02
#define KEY_A           0x04
#define KEY_C           0x06
#define KEY_V           0x19
#define KEY_X           0x1B
#define KEY_TAB         0x2B
#define KEY_EQUAL       0x2E
#define KEY_MINUS       0x2D

// --- Pin Definitions ---
#define FLEX_THUMB_PIN ADC1_CHANNEL_4 // GPIO 32 is ADC1_CHANNEL_4
#define FLEX_INDEX_PIN ADC1_CHANNEL_5 // GPIO 33 is ADC1_CHANNEL_5
#define FLEX_MIDDLE_PIN ADC1_CHANNEL_7 // GPIO 25 is ADC1_CHANNEL_7
#define FLEX_RING_PIN ADC1_CHANNEL_6  // GPIO 26 is ADC1_CHANNEL_6
#define VIBRATION_PIN 14

// --- Gesture Thresholds (CRITICAL: Calibrate these values!) ---
#define FLEX_STRAIGHT_THRESHOLD 1200 // Analog value BELOW which a finger is "straight".
#define FLEX_BENT_THRESHOLD 2000     // Analog value ABOVE which a finger is "bent".
#define CUT_ACCEL_THRESHOLD 20.0f    // Threshold for the "slice" motion for Cut.
#define TWIST_GYRO_THRESHOLD 4.0f    // Threshold for tab switching twist motion.
#define TILT_GYRO_THRESHOLD 3.5f     // Threshold for zoom tilt motion.
#define COOLDOWN_MS 1000             // Increased cooldown for more deliberate gestures.

// --- BLE HID Objects ---
NimBLEHIDDevice* hid;
NimBLECharacteristic* inputKeyboard;

// --- Sensor Objects ---
Adafruit_MPU6050 mpu;

// --- State Machine for Gestures ---
enum HandState {
    STATE_OPEN,
    STATE_FIST,
    STATE_POINTING,
    STATE_PINCH, // New state for zooming
    STATE_UNKNOWN // In-between states
};

HandState currentHandState = STATE_UNKNOWN;
HandState previousHandState = STATE_UNKNOWN;

unsigned long lastGestureTime = 0;
bool isConnected = false;

// --- Function Prototypes (Forward Declarations) ---
void triggerHapticFeedback(int duration_ms);
void sendMultiKey(uint8_t modifier, uint8_t key);
void sendMultiKey(const uint8_t keys[], uint8_t numKeys);


// --- Standard BLE Callbacks & Setup ---
class MyServerCallbacks : public NimBLEServerCallbacks {
    void onConnect(NimBLEServer* pServer) {
        Serial.println("Client connected");
        isConnected = true;
        triggerHapticFeedback(150);
    }
    void onDisconnect(NimBLEServer* pServer) {
        Serial.println("Client disconnected");
        isConnected = false;
        // Start advertising again to allow new connections
        NimBLEDevice::getAdvertising()->start();
        Serial.println("Advertising started");
    }
};

void setup() {
    Serial.begin(115200);
    Serial.println("Starting Custom Gesture Glove...");

    // Configure ADC pins for flex sensors
    adc1_config_width(ADC_WIDTH_BIT_12);
    adc1_config_channel_atten(FLEX_THUMB_PIN, ADC_ATTEN_DB_11);
    adc1_config_channel_atten(FLEX_INDEX_PIN, ADC_ATTEN_DB_11);
    adc1_config_channel_atten(FLEX_MIDDLE_PIN, ADC_ATTEN_DB_11);
    adc1_config_channel_atten(FLEX_RING_PIN, ADC_ATTEN_DB_11);
    
    pinMode(VIBRATION_PIN, OUTPUT);
    digitalWrite(VIBRATION_PIN, LOW);

    if (!mpu.begin()) {
        Serial.println("Failed to find MPU6050 chip");
        while (1) { delay(10); }
    }
    Serial.println("MPU6050 Found!");
    mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);

    // --- FINALIZED BLE HID Setup ---
    NimBLEDevice::init("Gesture Glove Custom");
    NimBLEDevice::setSecurityAuth(true, true, true);

    NimBLEServer* pServer = NimBLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());

    hid = new NimBLEHIDDevice(pServer);
    // The report ID for a keyboard is 1
    inputKeyboard = hid->getInputReport(1); 

    hid->setManufacturer("WearableTech");
    // REMOVED: The line below was causing the compilation error and is not essential.
    // hid->setPlugPlay(0x02, 0x05ac, 0x8242, 0x0111); 
    hid->setHidInfo(0x00, 0x01);

    hid->startServices();
    NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
    pAdvertising->setAppearance(HID_KEYBOARD);
    pAdvertising->addServiceUUID(hid->getHidService()->getUUID());
    pAdvertising->start();

    Serial.println("Advertising started. Waiting for connection...");
    Serial.println("--- CALIBRATE NOW ---");
    Serial.println("Open/close hand, point, and pinch to see flex values.");
    Serial.println("Adjust FLEX_STRAIGHT_THRESHOLD and FLEX_BENT_THRESHOLD.");
}

void loop() {
    if (isConnected) {
        updateHandState();
        
        if (millis() - lastGestureTime > COOLDOWN_MS) {
            detectGesture();
        }

        if(currentHandState != STATE_UNKNOWN) {
            previousHandState = currentHandState;
        }
    }
    delay(50);
}

// --- Core Logic Functions ---

void updateHandState() {
    int thumbFlex = adc1_get_raw(FLEX_THUMB_PIN);
    int indexFlex = adc1_get_raw(FLEX_INDEX_PIN);
    int middleFlex = adc1_get_raw(FLEX_MIDDLE_PIN);
    int ringFlex = adc1_get_raw(FLEX_RING_PIN);

    bool isFist = (thumbFlex > FLEX_BENT_THRESHOLD) &&
                  (indexFlex > FLEX_BENT_THRESHOLD) &&
                  (middleFlex > FLEX_BENT_THRESHOLD) &&
                  (ringFlex > FLEX_BENT_THRESHOLD);

    bool isOpen = (thumbFlex < FLEX_STRAIGHT_THRESHOLD) &&
                  (indexFlex < FLEX_STRAIGHT_THRESHOLD) &&
                  (middleFlex < FLEX_STRAIGHT_THRESHOLD) &&
                  (ringFlex < FLEX_STRAIGHT_THRESHOLD);
                  
    bool isPointing = (indexFlex < FLEX_STRAIGHT_THRESHOLD) &&
                      (middleFlex > FLEX_BENT_THRESHOLD) &&
                      (ringFlex > FLEX_BENT_THRESHOLD);

    bool isPinching = (thumbFlex > FLEX_BENT_THRESHOLD) &&
                      (indexFlex > FLEX_BENT_THRESHOLD) &&
                      (middleFlex < FLEX_STRAIGHT_THRESHOLD) &&
                      (ringFlex < FLEX_STRAIGHT_THRESHOLD);

    if (isFist) {
        currentHandState = STATE_FIST;
    } else if (isOpen) {
        currentHandState = STATE_OPEN;
    } else if (isPointing) {
        currentHandState = STATE_POINTING;
    } else if (isPinching) {
        currentHandState = STATE_PINCH;
    } else {
        currentHandState = STATE_UNKNOWN;
    }
}

void detectGesture() {
    if (previousHandState == STATE_FIST && currentHandState == STATE_OPEN) {
        Serial.println("Gesture: Paste (Fist -> Open)");
        sendMultiKey(KEY_LEFT_CTRL, KEY_V);
        return;
    }

    if (previousHandState == STATE_OPEN && currentHandState == STATE_FIST) {
        Serial.println("Gesture: Select All (Open -> Fist)");
        sendMultiKey(KEY_LEFT_CTRL, KEY_A);
        return;
    }
    
    if (previousHandState == STATE_POINTING && currentHandState == STATE_FIST) {
        Serial.println("Gesture: Copy (Pointing -> Fist)");
        sendMultiKey(KEY_LEFT_CTRL, KEY_C);
        return;
    }

    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    if (currentHandState == STATE_FIST) {
        if (abs(a.acceleration.x) > CUT_ACCEL_THRESHOLD || abs(a.acceleration.z) > CUT_ACCEL_THRESHOLD) {
            Serial.println("Gesture: Cut (Fist + Slice)");
            sendMultiKey(KEY_LEFT_CTRL, KEY_X);
            return;
        }
    }

    if (currentHandState == STATE_POINTING) {
        if (g.gyro.z > TWIST_GYRO_THRESHOLD) {
            Serial.println("Gesture: Next Tab (Point + Twist Right)");
            sendMultiKey(KEY_LEFT_CTRL, KEY_TAB);
            return;
        }
        if (g.gyro.z < -TWIST_GYRO_THRESHOLD) {
            Serial.println("Gesture: Prev Tab (Point + Twist Left)");
            uint8_t keys[] = {KEY_LEFT_CTRL, KEY_LEFT_SHIFT, KEY_TAB};
            sendMultiKey(keys, 3);
            return;
        }
    }

    if (currentHandState == STATE_PINCH) {
        if (g.gyro.x > TILT_GYRO_THRESHOLD) {
            Serial.println("Gesture: Zoom In (Pinch + Tilt Fwd)");
            sendMultiKey(KEY_LEFT_CTRL, KEY_EQUAL);
            return;
        }
        if (g.gyro.x < -TILT_GYRO_THRESHOLD) {
            Serial.println("Gesture: Zoom Out (Pinch + Tilt Back)");
            sendMultiKey(KEY_LEFT_CTRL, KEY_MINUS);
            return;
        }
    }
}

// --- Helper Functions ---

void sendMultiKey(uint8_t modifier, uint8_t key) {
    if (isConnected) {
        uint8_t keyReport[8] = {0};
        keyReport[0] = modifier;
        keyReport[2] = key;
        
        inputKeyboard->setValue(keyReport, sizeof(keyReport));
        inputKeyboard->notify();
        delay(20);
        
        memset(keyReport, 0, sizeof(keyReport));
        inputKeyboard->setValue(keyReport, sizeof(keyReport));
        inputKeyboard->notify();
        
        triggerHapticFeedback(100);
        lastGestureTime = millis();
        previousHandState = STATE_UNKNOWN;
    }
}

void sendMultiKey(const uint8_t keys[], uint8_t numKeys) {
    if (isConnected && numKeys == 3) {
        uint8_t keyReport[8] = {0};
        keyReport[0] = keys[0] | keys[1]; // Combine modifiers
        keyReport[2] = keys[2];
        
        inputKeyboard->setValue(keyReport, sizeof(keyReport));
        inputKeyboard->notify();
        delay(20);
        
        memset(keyReport, 0, sizeof(keyReport));
        inputKeyboard->setValue(keyReport, sizeof(keyReport));
        inputKeyboard->notify();
        
        triggerHapticFeedback(100);
        lastGestureTime = millis();
        previousHandState = STATE_UNKNOWN;
    }
}

void triggerHapticFeedback(int duration_ms) {
    digitalWrite(VIBRATION_PIN, HIGH);
    delay(duration_ms);
    digitalWrite(VIBRATION_PIN, LOW);
}

void printSensorValues() {
    int thumb = adc1_get_raw(FLEX_THUMB_PIN);
    int index = adc1_get_raw(FLEX_INDEX_PIN);
    int middle = adc1_get_raw(FLEX_MIDDLE_PIN);
    int ring = adc1_get_raw(FLEX_RING_PIN);
    Serial.printf("Flex - T:%d I:%d M:%d R:%d || State: %d\n", thumb, index, middle, ring, currentHandState);
    delay(200);
}