#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <algorithm>  // Add this for std::min

// Define GPIO pin
#define GPIO_PIN 13

// Initialize U8G2 display - rotation will be set in setup
U8G2_SH1106_128X64_NONAME_F_HW_I2C display(U8G2_R1);  // Default rotation



// Animation parameters
const uint8_t SAND_PARTICLES = 25;
const uint8_t ANIMATION_DELAY = 50;
const unsigned long HOURGLASS_DURATION = 60000; // 1 minute
const uint8_t NUM_FALLING_PARTICLES = 8;
const uint8_t PARTICLE_SPEED_MIN = 1;
const uint8_t PARTICLE_SPEED_MAX = 2;

// Hourglass dimensions
const uint8_t GLASS_WIDTH = 50;
const uint8_t GLASS_HEIGHT = 100;
const uint8_t GLASS_X = (64 - GLASS_WIDTH) / 2;
const uint8_t GLASS_Y = 14;
const uint8_t WALL_THICKNESS = 2;
const uint8_t TOP_THICKNESS = 5;
const uint8_t BASE_PROTRUSION = 2;
const uint8_t NECK_WIDTH = 2;
const uint8_t NECK_TOTAL = NECK_WIDTH + (WALL_THICKNESS * 2);
const uint8_t CURVE_STEPS = 15;
const uint8_t TOP_FILL_PERCENT = 60;
const uint8_t BOTTOM_FILL_PERCENT = 50;
const uint8_t DOME_MAX_HEIGHT = 15;  // Maximum height of the initial dome
const uint8_t SPREAD_THRESHOLD = 8;  // Height at which sand starts to spread more
const float DOME_CURVE_FACTOR = 0.7; // Controls dome roundness (0.5-1.0)

uint32_t topPixelCount = 0;    // Using uint32_t for larger numbers
uint32_t bottomPixelCount = 0;  // Using uint32_t for larger numbers


int calculateDomeHeight(int distanceFromCenter, int maxHeight) {
    float normalizedDist = (float)distanceFromCenter / (GLASS_WIDTH / 2);
    return maxHeight * (1 - pow(normalizedDist, DOME_CURVE_FACTOR));
}

// Structures for particles
struct Particle {
    int8_t x;
    int8_t y;
    int8_t velocity;
    bool active;
};

struct FallingParticle {
    int8_t x;
    int8_t y;
    int8_t speed;
    bool active;
};

// Global variables
Particle particles[SAND_PARTICLES];
FallingParticle fallingParticles[NUM_FALLING_PARTICLES];
unsigned long startTime;
bool isRunning = true;
uint8_t topFillPercent = TOP_FILL_PERCENT;
uint8_t bottomFillPercent = 0;
int16_t leftBoundary[GLASS_HEIGHT];
int16_t rightBoundary[GLASS_HEIGHT];

// Function declarations
void calculateBoundaries();
void initializeFallingParticles();
void bezierPoint(float t, int x0, int y0, int x1, int y1, int x2, int y2, float &outX, float &outY);
void drawTopBase(bool isTop);
void drawHourglass();
void updateFallingParticles();
void drawFallingParticles();
void updateSandLevels();
void drawTopSand();
void drawBottomSand();
void drawSand();

void checkGPIOAndRotation() {
    static bool lastPinState = HIGH;
    bool currentPinState = digitalRead(GPIO_PIN);
    
    if (currentPinState != lastPinState) {
        // Pin state changed
        display.setDisplayRotation(currentPinState ? U8G2_R3 : U8G2_R1);
        
        // Restart animation
        startTime = millis();
        topFillPercent = TOP_FILL_PERCENT;
        bottomFillPercent = 0;
        initializeFallingParticles();
        
        lastPinState = currentPinState;
    }
}

// Bezier curve calculation function
void bezierPoint(float t, int x0, int y0, int x1, int y1, int x2, int y2, float &outX, float &outY) {
    float mt = 1 - t;
    outX = mt * mt * x0 + 2 * mt * t * x1 + t * t * x2;
    outY = mt * mt * y0 + 2 * mt * t * y1 + t * t * y2;
}

