#include "hardware_led.hpp"
#include <Arduino.h>
#include <math.h>

#define DEBUG_LED
#ifdef DEBUG_LED
#define LOG(msg) Serial.println(msg)
#else
#define LOG(msg)
#endif

template<uint8_t PIN>
HardwareLed<PIN>::HardwareLed(uint8_t numPixels)
    : m_leds(new CRGB[numPixels > 0 ? numPixels : 1]),
      m_numPixels(numPixels > 0 ? numPixels : 1),
      m_currentConfig(hardware_LedConfig_init_default),
      m_isActive(false),
      m_startTime(0),
      m_pulseState(0),
      m_lastPulseTime(0),
      m_fadeIndex(0),
      m_lastFadeTime(0),
      m_fadeCurrentColor(0),
      m_fadeTargetColor(0),
      m_fadeProgress(0.0f),
      m_fadeStartTime(0),
      m_callback(nullptr) {}

template<uint8_t PIN>
HardwareLed<PIN>::~HardwareLed() {
    delete[] m_leds;
}

template<uint8_t PIN>
void HardwareLed<PIN>::begin() {
    FastLED.addLeds<WS2812B, PIN, GRB>(m_leds, m_numPixels).setCorrection(TypicalLEDStrip);
    FastLED.setMaxPowerInVoltsAndMilliamps(5, m_numPixels * 60); // Limit power
    FastLED.clear();
    FastLED.show();
    randomSeed(analogRead(0));
}

template<uint8_t PIN>
void HardwareLed<PIN>::end() {
    FastLED.clear();
    FastLED.show();
}

template<uint8_t PIN>
void HardwareLed<PIN>::update() {
    static unsigned long lastUpdate = 0;
    unsigned long now = millis();
    if (now - lastUpdate < 10) return; // Limit to ~100 FPS
    lastUpdate = now;

    if (!m_isActive) return;

    // Safe against millis() overflow due to unsigned arithmetic
    if (m_currentConfig.duration_ms > 0 && (now - m_startTime) >= m_currentConfig.duration_ms) {
        m_isActive = false;
        FastLED.clear();
        FastLED.show();
        if (m_callback) m_callback();
        return;
    }

    switch (m_currentConfig.which_animation_params) {
        case hardware_LedConfig_static_params_tag:
            applyStatic(m_currentConfig.animation_params.static_params);
            break;
        case hardware_LedConfig_pulse_params_tag:
            applyPulse(m_currentConfig.animation_params.pulse_params);
            break;
        case hardware_LedConfig_fade_params_tag:
            applyFade(m_currentConfig.animation_params.fade_params);
            break;
        case hardware_LedConfig_flicker_params_tag:
            applyFlicker(m_currentConfig.animation_params.flicker_params);
            break;
        default:
            LOG("Error: Unknown animation type");
            break;
    }
}

template<uint8_t PIN>
void HardwareLed<PIN>::set(const hardware_LedConfig& config) {
    if (config.brightness > 255) {
        LOG("Error: Brightness exceeds 255");
        return;
    }
    if (config.which_animation_params == hardware_LedConfig_fade_params_tag &&
        (config.animation_params.fade_params.colors_count == 0 ||
         config.animation_params.fade_params.colors_count > 5)) {
        LOG("Error: Invalid colors_count in FadeParams");
        m_isActive = false;
        FastLED.clear();
        FastLED.show();
        return;
    }
    if (config.which_animation_params == hardware_LedConfig_pulse_params_tag &&
        config.animation_params.pulse_params.speed_ms == 0) {
        LOG("Error: Pulse speed_ms cannot be zero");
        return;
    }
    m_currentConfig = config;
    m_startTime = millis();
    m_isActive = true;
    m_pulseState = 0;
    m_lastPulseTime = m_startTime;
    m_fadeIndex = 0;
    m_lastFadeTime = m_startTime;
    if (config.which_animation_params == hardware_LedConfig_fade_params_tag &&
        config.animation_params.fade_params.colors_count > 0) {
        m_fadeCurrentColor = config.animation_params.fade_params.colors[0];
        m_fadeTargetColor = config.animation_params.fade_params.colors[0];
        m_fadeProgress = 1.0f;
        m_fadeStartTime = m_startTime;
    }
    update();
}

template<uint8_t PIN>
void HardwareLed<PIN>::applyStatic(const hardware_StaticParams& params) {
    setColor(params.color, m_currentConfig.brightness);
}

