#include "led_animation.h"

#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "common/HSVTable.h"
#include "common/neo_colors.h"
#include "common/color_tools.h"

static const char* tag = "led_anim";


int AnimTestModeCount = 0;

ANIM_STATUS animStatus;
WHITE_FILL_STATUS whiteStatus;
HUE_PALLET_DISPENSER HuePalletDispenser;// (1, 1, 1);

/********************************************************************/

// Animation Loop Template
EXIT_TYPE Animation_Loop(ANIM_STATUS &stateMon, int msFreq, TickType_t duration, std::function<int()> callback){
	stateMon.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);
  EXIT_TYPE ret = EXIT_NORMAL;

  TickType_t startTicks = xTaskGetTickCount();
  
  for(;;){
    TickType_t  xLastWakeTime = xTaskGetTickCount();
    if(callback() == EXIT_FINISHED){ return EXIT_FINISHED; }

    if(ulTaskNotifyTake( pdTRUE, msFreq - ( xTaskGetTickCount() - xLastWakeTime ))){ // delay
      stateMon.busy = false;
      return EXIT_FROM_NOTIFY;
    } 

    // May have a duration limit
    if(duration){
      if((xLastWakeTime - startTicks) > duration){
        return EXIT_TIMEOUT;
      }
    }
  }

  return ret;
}

int Const_White_Fill(WHITE_FILL_STATUS &status,  FRONT_LIGHT light , float delayFactor, int countDown, int msFreq) { //int msRampUpTime,  int msRampDownTime, int msHoldTime, int msFreq, float minDuty, float maxDuty){ 
  status.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);
  
  // Linear brightness rise
  if(light.maxDuty > 100) { light.maxDuty = 100; }
  if(light.maxDuty < 1){ light.maxDuty = 1; }

  if (light.minDuty >= light.maxDuty) { light.minDuty = light.maxDuty -1; }
  if (light.minDuty < 0) { light.minDuty = 0; } 

  // Pre Delay
  if(delayFactor > 0.0){
    int waitDelay = int((float)countDown * delayFactor);
    ESP_LOGD(tag, "Const Light Delay: %d, countDown: %d", waitDelay, countDown);
    if(ulTaskNotifyTake( pdTRUE, waitDelay)){ // delay
      status.busy = false;
      return EXIT_FROM_NOTIFY;
    }
  }

  int rampSteps = (countDown - ((float)countDown * delayFactor)) / msFreq;
  float dutyStep = ( light.maxDuty - pwmOut[animProps.frontLight.relayIndex]->currDuty ) / rampSteps;
  if(dutyStep < 0.1) { dutyStep = 0.1; }

  //Log.noticeln("<front> upTime: %d, hold: %d, downTime: %d, msFreq: %d, minDuty: %F, maxDuty: %F", msRampUpTime, msHoldTime, msRampDownTime, msFreq, minDuty, maxDuty);
  //Log.noticeln("<front> dutyStep: %F", dutyStep);
  // Ramp Up
  for(;;){
    //calc new newDuty value
    float newDuty = pwmOut[animProps.frontLight.relayIndex]->currDuty + dutyStep;

    //check if dslrbooth jumped ahead and catch up
    if(delayFactor > 0){
      if(status.dslrCountStatus > newDuty && ((status.dslrCountStatus-10) < 90)){
        newDuty = status.dslrCountStatus; 
      }
    }

    // Set the new Duty 
    if(newDuty > light.maxDuty){ newDuty = light.maxDuty; } 
    pwmOut[animProps.frontLight.relayIndex]->setOutput(newDuty); // set  duty

    if(newDuty >= light.maxDuty) { break;} // break if complete

    //check if there was a trigger to leave function
    if(ulTaskNotifyTake( pdTRUE, msFreq )){ goto done; } // delay
  }

  // Hold Steady for msHoldTime
  //Serial.printf("Holding at %.1f\n\r", pwmOut[animProps.frontLight.relay]->currDuty );
  pwmOut[animProps.frontLight.relayIndex]->setOutput(light.maxDuty);
  if(ulTaskNotifyTake( pdTRUE, light.holdTime )){ goto done; } // delay

  // Linear Ramp Down to Min
  rampSteps = light.rampTime / msFreq;
  dutyStep = abs(( pwmOut[animProps.frontLight.relayIndex]->currDuty - light.minDuty) / rampSteps );
  if(dutyStep < .1) { dutyStep = .1; }
  //Serial.printf("stepDown: %.1f\n\r", dutyStep);

  for(;;){
    pwmOut[animProps.frontLight.relayIndex]->setOutput( pwmOut[animProps.frontLight.relayIndex]->currDuty - dutyStep ); // set  duty
    if( pwmOut[animProps.frontLight.relayIndex]->currDuty <= light.minDuty ) { break; } // break if complete

    //if(status.repeat){ goto done;}
    if(ulTaskNotifyTake( pdTRUE, msFreq )){ break; } // delay
  }

  done:
  pwmOut[animProps.frontLight.relayIndex]->setOutput(light.minDuty);
  status.busy = false;
  status.dslrCountStatus = 0;
  return 0;
}


