Flicker issue is not smooth when blinking

[BimoSora2]

I hope you fix the library to overcome flicker like this which is clearly not smooth.

VID-20250124-WA0000.mp4

#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <Adafruit_BMP280.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_INA219.h>
#include <SPI.h>
#include <Wire.h>
#include <LittleFS.h>
#include <TimeLib.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <DNSServer.h>
#include <ArduinoJson.h>
#include <Fonts/Swansea5pt7b.h>
#include <Fonts/Swansea6pt7b.h>
#include <Fonts/Open_24_Display_St17pt7b.h>

const char* ssid = "Time";         
const char* password = "12345678";    

#define TFT_MOSI  13
#define TFT_SCLK  14
#define TFT_DC    0
#define downButton 12

const int BATT_IMG_WIDTH = 22;
const int BATT_IMG_HEIGHT = 10;
const int ICON_WIDTH = 80;
const int ICON_HEIGHT = 80;

const int SMALL_ICON_WIDTH = 32;
const int SMALL_ICON_HEIGHT = 32;

const char* ICON_TEMP = "/temp_icon.bmp";
const char* ICON_PRESSURE = "/pressure_icon.bmp";
const char* ICON_ALTITUDE = "/altitude_icon.bmp";
const char* ICON_ANGLE = "/angle_icon.bmp";
const char* ICON_STEP = "/step_icon.bmp";
const char* ICON_SETTINGS = "/settings_icon.bmp";

const byte DNS_PORT = 53;
DNSServer dnsServer;
IPAddress apIP(172, 217, 28, 1);

ESP8266WebServer server(80);
Adafruit_ST7735 tft = Adafruit_ST7735(-1, TFT_DC, TFT_MOSI, TFT_SCLK, -1);
Adafruit_BMP280 bmp;
Adafruit_MPU6050 mpu;
Adafruit_INA219 ina219;

bool bmpEnabled = false;
bool mpuEnabled = false;

const float BATTERY_MAX = 4.2;
const float BATTERY_MIN = 3.0;

float temperature = 0, pressure = 0, altitude = 0;
float lastPressure = 0;
String tempC = "0.0 *C", tempF = "0.0 *F", pressureStr = "0.0 hPa", altitudeStr = "0.0 m";
String accX = "X: 0*", accY = "Y: 0*", accZ = "Z: 0*";
String batteryStatus = "100%";
String prevBatteryStatus = "";
String lastVoltageStr = "";
String prevTempC, prevTempF, prevPressureStr, prevAltitudeStr;
String prevAccX = "", prevAccY = "", prevAccZ = "";
String timeStr = "00:00";
String dateStr = "00/00/0000";
String prevTimeStr = "";
String prevDateStr = "";
String settingsText = "SETTING";
String prevSettingsText = "";
uint16_t backgroundColor = 0;
const uint16_t w = 80;
const uint16_t h = 160;

const int CONTENT_MARGIN = 9;  // Margin untuk semua konten (left, right, top, bottom)
const int BORDER_WIDTH = 80;   // Lebar border utama
const int BORDER_HEIGHT = 160; // Tinggi border utama

// Konstanta turunan untuk posisi border
const int BORDER_START_X = (tft.width() - BORDER_WIDTH) / 2;
const int BORDER_START_Y = (tft.height() - BORDER_HEIGHT) / 2;

unsigned long stepCount = 0;
String prevStepCount = "";
bool isFirstRun = true;

bool autoReturnEnabled = true;

bool bmpAvailable = false;
bool mpuAvailable = false;
bool ina219Available = false;
bool isInverted = true;
bool wifiInitialized = false;
volatile bool buttonPressed = false;
bool iconsDrawnTimeMenu = false;
int currentMenu = 1;

unsigned long lastButtonPress = 0;
const unsigned long AUTO_RETURN_TIMEOUT = 20000;

unsigned long buttonPressStartTime = 0;
const unsigned long LONG_PRESS_DURATION = 800;
const unsigned long DEBOUNCE_TIME = 0;
bool isButtonDown = false;
volatile bool longPressExecuted = false;

unsigned long sensorMillis = 0;
unsigned long stepMillis = 0;
unsigned long tempMillis = 0;
unsigned long pressureMillis = 0;
unsigned long altitudeMillis = 0;
const long sensorInterval = 50;
const long stepInterval = 1000;
const long tempInterval = 4000;
const long pressureInterval = 4000;
const long altitudeInterval = 4000;

const int WINDOW_SIZE = 10;  
float accelWindow[WINDOW_SIZE];
int windowIndex = 0;
unsigned long lastStepTime = 0;
const unsigned long MIN_STEP_INTERVAL = 150;
const float VARIANCE_THRESHOLD = 0.5;
const float STEP_MAGNITUDE_THRESHOLD = 1.5;
const float PEAK_THRESHOLD = 2.0;

class KalmanFilter {
private:
    float Q; // Process noise variance
    float R; // Measurement noise variance
    float P; // Estimation error variance
    float X; // State estimate
    float K; // Kalman gain
    bool initialized;

public:
    KalmanFilter(float processNoise = 0.001, float measurementNoise = 0.1) :
        Q(processNoise),
        R(measurementNoise),
        P(1.0),
        X(0),
        K(0),
        initialized(false) {}

    float update(float measurement) {
        if (!initialized) {
            X = measurement;
            initialized = true;
            return X;
        }

        P = P + Q;
        K = P / (P + R);
        X = X + K * (measurement - X);
        P = (1 - K) * P;

        return X;
    }

    void reset() {
        initialized = false;
        P = 1.0;
        X = 0;
        K = 0;
    }
};

