#include "ATALights.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include <Arduino.h>
#include <FastLED.h>
#include "Animations.h"
#include <functional>
#include "system.h"
#include "ColorPalettes.h"
//#include <crgb.h>

#define FASTLED_CORE 0
static const char* tag = "strips";

uint32_t whiteTimeout = 0;

TaskHandle_t Animation_Task_Handle;
LEDSTRIP_SETTINGS ledSettings[2];
volatile bool AnimationLooping = false;
ANIM_EVENT prevAnimEvent = {0};
QueueHandle_t animationQueue = xQueueCreate( 4, sizeof( ANIM_EVENT ) );

void Lights_Set_Animation(int animIndex, uint8_t red, uint8_t grn, uint8_t blu){

    // Trigger to exit curring looping animation
    if(AnimationLooping){
        AnimationLooping = false;
        xTaskNotifyGive( Animation_Task_Handle );
    }

    ANIM_EVENT event;
    event.AnimationIndex = animIndex;
    event.data.red = red;
    event.data.grn = grn;
    event.data.blu = blu;
    event.data.wht = 0;
    xQueueSend( animationQueue, &event, 25 );
}

void Lights_Set_ON(){
    Lights_Set_Animation(prevAnimEvent.AnimationIndex, prevAnimEvent.data.red, prevAnimEvent.data.grn, prevAnimEvent.data.blu);
}

void Lights_Set_OFF(){
    Lights_Set_Animation(OFF_INDEX, 0, 0, 0);
}

void Lights_Set_Brightness(uint8_t scale){
    FastLED.setBrightness(scale);
}

void Lights_Set_White(uint8_t val){
    //pwmOut[0]->setOutput(val / 2.5f);
}

void Init_Lights_Task(void){

    ledSettings[0].leds = new CRGB[ledSettings[0].size];
    Init_Strip(ledSettings[0].leds, ledSettings[0].pin, ledSettings[0].size, ledSettings[0].rgbOrder, ledSettings[0].chip, ledSettings[0].bright);
    ESP_LOGD(tag, "Initializing Strip1: Pin: %d, size: %d, order: %s, chip: %s", ledSettings[0].pin, ledSettings[0].size, ledSettings[0].rgbOrder, ledSettings[0].chip);
    

    ledSettings[1].leds = new CRGB[ledSettings[1].size];;
    Init_Strip(ledSettings[1].leds, ledSettings[1].pin, ledSettings[1].size, ledSettings[1].rgbOrder, ledSettings[1].chip, ledSettings[1].bright);
    ESP_LOGD(tag, "Initializing Strip2: Pin: %d, size: %d, order: %s, chip: %s", ledSettings[1].pin, ledSettings[1].size, ledSettings[1].rgbOrder, ledSettings[1].chip);

	xTaskCreatePinnedToCore(Lights_Control_Task, "Lights_Task", 1024*8, NULL, 1, &Animation_Task_Handle, FASTLED_CORE);
	ESP_LOGI(tag, "Lights Task Created...");
}


void Animation_Loop_Exit(void){
    if( Animation_Task_Handle ){ 
        xTaskNotifyGive( Animation_Task_Handle ); 
    }
}


EOrder GetEOrderByString(const String& rgbStr){
    ESP_LOGI(tag, "EOrder is %s", rgbStr.c_str());
    if (rgbStr.equalsIgnoreCase("RGB")) return RGB;
    else if (rgbStr.equalsIgnoreCase("RBG")) return RBG;
    else if (rgbStr.equalsIgnoreCase("GRB")) return GRB;
    else if (rgbStr.equalsIgnoreCase("GBR")) return GBR;
    else if (rgbStr.equalsIgnoreCase("BRG")) return BRG;
    else if (rgbStr.equalsIgnoreCase("BGR")) return BGR;
    else return RGB; // Default case
}


void LightsON(void){
    FastLED.show();
}


void LightsOff(void){
    FastLED.clear();
    FastLED.show();
}


/*
void setPixel(LEDSTRIP_SETTINGS& strip, int pixelIndex, CRGB col){
    int x = calcPixelIndex(strip, pixelIndex);
    leds1[x] = col;
}

inline int calcPixelIndex(LEDSTRIP_SETTINGS& strip, int index) {
  int x = (index + strip.shift) % strip.effSize;
  x = (x < 0) ? (x + strip.effSize) : x;
  return (x +strip.offset) % strip.effSize;
}
*/

