#include "pixel_stomp_mux.h"
#include <Arduino.h>

#define BMC_MUX_74HC165_DELAY 5

PixelStompMux::PixelStompMux(uint8_t dat, uint8_t ld, uint8_t clk, uint8_t di)
    : pin_dat(dat), pin_ld(ld), pin_clk(clk), pin_di(di),
      last_button_state(0) {
}

void PixelStompMux::begin() {
    pinMode(pin_di, INPUT);
    pinMode(pin_clk, OUTPUT);
    pinMode(pin_ld, OUTPUT);

    digitalWrite(pin_clk, LOW);
    digitalWrite(pin_ld, HIGH);

    delay(100);

    FastLED.addLeds<WS2812, 12, GRB>(leds, NUM_LEDS);
    FastLED.setBrightness(128);
    fill_solid(leds, NUM_LEDS, CRGB::Black);
    FastLED.show();

    Serial.printf("[MUX] Init DAT=%d LD=%d CLK=%d DI=%d\n",
                  pin_dat, pin_ld, pin_clk, pin_di);
    Serial.printf("[MUX] DI raw at init: %d\n", digitalRead(pin_di));
}

uint16_t PixelStompMux::read_buttons() {
    digitalWrite(pin_clk, LOW);
    delayMicroseconds(BMC_MUX_74HC165_DELAY);

    digitalWrite(pin_ld, LOW);
    delayMicroseconds(BMC_MUX_74HC165_DELAY);
    digitalWrite(pin_ld, HIGH);
    delayMicroseconds(BMC_MUX_74HC165_DELAY);

    uint8_t mux1 = 0;
    for (int i = 7; i >= 0; i--) {
        bitWrite(mux1, i, digitalRead(pin_di));
        digitalWrite(pin_clk, HIGH);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);
        digitalWrite(pin_clk, LOW);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);
    }

    uint8_t mux2 = 0;
    for (int i = 7; i >= 0; i--) {
        bitWrite(mux2, i, digitalRead(pin_di));
        digitalWrite(pin_clk, HIGH);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);
        digitalWrite(pin_clk, LOW);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);
    }

    uint16_t data = ((uint16_t)mux2 << 8) | mux1;
    last_button_state = data;
    return data;
}

bool PixelStompMux::is_button_pressed(uint8_t index) {
    if (index >= NUM_BUTTONS) return false;
    uint16_t state = read_buttons();
    return !(state & (1 << index));
}

void PixelStompMux::set_led_color(uint8_t index, uint8_t r, uint8_t g, uint8_t b) {
    if (index >= NUM_LEDS) return;
    leds[index] = CRGB(r, g, b);
}

void PixelStompMux::set_led_brightness(uint8_t brightness) {
    FastLED.setBrightness(brightness);
}

void PixelStompMux::clear_all() {
    fill_solid(leds, NUM_LEDS, CRGB::Black);
    FastLED.show();
    Serial.println("[MUX] LEDs cleared");
}

void PixelStompMux::show() {
    FastLED.show();
}

void PixelStompMux::dump() {
    uint16_t buttons = read_buttons();
    Serial.printf("[MUX] Buttons: 0x%04X (", buttons);
    for (int i = 15; i >= 0; i--) {
        Serial.print((buttons >> i) & 1);
    }
    Serial.println(")");

    for (int i = 0; i < NUM_BUTTONS; i++) {
        bool pressed = !(buttons & (1 << i));
        Serial.printf("  [%d] %s\n", i, pressed ? "PRESS" : "off");
    }
}

void PixelStompMux::probe() {
    Serial.println("[MUX] === Hardware Probe ===");

    for (int round = 0; round < 5; round++) {
        Serial.printf("--- Round %d ---\n", round);

        digitalWrite(pin_clk, LOW);
        digitalWrite(pin_ld, HIGH);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);

        digitalWrite(pin_ld, LOW);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);
        digitalWrite(pin_ld, HIGH);
        delayMicroseconds(BMC_MUX_74HC165_DELAY);

        uint8_t mux1 = 0;
        uint8_t mux2 = 0;
        for (int i = 7; i >= 0; i--) {
            bitWrite(mux1, i, digitalRead(pin_di));
            digitalWrite(pin_clk, HIGH);
            delayMicroseconds(BMC_MUX_74HC165_DELAY);
            digitalWrite(pin_clk, LOW);
            delayMicroseconds(BMC_MUX_74HC165_DELAY);
        }
        for (int i = 7; i >= 0; i--) {
            bitWrite(mux2, i, digitalRead(pin_di));
            digitalWrite(pin_clk, HIGH);
            delayMicroseconds(BMC_MUX_74HC165_DELAY);
            digitalWrite(pin_clk, LOW);
            delayMicroseconds(BMC_MUX_74HC165_DELAY);
        }
        uint16_t data = ((uint16_t)mux2 << 8) | mux1;
        Serial.printf("  Round %d: 0x%04X (MUX1=0x%02X MUX2=0x%02X)\n", round, data, mux1, mux2);

        delay(500);
    }

    Serial.println("  LED test: pixel 0 RED (max brightness)...");
    leds[0] = CRGB(255, 0, 0);
    FastLED.setBrightness(255);
    FastLED.show();
    delay(2000);
    leds[0] = CRGB::Black;
    FastLED.show();

    Serial.println("[MUX] === Probe Complete ===");
}