// TODO Roamer/Helio type White Fill
//int Const_White_Fill_Disc
/****************************************************************/
int Animation_White_Fill(LEDSTRIP& strip, ANIM_STATUS &stateMon, int msFillTime, rgbpixel_t& col, rgbpixel_t& baseCol)
{
  stateMon.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);

  int msDelay = msFillTime / strip.effSize;

  // Fill background first
  for(int i = 0; i < strip.effSize; i++){
    strip.pixels[i] = baseCol; // skips index calc
  }

  // Linear fill
  for(int i = 0; i < strip.effSize; i++){
    TickType_t xLastWakeTime = xTaskGetTickCount();
    strip.setPixel(i, col, 255);
    strip.show(true);
    if(ulTaskNotifyTake( pdTRUE, msDelay - (xTaskGetTickCount() - xLastWakeTime ) == pdTRUE )){ return 1; } // delay
  }

  stateMon.busy = false;
  return 0;
}

/********************************  WHITE FILL MIRRORED  *********************************/
EXIT_TYPE Animation_White_Fill_Mirrored(LEDSTRIP& strip, ANIMATION_EVENT& event)
{
  int msFillTime = 0;
  if(event.countDown > 0){ // use countdown if available 
    msFillTime = event.countDown;
  }else{
    msFillTime = map(event.param1, 0, 100, 0, 10000);
    if(msFillTime < 1000){ msFillTime = 1000; }
  }

  strip.powerDiv = (event.check4) ? 1 : 0;

  // TODO set msFreq from density
  int msFreq = 25; 
  int halfSize = (strip.effSize / 2);
  float steps = ((float)msFreq * (float)halfSize) / msFillTime;
  strip.fill(col_black, 0, strip.effSize); // clear/off all pixels

  float stepsTotal = steps;
  //int startIndex = 0;
  int endIndex = stepsTotal; 
  //uint8_t frac = 0;
  ESP_LOGD(tag, "whitefill-> msFillTime: %d, halfsize: %d, steps: %.3f, freq: %d, hue: %d", msFillTime, halfSize, steps, msFreq, event.hue);

  rgbpixel_t mainCol = GetColorFromHue(event.hue);
  //rgbpixel_t fracCol; //  = event.col1;

  int lastEndIndex = 0;
  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, [&](){

    stepsTotal += steps;
    endIndex = (int)stepsTotal;
    if(endIndex != lastEndIndex){
      for(int i = lastEndIndex; i < endIndex; i++){
        strip.setPixelMirrored(i, mainCol);
      }
      lastEndIndex = endIndex;
      strip.show(true);
    }

    if(endIndex > (halfSize -1)){ return EXIT_FINISHED; }

    /*
    frac = (stepsTotal - (int)stepsTotal) * 255;
    fracCol  = mainCol;
    scalePixel(fracCol, frac);
    linearizePixel(fracCol);
    strip.setPixelMirrored(endIndex + 1, fracCol);
    */
    //strip.show(true);

    return EXIT_NORMAL;
  });

  // TODO remove this trace later
  //Log.traceln("exited white fill");
  return ret;
}

float calculateSteps(int msFreq, float LEDCount, int msFillTime) {
  return (msFreq * LEDCount) / msFillTime;
}

void GetOptimizedStepInterval(float &steps, int &interval, int LEDCount, int msFillTime, int startInterval, float tolerance) 
{
  steps = calculateSteps(startInterval, LEDCount, msFillTime);

  while (std::abs(steps - std::round(steps)) > tolerance) {
      steps = calculateSteps(++startInterval, LEDCount, msFillTime);
  }
  interval = startInterval;
  steps = round(steps);
}


EXIT_TYPE Animation_Linear_Brighten(LEDSTRIP& strip, ANIMATION_EVENT& event){
  return EXIT_NORMAL;
}


EXIT_TYPE Animation_Tick_Fill(LEDSTRIP& strip, ANIMATION_EVENT& event){
  return EXIT_NORMAL;
}