// Then modify the KalmanFilter class declaration and other code before these functions
KalmanFilter tempKalman(0.001, 0.1);
KalmanFilter pressureKalman(0.01, 1.0);
KalmanFilter altitudeKalman(0.01, 2.0);
KalmanFilter accelXKalman(0.01, 0.5);
KalmanFilter accelYKalman(0.01, 0.5);
KalmanFilter accelZKalman(0.01, 0.5);
KalmanFilter gyroXKalman(0.01, 0.1);
KalmanFilter gyroYKalman(0.01, 0.1);
KalmanFilter gyroZKalman(0.01, 0.1);

struct FilteredAngles {
    float roll;  // X-axis rotation
    float pitch; // Y-axis rotation
    float yaw;   // Z-axis rotation
};

float gyroAngleX = 0;
float gyroAngleY = 0;
float gyroAngleZ = 0;
unsigned long lastGyroRead = 0;

struct FilteredMPUData {
    float x, y, z;
    float roll, pitch, yaw;  // Added these fields for angular data
};

float readFilteredTemperature() {
    if (!bmpAvailable || !bmpEnabled) return 0.0;
    float rawTemp = bmp.readTemperature();
    return tempKalman.update(rawTemp);
}

float readFilteredPressure() {
    if (!bmpAvailable || !bmpEnabled) return 0.0;
    float rawPressure = bmp.readPressure() / 100.0F;
    return pressureKalman.update(rawPressure);
}

float readFilteredAltitude() {
    if (!bmpAvailable || !bmpEnabled) return 0.0;
    float rawAltitude = bmp.readAltitude(1013.25);
    return altitudeKalman.update(rawAltitude);
}

FilteredMPUData readFilteredAcceleration() {
    FilteredMPUData filtered = {0, 0, 0, 0, 0, 0};
    if (!mpuAvailable || !mpuEnabled) return filtered;

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

    // Calculate time elapsed since last reading
    unsigned long now = micros();
    float dt = (now - lastGyroRead) / 1000000.0;
    lastGyroRead = now;

    // Filter accelerometer data
    filtered.x = accelXKalman.update(a.acceleration.x);
    filtered.y = accelYKalman.update(a.acceleration.y);
    filtered.z = accelZKalman.update(a.acceleration.z);

    // Filter gyroscope data
    float gyroX = gyroXKalman.update(g.gyro.x);
    float gyroY = gyroYKalman.update(g.gyro.y);
    float gyroZ = gyroZKalman.update(g.gyro.z);

    // Calculate pitch and roll from accelerometer
    float pitch = atan2(-filtered.x, sqrt(filtered.y * filtered.y + filtered.z * filtered.z));
    float roll = atan2(filtered.y, filtered.z);

    // Convert to degrees
    filtered.pitch = pitch * 180.0 / M_PI;
    filtered.roll = roll * 180.0 / M_PI;

    /*
    // Calculate yaw using gyroscope with drift compensation
    static float yawAngle = 0.0;
    const float GYRO_THRESHOLD = 0.02; // Threshold untuk menghilangkan noise gyro
    
    if (abs(gyroZ) > GYRO_THRESHOLD) {
        yawAngle += gyroZ * dt;
        
        // Normalize yaw angle to -180 to +180 degrees
        while (yawAngle > 180) yawAngle -= 360;
        while (yawAngle < -180) yawAngle += 360;
    }
    
    filtered.yaw = yawAngle;
    */

    return filtered;
}


float calculateFilteredAngle(float ax, float ay, float az) {
    static float filteredX = 0;
    static float filteredY = 0;
    static float filteredZ = 0;
    
    // Complementary filter coefficient
    const float ALPHA = 0.96;
    
    // Calculate raw angles
    float roll = atan2(ay, az) * 180.0 / M_PI;
    float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
    float yaw = atan2(ax, ay) * 180.0 / M_PI;
    
    // Apply complementary filter
    filteredX = ALPHA * filteredX + (1.0 - ALPHA) * roll;
    filteredY = ALPHA * filteredY + (1.0 - ALPHA) * pitch;
    filteredZ = ALPHA * filteredZ + (1.0 - ALPHA) * yaw;
    
    return filteredX; // Return filtered roll by default
}
void resetKalmanFilters() {
    tempKalman.reset();
    pressureKalman.reset();
    altitudeKalman.reset();
    accelXKalman.reset();
    accelYKalman.reset();
    accelZKalman.reset();
}

void enableBMP280() {
    if (!bmpEnabled && bmpAvailable) {
        bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                       Adafruit_BMP280::SAMPLING_X2,
                       Adafruit_BMP280::SAMPLING_X16,
                       Adafruit_BMP280::FILTER_X4,
                       Adafruit_BMP280::STANDBY_MS_4000);
        bmpEnabled = true;
        Serial.println("BMP280 enabled");
    }
}

void disableBMP280() {
    if (bmpEnabled && bmpAvailable) {
        bmp.setSampling(Adafruit_BMP280::MODE_SLEEP,
                       Adafruit_BMP280::SAMPLING_NONE,
                       Adafruit_BMP280::SAMPLING_NONE,
                       Adafruit_BMP280::FILTER_OFF,
                       Adafruit_BMP280::STANDBY_MS_4000);
        bmpEnabled = false;
        Serial.println("BMP280 disabled");
    }
}

void enableMPU6050() {
    if (!mpuEnabled && mpuAvailable) {
        mpu.enableSleep(false);
        mpuEnabled = true;
        Serial.println("MPU6050 enabled");
    }
}

void disableMPU6050() {
    if (mpuEnabled && mpuAvailable) {
        mpu.enableSleep(true);
        mpuEnabled = false;
        Serial.println("MPU6050 disabled");
    }
}

void handleRoot() {
    File file = LittleFS.open("/index.html", "r");
    if (!file) {
        server.send(404, "text/plain", "File not found");
        return;
    }
    server.streamFile(file, "text/html");
    file.close();
}