// Calculate the boundaries of the hourglass
void calculateBoundaries() {
    // ... (No changes in this function)
    int middleY = GLASS_Y + GLASS_HEIGHT / 2;
    for (int y = 0; y < GLASS_HEIGHT; y++) {
        float t;
        float xL, yL, xR, yR;

        if (y < GLASS_HEIGHT / 2) { // Top half
            t = (float)(y) / (GLASS_HEIGHT / 2);
            bezierPoint(t,
                        GLASS_X + WALL_THICKNESS - BASE_PROTRUSION, GLASS_Y,
                        GLASS_X + WALL_THICKNESS, GLASS_Y + GLASS_HEIGHT / 3,
                        GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + WALL_THICKNESS, middleY,
                        xL, yL);
            bezierPoint(t,
                        GLASS_X + GLASS_WIDTH - WALL_THICKNESS + BASE_PROTRUSION, GLASS_Y,
                        GLASS_X + GLASS_WIDTH - WALL_THICKNESS, GLASS_Y + GLASS_HEIGHT / 3,
                        GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - WALL_THICKNESS, middleY,
                        xR, yR);
        } else { // Bottom half
            t = (float)(y - GLASS_HEIGHT / 2) / (GLASS_HEIGHT / 2);
            bezierPoint(t,
                        GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + WALL_THICKNESS, middleY,
                        GLASS_X + WALL_THICKNESS, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3,
                        GLASS_X + WALL_THICKNESS - BASE_PROTRUSION, GLASS_Y + GLASS_HEIGHT,
                        xL, yL);
            bezierPoint(t,
                        GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - WALL_THICKNESS, middleY,
                        GLASS_X + GLASS_WIDTH - WALL_THICKNESS, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3,
                        GLASS_X + GLASS_WIDTH - WALL_THICKNESS + BASE_PROTRUSION, GLASS_Y + GLASS_HEIGHT,
                        xR, yR);
        }

        leftBoundary[y] = round(xL) + 1;
        rightBoundary[y] = round(xR) - 1;
    }
}

// Draw top or bottom base of the hourglass
void drawTopBase(bool isTop) {
    // ... (No changes in this function)
    int yPos = isTop ? GLASS_Y : GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS;
    int xExtension = 6;  // Amount to extend beyond glass width on EACH side

    // Original glass edges
    int glassStartX = GLASS_X;
    int glassEndX = GLASS_X + GLASS_WIDTH;

    // Base edges (extending beyond glass)
    int baseStartX = glassStartX - xExtension;
    int baseEndX = glassEndX + xExtension;

    // Draw main rectangle without corners
    for (int x = baseStartX + 2; x <= baseEndX - 2; x++) {
        display.drawPixel(x, yPos);    // Top line
        display.drawPixel(x, yPos + TOP_THICKNESS - 1);    // Bottom line
    }

    // Draw vertical sides without top and bottom pixels
    for (int y = yPos + 1; y < yPos + TOP_THICKNESS - 1; y++) {
        display.drawPixel(baseStartX, y);    // Left side
        display.drawPixel(baseEndX, y);      // Right side
    }

    // Draw rounded corners
    // Top-left corner
    display.drawPixel(baseStartX + 1, yPos);
    display.drawPixel(baseStartX + 1, yPos + 1);
    display.drawPixel(baseStartX, yPos + 1);

    // Top-right corner
    display.drawPixel(baseEndX - 1, yPos);
    display.drawPixel(baseEndX - 1, yPos + 1);
    display.drawPixel(baseEndX, yPos + 1);

    // Bottom-left corner
    display.drawPixel(baseStartX + 1, yPos + TOP_THICKNESS - 1);
    display.drawPixel(baseStartX + 1, yPos + TOP_THICKNESS - 2);
    display.drawPixel(baseStartX, yPos + TOP_THICKNESS - 2);

    // Bottom-right corner
    display.drawPixel(baseEndX - 1, yPos + TOP_THICKNESS - 1);
    display.drawPixel(baseEndX - 1, yPos + TOP_THICKNESS - 2);
    display.drawPixel(baseEndX, yPos + TOP_THICKNESS - 2);

    // Fill the base
    for (int x = baseStartX + 1; x < baseEndX; x++) {
        for (int y = yPos + 1; y < yPos + TOP_THICKNESS - 1; y++) {
            // display.drawPixel(x, y);
        }
    }
}
// Initialize falling particles
void initializeFallingParticles() {
    for (uint8_t i = 0; i < NUM_FALLING_PARTICLES; i++) {
        fallingParticles[i].active = false;
        fallingParticles[i].x = 0;
        fallingParticles[i].y = 0;
        fallingParticles[i].speed = 0;
    }
}