template<uint8_t PIN>
void HardwareLed<PIN>::applyPulse(const hardware_PulseParams& params) {
    unsigned long now = millis();
    float speed_ms = max(params.speed_ms, 1U); // Prevent division by zero
    float phase = fmod((now - m_startTime) / (float)speed_ms, 1.0f);
    float brightnessFactor = 0.5f + 0.5f * sin(2 * PI * phase);
    uint16_t pulseBrightness = roundf(m_currentConfig.brightness * brightnessFactor);
    if (pulseBrightness > 255) pulseBrightness = 255;
    setColor(params.color, (uint8_t)pulseBrightness);
}

template<uint8_t PIN>
void HardwareLed<PIN>::applyFade(const hardware_FadeParams& params) {
    if (params.colors_count == 0 || params.colors_count > 5) {
        LOG("Error: Invalid colors_count in FadeParams");
        return;
    }
    unsigned long now = millis();
    if (m_fadeProgress >= 1.0f) {
        m_fadeIndex = (m_fadeIndex + 1) % params.colors_count;
        m_fadeCurrentColor = m_fadeTargetColor;
        m_fadeTargetColor = params.colors[m_fadeIndex];
        m_fadeProgress = 0.0f;
        m_fadeStartTime = now;
    }
    float speed_ms = max(params.speed_ms, 1U); // Prevent division by zero
    m_fadeProgress = (now - m_fadeStartTime) / (float)speed_ms;
    if (m_fadeProgress > 1.0f) m_fadeProgress = 1.0f;
    uint32_t interpolatedColor = lerpColor(m_fadeCurrentColor, m_fadeTargetColor, m_fadeProgress);
    setColor(interpolatedColor, m_currentConfig.brightness);
}

template<uint8_t PIN>
void HardwareLed<PIN>::applyFlicker(const hardware_FlickerParams& params) {
    uint32_t intensity = min(params.intensity, 100U); // Clamp to 0-100
    uint8_t threshold = map(intensity, 0, 100, 0, 255);
    bool showPixel = random(255) < threshold;
    if (showPixel != (m_leds[0] != CRGB(0, 0, 0))) {
        if (showPixel) {
            setColor(params.color, m_currentConfig.brightness);
        } else {
            FastLED.clear();
            FastLED.show();
        }
    }
}

template<uint8_t PIN>
void HardwareLed<PIN>::setColor(uint32_t color, uint8_t brightness) {
    uint8_t r = (color >> 16) & 0xFF;
    uint8_t g = (color >> 8) & 0xFF;
    uint8_t b = color & 0xFF;
    uint16_t r_scaled = ((uint16_t)r * brightness) / 255;
    uint16_t g_scaled = ((uint16_t)g * brightness) / 255;
    uint16_t b_scaled = ((uint16_t)b * brightness) / 255;
    r = (r_scaled > 255) ? 255 : (uint8_t)r_scaled;
    g = (g_scaled > 255) ? 255 : (uint8_t)g_scaled;
    b = (b_scaled > 255) ? 255 : (uint8_t)b_scaled;
    for (uint16_t i = 0; i < m_numPixels; i++) {
        m_leds[i] = CRGB(r, g, b);
    }
    FastLED.show();
}

template<uint8_t PIN>
uint32_t HardwareLed<PIN>::lerpColor(uint32_t color1, uint32_t color2, float t) const {
    uint8_t r1 = (color1 >> 16) & 0xFF;
    uint8_t g1 = (color1 >> 8) & 0xFF;
    uint8_t b1 = color1 & 0xFF;
    uint8_t r2 = (color2 >> 16) & 0xFF;
    uint8_t g2 = (color2 >> 8) & 0xFF;
    uint8_t b2 = color2 & 0xFF;
    float r_f = r1 + (r2 - r1) * t;
    float g_f = g1 + (g2 - g1) * t;
    float b_f = b1 + (b2 - b1) * t;
    uint8_t r = (uint8_t)roundf(r_f);
    uint8_t g = (uint8_t)roundf(g_f);
    uint8_t b = (uint8_t)roundf(b_f);
    return (r << 16) | (g << 8) | b;
}

template<uint8_t PIN>
float HardwareLed<PIN>::estimateCurrent_mA() const {
    return m_numPixels * (m_currentConfig.brightness / 255.0f) * 60.0f;
}

// Explicit instantiation for pin D2 (GPIO 2)
template class HardwareLed<2>;