void handleSetTime() {
    if (server.hasArg("plain")) {
        String json = server.arg("plain");
        StaticJsonDocument<200> doc;
        DeserializationError error = deserializeJson(doc, json);

        if (error) {
            server.send(400, "text/plain", "Invalid JSON");
            return;
        }

        int year = doc["year"];
        int month = doc["month"];
        int day = doc["day"];
        int hour = doc["hour"];
        int minute = doc["minute"];
        int second = doc["second"];

        setTime(hour, minute, second, day, month, year);
        
        server.send(200, "text/plain", "Time set successfully");
    } else {
        server.send(400, "text/plain", "No data received");
    }
}

void setupWebServer() {
    WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));
    dnsServer.start(DNS_PORT, "*", apIP);
    
    server.onNotFound([]() {
        server.sendHeader("Location", String("http://") + apIP.toString(), true);
        server.send(302, "text/plain", "");
    });

    server.on("/", HTTP_GET, handleRoot);
    server.on("/settime", HTTP_POST, handleSetTime);
    server.begin();
    Serial.println("HTTP server started");
}

uint16_t safeColor565(int r, int g, int b) {
    r = constrain(r, 0, 255);
    g = constrain(g, 0, 255);
    b = constrain(b, 0, 255);
    uint8_t r5 = round(r * 31.0 / 255.0);
    uint8_t g6 = round(g * 63.0 / 255.0);
    uint8_t b5 = round(b * 31.0 / 255.0);
    return (r5 << 11) | (g6 << 5) | b5;
}

void setBackgroundColor(int r, int g, int b) {
    backgroundColor = safeColor565(r, g, b);
}

void clearScreen() {
    tft.fillScreen(backgroundColor);
    drawMargins();
}

void drawMargins() {
    // Gambar margin kiri dan kanan dengan warna hitam
    tft.fillRect(0, 0, CONTENT_MARGIN, tft.height(), safeColor565(0, 0, 0));
    tft.fillRect(tft.width() - CONTENT_MARGIN, 0, CONTENT_MARGIN, tft.height(), safeColor565(0, 0, 0));
}

void drawBorder() {
    drawMargins();
    int borderStartX = (tft.width() - BORDER_WIDTH) / 2;
    int borderStartY = (tft.height() - BORDER_HEIGHT) / 2;
    tft.drawFastHLine(borderStartX, borderStartY, BORDER_WIDTH, safeColor565(0, 0, 0));
    tft.drawFastHLine(borderStartX, borderStartY + BORDER_HEIGHT, BORDER_WIDTH, safeColor565(0, 0, 0));
    tft.drawFastVLine(borderStartX, borderStartY, BORDER_HEIGHT, safeColor565(0, 0, 0));
    tft.drawFastVLine(borderStartX + BORDER_WIDTH, borderStartY, BORDER_HEIGHT, safeColor565(0, 0, 0));
}

void clearBorderArea() {
    // Hitung posisi battery icon
    const int battX = BORDER_START_X + BORDER_WIDTH - BATT_IMG_WIDTH - CONTENT_MARGIN;
    const int battY = BORDER_START_Y + CONTENT_MARGIN;
    
    // Clear area diatas battery (jika ada)
    tft.fillRect(BORDER_START_X + 1, BORDER_START_Y + 1, 
                 BORDER_WIDTH - 2, battY - BORDER_START_Y - 1, 
                 backgroundColor);
    
    // Clear area kiri battery
    tft.fillRect(BORDER_START_X + 1, battY, 
                 battX - BORDER_START_X - 1, BATT_IMG_HEIGHT, 
                 backgroundColor);
    
    // Clear area kanan battery
    tft.fillRect(battX + BATT_IMG_WIDTH + 1, battY,
                 BORDER_START_X + BORDER_WIDTH - (battX + BATT_IMG_WIDTH) - 1, 
                 BATT_IMG_HEIGHT,
                 backgroundColor);
    
    // Clear area dibawah battery
    tft.fillRect(BORDER_START_X + 1, battY + BATT_IMG_HEIGHT,
                 BORDER_WIDTH - 2, 
                 (BORDER_START_Y + BORDER_HEIGHT - 1) - (battY + BATT_IMG_HEIGHT),
                 backgroundColor);
}

int adjustContentY(int y) {
    return BORDER_START_Y + y;  // Sekarang relatif terhadap border
}

uint32_t read32(File &f) {
    uint32_t result;
    ((uint8_t *)&result)[0] = f.read();
    ((uint8_t *)&result)[1] = f.read();
    ((uint8_t *)&result)[2] = f.read();
    ((uint8_t *)&result)[3] = f.read();
    return result;
}