/********************************  COLORED SNAKES  *********************************/
EXIT_TYPE Animation_Snakes(LEDSTRIP& strip, ANIMATION_EVENT& event, DIR_TYPE dirType, int cycles)
{
  bool mix = event.check1;
  bool rotate = event.check2;
  int sectors = constrain(event.param1/10, 1, 10);
  int colorCount = constrain(event.param2/10, 1, 10);
  strip.powerDiv = (event.check4) ? 1 : 0;
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  int snakeSize = trunc(roundf(strip.effSize / (float)sectors));

  HuePalletDispenser.Initialize(event.hue, event.hueRange, colorCount);
  ESP_LOGD(tag, "<snakes> size: %d, sectors: %d, freq: %d", snakeSize, sectors, msFreq);

  // Create Color Pallet Array
  rgbpixel_t col[colorCount];
  if(colorCount == 1){
    col[0] = GetColorFromHue(event.hue);
  }else{
    for(int i = 0; i < colorCount; i++){
      float hue = HuePalletDispenser.GetNextPalletHue();
      col[i] = HUEtoRGB( hue );
      //ESP_LOGD(tag, "<sectors> hue%d: ", hue);
    }
  }


  LED_DIR dir = (dirType == DT_FWD)? DIR_FWD : DIR_REV;
  int dirVal = (dir==DIR_FWD)? 1 : -1;
  int pixIndex = 0, rotateOffset = 0, colIndex = 0;              
  bool colorCycle = true;

  strip.fill({0,0,0}, 0, strip.effSize);

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {

    if(colorCycle){
      for(int sector = 0; sector < sectors; sector++){
        int pix = (sector * snakeSize) + pixIndex + rotateOffset;
        if(mix){
          strip.setPixel(pix * dirVal, (rgbpixel_t)col[(colIndex + sector)%colorCount]);
        }else{
          strip.setPixel(pix * dirVal, (rgbpixel_t)col[colIndex]);
        }
      }
    }else{
      for(int sector = 0; sector < sectors; sector++){
        int pix = (sector * snakeSize) + pixIndex + rotateOffset;
        strip.setPixel(pix * dirVal, {0,0,0});
      }
    }

    if(rotate){ // turn off front pixels
      strip.rotatePixels(dir);
      rotateOffset = ++rotateOffset % strip.effSize;
    }
    strip.show(true);

    pixIndex = ++pixIndex % snakeSize;

    if(pixIndex == 0){
      if(colorCycle){
        colorCycle = false;
      }else{
        // Check if there's an iteration limit
        if(cycles){
          if(--cycles <= 0){ return EXIT_FINISHED; }
        }
        // Once full cycle here
        if(dirType == DT_BOTH){
          dir = (dir == DIR_FWD)? DIR_REV : DIR_FWD;
          dirVal = -dirVal;
        }
        colorCycle = true;
        colIndex = ++colIndex % colorCount; // next color
      }
    }
    return EXIT_NORMAL;
  }));

  return ret;
}


/********************************  COLORED COMETS  *********************************/
#define COMET_SIZE_FACTOR 0.15
#define COMET_FADE_FACTOR1 128
#define COMET_FADE_FACTOR2 192

