// x-run: make img2cpi CC=clang
// x-run: ~/scripts/runc.sh % -Wall -Wextra -lm --- ~/images/boykisser.png cpi-images/boykisser.cpi
#include <math.h>
#include <assert.h>
#include <stb/stb_image.h>
#include <stb/stb_image_resize2.h>
#include <argp.h>
#include <stdio.h>
#include <getopt.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <strings.h>
#include <stdbool.h>
#include "cc-common.h"
#ifdef USE_OPENMP
#include <omp.h>
#endif

#define MAX_COLOR_DIFFERENCE 768
#define K_MEANS_ITERATIONS 4

struct cc_char {
  unsigned char character;
  unsigned char bg, fg;
};

struct arguments {
  bool fast_mode;
  int width, height;
  enum cpi_version {
    CPI_VERSION_AUTO,
    CPI_VERSION_RAW,
    CPI_VERSION_0,
    CPI_VERSION_1,
    CPI_VERSION_2,
  } cpi_version;
  enum placement {
    PLACEMENT_CENTER,
    PLACEMENT_COVER,
    PLACEMENT_TILE,
    PLACEMENT_FULL,
    PLACEMENT_EXTEND,
    PLACEMENT_FILL
  } placement;
  enum palette_type {
    PALETTE_DEFAULT,
    PALETTE_DEFAULT_GRAY,
    PALETTE_AUTO,
    PALETTE_PATH,
    PALETTE_LIST
  } palette_type;
  char *palette;
  char *input_path;
  char *output_path;
} args = {
  .fast_mode = false,
  .width = 4 * 8 - 1, // 4x3 blocks screen
  .height = 3 * 6 - 2,
  .cpi_version = CPI_VERSION_AUTO,
  .placement = PLACEMENT_FULL,
  .input_path = NULL,
  .output_path = NULL,
  .palette = NULL,
  .palette_type = PALETTE_DEFAULT // TODO(kc): change to PALETTE_AUTO when
                                  // k-means is implemented
};

struct image {
  int w, h;
  union color *pixels;
};

struct image_pal {
  int w, h;
  uint8_t *pixels;
  const struct palette *palette;
};

struct k_means_state {
  const struct image *items;
  struct palette *clusters;
  uint8_t *predicted_cluster;
  struct k_means_centroid_intermediate {
    struct {
      float r, g, b;
    } sums;
    size_t count;
    union color closest_present_item;
    float closest_present_distance;
  } *centroid_intermediate;
  size_t item_count;
};

bool parse_cmdline(int argc, char **argv);
void show_help(const char *progname, bool show_all, FILE *fp);

struct image *image_load(const char *fp);
struct image *image_new(int w, int h);
struct image *image_resize(struct image *original, int new_w, int new_h);
struct image_pal *image_quantize(struct image *original, const struct palette *palette);
void image_unload(struct image *img);

float get_color_difference(union color a, union color b);
float get_color_brightness(union color clr);

void get_size_keep_aspect(int w, int h, int dw, int dh, int *ow, int *oh);

void convert_2x3(const struct image_pal *img, struct cc_char *characters);
void convert_6x9(const struct image_pal *img, struct cc_char *characters);

int save_cpi_0(FILE *fp, const struct palette *pal, const struct cc_char *chars, int w, int h);
int save_cpi_1(FILE *fp, const struct palette *pal, const struct cc_char *chars, int w, int h);

// Only one global custom palette is maintained
struct palette *custom_palette_resize(uint8_t size);
struct palette *custom_palette_from(const struct palette *orig);

struct k_means_state k_means_init(const struct image *image, struct palette *starting_palette);
bool k_means_iteration(struct k_means_state *state);
void k_means_end(struct k_means_state *state);
struct palette *palette_k_means(const struct image *image, const struct palette *prototype);

const char *known_file_extensions[] = {
  ".png", ".jpg", ".jpeg", ".jfif", ".jpg", ".gif",
  ".tga", ".bmp", ".hdr", ".pnm", 0
};