void drawBMP(const char *filename, int16_t x, int16_t y, int16_t targetWidth, int16_t targetHeight) {
    File bmpFile = LittleFS.open(filename, "r");
    if (!bmpFile) {
        Serial.println("Failed to open BMP file");
        return;
    }

    // Read BMP header
    bmpFile.seek(0x0A);
    uint32_t imageOffset = read32(bmpFile);
    bmpFile.seek(0x12);
    uint32_t imageWidth = read32(bmpFile);
    uint32_t imageHeight = read32(bmpFile);

    // Calculate scaling while preserving aspect ratio
    float scaleX = (float)targetWidth / imageWidth;
    float scaleY = (float)targetHeight / imageHeight;
    float scale = min(scaleX, scaleY);  // Use the smaller scaling factor
    
    int16_t actualWidth = round(imageWidth * scale);
    int16_t actualHeight = round(imageHeight * scale);
    
    // Center the image within the target area
    x += (targetWidth - actualWidth) / 2;
    y += (targetHeight - actualHeight) / 2;

    // Check if this is a small icon (32x32 or smaller)
    bool isSmallIcon = (targetWidth <= 32 && targetHeight <= 32);

    if (isSmallIcon) {
        // Pre-calculate scaling ratios
        float scaleX = (float)imageWidth / actualWidth;
        float scaleY = (float)imageHeight / actualHeight;
        
        // Create temporary buffer for a single line of pixels
        uint8_t* lineBuffer = new uint8_t[imageWidth * 4];
        
        for (int16_t targetY = 0; targetY < actualHeight; targetY++) {
            // Calculate source Y position
            float srcY = (actualHeight - 1 - targetY) * scaleY;
            int srcYInt = (int)srcY;
            
            // Read the full line of source pixels
            bmpFile.seek(imageOffset + (srcYInt * imageWidth * 4));
            bmpFile.read(lineBuffer, imageWidth * 4);
            
            for (int16_t targetX = 0; targetX < actualWidth; targetX++) {
                // Calculate source X position
                float srcX = targetX * scaleX;
                int srcXInt = (int)srcX;
                
                // Get color values from buffer
                uint8_t b = lineBuffer[srcXInt * 4];
                uint8_t g = lineBuffer[srcXInt * 4 + 1];
                uint8_t r = lineBuffer[srcXInt * 4 + 2];
                uint8_t a = lineBuffer[srcXInt * 4 + 3];

                if (a > 127) {  // Only draw if pixel is visible
                    // For small icons, enhance contrast and sharpness
                    r = enhancePixel(r);
                    g = enhancePixel(g);
                    b = enhancePixel(b);

                    uint16_t color = safeColor565(r, g, b);
                    
                    // Check boundaries before drawing
                    if (x + targetX >= BORDER_START_X && 
                        x + targetX < BORDER_START_X + BORDER_WIDTH &&
                        y + targetY >= BORDER_START_Y && 
                        y + targetY < BORDER_START_Y + BORDER_HEIGHT) {
                        
                        tft.drawPixel(x + targetX, y + targetY, color);
                        
                        // Apply edge smoothing
                        if (targetX > 0 && targetY > 0 && 
                            targetX < actualWidth-1 && targetY < actualHeight-1) {
                            if (isEdge(lineBuffer, srcXInt, imageWidth)) {
                                smoothEdge(tft, x + targetX, y + targetY, color);
                            }
                        }
                    }
                }
            }
        }
        
        delete[] lineBuffer;
    } else {
        // Original scaling method for larger images
        for (int16_t row = 0; row < actualHeight; row++) {
            int16_t sourceRow = imageHeight - 1 - (int16_t)((float)row * imageHeight / actualHeight);
            bmpFile.seek(imageOffset + (sourceRow * imageWidth * 4));

            for (int16_t col = 0; col < actualWidth; col++) {
                int16_t sourceCol = (int16_t)((float)col * imageWidth / actualWidth);
                bmpFile.seek(imageOffset + (sourceRow * imageWidth * 4) + (sourceCol * 4));

                uint8_t b = bmpFile.read();
                uint8_t g = bmpFile.read();
                uint8_t r = bmpFile.read();
                uint8_t a = bmpFile.read();

                if (a > 127) {
                    // Check boundaries before drawing
                    if (x + col >= BORDER_START_X && 
                        x + col < BORDER_START_X + BORDER_WIDTH &&
                        y + row >= BORDER_START_Y && 
                        y + row < BORDER_START_Y + BORDER_HEIGHT) {
                        
                        tft.drawPixel(x + col, y + row, safeColor565(r, g, b));
                    }
                }
            }
        }
    }
    
    bmpFile.close();
}

uint8_t enhancePixel(uint8_t value) {
    // Increase contrast for small icons
    const float contrast = 1.2;  // Contrast enhancement factor
    const int brightness = 10;   // Brightness adjustment
    
    // Apply contrast
    float adjusted = ((value / 255.0f - 0.5f) * contrast + 0.5f) * 255.0f;
    
    // Apply brightness
    adjusted += brightness;
    
    // Ensure value stays in valid range
    return (uint8_t)constrain(adjusted, 0, 255);
}

// Helper function to detect edges in the image
bool isEdge(uint8_t* buffer, int pos, int width) {
    // Check if current pixel is significantly different from neighbors
    uint8_t current = buffer[pos * 4 + 3];  // Alpha channel
    uint8_t left = (pos > 0) ? buffer[(pos-1) * 4 + 3] : 0;
    uint8_t right = (pos < width-1) ? buffer[(pos+1) * 4 + 3] : 0;
    
    return (abs(current - left) > 127 || abs(current - right) > 127);
}

// Helper function to smooth edges
void smoothEdge(Adafruit_ST7735& tft, int16_t x, int16_t y, uint16_t color) {
    // Get RGB components
    uint8_t r = (color >> 11) << 3;
    uint8_t g = ((color >> 5) & 0x3F) << 2;
    uint8_t b = (color & 0x1F) << 3;
    
    // Create slightly darker version for edge smoothing
    uint16_t edgeColor = safeColor565(
        r * 0.8,
        g * 0.8,
        b * 0.8
    );
    
    // Apply edge smoothing only if needed
    if ((x + y) % 2 == 0) {
        tft.drawPixel(x, y, edgeColor);
    }
}

void drawMenuIcon(const char* iconPath, int y) {
    int xCenter = BORDER_START_X + (BORDER_WIDTH - ICON_WIDTH) / 2;
    y = BORDER_START_Y + y;  // Hapus referensi ke MARGIN_TOP
    
    if (LittleFS.exists(iconPath)) {
        drawBMP(iconPath, xCenter, y, ICON_WIDTH, ICON_HEIGHT);
    } else {
        Serial.printf("Icon not found: %s\n", iconPath);
    }
}

float getBatteryVoltage() {
    if (!ina219Available) return 0.0;
    
    float busVoltage = ina219.getBusVoltage_V();
    float shuntVoltage = ina219.getShuntVoltage_mV() / 1000.0;
    float voltage = busVoltage + shuntVoltage;
    
    return (voltage < 2.0 || voltage > 5.0) ? 0.0 : voltage;
}