EXIT_TYPE Animation_Comets(LEDSTRIP& strip,  ANIMATION_EVENT& event, DIR_TYPE dirType, int cycles){
  int sectorCount = constrain(event.param1/10, 1, 10); // cometCount also
  int colorCount = constrain(event.param2/10, 1, 10); // color divisions
  bool mix = event.check1;
  bool rotate = true;
  bool randomDecay = event.check2;
  bool shorterTail = event.check3;
  uint8_t fadeFactor = COMET_FADE_FACTOR1;
  if(shorterTail){ fadeFactor = COMET_FADE_FACTOR2; }
  strip.powerDiv = (event.check4) ? 1 : 0;
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  int sectorSize = trunc(roundf(strip.effSize / (float)sectorCount));

  int cometSize = (strip.effSize / sectorCount) * COMET_SIZE_FACTOR;
  if(cometSize < 1){ cometSize = 1;}

  HuePalletDispenser.Initialize(event.hue, event.hueRange, colorCount);
  ESP_LOGD(tag, "<sectors> size: %d, sectorCount: %d, freq: %d", sectorSize, sectorCount, msFreq);

  // Create Color Pallet Array
  rgbpixel_t col[colorCount];
  if(colorCount == 1){
    col[0] = GetColorFromHue(event.hue);
  }else{
    for(int i = 0; i < colorCount; i++){
      float hue = HuePalletDispenser.GetNextPalletHue();
      col[i] = HUEtoRGB( hue );
      //ESP_LOGD(tag, "<sectors> hue%d: ", hue);
    }
  }

  LED_DIR dir;
  if(dirType == DT_BOTH){
    dir = DIR_FWD;
  }else{
    dir = (dirType == DT_FWD)? DIR_FWD : DIR_REV;
  }

  strip.fill({0,0,0}, 0, strip.effSize);

  int rotateIndex = 0;
  int cycleCount = 0;
  int colIndex = 0;
  int rotOffset = 0;
  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {

    // Random Tail Decay here....
    if(randomDecay){
      for (int j = 0; j < strip.effSize; j++){
        if (random(10) > 5){
          fadeToBlackBy(strip.pixels[j], fadeFactor);  
        }
      }
    }else{ // Regular Tail Decay
      for (int j = 0; j < strip.effSize; j++){
        fadeToBlackBy(strip.pixels[j], fadeFactor);  
      }
    }

    // Draw Comets
    if(mix){
      for(int sector = 0; sector < sectorCount; sector++){
        strip.fill((rgbpixel_t)col[(colIndex + sector) % colorCount], rotOffset + (sector * sectorSize), cometSize);
      }
    }else{
      for(int sector = 0; sector < sectorCount; sector++){
        strip.fill((rgbpixel_t)col[colIndex], rotOffset + (sector * sectorSize), cometSize);
      }  
    }

    rotateIndex = ++rotateIndex % strip.effSize;

    if(dir == DIR_FWD){ rotOffset++; }
    else{ rotOffset--; }

    // full rotation
    if(rotateIndex == 0){
      // reverse rotation
      if(dirType == DT_BOTH){
        dir = (dir == DIR_FWD)? DIR_REV : DIR_FWD;
      }
      
      if(cycles){
        cycleCount++;
        if(cycleCount >= cycles){
          return EXIT_FINISHED;
        }
      }

      colIndex = ++colIndex % colorCount; // Increment color index
    }
    strip.show(true);

    return EXIT_NORMAL;
  }));

  return ret;
}

void fadeToBlackBy(rgbpixel_t& pixel, uint8_t fadeAmount) {
  pixel.red = (pixel.red <= 8) ? 0 : pixel.red - ((pixel.red * fadeAmount) >> 8);
  pixel.grn = (pixel.grn <= 8) ? 0 : pixel.grn - ((pixel.grn * fadeAmount) >> 8);
  pixel.blu = (pixel.blu <= 8) ? 0 : pixel.blu - ((pixel.blu * fadeAmount) >> 8);
}

inline uint8_t nscale8(uint8_t i, uint8_t scale) {
  return (((int)i * (int)(scale)) >> 8) + ((i && scale) ? 1 : 0);
}


/********************************  COLORED  SECTORS  *********************************/
EXIT_TYPE Animation_Sectors(LEDSTRIP& strip, ANIMATION_EVENT& event, DIR_TYPE dirType, int cycles){
  bool rotate = event.check2;
  int sectorCount = constrain(event.param1/10, 2, 10);
  int colorCount = constrain(event.param2/10, 1, 10);

  strip.powerDiv = (event.check4) ? 1 : 0;
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  int sectorSize = trunc(roundf(strip.effSize / (float)sectorCount));

  ESP_LOGD(tag, "hue: %d, range: %d\n", event.hue, event.hueRange);
  HuePalletDispenser.Initialize(event.hue, event.hueRange, colorCount);
  ESP_LOGD(tag, "size: %d, sectorCount: %d, colors: %d, freq: %d", sectorSize, sectorCount, colorCount, msFreq);

  // Create Color Pallet Array
  rgbpixel_t col[colorCount];
  if(colorCount == 1){
    col[0] = GetColorFromHue(event.hue);
  }else{
    for(int i = 0; i < colorCount; i++){
      float hue = HuePalletDispenser.GetNextPalletHue();
      col[i] = HUEtoRGB( hue );
      //ESP_LOGD(tag, "hue%d: ", hue);
    }
  }

  LED_DIR dir;
  if(dirType == DT_BOTH){
    dir = DIR_FWD;
  }else{
    dir = (dirType == DT_FWD)? DIR_FWD : DIR_REV;
  }

  strip.fill({0,0,0}, 0, strip.effSize);

  bool updateColors = true;
  int rotateIndex = 0;
  int cycleCount = 0;
  int colIndex = 0;

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {

    if(updateColors){ //
      for(int sector = 0; sector < sectorCount; sector++){
        strip.fill((rgbpixel_t)col[(colIndex + sector) % colorCount], sector * sectorSize, sectorSize);
      }
      colIndex = ++colIndex % colorCount; // Increment color index
      updateColors = false;
    }

    if(rotate){ // turn off front pixels
      strip.rotatePixels(dir);
      rotateIndex = ++rotateIndex % strip.effSize;

      // full rotation
      if(rotateIndex == 0){
        // reverse rotation
        if(dirType == DT_BOTH){
          dir = (dir == DIR_FWD)? DIR_REV : DIR_FWD;
        }
        
        if(cycles){
          cycleCount++;
          if(cycleCount >= cycles){
            return EXIT_FINISHED;
          }
        }

        updateColors = true;
      }
    }

    strip.show(true);
    return EXIT_NORMAL;
  }));

  return ret;
}