static const struct optiondocs {
  char shortopt;
  char *longopt;
  char *target;
  char *doc;
  struct optiondocs_choice { char *value; char *doc; } *choices;
} optiondocs[] = {
  { 'h', "help",        0,            "Show help", 0 },
  { 'f', "fast",        0,            "Use fast (old) method for picking characters and colors", 0 },
  { 'W', "width",       "width",      "Width in characters", 0 },
  { 'h', "height",      "height",     "Height in characters", 0 },
  { 'P', "palette",     "palette",    "Use specific palette.\n"
             "                        `auto` uses automatic selection\n"
             "                        `default` uses default palette\n"
             "                        `defaultgray` uses default grayscale palette\n"
             "                        `list:#RRGGBB,#RRGGBB,...` uses hard-coded one\n"
             "                        `txt:PATH` reads hex colors from each line in a file\n", 0 },
  { 'V', "cpi_version", "version",    "Force specific version of CPI",
    (struct optiondocs_choice[]) {
      { "-2,raw", "Use raw format. No headers, no palette, just characters and colors" },
      { "-1,auto", "Choose best available" },
      { "0", "OG CPI, 255x255, uncompressed" },
      { "1", "CPIv1, huge images, uncompressed" },
      { "255", "In-dev version, may not work" },
      { 0, 0 } } },
  { 'p', "placement",   "placement",  "Image placement mode (same as in hsetroot)",
    (struct optiondocs_choice[]){
      { "center", "Render image centered on the canvas" },
      { "cover",  "Centered on screen, scaled to fill fully" },
      { "tile",   "Render image tiled" },
      { "full",   "Use maximum aspect ratio" },
      { "extend", "Same as \"full\", but filling borders" },
      { "fill",   "Stretch to fill" },
      { 0, 0 } } },
  { 0,   0,             "input.*",    "Input file path", 0 },
  { 0,   0,             "output.cpi", "Output file path", 0 },
  { 0 }
};

int main(int argc, char **argv) {
  if (!parse_cmdline(argc, argv)) {
    show_help(argv[0], false, stderr);
    fprintf(stderr, "Fatal error occurred, exiting.\n");
    return EXIT_FAILURE;
  }
  struct image *src_image = image_load(args.input_path);
  if (!src_image) {
    fprintf(stderr, "Error: failed to open the file\n");
    return EXIT_FAILURE;
  }

  struct image *canvas;
  if (args.fast_mode) {
    canvas = image_new(args.width * 2, args.height * 3);
  } else {
    canvas = image_new(args.width * 6, args.height * 9);
  }

  if (!canvas) {
    fprintf(stderr, "Error: failed to allocate second image buffer\n");
    return EXIT_FAILURE;
  }

  // TODO: load palette, maybe calculate it too? k-means?
  const struct palette *palette = &cc_default_palette;
  switch (args.palette_type) {
    case PALETTE_DEFAULT: palette = &cc_default_palette; break;
    case PALETTE_DEFAULT_GRAY: palette = &cc_default_gray_palette; break;
    case PALETTE_AUTO: palette = palette_k_means(src_image, &cc_default_palette); break;
    case PALETTE_LIST: assert(0 && "Not implemented"); break;
    case PALETTE_PATH: assert(0 && "Not implemented"); break;
    default: assert(0 && "Unreachable");
  }

  // TODO: properly scale
  struct image *scaled_image;
  {
    int new_w, new_h;
    get_size_keep_aspect(src_image->w, src_image->h, canvas->w, canvas->h, &new_w, &new_h);

    scaled_image = image_resize(src_image, new_w, new_h);
    if (!scaled_image) {
      fprintf(stderr, "Error: failed to open the file\n");
      return EXIT_FAILURE;
    }
  }

  // TODO: position image properly
  int small_w = scaled_image->w < canvas->w ? scaled_image->w : canvas->w;
  int small_h = scaled_image->h < canvas->h ? scaled_image->h : canvas->h;
  for (int y = 0; y < small_h; y++) {
    memcpy(&canvas->pixels[y * canvas->w],
           &scaled_image->pixels[y * scaled_image->w],
           small_w * sizeof(union color));
  }

  // TODO: actually do stuff
  struct cc_char *characters = calloc(args.width * args.height, sizeof(struct cc_char));

  struct image_pal *quantized_image = image_quantize(canvas, palette);
  if (!quantized_image) {
    fprintf(stderr, "Error: failed to open the file\n");
    return EXIT_FAILURE;
  }

  if (args.fast_mode) {
    convert_2x3(quantized_image, characters);
  } else {
    convert_6x9(quantized_image, characters);
  }

  FILE *fp = fopen(args.output_path, "wb");
  if (args.width < 256) {
    save_cpi_0(fp, palette, characters, args.width, args.height);
  } else {
    save_cpi_1(fp, palette, characters, args.width, args.height);
  }
  fclose(fp);

  image_unload(src_image);
  image_unload(canvas);
  return EXIT_SUCCESS;
}

