/* 
 Adapted from sutaburosu's AA_lines.ino code. Many thanks!
*/

#include <FastLED.h>

#define WIDTH 64
#define HEIGHT 26
#define NUM_LEDS (WIDTH * HEIGHT)

CRGB leds[NUM_LEDS + 1];

CRGBPalette16 rainbowPalette = {
  0xFF0000, 0x7F0000, 0xAB5500, 0x552A00, 0xABAB00, 0x555500, 0x00FF00, 0x007F00,
  0x00AB55, 0x00552A, 0x0000FF, 0x00007F, 0x5500AB, 0x2A0055, 0xAB0055, 0x55002A
};

CRGBPalette16 greenStripe = {
  0x00FF00, 0x00FF00, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
  0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000
};

CRGBPalette16 yellowStripe = {
  0xFFFF00, 0xFFFF00, 0xFFFF00, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
  0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000
};

CRGBPalette16 fadeFactorPalette = {
  0xFF0000, 0xFF0000, 0xFF0000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
  0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000
};


CRGBPalette16 whiteStripe = {
  0xFFFFFF, 0xFFFFFF, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000,
  0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000, 0x000000
};

typedef enum {
  RADAR,
  RADIATE,
  PAUSE,
  NONE
} FixMode ;

boolean _twist = false;

void setup() 
{
  FastLED.addLeds<NEOPIXEL, 19>(leds, NUM_LEDS);
  Serial.begin(115200);
  // Serial.print(sizeof(CRGB));
}

// This is probably not good programming, idk.
#define ENABLE_CROSSFADE true
#define DISABLE_CROSSFADE false
#define ANIM_FOREGROUND true
#define ANIM_BACKGROUND false

int ring_radius = 0;

void loop()
{
  int tslider = analogRead(33); // pot 1
  // int tslider = beatsin16(30, 512, 800);
  int rslider = analogRead(35); // pot 2
  int lslider = analogRead(36);  // pot 3
  float bpm = map(tslider,0,1024,0.0,120.0);
  // float rotationSpeedFloat = rotationSpeed;
  float mappedRSlider = fmap(rslider,0,1024,-8.0,8.0);
  int mappedLSlider = map(lslider,0,1024,0,20);

  sup(mappedRSlider,bpm,mappedLSlider,LavaColors_p, LINEARBLEND,ANIM_BACKGROUND,DISABLE_CROSSFADE);
  // sup(mappedRSlider,bpm,mappedLSlider,whiteStripe, LINEARBLEND,ANIM_FOREGROUND,DISABLE_CROSSFADE);

  // EVERY_N_SECONDS(3) {
  //   Serial.println("pulse");
  //   ring_radius = 1;
  // }

  // EVERY_N_MILLISECONDS(10) {
  //   if( ring_radius >= 1) {
  //     ring_radius++;
  //     for (byte y = 0; y < HEIGHT; y++) {
  //       leds[XY(ring_radius, y)] = CRGB::White;
  //     }
  //     if( ring_radius == WIDTH) {
  //       ring_radius = 0;
  //     }
  //     // Serial.println(ring_radius);
  //   }
  // }
  FastLED.show();
}

uint16_t XY(uint8_t x, uint8_t y) {
  if (x >= WIDTH) return NUM_LEDS;
  if (y >= HEIGHT) return NUM_LEDS;
  return y * WIDTH + x;
}

void crossfade(CRGB *a, const CRGB *b, uint8_t amount) {
  uint8_t rev = 255 - amount;
  a->red   = (a->red   * amount + b->red   * rev) >> 8;
  a->green = (a->green * amount + b->green * rev) >> 8;
  a->blue  = (a->blue  * amount + b->blue  * rev) >> 8;
}

void qaddColors(CRGB *a, const CRGB *b) {
  a->red   = qadd8(a->red,b->red);
  a->green = qadd8(a->green,b->green);
  a->blue  = qadd8(a->blue,b->blue);
}