/********************************  DASHES  SECTORS  *********************************/
EXIT_TYPE Animation_Dashes(LEDSTRIP& strip, ANIMATION_EVENT& event, DIR_TYPE dirType, int cycles){
  int sectorCount = constrain(event.param1/10, 1, 10);
  int colorCount = constrain(event.param2/10, 1, 10);
  bool mix = event.check1;
  bool rotate = event.check2;

  strip.powerDiv = (event.check4) ? 1 : 0;
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  int sectorSize = trunc(roundf(strip.effSize / (float)sectorCount));

  HuePalletDispenser.Initialize(event.hue, event.hueRange, colorCount);
  ESP_LOGD(tag, "<sectors> size: %d, sectorCount: %d, freq: %d", sectorSize, sectorCount, msFreq);

  // Create Color Pallet Array
  rgbpixel_t col[colorCount];
  if(colorCount == 1){
    col[0] = GetColorFromHue(event.hue);
  }else{
    for(int i = 0; i < colorCount; i++){
      float hue = HuePalletDispenser.GetNextPalletHue();
      col[i] = HUEtoRGB( hue );
      //ESP_LOGD(tag, "<sectors> hue%d: ", hue);
    }
  }

  LED_DIR dir;
  if(dirType == DT_BOTH){
    dir = DIR_FWD;
  }else{
    dir = (dirType == DT_FWD)? DIR_FWD : DIR_REV;
  }

  strip.fill({0,0,0}, 0, strip.effSize);

  bool updateColors = true;
  int rotateIndex = 0;
  int cycleCount = 0;
  int colIndex = 0;

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {
    if(updateColors){ //
      if(mix){
        for(int sector = 0; sector < sectorCount; sector++){
          strip.fill((rgbpixel_t)col[(colIndex + sector) % colorCount], sector * sectorSize, sectorSize/2);
        }
      }else{
        for(int sector = 0; sector < sectorCount; sector++){
          strip.fill((rgbpixel_t)col[colIndex], sector * sectorSize, sectorSize/2);
        }  
      }

      colIndex = ++colIndex % colorCount; // Increment color index
      updateColors = false;
    }

    if(rotate){ // turn off front pixels
      strip.rotatePixels(dir);
      rotateIndex = ++rotateIndex % strip.effSize;

      // full rotation
      if(rotateIndex == 0){
        // reverse rotation
        if(dirType == DT_BOTH){
          dir = (dir == DIR_FWD)? DIR_REV : DIR_FWD;
        }
        
        if(cycles){
          cycleCount++;
          if(cycleCount >= cycles){
            return EXIT_FINISHED;
          }
        }

        updateColors = true;
      }
    }

    strip.show(true);

    return EXIT_NORMAL;
  }));

  return ret;
}


/****************************************************************/
void Animation_TransTo_Color(LEDSTRIP& strip, ANIM_STATUS &stateMon, int msTransTime, int msFreq, rgbpixel_t newCol)
{
  stateMon.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);

  int numSteps = (msTransTime / msFreq);
  if (numSteps == 0) { return; }
    
  for (uint8_t step = 0; step < numSteps; step++) {
    TickType_t xLastWakeTime = xTaskGetTickCount();

    for (size_t i = 0; i < strip.effSize; i++) {
      rgbpixel_t colorDiff = {
        .red = static_cast<uint8_t>((newCol.red - strip.pixels[i].red) / numSteps),
        .grn = static_cast<uint8_t>((newCol.grn - strip.pixels[i].grn) / numSteps),
        .blu = static_cast<uint8_t>((newCol.blu - strip.pixels[i].blu) / numSteps)
      };
      strip.pixels[i].red += colorDiff.red;
      strip.pixels[i].grn += colorDiff.grn;
      strip.pixels[i].blu += colorDiff.blu;
    }

    strip.show(true);
    if(ulTaskNotifyTake( pdTRUE, msFreq - (xTaskGetTickCount() - xLastWakeTime ) == pdTRUE )){ return; } // delay
  }
  stateMon.busy = false;
}