void updateFallingParticles() {
    int middleY = GLASS_Y + GLASS_HEIGHT / 2;
    int neckLeft = GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + WALL_THICKNESS + 1;
    int neckWidth = NECK_WIDTH - 2;
    int bottomLimit = GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS - (bottomFillPercent * GLASS_HEIGHT / 200);

    // Activate new particles
    for (uint8_t i = 0; i < NUM_FALLING_PARTICLES; i++) {
        if (!fallingParticles[i].active && random(100) < 30 && topFillPercent > 0) {
            fallingParticles[i].active = true;
            fallingParticles[i].x = neckLeft + random(neckWidth);
            fallingParticles[i].y = middleY;
            fallingParticles[i].speed = random(PARTICLE_SPEED_MIN, PARTICLE_SPEED_MAX + 1);
        }
    }

    // Update active particles
    for (uint8_t i = 0; i < NUM_FALLING_PARTICLES; i++) {
        if (fallingParticles[i].active) {
            fallingParticles[i].y += fallingParticles[i].speed;

            // Reduced horizontal movement chance
            if (random(100) < 15) { // Reduced to 15%
                fallingParticles[i].x += random(-1, 2);
                // Keep within boundaries
                int currentY = fallingParticles[i].y - GLASS_Y;
                if (currentY >= 0 && currentY < GLASS_HEIGHT) {
                    fallingParticles[i].x = constrain(fallingParticles[i].x,
                                                      leftBoundary[currentY],
                                                      rightBoundary[currentY]);
                }
            }

            // Deactivate if reached bottom fill level
            if (fallingParticles[i].y >= bottomLimit) {
                fallingParticles[i].active = false;
            }
        }
    }
}

// Draw the falling particles
void drawFallingParticles() {
    for (uint8_t i = 0; i < NUM_FALLING_PARTICLES; i++) {
        if (fallingParticles[i].active) {
            display.drawPixel(fallingParticles[i].x, fallingParticles[i].y);
        }
    }
}

// Update sand levels based on time
void updateSandLevels() {
    unsigned long elapsedTime = millis() - startTime;
    float progress = (float)elapsedTime / HOURGLASS_DURATION;
    
    // Enhanced non-linear function for more realistic hourglass behavior
    float topProgressFactor;
    if (progress <= 1.0) {
        // This formula creates three distinct phases:
        // 1. Slow initial drop (wide part)
        // 2. Accelerating middle section (curved part)
        // 3. Fast final drop (neck part)
        float x = progress;
        // Cubic function with adjustable parameters
        topProgressFactor = 0.3 * pow(x, 3) + 0.7 * x;
        
        // Add small random variations for more natural look
        float randomFactor = 1.0 + (random(-10, 11) / 1000.0); // ±1% variation
        topProgressFactor *= randomFactor;
    } else {
        topProgressFactor = 1.0;
    }
    
    // Calculate new fill percentages
    topFillPercent = TOP_FILL_PERCENT * (1.0 - topProgressFactor);
    
    // Bottom chamber fills proportionally to top chamber's emptying
    bottomFillPercent = BOTTOM_FILL_PERCENT * topProgressFactor;
    
    // Constrain values
    topFillPercent = constrain(topFillPercent, 0, TOP_FILL_PERCENT);
    bottomFillPercent = constrain(bottomFillPercent, 0, BOTTOM_FILL_PERCENT);
}

// Draw the sand in both chambers
// Function to draw sand in top chamber
void drawTopSand() {
    int middleY = GLASS_Y + GLASS_HEIGHT / 2;
    
    if (topFillPercent > 0) {
        int topHeight = (GLASS_HEIGHT / 2 - TOP_THICKNESS) * topFillPercent / 100;
        int sandTop = middleY - topHeight;
        
        for (int y = middleY - 1; y >= sandTop; y--) {
            if (y >= GLASS_Y + TOP_THICKNESS) {
                int leftX = leftBoundary[y - GLASS_Y];
                int rightX = rightBoundary[y - GLASS_Y];

                if (y == sandTop) {
                    // Slightly uneven surface at the top
                    for (int x = leftX; x <= rightX; x++) {
                        if (random(100) < 90) {
                            display.drawPixel(x, y);
                            topPixelCount++;
                        }
                    }
                } else {
                    // Fill complete rows
                    for (int x = leftX; x <= rightX; x++) {
                        display.drawPixel(x, y);
                        topPixelCount++;
                    }
                }
            }
        }
    }
}

