#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <SPI.h>
#include <U8g2lib.h>

// OLED Display Configuration for Waveshare 1.51" Transparent OLED
// SSD1309 128x64 SPI Interface
#define OLED_CLK  18   // SPI Clock (SCL/SCK)
#define OLED_MOSI 23   // SPI Data (SDA/MOSI) 
#define OLED_RES  16   // Reset
#define OLED_DC   17   // Data/Command
#define OLED_CS   5    // Chip Select

// BLE Configuration
#define SERVICE_UUID        "12345678-1234-1234-1234-123456789abc"
#define BATTERY_CHAR_UUID   "87654321-4321-4321-4321-cba987654321"
#define SPEED_CHAR_UUID     "11111111-2222-3333-4444-555555555555"
#define POWER_CHAR_UUID     "22222222-3333-4444-5555-666666666666"
#define LIGHT_CHAR_UUID     "33333333-4444-5555-6666-777777777777"
#define DEVICE_NAME         "mirror-hud"

// Display object for SSD1309 - Hardware SPI
U8G2_SSD1309_128X64_NONAME0_F_4W_HW_SPI u8g2(U8G2_R0, OLED_CS, OLED_DC, OLED_RES);

// Alternative display objects to try if the above doesn't work:
// U8G2_SSD1306_128X64_NONAME_F_4W_HW_SPI u8g2_alt1(U8G2_R0, OLED_CS, OLED_DC, OLED_RES);
// U8G2_SSD1309_128X64_NONAME0_F_4W_SW_SPI u8g2_alt2(U8G2_R0, OLED_CLK, OLED_MOSI, OLED_CS, OLED_DC, OLED_RES);

// BLE Variables
BLEServer* pServer = nullptr;
BLEService* pService = nullptr;
BLECharacteristic* pBatteryCharacteristic = nullptr;
BLECharacteristic* pSpeedCharacteristic = nullptr;
BLECharacteristic* pPowerCharacteristic = nullptr;
BLECharacteristic* pLightCharacteristic = nullptr;
bool deviceConnected = false;
bool oldDeviceConnected = false;

// Data variables
float batteryVoltage = 0.0; // Voltage instead of percentage
float speed = 0.0;          // Speed in km/h
float power = 0.0;          // Power in watts
bool lightOn = false;       // Headlight status
unsigned long lastUpdate = 0;
const unsigned long UPDATE_INTERVAL = 250; // Update display every 250ms (reduce load)
bool dataReceived = false;  // Flag to track if we've received data from glove
// BLE Server Callbacks
class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
        deviceConnected = true;
        Serial.println("BLE Client connected to mirror-hud");
    }

    void onDisconnect(BLEServer* pServer) {
        deviceConnected = false;
        Serial.println("BLE Client disconnected from mirror-hud");
    }
};

// Battery Level Characteristic Callback
class BatteryCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) {
        std::string value = pCharacteristic->getValue();
        
        if (value.length() == 4) { // Expecting 4 bytes for float
            memcpy(&batteryVoltage, value.data(), sizeof(float));
            dataReceived = true;  // Mark that we've received data
            Serial.print("Received battery voltage: ");
            Serial.print(batteryVoltage);
            Serial.println("V");
        }
    }
};

// Speed Characteristic Callback
class SpeedCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) {
        std::string value = pCharacteristic->getValue();
        
        if (value.length() == 4) { // Expecting 4 bytes for float
            memcpy(&speed, value.data(), sizeof(float));
            dataReceived = true;  // Mark that we've received data
            Serial.print("Received speed: ");
            Serial.print(speed);
            Serial.println(" km/h");
        }
    }
};

// Power Characteristic Callback
class PowerCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) {
        std::string value = pCharacteristic->getValue();
        
        if (value.length() == 4) { // Expecting 4 bytes for float
            memcpy(&power, value.data(), sizeof(float));
            dataReceived = true;  // Mark that we've received data
            Serial.print("Received power: ");
            Serial.print(power);
            Serial.println(" W");
        }
    }
};

// Light Characteristic Callback
class LightCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) {
        std::string value = pCharacteristic->getValue();
        