/********************************  HUE SPECTRUM MIRRORED  *********************************/

EXIT_TYPE Animation_Hue_Spectrum_Mirrored(LEDSTRIP& strip, ANIMATION_EVENT& event, TickType_t duration){
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  strip.powerDiv = (event.check4) ? 1 : 0;
  Fill_Hue_Spectrum_Mirrored(strip, event.hue, event.hueRange);

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, duration, std::function<int()>([&]() -> int {
    strip.show(true);
    strip.rotatePixels(DIR_FWD);
    return EXIT_NORMAL;
  }));

  return ret;
}

void Fill_Hue_Spectrum(LEDSTRIP& strip, float hue, float range)
{
  HuePalletDispenser.Initialize(hue, range, strip.effSize);
  for(int i = 0; i < strip.effSize; i++){
    strip.setPixel(i, HUEtoRGB( HuePalletDispenser.GetNextPalletHue() ));
  }
}

void Fill_Hue_Spectrum_Mirrored(LEDSTRIP& strip, float hue1, float hue2)
{
  HuePalletDispenser.Initialize(hue1, hue2, strip.effSize / 2.0);

  for(int i = 0; i < strip.effSize/2; i++){
    strip.setPixelMirrored(i, HUEtoRGB( HuePalletDispenser.GetNextPalletHue() ));
  }
}


/********************************  PULSE COLOR CYCLING  *********************************/
//TODO Add Linearizing for smoother transitions
#define PWR_STEP 8
EXIT_TYPE Animation_Pulse_Color_Cycling(LEDSTRIP& strip, ANIMATION_EVENT& event){
  int colorSteps = event.param1/10.0;
  if(colorSteps < 3){ colorSteps = 2; }

  strip.powerDiv = (event.check4) ? 1 : 0;
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);

  HuePalletDispenser.Initialize(event.hue, event.hueRange, colorSteps);

  int pwrStep = PWR_STEP;
  int pwr = pwrStep;

  rgbpixel_t col = HUEtoRGB(HuePalletDispenser.GetNextPalletHue());

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {
    // ramp up/down brightness
    rgbpixel_t col_pwr = col;
    scalePixel(col_pwr, (uint8_t)pwr);
    linearizePixel(col_pwr);
    strip.fill(col_pwr, 0, strip.effSize);
    strip.show(true);

    pwr += pwrStep;
    if(pwr >= (256-PWR_STEP) || pwr <= PWR_STEP){ 
      pwrStep = -pwrStep; // switch brightness direction

      // Change color
      if(pwrStep > 0){
         col = HUEtoRGB(HuePalletDispenser.GetNextPalletHue());
      }
    }

    return EXIT_NORMAL;
  }));

  return ret;
}


/********************************  RAINBOW  *********************************/
// TODO Need more Red for rainboow
EXIT_TYPE Animation_Rainbow(LEDSTRIP& strip, ANIMATION_EVENT& event, TickType_t duration){
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  double hueStep = 359.2 / (strip.effSize-1);

  strip.powerDiv = (event.check4) ? 1 : 0;

  for(int i = 0; i < strip.effSize; i++){
    strip.setPixel(i, HUEtoRGB((int)round(i*hueStep)));
  }

  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, duration, std::function<int()>([&]() -> int {
    strip.rotatePixels(DIR_FWD);
    strip.show(true);
    return EXIT_NORMAL;
  }));

  return ret;
}


/********************************  FIRE ANIMATION *********************************/
//TODO Get these fire params from json
#define FIRE_SPARK_PIXEL_PERCENT 15
#define MIN_FIRE_COOLING 20
#define MAX_FIRE_COOLING 100
#define MIN_FIRE_SPARKING 50
#define MAX_FIRE_SPARKING 200
uint8_t *heat;
uint8_t sparkCount;  