int _write_palette_full(FILE *fp, const struct palette *pal) {
  int written = 0;
  assert(pal->count == 16 && "Invalid palette size");
  for (int i = 0; i < 16; i++) {
    written += fputc(pal->colors[i].rgba.r, fp);
    written += fputc(pal->colors[i].rgba.g, fp);
    written += fputc(pal->colors[i].rgba.b, fp);
  }
  return written;
}

int _write_pixeldata_v0(FILE *fp, const struct cc_char *chars, int w, int h) {
  int written = 0;
  for (int i = 0; i < w * h; i++) {
    written += fputc(chars[i].character, fp);
    written += fputc(chars[i].bg | (chars[i].fg << 4), fp);
  }
  return written;
}

int save_cpi_0(FILE *fp, const struct palette *pal, const struct cc_char *chars, int w, int h) {
  int written = 0;
  written += fwrite("CCPI", 1, 4, fp);
  written += fputc(w, fp);
  written += fputc(h, fp);
  written += fputc(0x00, fp);
  written += _write_palette_full(fp, pal);
  written += _write_pixeldata_v0(fp, chars, w, h);
  return written;
}

int save_cpi_1(FILE *fp, const struct palette *pal, const struct cc_char *chars, int w, int h) {
  int written = 0;
  written += fwrite("CPI\x01", 1, 4, fp);
  written += write_varint(fp, w);
  written += write_varint(fp, h);
  written += _write_palette_full(fp, pal);
  written += _write_pixeldata_v0(fp, chars, w, h);
  return written;
}

bool parse_cmdline(int argc, char **argv) {
  static struct option options[] = {
    { "help",         no_argument,        0, 'h' },
    { "fast",         no_argument,        0, 'f' },
    { "width",        required_argument,  0, 'W' },
    { "height",       required_argument,  0, 'H' },
    { "cpi_version",  required_argument,  0, 'V' },
    { "placement",    required_argument,  0, 'p' },
    { "palette",      required_argument,  0, 'P' },
    { 0,              0,                  0,  0  }
  };

  while (true) {
    int option_index = 0;
    int c = getopt_long(argc, argv, "hfW:H:V:p:P:", options, &option_index);
    if (c == -1) break;
    if (c == 0) c = options[option_index].val;
    if (c == '?') break;

    switch (c) {
      case 'h': // --help
        show_help(argv[0], true, stdout);
        exit(EXIT_SUCCESS);
        break;
      case 'f': // --fast
        args.fast_mode = true;
        if (args.cpi_version != CPI_VERSION_AUTO) {
          fprintf(stderr, "Warning: text mode ignores version\n");
        }
        break;
      case 'W': // --width
        args.width = atoi(optarg);
        break;
      case 'H': // --height
        args.height = atoi(optarg);
        break;
      case 'V': // --cpi_version
        {
          if (0 == strcmp(optarg, "auto") || 0 == strcmp(optarg, "-1")) {
            args.cpi_version = CPI_VERSION_AUTO;
          } else if (0 == strcmp(optarg, "raw") || 0 == strcmp(optarg, "-2")) {
            args.cpi_version = CPI_VERSION_RAW;
          } else if (0 == strcmp(optarg, "0")) {
            args.cpi_version = CPI_VERSION_0;
          } else if (0 == strcmp(optarg, "1")) {
            args.cpi_version = CPI_VERSION_1;
          } else if (0 == strcmp(optarg, "2")) {
            args.cpi_version = CPI_VERSION_2;
          }
        }
        break;
      case 'p': // --placement
        if (0 == strcmp(optarg, "center")) {
          args.placement = PLACEMENT_CENTER;
        } else if (0 == strcmp(optarg, "cover")) {
          args.placement = PLACEMENT_COVER;
        } else if (0 == strcmp(optarg, "tile")) {
          args.placement = PLACEMENT_TILE;
        } else if (0 == strcmp(optarg, "full")) {
          args.placement = PLACEMENT_FULL; }
        else if (0 == strcmp(optarg, "extend")) {
          args.placement = PLACEMENT_EXTEND;
        } else if (0 == strcmp(optarg, "fill")) {
          args.placement = PLACEMENT_FILL;
        } else {
          fprintf(stderr, "Error: invaild placement %s\n", optarg);
          return false;
        }
        break;
      case 'P': // --palette
        if (0 == strcmp(optarg, "default")) {
          args.palette_type = PALETTE_DEFAULT;
        } else if (0 == strcmp(optarg, "defaultgray")) {
          args.palette_type = PALETTE_DEFAULT_GRAY;
        } else if (0 == strcmp(optarg, "auto")) {
          args.palette_type = PALETTE_AUTO;
        } else if (0 == strncmp(optarg, "list:", 5)) {
          args.palette_type = PALETTE_LIST;
          args.palette = &optarg[5];
        } else {
          fprintf(stderr, "Error: invaild palette %s\n", optarg);
          return false;
        }
        break;
    }
  }

  if (optind == argc) {
    fprintf(stderr, "Error: no input file provided\n");
    return false;
  } else if (optind + 1 == argc) {
    fprintf(stderr, "Error: no output file provided\n");
    return false;
  } else if ((argc - optind) != 2) {
    fprintf(stderr, "Error: too many arguments\n");
    return false;
  }

  args.input_path = argv[optind];
  args.output_path = argv[optind + 1];

  const char *extension = strrchr(args.input_path, '.');
  if (!extension) {
    fprintf(stderr, "Warning: no file extension, reading may fail!\n");
  } else {
    bool known = false;
    for (int i = 0; known_file_extensions[i] != 0; i++) {
      if (0 == strcasecmp(known_file_extensions[i], extension)) {
        known = true;
        break;
      }
    }
    if (!known) {
      fprintf(stderr, "Warning: unknown file extension %s, reading may fail!\n", extension);
    }
  }

  return true;
}