        if (value.length() == 1) { // Expecting 1 byte for boolean
            lightOn = (value[0] != 0);
            dataReceived = true;  // Mark that we've received data
            Serial.print("Received light status: ");
            Serial.println(lightOn ? "ON" : "OFF");
        }
    }
};
void initBLE() {
    Serial.println("Initializing BLE...");
    
    // Initialize BLE
    BLEDevice::init(DEVICE_NAME);
    
    // Create BLE Server
    pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());
    
    // Create BLE Service
    pService = pServer->createService(SERVICE_UUID);
    
    // Create Battery Characteristic
    pBatteryCharacteristic = pService->createCharacteristic(
        BATTERY_CHAR_UUID,
        BLECharacteristic::PROPERTY_READ |
        BLECharacteristic::PROPERTY_WRITE |
        BLECharacteristic::PROPERTY_NOTIFY
    );
    pBatteryCharacteristic->setCallbacks(new BatteryCallbacks());
    pBatteryCharacteristic->addDescriptor(new BLE2902());
    
    // Create Speed Characteristic
    pSpeedCharacteristic = pService->createCharacteristic(
        SPEED_CHAR_UUID,
        BLECharacteristic::PROPERTY_READ |
        BLECharacteristic::PROPERTY_WRITE |
        BLECharacteristic::PROPERTY_NOTIFY
    );
    pSpeedCharacteristic->setCallbacks(new SpeedCallbacks());
    pSpeedCharacteristic->addDescriptor(new BLE2902());
    
    // Create Power Characteristic
    pPowerCharacteristic = pService->createCharacteristic(
        POWER_CHAR_UUID,
        BLECharacteristic::PROPERTY_READ |
        BLECharacteristic::PROPERTY_WRITE |
        BLECharacteristic::PROPERTY_NOTIFY
    );
    pPowerCharacteristic->setCallbacks(new PowerCallbacks());
    pPowerCharacteristic->addDescriptor(new BLE2902());
    
    // Create Light Characteristic
    pLightCharacteristic = pService->createCharacteristic(
        LIGHT_CHAR_UUID,
        BLECharacteristic::PROPERTY_READ |
        BLECharacteristic::PROPERTY_WRITE |
        BLECharacteristic::PROPERTY_NOTIFY
    );
    pLightCharacteristic->setCallbacks(new LightCallbacks());
    pLightCharacteristic->addDescriptor(new BLE2902());
    
    // Start the service
    pService->start();
    
    // Start advertising
    BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
    pAdvertising->addServiceUUID(SERVICE_UUID);
    pAdvertising->setScanResponse(false);
    pAdvertising->setMinPreferred(0x0);
    BLEDevice::startAdvertising();
    
    Serial.println("BLE Server started, advertising as 'mirror-hud'");
}
void initDisplay() {
    Serial.println("=== OLED Display Initialization (SPI) ===");
    
    // Configure SPI pins
    Serial.println("Configuring SPI pins...");
    pinMode(OLED_CS, OUTPUT);
    pinMode(OLED_DC, OUTPUT);
    pinMode(OLED_RES, OUTPUT);
    
    // Manual reset sequence
    Serial.println("Performing manual reset...");
    digitalWrite(OLED_RES, LOW);
    delay(100);
    digitalWrite(OLED_RES, HIGH);
    delay(100);
    
    // Initialize SPI
    Serial.println("Initializing SPI...");
    SPI.begin(OLED_CLK, -1, OLED_MOSI, OLED_CS); // CLK, MISO(-1), MOSI, CS
    
    Serial.print("SPI pins - CLK:");
    Serial.print(OLED_CLK);
    Serial.print(" MOSI:");
    Serial.print(OLED_MOSI);
    Serial.print(" CS:");
    Serial.print(OLED_CS);
    Serial.print(" DC:");
    Serial.print(OLED_DC);
    Serial.print(" RES:");
    Serial.println(OLED_RES);
    
    // Initialize U8g2 with SPI
    Serial.println("Initializing U8g2 library for SPI...");
    
    bool displayInit = u8g2.begin();
    if (displayInit) {
        Serial.println("✓ U8g2 SPI initialization successful!");
    } else {
        Serial.println("✗ U8g2 SPI initialization failed, but continuing...");
    }
    
    // Force display setup
    u8g2.clearBuffer();
    u8g2.setDisplayRotation(U8G2_R0);
    u8g2.setPowerSave(0);  // Wake up display
    
    // Test with simple elements
    Serial.println("Testing SPI display...");
    u8g2.clearBuffer();
    u8g2.drawPixel(64, 32);  // Center pixel
    u8g2.sendBuffer();
    delay(500);
    
    // Test with rectangle
    u8g2.clearBuffer();
    u8g2.drawBox(10, 10, 20, 20);
    u8g2.sendBuffer();
    delay(500);
    
    // Show "tangerine" boot screen
    Serial.println("Displaying tangerine boot screen via SPI...");
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_ncenB18_tr);
    u8g2.drawStr(15, 35, "tangerine");
    
    // Add border to confirm display is working
    u8g2.drawFrame(0, 0, 128, 64);
    u8g2.sendBuffer();
    
    Serial.println("SPI OLED should now show 'tangerine'!");
    delay(3000);  // Show "tangerine" for 3 seconds
    
    Serial.println("=== SPI OLED Display Initialization Complete ===");
}
void updateDisplay() {
    u8g2.clearBuffer();
    
    if (!dataReceived) {
        // Show "tangerine" screen when no data received
        u8g2.setFont(u8g2_font_ncenB18_tr);
        u8g2.drawStr(15, 35, "tangerine");
        
        // Show connection status
        u8g2.setFont(u8g2_font_6x10_tr);
        if (deviceConnected) {
            u8g2.drawStr(30, 55, "Connected");
        } else {
            u8g2.drawStr(25, 55, "Waiting...");
        }
    } else {
        // Show EUC data when received from glove
        
        // Connection status indicator
        if (deviceConnected) {
            u8g2.setFont(u8g2_font_4x6_tr);
            u8g2.drawStr(100, 8, "CONN");
        } else {
            u8g2.setFont(u8g2_font_4x6_tr);
            u8g2.drawStr(95, 8, "DISC");
        }
        
        // Speed Display - Large and prominent
        u8g2.setFont(u8g2_font_ncenB24_tn);  // Increased from 18 to 24
        char speedStr[10];
        dtostrf(speed, 4, 1, speedStr);
        u8g2.drawStr(5, 35, speedStr);  // Moved left to make room for larger font
        
        // "K" instead of "km/h"
        u8g2.setFont(u8g2_font_ncenB12_tr);  // Larger K
        u8g2.drawStr(85, 35, "K");
        
        // Power Display where km/h used to be
        u8g2.setFont(u8g2_font_ncenB08_tr);
        char powerStr[15];
        sprintf(powerStr, "%.0fW", power);
        u8g2.drawStr(100, 35, powerStr);
        
        // Battery Voltage Display
        u8g2.setFont(u8g2_font_ncenB08_tr);
        char voltStr[15];
        sprintf(voltStr, "%.1fV", batteryVoltage);
        u8g2.drawStr(10, 55, voltStr);
        
        // 5-Bar Battery Display based on actual voltage range
        // 134.4V = 100%, 117.7V = 44% 
        float minVoltage = 104.58;   // Calculated 0% voltage
        float maxVoltage = 134.4;    // 100% voltage
        
        // Calculate number of bars to display (0-5)
        int numBars = 0;
        if (batteryVoltage >= 134.40) numBars = 5;      // 100%+
        else if (batteryVoltage >= 128.44) numBars = 4; // 80%+
        else if (batteryVoltage >= 122.47) numBars = 3; // 60%+
        else if (batteryVoltage >= 116.51) numBars = 2; // 40%+
        else if (batteryVoltage >= 110.54) numBars = 1; // 20%+
        else numBars = 0;                               // <20%
        
        // Draw 5 individual battery bars (each 8px wide, 2px spacing)
        int barX = 70;  // Starting X position
        int barY = 48;  // Y position
        int barWidth = 8;
        int barHeight = 8;
        int barSpacing = 2;
        
        for (int i = 0; i < 5; i++) {
            int x = barX + i * (barWidth + barSpacing);
            
            // Draw bar outline
            u8g2.drawFrame(x, barY, barWidth, barHeight);
            
            // Fill bar if it should be active
            if (i < numBars) {
                u8g2.drawBox(x + 1, barY + 1, barWidth - 2, barHeight - 2);
            }
        }
        
        // Show percentage for debugging
        float percentage = ((batteryVoltage - minVoltage) / (maxVoltage - minVoltage)) * 100.0;
        if (percentage < 0) percentage = 0;
        if (percentage > 100) percentage = 100;
        
        char percentStr[8];
        sprintf(percentStr, "%.0f%%", percentage);
        u8g2.setFont(u8g2_font_4x6_tr);
        u8g2.drawStr(70, 63, percentStr);
        
        // Headlight icon at bottom left
        if (lightOn) {
            // Draw filled headlight icon when on
            u8g2.drawCircle(8, 58, 4);      // Outer circle
            u8g2.drawDisc(8, 58, 2);        // Inner filled circle
            u8g2.drawLine(8, 54, 8, 50);    // Light beam line
            u8g2.drawLine(6, 55, 4, 53);    // Left beam
            u8g2.drawLine(10, 55, 12, 53);  // Right beam
        } else {
            // Draw outline headlight icon when off
            u8g2.drawCircle(8, 58, 4);      // Outer circle only
            u8g2.drawCircle(8, 58, 2);      // Inner circle outline
        }
    }
    
    // Send buffer to display
    u8g2.sendBuffer();
}
void setup() {
    Serial.begin(115200);
    delay(1000);  // Give time for serial to initialize
    Serial.println();
    Serial.println("=== Mirror HUD Display Starting ===");
    Serial.println("ESP32 Standard Board");
    Serial.printf("Free heap: %d bytes\n", ESP.getFreeHeap());
    
    // Initialize OLED display first
    initDisplay();
    
    // Initialize BLE
    initBLE();
    
    Serial.println("=== Mirror HUD Ready! ===");
    Serial.printf("Free heap after init: %d bytes\n", ESP.getFreeHeap());
}

void loop() {
    // Handle BLE disconnection
    if (!deviceConnected && oldDeviceConnected) {
        delay(500); // Give time for BLE stack
        pServer->startAdvertising(); // Restart advertising
        Serial.println("Restarting BLE advertising...");
        oldDeviceConnected = deviceConnected;
    }
    
    // Handle BLE connection
    if (deviceConnected && !oldDeviceConnected) {
        oldDeviceConnected = deviceConnected;
    }
    
    // Update display at regular intervals
    if (millis() - lastUpdate > UPDATE_INTERVAL) {
        updateDisplay();
        lastUpdate = millis();
    }
    
    delay(10);
}