int getBatteryPercentage(float voltage) {
    float percentage = ((voltage - BATTERY_MIN) / (BATTERY_MAX - BATTERY_MIN)) * 100;
    return round(constrain(percentage, 0, 100));
}

void updateBatteryStatus() {
    float voltage = getBatteryVoltage();
    String currentVoltageStr = String(voltage, 2);
    
    if (currentVoltageStr != lastVoltageStr) {
        lastVoltageStr = currentVoltageStr;
        int percentage = getBatteryPercentage(voltage);
        
        if (percentage <= 3) {
            batteryStatus = "0%";
        } else if (percentage <= 20) {
            batteryStatus = "20%";
        } else if (percentage <= 50) {
            batteryStatus = "50%";
        } else {
            batteryStatus = "100%";
        }
        
        if (batteryStatus != prevBatteryStatus) {
            displayBatteryStatus();
        }
    }
}

void displayBatteryStatus() {
    // Adjust battery position relative to border
    const int battX = BORDER_START_X + BORDER_WIDTH - BATT_IMG_WIDTH - CONTENT_MARGIN;
    const int battY = BORDER_START_Y + CONTENT_MARGIN;
    
    if (batteryStatus != prevBatteryStatus) {
        tft.fillRect(battX, battY, BATT_IMG_WIDTH, BATT_IMG_HEIGHT, backgroundColor);
        
        const char* batteryImage = batteryStatus == "100%" ? "/battery_100.bmp" :
                          batteryStatus == "50%" ? "/battery_50.bmp" :
                          batteryStatus == "20%" ? "/battery_20.bmp" : "/battery_0.bmp";
        
        drawBMP(batteryImage, battX, battY, BATT_IMG_WIDTH, BATT_IMG_HEIGHT);
        prevBatteryStatus = batteryStatus;
    }
}

void drawText(const String& text, int x, int y, uint16_t textColor, int paddingX = 5) {
    int16_t x1, y1;
    uint16_t w, h;
    
    tft.getTextBounds(text, x, y, &x1, &y1, &w, &h);
    int actualX = x;
    tft.fillRect(actualX - paddingX, y1, w + (paddingX * 2), h, backgroundColor);
    tft.setTextColor(textColor);
    tft.setCursor(actualX, y);
    tft.print(text);
}

void drawCenteredText(const String& text, int y, uint16_t textColor, int paddingX = 5, int offsetX = 0) {
    int16_t x1, y1;
    uint16_t w, h;
    
    tft.getTextBounds(text, 0, 0, &x1, &y1, &w, &h);
    int availableWidth = BORDER_WIDTH - (CONTENT_MARGIN * 2);
    int x = BORDER_START_X + CONTENT_MARGIN + (availableWidth - w) / 2 + offsetX;
    y = BORDER_START_Y + y;  // Hapus referensi ke MARGIN_TOP
    
    drawText(text, x, y, textColor, paddingX);
}

void drawDottedLine(int y, int length, int dotSpacing, uint16_t color) {
    int borderStartX = (tft.width() - 80) / 2;
    int xStart = borderStartX + (80 - length) / 2;
    int xEnd = xStart + length;
    y = adjustContentY(y);
    
    for (int x = xStart; x <= xEnd; x += dotSpacing) {
        tft.drawPixel(x, y, color);
    }
}

void drawSmallIcon(const char* iconPath, int x, int y) {
    // Debug info
    Serial.printf("Drawing icon at x:%d y:%d with size %dx%d\n", 
                  x, y, SMALL_ICON_WIDTH, SMALL_ICON_HEIGHT);
    
    // Constrain position within border bounds
    x = constrain(x, BORDER_START_X + CONTENT_MARGIN, 
                 BORDER_START_X + BORDER_WIDTH - SMALL_ICON_WIDTH - CONTENT_MARGIN);
    y = constrain(y, BORDER_START_Y + CONTENT_MARGIN, 
                 BORDER_START_Y + BORDER_HEIGHT - SMALL_ICON_HEIGHT - CONTENT_MARGIN);
    
    if (LittleFS.exists(iconPath)) {
        drawBMP(iconPath, x, y, SMALL_ICON_WIDTH, SMALL_ICON_HEIGHT);
    } else {
        Serial.printf("Small icon not found: %s\n", iconPath);
    }
}

void clearTextArea(int x, int y, const String& text) {
    int16_t x1, y1;
    uint16_t w, h;
    
    tft.getTextBounds(text, x, y, &x1, &y1, &w, &h);
    tft.fillRect(x, y1, w + 2, h + 2, backgroundColor);
}

void updateTimeAndDate() {
    String hourStr = (hour() < 10 ? "0" : "") + String(hour());
    String minStr = (minute() < 10 ? "0" : "") + String(minute());
    timeStr = hourStr + ":" + minStr;

    String dayStr = (day() < 10 ? "0" : "") + String(day());
    String monthStr = (month() < 10 ? "0" : "") + String(month());
    String yearStr = String(year());
    dateStr = dayStr + " / " + monthStr + " / " + yearStr;
}