EXIT_TYPE Animation_Fire(LEDSTRIP& strip, ANIMATION_EVENT& event){
  int msFreq = CalcEventInterval(event.speed, MIN_LED_UPDATE_INTERVAL, 100);
  bool cycleColors = event.check1;
  uint8_t sparking = map(event.param1, 0, 100, MIN_FIRE_SPARKING, MAX_FIRE_SPARKING); // input, input max, output min, out max
  uint8_t cooling = map(event.param2, 0, 100, MIN_FIRE_COOLING, MAX_FIRE_COOLING);

  strip.powerDiv = (event.check4) ? 1 : 0;

  int duration = map(event.hueRange, 0, 360, 0, 60000 ); // upto 1 minute
  if(cycleColors && duration == 0) { duration = 10000; }
  if(!cycleColors && duration > 0) { duration = 0; } 

  FIRE_COLOR fireColor = RED_FIRE;

  // Choose which Color comes closest
  if(event.hue = -1 || event.hue == -2) { event.hue == 0;}
  rgbpixel_t col = HUEtoRGB(event.hue);
  if (col.red >= col.grn && col.red >= col.blu) {
      fireColor = RED_FIRE;
  } else if (col.grn >= col.red && col.grn >= col.blu) {
      fireColor = GREEN_FIRE;
  } else {
      fireColor = BLUE_FIRE;
  }

  EXIT_TYPE ret;
  while(1){
      ret = Animation_Loop( animStatus, msFreq, duration, std::function<int()>([&]() -> int {
      Fire_Update( strip, fireColor, cooling, sparking);
      strip.show(true);
      return EXIT_NORMAL;
    }));

    if(ret == EXIT_TIMEOUT){ 
      fireColor = (FIRE_COLOR)(((int)fireColor + 1) % 3);
      continue; 
    }
    break;
  }

  return ret;
}

void LEDStrip_FireInit(LEDSTRIP& strip){
  sparkCount = (strip.effSize/2 * FIRE_SPARK_PIXEL_PERCENT) / 100;
  heat = new uint8_t[strip.effSize/2];
	for(int i = 0; i < (strip.effSize/2); i++){
		heat[i] = 0;
  }

  srand(70);
}

void Fire_Update( LEDSTRIP& strip, FIRE_COLOR fire, int Cooling, int Sparking) {
  int cooldown;
 
	// Step 1.  Cool down every cell a little
	for( int i = 0; i < (strip.effSize/2); i++) {
		cooldown = rand() % ((Cooling * 10) / (strip.effSize/2) + 2);
		
		if(cooldown > heat[i]) {
			heat[i] = 0;
		}else {
			heat[i] -= cooldown;
		}
	}
	
	// Step 2. Heat from each cell drifts 'up' and diffuses a little
	// Starts from farthest and work down to the source
	for( int k = (strip.effSize/2) - 1; k >= 2; k--) {
		heat[k] = (heat[k - 1] + heat[k - 2] + heat[k - 2]) / 3;
	}
		
	// Step 3.  Randomly ignite new 'sparks' near the bottom
	if( (rand() % 255) < Sparking ) {
		int y = (rand() % sparkCount);
		heat[y] += 160 + (rand() % 95);
	}

	// Step 4.  Convert heat to LED colors
	for( int j = 0; j < strip.effSize/2; j++) {	
    strip.setPixelMirrored(j, GetPixelHeatColor(fire, heat[j]));
	}
}

rgbpixel_t GetPixelHeatColor ( FIRE_COLOR fire, uint8_t temperature){
	rgbpixel_t led;
	
  // Scale 'heat' down from 0-255 to 0-191
  //uint8_t T = ((uint16_t)(temperature*191))>>8;
	uint8_t T = (uint16_t)(temperature*120) >> 8;
 
  // calculate ramp up from (heat ramp)
  int hr = T & 0x3F; // 0..63
  hr <<= 2; // scale up to 0..252
	
	//  Choose Fire Color
	switch(fire){
		case RED_FIRE:{
			//if((hr + 32) & 255){ hr = 255; }
      if((hr + 32) > 255){ hr = 255; }
			// figure out which third of the spectrum we're in:
			if( T > 117) {                     				// hottest
				led = (rgbpixel_t){ .red=120, .grn=120, .blu=(uint8_t)hr	};   // spark color
			} else if( T > 48 ) {             					// middle
				led = (rgbpixel_t){ .red=255, .grn=(uint8_t)hr, .blu=0 }; 	 // flame color 
			} else{																			// coolest
				led = (rgbpixel_t){ .red=(uint8_t)hr, .grn=0, .blu=0 };		 // cooling color	
			}				
			break;
		}
		case BLUE_FIRE:{
			if( T > 117) {                     					// hottest
				led = (rgbpixel_t){	.red=120, .grn=(uint8_t)hr, .blu=120 };  
			} else if( T > 48 ) {             					// middle
				led = (rgbpixel_t){	.red=(uint8_t)hr, .grn=0, .blu=255	};
			} else {                               			// coolest
				led = (rgbpixel_t){	.red=0, .grn=0,.blu=(uint8_t)hr };
			}
			break;
		}
		default:{ // GRN_FIRE
			if( T > 117) {                    	 				// hottest
				led = (rgbpixel_t){	.red=(uint8_t)hr, .grn=120, .blu=120	};  
			}else if ( T > 48 ) {             					// middle
				led = (rgbpixel_t){	.red=0, .grn=255, .blu=(uint8_t)hr	};
			}else {                               			// coolest
				led = (rgbpixel_t){	.red=0, .grn=(uint8_t)hr, .blu=0	};
			}
			break;
		}
	}

	return led;
}