inline void setPixel1(LEDSTRIP_SETTINGS& leds, int pixelIndex, const CRGB col) {
    register uint16_t x = pixelIndex + ledSettings[0].shift;
    // If strip.effSize is power of 2, use faster bit masking
    if ((leds.effSize & (leds.effSize - 1)) == 0) {
        x = (x < 0) ? ((x + leds.effSize) & (leds.effSize - 1)) : (x & (leds.effSize - 1));
        leds.leds[(x + leds.offset) & (leds.effSize - 1)] = col;
    } else {
        // For non-power-of-2 sizes, still need modulo
        x = (x < 0) ? ((x + ledSettings[0].effSize) % ledSettings[0].effSize) : (x % ledSettings[0].effSize);
        ledSettings[0].leds[(x + ledSettings[0].offset) % ledSettings[0].effSize] = col;
    }
}


inline void setPixel2(int pixelIndex, const CRGB col) {
    register uint16_t x = pixelIndex + ledSettings[1].shift;
    // If strip.effSize is power of 2, use faster bit masking
    if ((ledSettings[1].effSize & (ledSettings[1].effSize - 1)) == 0) {
        x = (x < 0) ? ((x + ledSettings[1].effSize) & (ledSettings[1].effSize - 1)) : (x & (ledSettings[1].effSize - 1));
        ledSettings[1].leds[(x + ledSettings[1].offset) & (ledSettings[1].effSize - 1)] = col;
    } else {
        // For non-power-of-2 sizes, still need modulo
        x = (x < 0) ? ((x + ledSettings[1].effSize) % ledSettings[1].effSize) : (x % ledSettings[1].effSize);
        ledSettings[1].leds[(x + ledSettings[1].offset) % ledSettings[1].effSize] = col;
    }
}


void Init_Strip(CRGB* leds, uint8_t pin, int size, const String& colorOrder, const String& chipType, uint8_t bright) {
    // Validate inputs
    if (!leds || size <= 0) {
        ESP_LOGE(tag, "Invalid LED array or size");
        return;
    }

    // Convert strings to uppercase once
    String chipUpper = chipType;
    String orderUpper = colorOrder;
    chipUpper.toUpperCase();
    orderUpper.toUpperCase();

    // Validate pin (using AND for correct logic)
    if (pin != 3 && pin != 9 && pin != 46) {
        ESP_LOGW(tag, "Invalid pin %d, defaulting to 3", pin);
        pin = 3;
    }

    EOrder rgbOrder = GetEOrderByString(orderUpper);

    if(pin == 3){
        // First level: Chip type selection
        if (chipUpper == "WS2812B" || chipUpper == "SK6812") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2812B, 3, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2812B, 3, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2812B, 3, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2812B, 3, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2812B, 3, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2812B, 3, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2812B, 3, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "SK6812") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<SK6812, 3, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<SK6812, 3, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 3, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<SK6812, 3, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<SK6812, 3, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<SK6812, 3, BGR>(leds, size); break;
                default: FastLED.addLeds<SK6812, 3, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2811_400") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2811_400, 3, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2811_400, 3, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2811_400, 3, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2811_400, 3, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2811_400, 3, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2811_400, 3, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2811_400, 3, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2815") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2815, 3, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2815, 3, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 3, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2815, 3, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2815, 3, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2815, 3, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2815, 3, GRB>(leds, size); break;
            }
        }
        else {
            // Default to WS2812B if unknown chip type
            ESP_LOGW(tag, "Unknown LED chip type: %s, defaulting to WS2812B", chipType.c_str());
            FastLED.addLeds<WS2812B, 3, GRB>(leds, size);
        }
        ESP_LOGI(tag, "Initialized %s LED strip with %d LEDs on pin %d", chipType.c_str(), size, pin);
    }
    else if(pin == 9){
        // First level: Chip type selection
        if (chipUpper == "WS2812B") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2812B, 9, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2812B, 9, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2812B, 9, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2812B, 9, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2812B, 9, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2812B, 9, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2812B, 9, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "SK6812") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<SK6812, 9, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<SK6812, 9, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 9, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<SK6812, 9, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<SK6812, 9, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<SK6812, 9, BGR>(leds, size); break;
                default: FastLED.addLeds<SK6812, 9, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2811_400") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2811_400, 9, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2811_400, 9, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2811_400, 9, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2811_400, 9, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2811_400, 9, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2811_400, 9, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2811_400, 9, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2815") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2815, 9, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2815, 9, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 9, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2815, 9, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2815, 9, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2815, 9, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2815, 9, GRB>(leds, size); break;
            }
        }
        else {
            // Default to WS2812B if unknown chip type
            ESP_LOGW(tag, "Unknown LED chip type: %s, defaulting to WS2812B", chipType.c_str());
            FastLED.addLeds<WS2812B, 9, GRB>(leds, size);
        }
        ESP_LOGI(tag, "Initialized %s LED strip with %d LEDs on pin %d", chipType.c_str(), size, pin);
    }
    else if(pin == 46){
        // First level: Chip type selection
        if (chipUpper == "WS2812B" || chipUpper == "SK6812") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2812B, 46, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2812B, 46, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2812B, 46, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2812B, 46, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2812B, 46, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2812B, 46, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2812B, 46, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "SK6812") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<SK6812, 46, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<SK6812, 46, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 46, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<SK6812, 46, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<SK6812, 46, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<SK6812, 46, BGR>(leds, size); break;
                default: FastLED.addLeds<SK6812, 46, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2811_400") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2811_400, 46, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2811_400, 46, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2811_400, 46, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2811_400, 46, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2811_400, 46, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2811_400, 46, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2811_400, 46, GRB>(leds, size); break;
            }
        }
        else if (chipUpper == "WS2815") {
            switch(rgbOrder) {
                case RGB: FastLED.addLeds<WS2815, 46, RGB>(leds, size); break;
                case RBG: FastLED.addLeds<WS2815, 46, RBG>(leds, size); break;
                case GRB: FastLED.addLeds<WS2815, 46, GRB>(leds, size); break;
                case GBR: FastLED.addLeds<WS2815, 46, GBR>(leds, size); break;
                case BRG: FastLED.addLeds<WS2815, 46, BRG>(leds, size); break;
                case BGR: FastLED.addLeds<WS2815, 46, BGR>(leds, size); break;
                default: FastLED.addLeds<WS2815, 46, GRB>(leds, size); break;
            }
        }
        else {
            // Default to WS2812B if unknown chip type
            ESP_LOGW(tag, "Unknown LED chip type: %s, defaulting to WS2812B", chipType.c_str());
            FastLED.addLeds<WS2812B, 9, GRB>(leds, size);
        }
        ESP_LOGI(tag, "Initialized %s LED strip with %d LEDs on pin %d", chipType.c_str(), size, pin);
    }
    
    FastLED.setBrightness(bright);
}