void show_help(const char *progname, bool show_all, FILE *fp) {
  fprintf(fp, "usage: %s", progname);
  for (int i = 0; optiondocs[i].doc != 0; i++) {
    struct optiondocs doc = optiondocs[i];
    fprintf(fp, " [");
    if (doc.shortopt) fprintf(fp, "-%c", doc.shortopt);
    if (doc.shortopt && doc.longopt) fprintf(fp, "|");
    if (doc.longopt) fprintf(fp, "--%s", doc.longopt);
    if (doc.target) {
      if (doc.shortopt || doc.longopt) fprintf(fp, " ");
      fprintf(fp, "%s", doc.target);
    }
    fprintf(fp, "]");
  }
  fprintf(fp, "\n");

  if (!show_all) return;

  fprintf(fp, "\n");
  fprintf(fp, "ComputerCraft Palette Image converter\n");
  fprintf(fp, "\n");
  fprintf(fp, "positional arguments:\n");
  for (int i = 0; optiondocs[i].doc != 0; i++) {
    struct optiondocs doc = optiondocs[i];
    if (!doc.shortopt && !doc.longopt) {
      fprintf(fp, "  %s\t%s\n", doc.target, doc.doc);
    }
  }
  fprintf(fp, "\n");
  fprintf(fp, "options:\n");
  for (int i = 0; optiondocs[i].doc != 0; i++) {
    struct optiondocs doc = optiondocs[i];
    if (!doc.shortopt && !doc.longopt) { continue; }
    fprintf(fp, "  ");
    int x = 2;
    if (doc.shortopt) { fprintf(fp, "-%c", doc.shortopt); x += 2; }
    if (doc.shortopt && doc.longopt) { fprintf(fp, ", "); x += 2; }
    if (doc.longopt) { fprintf(fp, "--%s", doc.longopt); x += strlen(doc.longopt) + 2; }
    if (doc.choices) {
      fprintf(fp, " {");
      for (int j = 0; doc.choices[j].value != 0; j++) {
        if (j > 0) { fprintf(fp, ","); x += 1; }
        fprintf(fp, "%s", doc.choices[j].value);
        x += strlen(doc.choices[j].value);
      }
      fprintf(fp, "}");
      x += 3;
    } else if (doc.target) {
      fprintf(fp, " ");
      fprintf(fp, "%s", doc.target);
      x += strlen(doc.target) + 1;
    }
    if (x > 24) fprintf(fp, "\n%24c", ' ');
    else fprintf(fp, "%*c", 24 - x, ' ');

    fprintf(fp, "%s\n", doc.doc);

    if (doc.choices) {
      for (int j = 0; doc.choices[j].value != 0; j++) {
        fprintf(fp, "%26c", ' ');
        if (doc.shortopt) fprintf(fp, "-%c ", doc.shortopt);
        else if (doc.longopt) fprintf(fp, "--%s", doc.longopt);
        fprintf(fp, "%-12s %s\n", doc.choices[j].value, doc.choices[j].doc);
      }
    }
  }
}