void displayTimeAndDate() {
    // Adjust base position relative to border
    int yBase = BORDER_START_Y + 70;
    int yStep = 10;
    
    // Display time with direct centering
    tft.setFont(&Open_24_Display_St17pt7b);
    if (timeStr != prevTimeStr) {
        int16_t x1, y1;
        uint16_t w, h;
        tft.getTextBounds(timeStr, 0, 0, &x1, &y1, &w, &h);
        
        // Calculate center position for time
        int x = BORDER_START_X + (BORDER_WIDTH - w) / 2;
        int y = yBase + yStep + 0;
        
        // Clear previous time area
        tft.fillRect(x + x1, y + y1, w, h, backgroundColor);
        
        // Draw centered time
        tft.setTextColor(safeColor565(255, 255, 255));
        tft.setCursor(x, y);
        tft.print(timeStr);
        
        prevTimeStr = timeStr;
    }
    
    // Display date with direct centering
    tft.setFont(&Swansea5pt7b);
    if (dateStr != prevDateStr) {
        int16_t x1, y1;
        uint16_t w, h;
        tft.getTextBounds(dateStr, 0, 0, &x1, &y1, &w, &h);
        
        // Calculate center position for date
        int x = BORDER_START_X + (BORDER_WIDTH - w) / 2;
        int y = yBase + yStep + 15;
        
        // Clear previous date area
        tft.fillRect(x + x1, y + y1, w, h, backgroundColor);
        
        // Draw centered date
        tft.setTextColor(safeColor565(221, 125, 64));
        tft.setCursor(x, y);
        tft.print(dateStr);
        
        prevDateStr = dateStr;
    }

    if (currentMenu == 1) {
        tft.setFont(&Swansea6pt7b);
        
        // Calculate proper icon positions
        int iconOffset = SMALL_ICON_HEIGHT / 2;
        
        int tempIconY = yBase + yStep + 30;
        int altIconY = yBase + yStep + 70;
        
        if (!iconsDrawnTimeMenu) {
            // Draw icons with adjusted positions and centering
            drawSmallIcon(ICON_TEMP, 
                        BORDER_START_X + CONTENT_MARGIN, 
                        tempIconY - iconOffset);
                        
            drawSmallIcon(ICON_ALTITUDE, 
                        BORDER_START_X + CONTENT_MARGIN, 
                        altIconY - iconOffset);
                        
            iconsDrawnTimeMenu = true;
        }

        // Update temperature value
        if (millis() - tempMillis >= tempInterval) {
            tempMillis = millis();
            
            if (bmpAvailable && bmpEnabled) {
                temperature = readFilteredTemperature();
                tempC = String(temperature, 1) + " *C";
            }
        }
            
        if (tempC != prevTempC) {
            int16_t x1, y1;
            uint16_t w, h;
            tft.getTextBounds(tempC, 0, 0, &x1, &y1, &w, &h);
            
            int textX = BORDER_START_X + BORDER_WIDTH - w - CONTENT_MARGIN;
            int textY = yBase + yStep + 38;
            
            tft.fillRect(textX, textY - h + y1, w, h, backgroundColor);
            tft.setTextColor(safeColor565(255, 255, 255));
            tft.setCursor(textX, textY);
            tft.print(tempC);
            prevTempC = tempC;
        }

        // Update altitude value
        if (millis() - altitudeMillis >= altitudeInterval) {
            altitudeMillis = millis();
            
            if (bmpAvailable && bmpEnabled) {
                altitude = bmp.readAltitude(1013.25);
                altitudeStr = String(altitude, 1) + " m";
            }
        }
            
        if (altitudeStr != prevAltitudeStr) {
            int16_t x1, y1;
            uint16_t w, h;
            tft.getTextBounds(altitudeStr, 0, 0, &x1, &y1, &w, &h);
            
            int textX = BORDER_START_X + BORDER_WIDTH - w - CONTENT_MARGIN;
            int textY = yBase + yStep + 63;
            
            tft.fillRect(textX, textY - h + y1, w, h, backgroundColor);
            tft.setTextColor(safeColor565(24, 218, 61));
            tft.setCursor(textX, textY);
            tft.print(altitudeStr);
            prevAltitudeStr = altitudeStr;
        }
    }
}

float calculateAngle(float ax, float ay, float az) {
    // Ensure we don't divide by zero
    if (ax == 0 && ay == 0 && az == 0) return 0;
    
    // Calculate angles using arctan2 for better quadrant handling
    // For X (Roll) - rotation around Y axis
    float roll = atan2(ay, az) * 180.0 / M_PI;
    
    // For Y (Pitch) - rotation around X axis
    float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
    
    // For Z (Yaw) - can't be accurately determined with just accelerometer
    // Would need magnetometer for true heading
    float yaw = atan2(ax, ay) * 180.0 / M_PI;
    
    return roll; // Return roll by default, modify based on which angle you're calculating
}

float calculateMovingAverage() {
    float sum = 0;
    for (int i = 0; i < WINDOW_SIZE; i++) {
        sum += accelWindow[i];
    }
    return sum / WINDOW_SIZE;
}

float calculateMovingVariance(float average) {
    float variance = 0;
    for (int i = 0; i < WINDOW_SIZE; i++) {
        variance += pow(accelWindow[i] - average, 2);
    }
    return variance / WINDOW_SIZE;
}

void IRAM_ATTR ISR_downButton() {
    static unsigned long lastInterruptTime = 0;
    unsigned long interruptTime = millis();
    
    if (digitalRead(downButton) == LOW) {
        if (interruptTime - lastInterruptTime > DEBOUNCE_TIME) {
            buttonPressStartTime = interruptTime;
            isButtonDown = true;
            longPressExecuted = false;
        }
    } else {
        if (isButtonDown) {
            unsigned long pressDuration = interruptTime - buttonPressStartTime;
            
            if (pressDuration >= LONG_PRESS_DURATION && currentMenu == 6) {
                stepCount = 0;
                prevStepCount = "";
                longPressExecuted = true;
            } else if (pressDuration < LONG_PRESS_DURATION && !longPressExecuted) {
                buttonPressed = true;
            }
            isButtonDown = false;
        }
    }
    lastInterruptTime = interruptTime;
}