// With thanks to Steve Dommett; github.com/sutaburosu
void sup(float rotationSpeed, float bpm, uint8_t lineWidth, CRGBPalette16 palette, TBlendType blendType, boolean foreground, boolean enableCrossfade) {
  uint32_t yHueDelta ;
  uint32_t xHueDelta ;
  static uint32_t lastMillis = 32767; // int16_t 32767/2  (for rotationspeed=0 test)
  static uint8_t lastIndex4bit = 15;
  float rotationSpeedFloat = fmap((float)rotationSpeed,(float)0,(float)255,0.0,60.0); // Between -6 and 6
  // float rotationSpeedFloat = rotationSpeed;
  // float rotationSpeedFloat = 0.0;
  FixMode fixMode = NONE;
  
  uint32_t ms = millis();

  if( fixMode == RADIATE ){
    lastMillis = 32767;  // yHueDelta = sin16(32767) = 0
    rotationSpeedFloat = 0.0;
  }
  if( fixMode == RADAR ){
    lastMillis = 16383;  // xHueDelta = cos16(16383) = 0
    rotationSpeedFloat = 0.0;
  }

  // (abs(sin(x))^3 * sin(x)

// https://math.stackexchange.com/questions/4014462/translating-a-sin-curve-into-one-that-lingers-on-its-maximums-longer

 if( rotationSpeedFloat != 0.0 ) {
    yHueDelta = (int32_t)sin16((int16_t)round(ms * rotationSpeedFloat)) * lineWidth;
    xHueDelta = (int32_t)cos16((int16_t)round(ms * rotationSpeedFloat)) * lineWidth;
    lastMillis = ms;
  } else {
    yHueDelta = (int32_t)sin16(lastMillis) * lineWidth;
    xHueDelta = (int32_t)cos16(lastMillis) * lineWidth;
  }

  
  int32_t startHue = beat88(round(bpm * 64));
  startHue = startHue << 7 ;
  int32_t lineStartHue = startHue - (HEIGHT + 2) / 2 * yHueDelta;
  // Serial.print(ms);
  // Serial.print(" * ");
  // Serial.print(mappedTranslationSpeed);
  // Serial.print(" = ");
  // Serial.print(startHue);
  // Serial.print(" linestart: ");
  // Serial.print(lineStartHue);
  // Serial.print(" linestart>>7: ");
  // Serial.print(lineStartHue >> 7);
  

  for (byte y = 0; y < HEIGHT; y++) {
    uint32_t pixelHue = lineStartHue - (64 + 2) / 2 * xHueDelta;
    lineStartHue += yHueDelta;
    // Serial.print(ms);
    // Serial.print("   pixHue32: ");
    // Serial.print("   ");
    // Serial.print(pixelHue);
    // Serial.print("   index16: ");
    uint16_t myIndex = pixelHue >> 7;
    // Serial.print(myIndex);
    uint8_t index_4bit = myIndex >> 12;
    // Serial.print("  index4: ");

    // if( lastIndex4bit != index_4bit ) {
    //   Serial.print(ms - lastMark);
    //   Serial.print("   ");
    //   Serial.print(index_4bit);
    //   Serial.println();
    //   lastIndex4bit = index_4bit;
    //   lastMark = ms;
    // }

    for (byte x = 0; x < WIDTH; x++) {
      leds[XY(x, y)] = ColorFromPaletteExtended(palette, pixelHue >> 7, 255, blendType);
      pixelHue += xHueDelta;
    }
    // if( y == 0 ) {
    //   uint16_t myIndex = pixelHue >> 7;
    //   uint8_t index_4bit = myIndex >> 12;
    //   if( index_4bit == 0 && !mark) {
    //       mark = true; // flag!
    //       uint32_t currentPaletteInterval = millis() - lastMark;
    //       Serial.println("Mark! Interval: " + (String)currentPaletteInterval + " lastMark: " + (String)lastMark);
    //       lastMark = millis();
    //   } else if( index_4bit != 0 && mark ) {
    //     mark = false; // reset flag
    //   }
    //   Serial.println(index_4bit);
    // }

    // if( ColorFromPaletteExtended(palette, pixelHue >> 7, 255, blendType) == CRGB::White ) {
    //   Serial.println(pixelHue >> 7);
    // }
  }
  delay(2);
}


// from: https://github.com/FastLED/FastLED/pull/202
CRGB ColorFromPaletteExtended(const CRGBPalette16& pal, uint16_t index, uint8_t brightness, TBlendType blendType) {
  // Extract the four most significant bits of the index as a palette index.
  uint8_t index_4bit = (index >> 12);
  // Calculate the 8-bit offset from the palette index.
  uint8_t offset = (uint8_t)(index >> 4);
  // Get the palette entry from the 4-bit index
  const CRGB* entry = &(pal[0]) + index_4bit;
  uint8_t red1   = entry->red;
  uint8_t green1 = entry->green;
  uint8_t blue1  = entry->blue;

  uint8_t blend = offset && (blendType != NOBLEND);
  if (blend) {
    if (index_4bit == 15) {
      entry = &(pal[0]);
    } else {
      entry++;
    }

    // Calculate the scaling factor and scaled values for the lower palette value.
    uint8_t f1 = 255 - offset;
    red1   = scale8_LEAVING_R1_DIRTY(red1,   f1);
    green1 = scale8_LEAVING_R1_DIRTY(green1, f1);
    blue1  = scale8_LEAVING_R1_DIRTY(blue1,  f1);

    // Calculate the scaled values for the neighbouring palette value.
    uint8_t red2   = entry->red;
    uint8_t green2 = entry->green;
    uint8_t blue2  = entry->blue;
    red2   = scale8_LEAVING_R1_DIRTY(red2,   offset);
    green2 = scale8_LEAVING_R1_DIRTY(green2, offset);
    blue2  = scale8_LEAVING_R1_DIRTY(blue2,  offset);
    cleanup_R1();

    // These sums can't overflow, so no qadd8 needed.
    red1   += red2;
    green1 += green2;
    blue1  += blue2;
  }
  if (brightness != 255) {
    // nscale8x3_video(red1, green1, blue1, brightness);
    nscale8x3(red1, green1, blue1, brightness);
  }
  return CRGB(red1, green1, blue1);
}

float fmap(float x, float a, float b, float c, float d)
{
      float f=x/(b-a)*(d-c)+c;
      return f;
}

String uint64ToString(uint64_t input) {
  String result = "";
  uint8_t base = 10;

  do {
    char c = input % base;
    input /= base;

    if (c < 10)
      c +='0';
    else
      c += 'A' - 10;
    result = c + result;
  } while (input);
  return result;
}