struct image *image_load(const char *fp) {
  struct image *img = calloc(1, sizeof(struct image));
  if (!img) return NULL;
  img->pixels = (union color*)stbi_load(fp, &img->w, &img->h, 0, 4);
  if (!img->pixels) {
    free(img);
    return NULL;
  }
  return img;
}

struct image *image_new(int w, int h) {
  struct image *img = calloc(1, sizeof(struct image));
  if (!img) return NULL;
  img->pixels = calloc(h, sizeof(union color) * w);
  img->w = w;
  img->h = h;
  if (!img->pixels) {
    free(img);
    return NULL;
  }
  return img;
}

struct image *image_resize(struct image *original, int new_w, int new_h) {
  struct image *resized = image_new(new_w, new_h);
  if (!resized) return NULL;
  stbir_resize_uint8_srgb((unsigned char *)original->pixels, original->w, original->h, 0,
                          (unsigned char *)resized->pixels, resized->w, resized->h, 0,
                          STBIR_RGBA);
  return resized;
}

void image_unload(struct image *img) {
  free(img->pixels);
  free(img);
}

void get_size_keep_aspect(int w, int h, int dw, int dh, int *ow, int *oh)
{
  *ow = dw;
  *oh = dh;
  float ratio = (float)w / (float)h;
  float ratio_dst = (float)dw / (float)dh;
  int tmp_1, tmp_2;
  if (ratio_dst >= ratio)
  {
    tmp_1 = floor(dh * ratio);
    tmp_2 = ceil(dh * ratio);
    if (fabsf(ratio - (float)tmp_1 / dh) < fabsf(ratio - (float)tmp_2 / dh))
      *ow = tmp_1 < 1 ? 1 : tmp_1;
    else
      *ow = tmp_2 < 1 ? 1 : tmp_2;
  }
  else
  {
    tmp_1 = floor(dw / ratio);
    tmp_2 = ceil(dw / ratio);
    if (tmp_2 == 0 ||
        fabs(ratio - (float)dw / tmp_1) < fabs(ratio - (float)dw / tmp_2))
      (*oh) = tmp_1 < 1 ? 1 : tmp_1;
    else
      (*oh) = tmp_2 < 1 ? 1 : tmp_2;
  }
}

struct image_pal *image_quantize(struct image *original, const struct palette *palette) {
  struct image_pal *out = calloc(1, sizeof(struct image_pal));
  out->w = original->w;
  out->h = original->h;
  out->pixels = calloc(original->w, original->h);
  out->palette = palette;

  for (int i = 0; i < out->w * out->h; i++) {
    int closest_color = 0;
    float closest_distance = 1e20;
    for (int color = 0; color < palette->count; color++) {
      float dist = get_color_difference(palette->colors[color], original->pixels[i]);
      if (dist <= closest_distance) {
        closest_distance = dist;
        closest_color = color;
      }
    }
    out->pixels[i] = closest_color;
  }

  return out;
}

float get_color_difference(union color a, union color b) {
  int dr = a.rgba.r - b.rgba.r,
      dg = a.rgba.g - b.rgba.g,
      db = a.rgba.b - b.rgba.b;
  return dr * dr + dg * dg + db * db;
}

float get_color_brightness(union color clr) {
  return get_color_difference(clr, (union color){ .v = 0 });
}