void setup() {
    Serial.begin(115200);
    Serial.println("Booting...");
    
    ina219Available = ina219.begin();
    if (!ina219Available) Serial.println("INA219 not found! Battery monitoring disabled");
    
    if (!LittleFS.begin()) {
        Serial.println("LittleFS initialization failed!");
        return;
    }

    Dir dir = LittleFS.openDir("/");
    Serial.println("Files in LittleFS:");
    while (dir.next()) {
        Serial.printf(" - %s (%d bytes)\n", dir.fileName().c_str(), dir.fileSize());
    }

    pinMode(downButton, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(downButton), ISR_downButton, CHANGE);

    tft.initR(INITR_GREENTAB);
    tft.setSPISpeed(40000000);
    delay(10);
    tft.invertDisplay(isInverted);
    setBackgroundColor(0, 0, 0);
    clearScreen();

    bmpAvailable = bmp.begin(0x76);
    if (!bmpAvailable) Serial.println("BMP280 not found! Values set to 0");

    mpuAvailable = mpu.begin();
    if (!mpuAvailable) Serial.println("MPU6050 not found! Values set to 0");

    if (bmpAvailable) {
        disableBMP280();
    }

    if (mpuAvailable) {
        disableMPU6050();
    }

    setTime(0, 0, 0, 1, 1, 2000);
    lastButtonPress = millis();
    
    drawBorder();
    updateBatteryStatus();
    displayBatteryStatus();
}

