#include "ws2812b_writer.h"

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "esp_log.h"
#include "pattern/twinkle.h"

const int NUMBER_PARAMS =
#define STATE_PARAM(...) +1
#include "state_params.i"
#undef STATE_PARAM
    ;

#define SIZE 300
/* 60 FPS -> ~16.67ms per frame. Keep time scaling aligned. */
#define FRAME_TIME_DELTA 167

/* Returns round(sin(2πn / 256) * 127.5 + 127.5). */
uint8_t byte_sin(uint8_t n);

static inline uint8_t byte_scale(uint8_t n, uint8_t sc)
{
  return n * sc / 255;
}

#define min(a, b) (a) < (b) ? (a) : (b)
#define max(a, b) (a) > (b) ? (a) : (b)

/* returns (in / 256)^n * 256. */
uint8_t amp(uint8_t in, uint8_t n);

/* Per-channel calibration to balance the LEDs (boost red, soften green/blue).
 */
#define CAL_R_NUM 140 /* 1.40x */
#define CAL_G_NUM 90  /* 0.90x */
#define CAL_B_NUM 90  /* 0.90x */
#define CAL_DEN 100

void reset_parameters(ws_params_t* params)
{
  if (params->state_lock) {
    xSemaphoreTake(params->state_lock, portMAX_DELAY);
  }
  memset(&params->state, 0, sizeof(params->state));

#define STATE_PARAM(type, attr, displ, def, ...) params->state.attr = def;
#include "state_params.i"
#undef STATE_PARAM
  if (params->state_lock) {
    xSemaphoreGive(params->state_lock);
  }
}

struct rgba {
  uint8_t r;
  uint8_t g;
  uint8_t b;
  uint8_t a;
};

uint32_t asi(struct rgba rgb)
{
  return (rgb.r << 16) | (rgb.g << 8) | rgb.b;
}
struct rgba get_rgba(int x, int64_t time, const ws_state_t* state)
{
  uint32_t red = 0, green = 0, blue = 0;

  uint8_t brightness = state->brightness;
  if (!state->use_static_color) {
    red = byte_scale(
              byte_sin((uint8_t)(time / 1000 + x * 4)), 255 - state->n_red) +
          state->n_red;
    green = 255 - red;

    if (state->cool) {
      uint32_t tmp = green;
      green = blue;
      blue = tmp;
    }

    /* Add a little white flair that comes around every once in a while. */

    uint32_t whitesum = 0;
    if (state->n_snow) {
      uint32_t white[] = {
          /* Parallax "snow" */
          time / 179 + x * 8,
          time / 193 + x * 12,
          time / 211 + x * 16,
          time / 233 + x * 8,
          // (state->time + 128) / 233 + x * 8,
      };

      for (int i = 0; i < sizeof(white) / sizeof(uint32_t); ++i) {
        if ((white[i] / 256) % state->n_snow == 0) {
          white[i] = amp(byte_sin(white[i]), 20);
        } else {
          white[i] = 0;
        }
        whitesum += white[i];
      }
    }

    red = min(red + whitesum, 255);
    green = min(green + whitesum, 255);
    blue = min(blue + whitesum, 255);
  } else {
    red = (state->static_color >> 16) & 0xff;
    green = (state->static_color >> 8) & 0xff;
    blue = state->static_color & 0xff;
  }

  if (state->invert) {
    red = 255 - red;
    green = 255 - green;
    blue = 255 - blue;
  }

  /* Channel calibration to balance perceived brightness. */
  red = min((red * CAL_R_NUM) / CAL_DEN, 255);
  green = min((green * CAL_G_NUM) / CAL_DEN, 255);
  blue = min((blue * CAL_B_NUM) / CAL_DEN, 255);

  uint8_t twink = 0xff;
  if (state->twinkle) {
    twink = twinkle(time / 300, x, state->twink_amt);
  }

  red = byte_scale(
      red, max(byte_scale(brightness, twink), state->base_brightness));
  green = byte_scale(
      green, max(byte_scale(brightness, twink), state->base_brightness));
  blue = byte_scale(
      blue, max(byte_scale(brightness, twink), state->base_brightness));

  return (struct rgba){red, green, blue, 0xff};
}

/* return a new color which is an interoplation between c1 and c2 by the
 * provided amount. The semantic amount is amt / 255.0. */
struct rgba interpolate(struct rgba c1, struct rgba c2, uint8_t amt)
{
#define scale(a, b, s) ((a * (255 - s) + b * s) / 255)

  return (struct rgba){scale(c1.r, c2.r, amt),
                       scale(c1.g, c2.g, amt),
                       scale(c1.b, c2.b, amt),
                       0xff};
#undef scale
}