// Function to draw sand in bottom chamber
void drawBottomSand() {
    int middleY = GLASS_Y + GLASS_HEIGHT / 2;
    
    if (bottomFillPercent > 0) {
        int sandBottom = GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS;
        int maxFillHeight = (GLASS_HEIGHT / 2 - TOP_THICKNESS) * bottomFillPercent / 100;
        int centerX = GLASS_X + GLASS_WIDTH / 2;
        
        // Calculate current dome height based on fill percentage
        int currentDomeHeight = std::min(maxFillHeight, (int)DOME_MAX_HEIGHT);
        int spreadHeight = maxFillHeight - currentDomeHeight;
        
        // Draw the main sand body (if any)
        if (spreadHeight > 0) {
            int flatSandTop = sandBottom - spreadHeight;
            
            // Draw the flat accumulated sand
            for (int y = sandBottom - 1; y >= flatSandTop; y--) {
                if (y >= middleY) {
                    int leftX = leftBoundary[y - GLASS_Y];
                    int rightX = rightBoundary[y - GLASS_Y];
                    
                    for (int x = leftX; x <= rightX; x++) {
                        display.drawPixel(x, y);
                        bottomPixelCount++;
                    }
                }
            }
            
            // Adjust sandBottom for dome drawing
            sandBottom = flatSandTop;
        }
        
        // Draw the dome shape with smoother top
        for (int y = sandBottom; y >= sandBottom - currentDomeHeight; y--) {
            if (y >= middleY) {
                int leftX = leftBoundary[y - GLASS_Y];
                int rightX = rightBoundary[y - GLASS_Y];
                
                for (int x = leftX; x <= rightX; x++) {
                    int distFromCenter = abs(x - centerX);
                    int domeHeightAtDist = calculateDomeHeight(distFromCenter, currentDomeHeight);
                    
                    if (sandBottom - y <= domeHeightAtDist) {
                        // Only add randomness at the very top edge of the dome
                        if (sandBottom - y == domeHeightAtDist) {
                            // Increased randomness at the dome's edge
                            if (random(100) < 70) { // 70% chance to skip pixel at the edge
                                continue;
                            }
                        }
                        display.drawPixel(x, y);
                        bottomPixelCount++;
                    }
                }
            }
        }
    }
}
        
        // Add some randomness to the top surface
   /*     int topSurfaceY = sandBottom - currentDomeHeight;
        if (topSurfaceY >= middleY) {
            int leftX = leftBoundary[topSurfaceY - GLASS_Y];
            int rightX = rightBoundary[topSurfaceY - GLASS_Y];
            
            for (int x = leftX; x <= rightX; x++) {
                if (random(100) < 20) {
                    display.drawPixel(x, topSurfaceY - 1);
                    bottomPixelCount++;
                }
            }
        }
    }
}
*/

// Main draw sand function that calls both chambers
void drawSand() {
    topPixelCount = 0;
    bottomPixelCount = 0;
    
    drawTopSand();
    drawBottomSand();
}