void loop() {
    updateBatteryStatus();
    
    if (wifiInitialized) {
        dnsServer.processNextRequest();
        server.handleClient();
    }
    
    if (autoReturnEnabled && (millis() - lastButtonPress >= AUTO_RETURN_TIMEOUT)) {
        currentMenu = 1;
        clearBorderArea();
        prevTimeStr = "";
        prevDateStr = "";
        prevTempC = "";
        prevTempF = "";
        prevPressureStr = "";
        prevAltitudeStr = "";
        prevAccX = "";
        prevAccY = "";
        prevAccZ = "";
        prevSettingsText = "";
        drawBorder();
        lastButtonPress = millis();
        
        disableMPU6050();
        enableBMP280();  // Menu 1 needs BMP280 for temperature display
    }

    if (buttonPressed) {
        buttonPressed = false;
        lastButtonPress = millis();
        resetKalmanFilters();
        
        int nextMenu;
        if (currentMenu == 1) {
            nextMenu = 2;  // Dari homescreen ke menu 2
        } else {
            nextMenu = (currentMenu % 7) + 1;
            if (nextMenu == 1) {  // Skip menu 1 dalam rotasi
                nextMenu = 2;
            }
        }
        
        if (nextMenu == 6 && currentMenu != 6) {
            stepCount = 0;
            prevStepCount = "";
            isFirstRun = true;
        }
        
        disableBMP280();
        disableMPU6050();
        
        switch (nextMenu) {
            case 1:
                enableBMP280();
                break;
            case 2:
                enableBMP280();
                break;
            case 3:
                enableBMP280();
                break;
            case 4:
                enableBMP280();
                break;
            case 5:
                enableMPU6050();
                break;
            case 6:
                enableMPU6050();
                break;
        }
        
        currentMenu = nextMenu;
        clearBorderArea();

        prevTimeStr = "";
        prevDateStr = "";
        prevTempC = "";
        prevTempF = "";
        prevPressureStr = "";
        prevAltitudeStr = "";
        prevAccX = "";
        prevAccY = "";
        prevAccZ = "";
        prevSettingsText = "";
        iconsDrawnTimeMenu = false;
        
        if (currentMenu == 2) {
            tempMillis = 0;
        }

        if (currentMenu == 1) {
            tempMillis = 0;
            altitudeMillis = 0;
        }
        
        if (currentMenu != 6 && wifiInitialized) {
            WiFi.softAPdisconnect(true);
            server.close();
            wifiInitialized = false;
            Serial.println("WiFi AP stopped");
        }
        
        drawBorder();
    }

    updateTimeAndDate();

    switch (currentMenu) {
        case 1: 
            autoReturnEnabled = false;
            displayTimeAndDate();
            break;

       case 2: {
            autoReturnEnabled = true;
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_TEMP, CONTENT_MARGIN + 20);
            
            if (millis() - tempMillis >= tempInterval) {
                tempMillis = millis();
                
                if (bmpAvailable && bmpEnabled) {
                    temperature = readFilteredTemperature();
                    tempC = String(temperature, 1) + " *C";
                    tempF = String((temperature * 9/5) + 32, 1) + " *F";
                }
            }
                
            int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
            int yStep = 30;
            
            if (tempC != prevTempC) {
                drawCenteredText(tempC, yBase + yStep + 0, safeColor565(255, 255, 255));
                prevTempC = tempC;
            }
            
            drawDottedLine(yBase + yStep + 7, 55, 4, safeColor565(255, 255, 255));
            
            if (tempF != prevTempF) {
                drawCenteredText(tempF, yBase + yStep + 22, safeColor565(221, 125, 64));
                prevTempF = tempF;
            }
        }
        break;

        case 3: {
            autoReturnEnabled = true;
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_PRESSURE, CONTENT_MARGIN + 20);
            
            if (millis() - pressureMillis >= pressureInterval) {
                pressureMillis = millis();
                
                if (bmpAvailable && bmpEnabled) {
                    pressure = readFilteredPressure();
                    lastPressure = pressure;
                    pressureStr = String(pressure, 1) + " hPa";
                }
            }
                
            int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
            int yStep = 30;
            
            if (pressureStr != prevPressureStr) {
                drawCenteredText(pressureStr, yBase + yStep + 0, safeColor565(255, 100, 100));
                prevPressureStr = pressureStr;
            }
        }
        break;

        case 4: {
            autoReturnEnabled = true;
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_ALTITUDE, CONTENT_MARGIN + 20);
            
            if (millis() - altitudeMillis >= altitudeInterval) {
                altitudeMillis = millis();
                
                if (bmpAvailable && bmpEnabled) {
                    altitude = readFilteredAltitude();
                    altitudeStr = String(altitude, 1) + " m";
                }
            }
                
            int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
            int yStep = 30;
            
            if (altitudeStr != prevAltitudeStr) {
                drawCenteredText(altitudeStr, yBase + yStep + 0, safeColor565(24, 218, 61));
                prevAltitudeStr = altitudeStr;
            }
        }
        break;

        case 5: {
            autoReturnEnabled = true;
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_ANGLE, CONTENT_MARGIN + 20);
            
            if (millis() - sensorMillis >= sensorInterval) {
                sensorMillis = millis();
                
                if (mpuAvailable && mpuEnabled) {
                    FilteredMPUData filtered = readFilteredAcceleration();
                    
                    // Update display strings with angular data
                    accX = "X: " + String(filtered.roll, 1) + "*";   // Left-Right tilt
                    accY = "Y: " + String(filtered.pitch, 1) + "*";  // Forward-Backward tilt
                    /*
                      accZ = "Z: " + String(filtered.yaw, 1) + "*";    // Rotation
                    */
                }
            }
                
            int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
            int yStep = 30;
            
            if (accX != prevAccX) {
                drawCenteredText(accX, yBase + yStep + 0, safeColor565(221, 67, 196));
                prevAccX = accX;
            }
            if (accY != prevAccY) {
                drawCenteredText(accY, yBase + yStep + 15, safeColor565(101, 111, 188));
                prevAccY = accY;
            }
            /*
              if (accZ != prevAccZ) {
                  drawCenteredText(accZ, yBase + yStep + 30, safeColor565(249, 218, 188));
                  prevAccZ = accZ;
              }
            */
          }
          break;

        case 6: {
            autoReturnEnabled = false;
            static const unsigned long MAX_STEPS = 99999999;
            static KalmanFilter accelMagKalman(0.01, 0.1); // Process noise, measurement noise
            static bool isKalmanInitialized = false;
            
            if (isButtonDown && (millis() - buttonPressStartTime) >= LONG_PRESS_DURATION && currentMenu == 6) {
                stepCount = 0;
                prevStepCount = "";
                isButtonDown = false;
                buttonPressed = false;
                isKalmanInitialized = false;
                accelMagKalman.reset();
            }
            
            if (isFirstRun) {
                for (int i = 0; i < WINDOW_SIZE; i++) {
                    accelWindow[i] = 0;
                }
                isFirstRun = false;
                isKalmanInitialized = false;
                accelMagKalman.reset();
            }
            
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_STEP, CONTENT_MARGIN + 20);
            
            if (mpuAvailable && mpuEnabled) {
                sensors_event_t a, g, temp;
                mpu.getEvent(&a, &g, &temp);
                
                // Calculate acceleration magnitude with gravity compensation
                float rawMagnitude = sqrt(
                    a.acceleration.x * a.acceleration.x + 
                    a.acceleration.y * a.acceleration.y + 
                    (a.acceleration.z - 9.81) * (a.acceleration.z - 9.81)
                );
                
                // Apply Kalman filter to smooth the magnitude
                float filteredMagnitude = accelMagKalman.update(rawMagnitude);
                
                // Noise threshold with hysteresis
                const float NOISE_THRESHOLD_HIGH = 0.9;
                const float NOISE_THRESHOLD_LOW = 0.7;
                static bool isAboveNoise = false;
                
                if (filteredMagnitude > NOISE_THRESHOLD_HIGH) {
                    isAboveNoise = true;
                } else if (filteredMagnitude < NOISE_THRESHOLD_LOW) {
                    isAboveNoise = false;
                }
                
                if (isAboveNoise) {
                    accelWindow[windowIndex] = filteredMagnitude;
                } else {
                    accelWindow[windowIndex] = 0;
                }
                windowIndex = (windowIndex + 1) % WINDOW_SIZE;
                
                float movingAvg = calculateMovingAverage();
                float movingVar = calculateMovingVariance(movingAvg);
                
                static bool isPeak = false;
                static float peakValue = 0;
                static float valleyValue = 0;

                // Peak detection with enhanced conditions
                if (filteredMagnitude > movingAvg && !isPeak && 
                    movingVar > VARIANCE_THRESHOLD && 
                    (millis() - lastStepTime) > MIN_STEP_INTERVAL &&
                    isAboveNoise) {
                    
                    isPeak = true;
                    peakValue = filteredMagnitude;
                } 
                else if (filteredMagnitude < movingAvg && isPeak) {
                    valleyValue = filteredMagnitude;
                    
                    float stepMagnitude = peakValue - valleyValue;
                    
                    // Enhanced step validation
                    if (stepMagnitude > STEP_MAGNITUDE_THRESHOLD && 
                        peakValue > PEAK_THRESHOLD) {
                        
                        // Additional validation using variance
                        if (movingVar > VARIANCE_THRESHOLD * 1.2) {
                            stepCount++;
                            if (stepCount >= MAX_STEPS) {
                                stepCount = 0;
                            }
                            lastStepTime = millis();
                        }
                    }
                    isPeak = false;
                }
            }
            
            String currentStepCount = String(stepCount);
            int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
            int yStep = 30;
            
            if (currentStepCount != prevStepCount) {
                drawCenteredText(currentStepCount, yBase + yStep + 0, safeColor565(255, 255, 255));
                prevStepCount = currentStepCount;
            }
        }
        break;

        case 7: {
            autoReturnEnabled = false;
            if (!wifiInitialized) {
                WiFi.mode(WIFI_AP);
                WiFi.softAP(ssid, password);
                setupWebServer();
                wifiInitialized = true;
                Serial.println("Access Point Started");
                Serial.print("IP Address: ");
                Serial.println(WiFi.softAPIP());
            }
            
            tft.setFont(&Swansea6pt7b);
            drawMenuIcon(ICON_SETTINGS, CONTENT_MARGIN + 25);
            
            if (settingsText != prevSettingsText) {
                int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
                int yStep = 30;
                drawCenteredText(settingsText, yBase + yStep + 0, safeColor565(255, 255, 255));
                prevSettingsText = settingsText;
            }
        }
        break;
    }
}