void Lights_Control_Task_Resume(void){
    vTaskResume(Animation_Task_Handle);
}


void Lights_Control_Task(void *parameters){

    ANIM_EVENT AnimEvent;
    CRGB col;
    COLOR_PACK colorPack;
    CRGBPalette16 firePalette;

    ESP_LOGD(tag, "Lights Control Task Entered & Suspended...");
    vTaskSuspend(NULL);
    ESP_LOGD(tag, "Lights Control Task Resumed...");
    vTaskDelay(1000);

    Lights_Set_Brightness(48);
    Lights_Set_Animation(1, 0, 0, 0); // set rainbow animation


    while (true) {
        if (xQueueReceive(animationQueue, &AnimEvent, portMAX_DELAY) == pdTRUE) {
            ESP_LOGI(tag, "New Animation Event: Index: %d", AnimEvent.AnimationIndex);
            switch (AnimEvent.AnimationIndex) {
                case -3: // Set Pixel by index
                    if (AnimEvent.data.data[7] >= 0 && AnimEvent.data.data[7] < ledSettings[0].size) {
                        ledSettings[0].leds[AnimEvent.data.data[7]] = CRGB(AnimEvent.data.red, AnimEvent.data.grn, AnimEvent.data.blu);
                        FastLED.show();
                    } else {
                        ESP_LOGW(tag, "Pixel index out of range: %d", AnimEvent.data.data[7]);
                    }
                    break;
                case -2: // Fill Static Color
                    col = CRGB(AnimEvent.data.red, AnimEvent.data.grn, AnimEvent.data.blu);
                    fill_solid(ledSettings[0].leds, ledSettings[0].size, col);
                    FastLED.show();
                    ESP_LOGD(tag, "Static Color");
                    break;
                case -1:
                    FastLED.clear();
                    FastLED.show();
                    ESP_LOGD(tag, "LEDs Off");
                    break;
                case 0:
                    break;
                case 1:
                    Anim_Rainbow(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, 30);
                    break;
                case 2:
                    Anim_TimedFill(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, CRGB::Black, CRGB::White, 1000, ledSettings[0].shift);
                    whiteTimeout = 20;
                    break;
                case 3:
                    Anim_TimedFill(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, CRGB::Black, CRGB::White, 2000, ledSettings[0].shift);
                    whiteTimeout = 20;
                    break;
                case 4:
                    Anim_TimedFill(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, CRGB::Black, CRGB::White, 3000, ledSettings[0].shift);
                    whiteTimeout = 20;
                    break;
                case 5:
                    createFirePalette(firePalette, CRGB::Red, CRGB::OrangeRed, CRGB::Orange);
                    Anim_Fire(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, 60, firePalette, ledSettings[0].shift);
                    break;
                case 6:
                    createFirePalette(firePalette, CRGB::DarkGreen, CRGB::Green, CRGB::LightGreen);
                    Anim_Fire(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, 60, firePalette, ledSettings[0].shift);
                    break;
                case 7:
                    createFirePalette(firePalette, CRGB::DarkBlue, CRGB::Blue, CRGB::LightBlue);
                    Anim_Fire(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, 60, firePalette, ledSettings[0].shift);
                    break;
                case 8:
                    createFirePalette(firePalette, CRGB::Purple, CRGB::Blue, CRGB::Violet);
                    Anim_Fire(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, 60, firePalette, ledSettings[0].shift);
                    break;
                case 9:
                    loadColorPack(colorPack, colorPack_USA);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 1, 80);
                    break;
                case 10:
                    loadColorPack(colorPack, colorPack_MEXICO);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 1, 80);
                    break;
                case 11:
                    loadColorPack(colorPack, colorPack_RedBlack);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 4, 80);
                    break;
                case 12:
                    loadColorPack(colorPack, colorPack_YellowBlack);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 4, 80);
                    break;
                case 13:
                    loadColorPack(colorPack, colorPack_GreenBlack);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 4, 80);
                    break;
                case 14:
                    loadColorPack(colorPack, colorPack_BlueBlack);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 4, 80);
                    break;
                case 15:
                    loadColorPack(colorPack, colorPack_VioletBlack);
                    Anim_Color_Sectors(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 4, 80);
                    break;
                case 16:
                    loadColorPack(colorPack, colorPack_RAINBOW);
                    Anim_Comets(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 80, RANDOM_DECAY, true, 1);
                    break;
                case 17:
                    loadColorPack(colorPack, colorPack_USA);
                    Anim_Comets(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 80, LINEAR_DECAY, true, 1);
                    break;
                case 18:
                    loadColorPack(colorPack, colorPack_USA);
                    Anim_Comets(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 80, RANDOM_DECAY, false, 1);
                    break;
                case 19:
                    loadColorPack(colorPack, colorPack_USA);
                    Anim_Comets(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 80, RANDOM_DECAY, true, 2);
                    break;
                case 20:
                    loadColorPack(colorPack, colorPack_RAINBOW);
                    Anim_Comets(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 80, RANDOM_DECAY, true, 1);
                    break;
                case 21:
                    loadColorPack(colorPack, colorPack_RAINBOW);
                    Anim_ColorBreath(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 7000, 90);
                    break;
                case 22:
                    loadColorPack(colorPack, colorPack_USA);
                    Anim_GradientRotate(AnimationLooping, ledSettings[0].leds, ledSettings[0].size, colorPack, 50);
                    break;
                default:
                    ESP_LOGW(tag, "Loop default");
                    break;
            }

            AnimationLooping = false;
            prevAnimEvent = AnimEvent;
            ESP_LOGD(tag, "Going to Queue to Wait");
        }
    }
}


void createFirePalette(CRGBPalette16& palette, CRGB color1, CRGB color2, CRGB color3) {
    for (uint8_t i = 0; i < 16; i++) {
        if (i < 3) palette[i] = CRGB::Black;
        else if (i < 7) palette[i] = color1;
        else if (i < 10) palette[i] = color2;
        else if (i < 15) palette[i] = color3;
        else palette[i] = CRGB::White;
    }
}

void loadColorPack(COLOR_PACK& dest, const COLOR_PACK& src) {
    memcpy_P(&dest, &src, sizeof(COLOR_PACK));
}