// Draw the hourglass frame - THIS WAS LIKELY MISSING OR INCOMPLETE
void drawHourglass() {
    int middleY = GLASS_Y + GLASS_HEIGHT / 2;

    // Draw the filled walls
    for (int y = GLASS_Y + TOP_THICKNESS; y < GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS; y++) {
        float t;
        float xL1, yL1, xR1, yR1;    // Inner curve points
        float xL2, yL2, xR2, yR2;    // Outer curve points

        if (y < middleY) { // Top half
            t = (float)(y - (GLASS_Y + TOP_THICKNESS)) / (GLASS_HEIGHT / 2 - TOP_THICKNESS);
            // Inner curves
            bezierPoint(t, GLASS_X + WALL_THICKNESS - 1, GLASS_Y + TOP_THICKNESS, GLASS_X + WALL_THICKNESS - 1, GLASS_Y + GLASS_HEIGHT / 3, GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + WALL_THICKNESS - 1, middleY, xL1, yL1);
            bezierPoint(t, GLASS_X + GLASS_WIDTH - WALL_THICKNESS + 1, GLASS_Y + TOP_THICKNESS, GLASS_X + GLASS_WIDTH - WALL_THICKNESS + 1, GLASS_Y + GLASS_HEIGHT / 3, GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - WALL_THICKNESS + 1, middleY, xR1, yR1);
            // Outer curves
            bezierPoint(t, GLASS_X - BASE_PROTRUSION + 1, GLASS_Y + TOP_THICKNESS, GLASS_X + 1, GLASS_Y + GLASS_HEIGHT / 3, GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + 1, middleY, xL2, yL2);
            bezierPoint(t, GLASS_X + GLASS_WIDTH + BASE_PROTRUSION - 1, GLASS_Y + TOP_THICKNESS, GLASS_X + GLASS_WIDTH - 1, GLASS_Y + GLASS_HEIGHT / 3, GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - 1, middleY, xR2, yR2);
        } else { // Bottom half
            t = (float)(y - middleY) / (GLASS_HEIGHT / 2 - TOP_THICKNESS);
            // Inner curves
            bezierPoint(t, GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + WALL_THICKNESS - 1, middleY, GLASS_X + WALL_THICKNESS - 1, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3, GLASS_X + WALL_THICKNESS - 1, GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS, xL1, yL1);
            bezierPoint(t, GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - WALL_THICKNESS + 1, middleY, GLASS_X + GLASS_WIDTH - WALL_THICKNESS + 1, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3, GLASS_X + GLASS_WIDTH - WALL_THICKNESS + 1, GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS, xR1, yR1);
            // Outer curves
            bezierPoint(t, GLASS_X + (GLASS_WIDTH - NECK_TOTAL) / 2 + 1, middleY, GLASS_X + 1, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3, GLASS_X - BASE_PROTRUSION + 1, GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS, xL2, yL2);
            bezierPoint(t, GLASS_X + GLASS_WIDTH - (GLASS_WIDTH - NECK_TOTAL) / 2 - 1, middleY, GLASS_X + GLASS_WIDTH - 1, GLASS_Y + GLASS_HEIGHT - GLASS_HEIGHT / 3, GLASS_X + GLASS_WIDTH + BASE_PROTRUSION - 1, GLASS_Y + GLASS_HEIGHT - TOP_THICKNESS, xR2, yR2);
        }

        // Draw the walls
        int xL2i = round(xL2);
        int xR2i = round(xR2);

        display.drawPixel(xL2i, y);      // Left wall outer
        display.drawPixel(xL2i + 1, y);   // Left wall inner
        display.drawPixel(xR2i, y);      // Right wall outer
        display.drawPixel(xR2i - 1, y);   // Right wall inner
    }

    // Draw top and bottom bases
    drawTopBase(true);
    drawTopBase(false);
}

void setup() {
    // Initialize GPIO13 as output and set it HIGH
    pinMode(GPIO_PIN, OUTPUT);
    digitalWrite(GPIO_PIN, HIGH);
    
    // Initialize display with rotation based on GPIO state
    if (digitalRead(GPIO_PIN)) {
        display.setDisplayRotation(U8G2_R3);
    } else {
        display.setDisplayRotation(U8G2_R1);
    }
    
    // Initialize display
    display.begin();
    display.setFont(u8g2_font_6x10_tf);

    // Calculate boundaries for the hourglass shape
    calculateBoundaries();

    // Initialize particles
    initializeFallingParticles();

    // Set start time
    startTime = millis();

    // Initialize random seed
    randomSeed(os_random());
}

void loop() {
    // Check GPIO state and handle rotation if needed
    checkGPIOAndRotation();

    // Calculate progress
    unsigned long elapsedTime = millis() - startTime;
    int progress = map(elapsedTime, 0, HOURGLASS_DURATION, 0, 100);
    progress = constrain(progress, 0, 100);

    // Begin drawing
    display.clearBuffer();

    // Draw progress percentage
    char progressStr[5];
    sprintf(progressStr, "%d%%", progress);
    display.drawStr(23, 8, progressStr);
    display.drawStr(1,127,"Sand Clock");

    // Draw all hourglass elements
    drawHourglass();
    updateSandLevels();
    drawSand();
    updateFallingParticles();
    drawFallingParticles();

    // Send the buffer to the display
    display.sendBuffer();

    // Check if time's up
    if (elapsedTime >= HOURGLASS_DURATION) {
        // Instead of showing "Time's Up", just keep showing the final state
        startTime = millis() - HOURGLASS_DURATION;  // This keeps the progress at 100%
    }

    delay(ANIMATION_DELAY);
}