void convert_2x3(const struct image_pal *img, struct cc_char *characters) {
  int w = img->w / 2, h = img->h / 3;
  for (int y = 0; y < h; y++) {
    for (int x = 0; x < w; x++) {
      unsigned char darkest_i = 0, brightest_i = 0;
      float darkest_diff = 0xffffff, brightest_diff = 0;

      for (int oy = 0; oy < 3; oy++) {
        for (int ox = 0; ox < 2; ox++) {
          unsigned char pix = img->pixels[ox + (x + (y * 3 + oy) * w) * 2];
          float brightness = get_color_brightness(img->palette->colors[pix]);
          if (brightness >= brightest_diff) {
            brightest_i = pix;
            brightest_diff = brightness;
          }
          if (brightness <= darkest_diff) {
            darkest_i = pix;
            darkest_diff = brightness;
          }
        }
      }

      unsigned char bitmap = 0;
      const static unsigned char pixel_bits[3][2] = { { 1, 2}, { 4, 8 }, { 16, 0 } };
      for (int oy = 0; oy < 3; oy++) {
        for (int ox = 0; ox < 2; ox++) {
          if (ox == 1 && oy == 2) continue;
          unsigned char pix = img->pixels[ox + (x + (y * 3 + oy) * w) * 2];
          float diff_bg = get_color_difference(img->palette->colors[darkest_i], img->palette->colors[pix]);
          float diff_fg = get_color_difference(img->palette->colors[brightest_i], img->palette->colors[pix]);
          if (diff_fg < diff_bg) {
            bitmap |= pixel_bits[oy][ox];
          }
        }
      }

      {
        unsigned char pix = img->pixels[1 + (x + (y * 3 + 2) * w) * 2];
        float diff_bg = get_color_difference(img->palette->colors[darkest_i], img->palette->colors[pix]);
        float diff_fg = get_color_difference(img->palette->colors[brightest_i], img->palette->colors[pix]);
        if (diff_fg < diff_bg) {
          bitmap ^= 31;
          unsigned char tmp = darkest_i;
          darkest_i = brightest_i;
          brightest_i = tmp;
        }
      }

      characters[x + y * w].character = 0x80 + bitmap;
      characters[x + y * w].bg = darkest_i;
      characters[x + y * w].fg = brightest_i;
    }
  }
}

void convert_6x9(const struct image_pal *img, struct cc_char *characters) {
  int w = img->w / 6, h = img->h / 9;
  float palette_self_diffs[0x100][0x10] = {{(float) 0xffffff}};
  for (int input_color = 0x0; input_color < 0x100 && input_color < img->palette->count; input_color++) {
    for (int output_color = 0x0; output_color < 0x10 && output_color < img->palette->count; output_color++) {
      palette_self_diffs[input_color][output_color] = get_color_difference(img->palette->colors[input_color], img->palette->colors[output_color]);
    }
  }

  for (int y = 0; y < h; y++) {
#ifdef USE_OPENMP
#pragma omp parallel for
#endif
    for (int x = 0; x < w; x++) {
      float chunk_palette_diffs[6][9][0x10] = {{{(float) 0xffffff}}};
      for (int ox = 0; ox < 6; ox++) {
        for (int oy = 0; oy < 9; oy++) {
          uint8_t pixel_unresolved = img->pixels[
            ox + (x + (y * 9 + oy) * w) * 6
          ];
          for (int color = 0x0; color < 0x10 && color < img->palette->count; color++) {
            chunk_palette_diffs[ox][oy][color] = palette_self_diffs[pixel_unresolved][color];
          }
        }
      }

      float min_diff = 0xffffff;
      char closest_sym = 0x00, closest_color = 0xae;
      for (int sym = 0x01; sym <= 0xFF; sym++) {
        if (sym == '\t' || sym == '\n' || sym == '\r' || sym == '\x0e') {
          continue;
        }
        for (int color = 0x00; color <= 0xff; color++) {
          float difference = 0;
          for (int oy = 0; oy < 9; oy++) {
            unsigned char sym_line = cc_font_atlas[sym][oy];
            for (int ox = 0; ox < 6; ox++) {
              bool lit = sym_line & (0x80 >> ox);
              difference += chunk_palette_diffs[ox][oy][lit ? color >> 4 : color & 0xF];
            }
          }
          if (difference <= min_diff) {
            min_diff = difference;
            closest_sym = sym;
            closest_color = color;
          }
        }
      }
      characters[x + y * w].character = closest_sym;
      characters[x + y * w].bg = closest_color & 0xF;
      characters[x + y * w].fg = closest_color >> 4;
    }
  }
}

struct {
  uint8_t count;
  union color colors[256];
} custom_palette_data;

struct palette *custom_palette_resize(uint8_t size) {
  custom_palette_data.count = size;
  return (struct palette*)&custom_palette_data;
}

