#include <Arduino.h>
#include <FastLED.h>
#include <ESP32Servo.h>
#include "palettes.h"

#define SERVO_PIN 15

#define SERVO_CLOSED 85
#define SERVO_OPEN 40

#define BRIGHTNESS_CLOSED 100
#define BRIGHTNESS_OPEN 255

#define LED_PIN 18
#define NUM_LEDS 8

int servoPosition = SERVO_CLOSED;

CRGBPalette16 getPalette(int paletteIndex);

CRGB leds[NUM_LEDS]; // create an array of LED pixels
Servo servo;

int servoToBrightness(int servo)
{
  int brightness = map(servo, SERVO_CLOSED, SERVO_OPEN, 0, 100);
  brightness = brightness * brightness;
  brightness = map(brightness, 0, 100 * 100, BRIGHTNESS_CLOSED, BRIGHTNESS_OPEN);
  return brightness;
}

void setup()
{
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS); // initialize FastLED
  servo.attach(SERVO_PIN);
  servo.write(servoPosition);
  for (int i = 0; i < NUM_LEDS; i++)
  {
    leds[i] = CRGB::Purple;
  }
  FastLED.show();
}

void loop()
{
  // put your main code here, to run repeatedly:
  delay(50);                                            // this speeds up the simulation
  int baseWave = beatsin8(5, SERVO_OPEN, SERVO_CLOSED); // Base sine wave
  int stepHeight = 3;
  int stepWave1 = beatsin8(15, -stepHeight, stepHeight);         // Twice the frequency, smaller amplitude
  int stepWave2 = beatsin8(15, -stepHeight / 2, stepHeight / 2); // Even higher frequency, smaller amplitude

  // servoPosition = baseWave + stepWave1 + stepWave2;
  // servoPosition = max(SERVO_OPEN, min(servoPosition, SERVO_CLOSED));
  servoPosition = baseWave;

  // depending on the servo position, select the palette and brightness to write to array leds,
  // fully closed should a random palette from _magentaPalettes, and fully open from _yellowPalettes.
  // The brightness should be mapped from SERVO_CLOSED to SERVO_OPEN to BRIGHTNESS_CLOSED to BRIGHTNESS_OPEN using servoToBrightness
  // Blending between palettes should be done using blendPaletteTowardPalette or FastLED::blend
  
  // Assuming _magentaPalettes and _yellowPalettes are arrays of CRGBPalette16
  CRGBPalette16 openPalette;
  CRGBPalette16 closedPalette;
  CRGBPalette16 currentPalette;
  

  if (servoPosition == SERVO_CLOSED) {
    // Select a random palette from _magentaPalettes
    int paletteIndex = random(sizeof(_magentaPalettes) / sizeof(_magentaPalettes[0]));
    openPalette = getPalette(_magentaPalettes[paletteIndex]);
    Serial.println(paletteIndex);
  } else if (servoPosition == SERVO_OPEN) {
    // Select a random palette from _yellowPalettes
    int paletteIndex = random(sizeof(_yellowPalettes) / sizeof(_yellowPalettes[0]));
    closedPalette = getPalette(_yellowPalettes[paletteIndex]);
    Serial.println(paletteIndex);
  }

  int8_t blendAmount = map(servoPosition, SERVO_CLOSED, SERVO_OPEN, 0, 255);

  // Blend towards the target palette 
  currentPalette = blend(openPalette, closedPalette, leds, NUM_LEDS, blendAmount);

  // Write the palette and brightness to the leds
  for (int i = 0; i < NUM_LEDS; i++) {
    leds[i] = ColorFromPalette(currentPalette, i, servoToBrightness(servoPosition));
  }

  FastLED.show();
  servo.write(servoPosition);
}


CRGBPalette16 getPalette(int paletteIndex) {
    if( paletteIndex < NUM_PALETTES ) {
      return _palettes[paletteIndex]; // this is kinda nasty
    } else if( paletteIndex >= NUM_PALETTES && paletteIndex < TOTAL_PALETTES ) {
      return gGradientPalettes[paletteIndex-NUM_PALETTES]; // this is kinda nasty
    }
    return _palettes[0]; // safe default
}