void calculate_colors(const ws_state_t* state, ws2812b_buffer_t* buffer)
{
  for (int i = 0; i < SIZE; ++i) {
    int x_scale = state->x_scale ? state->x_scale : 1;
    int x = i * 255 / x_scale;

    if (!state->power) {
      ws2812b_buffer_set_rgb(buffer, i, 0, 0, 0);
      continue;
    }
    struct rgba c;

    int64_t real_time = state->time * FRAME_TIME_DELTA;
    int32_t speed_scaled = ((int32_t)state->speed * 1000) / 255;
    if (speed_scaled % 1000 == 0) {
      // speed = 1000 is the "canonical" time speed. If speed is divisible by
      // 1000, then no interpolation is required.
      c = get_rgba(x, (real_time * speed_scaled) / 1000, state);
    } else {
      int64_t adj_time = real_time * speed_scaled;
      int64_t bot = adj_time / 1000;
      int64_t top =
          adj_time / 1000 + FRAME_TIME_DELTA * (adj_time >= 0 ? 1 : -1);
      int64_t mod = adj_time % 1000;
      if (mod < 0) {
        mod += 1000;
      }
      uint8_t interp = (uint8_t)((mod * 255) / 1000);

      struct rgba c1 = get_rgba(x, bot, state);
      struct rgba c2 = get_rgba(x, top, state);

      if (i == 5 && asi(c1) > 0) {
        ESP_LOGI(
            "main",
            "%06x %06x %d = %06x",
            asi(c1),
            asi(c2),
            interp,
            asi(interpolate(c1, c2, interp)));
      }

      c = interpolate(c1, c2, interp);
    }

    ws2812b_buffer_set_rgb(buffer, i, c.r, c.g, c.b);
  }
}

portTASK_FUNCTION(ws2812b_write_task, params)
{
  ws_params_t* ws_params = (ws_params_t*)params;

  ws2812b_buffer_t* buffer = ws2812b_new_buffer(SIZE);
  TickType_t last_wake = xTaskGetTickCount();
  const TickType_t period = pdMS_TO_TICKS(17); /* ~60 FPS */

  for (;;) {
    // Copy the parameters to avoid a change occuring during the calculation.
    ws_state_t state_snapshot;
    if (xSemaphoreTake(ws_params->state_lock, portMAX_DELAY) != pdTRUE) {
      vTaskDelayUntil(&last_wake, period);
      continue;
    }
    state_snapshot = ws_params->state;
    xSemaphoreGive(ws_params->state_lock);

    calculate_colors(&state_snapshot, buffer);
    if (ws2812b_write(ws_params->drv, buffer) != ESP_OK) {
      /* If we fail to write, back off briefly instead of hammering the SPI
       * bus, which can otherwise cause visible flicker. */
      vTaskDelayUntil(&last_wake, period);
      continue;
    }
    /* Advance time once per frame. */
    if (xSemaphoreTake(ws_params->state_lock, portMAX_DELAY) == pdTRUE) {
      ws_params->state.time += 1;
      xSemaphoreGive(ws_params->state_lock);
    }
    vTaskDelayUntil(&last_wake, period);
  }
}

uint8_t sintable[256] = {
    128, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 162, 165, 167, 170,
    173, 176, 179, 182, 185, 188, 190, 193, 196, 198, 201, 203, 206, 208, 211,
    213, 215, 218, 220, 222, 224, 226, 228, 230, 232, 234, 235, 237, 238, 240,
    241, 243, 244, 245, 246, 248, 249, 250, 250, 251, 252, 253, 253, 254, 254,
    254, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 252, 251,
    250, 250, 249, 248, 246, 245, 244, 243, 241, 240, 238, 237, 235, 234, 232,
    230, 228, 226, 224, 222, 220, 218, 215, 213, 211, 208, 206, 203, 201, 198,
    196, 193, 190, 188, 185, 182, 179, 176, 173, 170, 167, 165, 162, 158, 155,
    152, 149, 146, 143, 140, 137, 134, 131, 128, 124, 121, 118, 115, 112, 109,
    106, 103, 100, 97,  93,  90,  88,  85,  82,  79,  76,  73,  70,  67,  65,
    62,  59,  57,  54,  52,  49,  47,  44,  42,  40,  37,  35,  33,  31,  29,
    27,  25,  23,  21,  20,  18,  17,  15,  14,  12,  11,  10,  9,   7,   6,
    5,   5,   4,   3,   2,   2,   1,   1,   1,   0,   0,   0,   0,   0,   0,
    0,   1,   1,   1,   2,   2,   3,   4,   5,   5,   6,   7,   9,   10,  11,
    12,  14,  15,  17,  18,  20,  21,  23,  25,  27,  29,  31,  33,  35,  37,
    40,  42,  44,  47,  49,  52,  54,  57,  59,  62,  65,  67,  70,  73,  76,
    79,  82,  85,  88,  90,  93,  97,  100, 103, 106, 109, 112, 115, 118, 121,
    124,
};

uint8_t byte_sin(uint8_t n)
{
  return sintable[n];
}

uint8_t amp(uint8_t in, uint8_t n)
{
  uint32_t out = in;

  for (int i = 0; i < n; ++i) {
    out = (out * in) / 256;
  }

  return min(out, 255);
}