struct palette *custom_palette_from(const struct palette *orig) {
  custom_palette_data.count = orig->count;
  for (int i = 0; i < custom_palette_data.count; i++) {
    custom_palette_data.colors[i] = orig->colors[i];
  }
  return (struct palette*)&custom_palette_data;
}

struct k_means_state k_means_init(const struct image *image, struct palette *starting_palette) {
  size_t item_count = image->w * image->h;
  uint8_t cluster_count = starting_palette->count;
  struct k_means_state state = {
    .items = image,
    .clusters = starting_palette,
    .predicted_cluster = calloc(image->w, image->h),
    .centroid_intermediate = calloc(cluster_count, sizeof(struct k_means_centroid_intermediate)),
    .item_count = item_count,
  };
  if (state.centroid_intermediate) {
    for (size_t i = 0; i < cluster_count; i++) {
      state.centroid_intermediate[i].closest_present_item = starting_palette->colors[i];
      state.centroid_intermediate[i].closest_present_distance = 1e20;
    }
  }
  return state;
}

bool k_means_iteration(struct k_means_state *state) {
  if (!state->predicted_cluster || !state->centroid_intermediate) {
    return false;
  }
  bool changed = false;

  // Find closest cluster
  for (int i = 0; i < state->item_count; i++) {
    int closest_cluster = 0;
    float closest_distance = 1e20;
    for (int cluster = 0; cluster < state->clusters->count; cluster++) {
      union color item = state->items->pixels[i];
      float dist = get_color_difference(state->clusters->colors[cluster], item);
      if (dist <= closest_distance) {
        closest_distance = dist;
        closest_cluster = cluster;
      }
      if (dist < state->centroid_intermediate[cluster].closest_present_distance) {
        bool can_update = true;
        for (int other_cluster = 0; other_cluster < state->clusters->count; other_cluster++) {
          if (other_cluster == cluster) {
            continue;
          }
          if (state->centroid_intermediate[other_cluster].closest_present_item.v == item.v) {
            can_update = false;
            break;
          }
        }
        if (can_update) {
          state->centroid_intermediate[cluster].closest_present_item = item;
          state->centroid_intermediate[cluster].closest_present_distance = dist;
        }
      }
    }
    if (!changed) {
      changed = state->predicted_cluster[i] != closest_cluster;
    }
    state->predicted_cluster[i] = closest_cluster;
    state->centroid_intermediate[closest_cluster].count += 1;
    state->centroid_intermediate[closest_cluster].sums.r += state->items->pixels[i].rgba.r;
    state->centroid_intermediate[closest_cluster].sums.g += state->items->pixels[i].rgba.g;
    state->centroid_intermediate[closest_cluster].sums.b += state->items->pixels[i].rgba.b;
  }

  // Update centroids
  for (int i = 0; i < state->clusters->count; ++i) {
    struct k_means_centroid_intermediate intermediate = state->centroid_intermediate[i];
    if (intermediate.count) {
      union color centroid = {
        .rgba = {
          .r = intermediate.sums.r / intermediate.count,
          .g = intermediate.sums.g / intermediate.count,
          .b = intermediate.sums.b / intermediate.count,
          .a = 0xff,
        }
      };
      if (!changed) {
        changed = state->clusters->colors[i].v != centroid.v;
      }
      state->clusters->colors[i] = centroid;
    } else {
      // No pixels are closest to this color
      // Warp the centroid onto the closest item
      state->clusters->colors[i] = intermediate.closest_present_item;
    }
    state->centroid_intermediate[i] = (struct k_means_centroid_intermediate) { .sums = {0, 0, 0}, .count = 0, .closest_present_item = state->clusters->colors[i], .closest_present_distance = 1e20 };
  }

  return changed;
}

void k_means_end(struct k_means_state *state) {
  if (state->predicted_cluster) {
    free(state->predicted_cluster);
  }
  if (state->centroid_intermediate) {
    free(state->centroid_intermediate);
  }
}

struct palette *palette_k_means(const struct image *image, const struct palette *prototype) {
  if (!prototype) {
    prototype = &cc_default_palette;
  }
  struct palette *palette = custom_palette_from(prototype);

  struct k_means_state state = k_means_init(image, palette);
  for (int i = 0; i < K_MEANS_ITERATIONS; i++) {
    if (!k_means_iteration(&state)) {
      fprintf(stderr, "early k-means stop at iteration %d\n", i);
      break;
    }
  }
  k_means_end(&state);

  return palette;
}