/********************************  Serial Out Toggle  *********************************/
EXIT_TYPE Animation_Serial_Test2(LEDSTRIP& strip, int msFreq, const char* s1, const char* s2){
  //Animation_Loop( animStatus, msFreq, [&](){
  EXIT_TYPE ret = Animation_Loop( animStatus, msFreq, INFINITE_LOOP, std::function<int()>([&]() -> int {
    static bool x = false;

    if(x){
      Serial.print("[ "); Serial.print( s1 ); Serial.print(" ]\r");
    }else{
      Serial.print("[ "); Serial.print( s2 ); Serial.print(" ]\r");
    }
    x = !x;

    strip.show(true);
    return EXIT_NORMAL;
  }));

  return ret;
}


/********************************        *********************************/

void Animation_SparkleHue(LEDSTRIP& strip, ANIM_STATUS &stateMon, int msFreq, uint8_t hueRange, uint8_t hue, uint8_t sat, uint8_t val)
{
  stateMon.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);

  for(;;){
    TickType_t  xLastWakeTime = xTaskGetTickCount();
  
    //decay all leds
    for(int i = 0; i < strip.effSize; i++){
    //  strip.scale(strip.pixels[i], 0.5 * 0xFF);
    }

    // Random Pixel 
    //int index = random(strip.effSize);
    //uint16_t h = (random(hueRange * 2) - hueRange) % HUE_MAX;
    //strip.setPixel(index, h, sat, val);

    strip.show(true);
    if(ulTaskNotifyTake( pdTRUE, msFreq - (xTaskGetTickCount() - xLastWakeTime ) == pdTRUE )){ return; } // delay
  }

  stateMon.busy = false;
}

void Animation_SparkleColor(LEDSTRIP& strip, ANIM_STATUS &stateMon, int msFreq, rgbpixel_t col)
{
  stateMon.busy = true;
  ulTaskNotifyTake( pdTRUE, 0);

  for(;;){
    TickType_t  xLastWakeTime = xTaskGetTickCount();

    //decay all leds
    for(int i = 0; i < strip.effSize; i++){
     // strip.scale(strip.pixels[i], 0.5 * 0xFF);
    }

    // Random Pixel 
    int index = random(strip.effSize);
    strip.pixels[index] = col;

    strip.show(true);
    if(ulTaskNotifyTake( pdTRUE, msFreq - (xTaskGetTickCount() - xLastWakeTime ) == pdTRUE )){ return; } // delay
  }

  stateMon.busy = false;
}



/********************************  test Functions  *********************************/
EXIT_TYPE Animation_SetPixel(LEDSTRIP& strip, ANIMATION_EVENT& event){
  return EXIT_NORMAL;
}

EXIT_TYPE Animation_Clear(LEDSTRIP& strip, ANIMATION_EVENT& event){
  return EXIT_NORMAL;
}








/********************************        *********************************/

// This takes into account that the web color picker hue is reversed
float GetHueRangeWeb(float hue1, float hue2){
  float rng;
  if (hue1 > hue2) { rng = hue1 - hue2; }
  else { rng = 360.0 + hue1 - hue2; }

  if (rng < 0.0) { rng += 360.0; }
  else if (rng > 360.0) { rng -= 360.0; }

  return rng;
}

float GetHueRange(float hue1, float hue2){
  float range = hue2 - hue1;
  if(hue2 < hue1){ range += 360.0; }
  //Log.noticeln("<anims> hue range: %d", hueRange);
  return range;
}



/********************************  SPEED CALC HELPER FUNCTIONS  *********************************/

void Init_Speed_Range(float startSpeed, float endSpeed, int steps){

}

float GetNextSpeed(void){
  return 0.0;
}



int CalcEventInterval(float speed, int min, int max){
  int range = max - min;
  return (max - ((speed/100.0) * range));
}

rgbpixel_t GetColorFromHue(int hue){
  if(hue == -1){
    return col_white;
  }else if(hue == -2){
    return col_black;
  }else if(hue >= 0 && hue <= 360){
    return HUEtoRGB(hue);
  }
  return col_black;
}