/*
  uLisp GIF Decode/Encode Extension v1 - 10th February 2026
  See http://www.ulisp.com/show?3IUQ
*/

#include <SPI.h>
#include <SD.h>

typedef struct {
  int16_t ptr;
  uint8_t code;
} cell_t;

uint16_t Colour (int r, int g, int b) {
  return (r & 0xf8)<<8 | (g & 0xfc)<<3 | b>>3;
}

// Globals
const int TableSize = 4096;
cell_t Table[TableSize]; // Translation table
uint8_t Block[255]; 
uint16_t ColourTable[256];
int Pixel = 0;

// Utilities **********************************************

/*
  InitialiseTable - initialises the translation table.
*/
void InitialiseTable (int colours) {
  for (int c = 0; c<colours; c++) {
    Table[c].ptr = -1; Table[c].code = c;
  }
}

/*
  IntegerLength -  number of bits needed to represent n
*/
int IntegerLength (unsigned int n) {
  return n ? 32 - __builtin_clz(n) : 0;
}

// Display GIF **********************************************

/*
  GetNBits - reads the next n bits from the bitstream. It keeps a bit buffer in the variable buf, with nbuf
  specifying how many bits are still available. If there are fewer than n bits in buf another byte is read
  from the input stream and appended to the buffer.
*/
int GetNBits (gfun_t gfun, int n) {
  static uint8_t nbuf = 0, blockptr = 0;
  static uint32_t buf = 0;
  if (n > 0) {
    while (nbuf < n) {
      if (blockptr == 0) blockptr = gfun();
      buf = ((uint32_t)gfun() << nbuf) | buf;
      blockptr--; nbuf = nbuf + 8;
    }
    int result = ((1 << n) - 1) & buf;
    buf = buf >> n; nbuf = nbuf - n;
    return result;
  } else { // n == 0
    nbuf = 0; buf = 0; blockptr = 0;
    return 0;
  }
}

/*
  FirstPixel - for a table element p, follows the pointers to find the first pixel colour in the sequence it represents.
*/
uint8_t FirstPixel (int p) {
  uint8_t last;
  do {
    last = Table[p].code;
    p = Table[p].ptr;
  } while (p != -1);
  return last;
}

/*
  WritePixel - calls a Lisp function with parameters x, y, and c to plot a pixel.
*/
void WritePixel (object *fn, uint16_t x, uint16_t y, uint16_t c) {
  if (fn == NULL) {
    #if defined(gfxsupport)
      tft.drawPixel(x, y, c);
    #endif
  } else {
    object* list = cons(number(x), cons(number(y), (cons(number(c), NULL))));
    apply(fn, list, NULL);
  }
}

/*
  PlotSequence - plots the sequence of pixels represented by the table element p. It avoids the need for recursion by
  plotting pixels backwards.
*/
void PlotSequence (object *fn, int p, uint16_t width) {
  // Measure backtrack
  int i = 0; int16_t rest = p;
  while (rest != -1) {
    rest = Table[rest].ptr;
    i++;
  }
  // Plot backwards
  Pixel = Pixel + i - 1;
  rest = p;
  while (rest != -1) {
    WritePixel(fn, Pixel%width, Pixel/width, ColourTable[Table[rest].code]);
    Pixel--;
    rest = Table[rest].ptr;
  }
  Pixel = Pixel + i + 1;
  #if defined(ARDUINO_M5STACK_TAB5)
  if (!fn) tft.display();
  #endif
}

/*
  SkipNBytes - skips a series of unneeded bytes in the input stream.
*/
void SkipNBytes (gfun_t gfun, int n) {
  for (int i=0; i<n; i++) gfun();
}

/*
  Power2 - checks that its argument is a power of two. It is used to increase the bit length of codes
  being read when the size of the table reaches a power of two.
*/
boolean Power2 (int x) {
  return (x & (x - 1)) == 0;
}

int ReadInt (gfun_t gfun) {
  return gfun() | gfun()<<8;
}

/*
  (decode-gif stream [function])
  Decodes a GIF file from the stream and plots it.
  Alternatively a function fn(x y colours) can be provided that plots the colour index at x,y.
*/
object *fn_decodegif (object *args, object *env) {
  (void) env;
  gfun_t gfun = gstreamfun(args);
  args = cdr(args);
  object *fn = NULL;
  if (args != NULL) fn = first(args);
  const char *head = "GIF8*a"; // GIF87a or GIF89a
  for (int p=0; head[p]; p++) {
    if (gfun() != head[p] && head[p] != '*') error2("not a valid GIF");
  }
  int width = ReadInt(gfun);
  ReadInt(gfun); // Height
  uint8_t field = gfun();
  SkipNBytes(gfun, 2); // background, and aspect
  uint8_t colourbits = max(1 + (field & 7), 2);
  int colours = 1<<colourbits;
  int clr = colours;
  int end = 1 + clr;
  int free = 1 + end;
  uint8_t bits = 1 + colourbits;
  Pixel = 0;

  // Parse colour table
  for (int c = 0; c<colours; c++) {
    ColourTable[c] = Colour(gfun(), gfun(), gfun());
  }
  
  InitialiseTable(colours);
  
  // Parse blocks
  do {
    uint8_t header = gfun();
    if (header == 0x2C) { // Image block
      SkipNBytes(gfun, 8);
      uint8_t packed = gfun();
      if (packed & 0x80) error2("local colour tables not supported");
      if (packed & 0x40) error2("interlaced GIFs not supported");
      SkipNBytes(gfun, 1);
      GetNBits(gfun, 0); // Reset nbuf, buf, and blockptr
      int code = -1, last = -1;
      do {
        last = code;
        code = GetNBits(gfun, bits);
        if (code == clr) {
          free = 1 + end;
          bits = 1 + colourbits;
          code = -1;
        } else if (code == end) {
          break;
        } else if (last == -1) {
          PlotSequence(fn, code, width);
        } else if (code <= free) {
          if (free < TableSize) {
            Table[free].ptr = last;
            if (code == free) Table[free].code = FirstPixel(last);
            else Table[free].code = FirstPixel(code);
            free++;
            if (Power2(free) && (free < TableSize)) bits++;
          }
          PlotSequence(fn, code, width); 
        }
      } while (true);
      if (gfun() != 0) error2("invalid end of file");
    } else if (header == 0x21) { // Extension block
      uint8_t gce = gfun();
      if (gce == 0xff) { // Application extension
        SkipNBytes(gfun, 1);
        SkipNBytes(gfun, 8);
        const char *xmp = "XMP"; // Adobe extension
        for (int p=0; xmp[p]; p++) {
	        if (gfun() != xmp[p]) error2("unrecognised application extension");
	      }
        int length;
        do {
          length = gfun();
          SkipNBytes(gfun, length);
        } while (length != 0);
      } else { // Other extension
        int length = gfun();
        SkipNBytes(gfun, 1 + length);
      }
    } else if (header == 0x3b) { // Terminating byte
      return nil;
    }
  } while (true);
}

// Encode GIF **********************************************

// Works: 8, 64, 256

const int factor=0, divisor = 1, rescale = 2;

const uint8_t PosteriseData [8][3][3] {
  //factors    divisors   rescales
  { {0, 0, 0}, {0, 0, 0}, {0, 0, 0} },             // 2 not used
  { {0, 0, 0}, {0, 0, 0}, {0, 0, 0} },             // 4 not used
  { {2, 2, 2}, {16, 32, 16}, {0x80, 0x80, 0x80} }, // 8
  { {2, 3, 2}, {16, 22, 16}, {0x80, 0xb0, 0x80} }, // 16
  { {3, 3, 3}, {11, 22, 11}, {0xb0, 0xb0, 0xb0} }, // 32
  { {4, 4, 4}, {8, 16, 8}, {0xc0, 0xc0, 0xc0} },   // 64
  { {5, 5, 5}, {7, 13, 7}, {0xe0, 0xd0, 0xe0} },   // 128
  { {6, 7, 6}, {6, 10, 6}, {0xf0, 0xf0, 0xf0} }    // 256
};

/*
  EmitTable - writes out the colour table for the specified number of colours.
*/
void EmitTable (pfun_t pfun, int colours) {
  uint8_t bits = IntegerLength(colours - 1);
  const uint8_t (*entry)[3] = PosteriseData[bits - 1];
  uint8_t product = entry[factor][0] * entry[factor][1] * entry[factor][2];
  if (colours == 2) {
    pfun(0); pfun(0); pfun(0); pfun(0xff); pfun(0xff); pfun(0xff);
  } else if (colours == 4) {
    pfun(0); pfun(0); pfun(0); pfun(0); pfun(0xff); pfun(0); 
    pfun(0xff); pfun(0); pfun(0); pfun(0xff); pfun(0xff); pfun(0xff);
  } else {
    for (int r=0; r<entry[factor][0]; r++) {
      for (int g=0; g<entry[factor][1]; g++) {
        for (int b=0; b<entry[factor][2]; b++) {
          pfun((((r * entry[divisor][0]) << 3) * 0xff) / entry[rescale][0]);
          pfun((((g * entry[divisor][1]) << 2) * 0xff) / entry[rescale][1]);
          pfun((((b * entry[divisor][2]) << 3) * 0xff) / entry[rescale][2]);
        }
      }
    }
    for (int w=0; w<colours - product; w++) { pfun(0); pfun(0); pfun(0); }
  }
}

/*
  Posterise - converts a 16-bit colour col565 to an 8-bit colour index in the 
  specified number of colours by posterising it, and returns it.
*/
uint8_t Posterise (uint16_t col565, int colours) {
  uint8_t bits = IntegerLength(colours - 1);
  const uint8_t (*entry)[3] = PosteriseData[bits - 1];
  uint8_t r = col565>>11 & 0x1f, g = col565>>5 & 0x3f, b = col565 & 0x1f;
  if (colours == 2) {
    if (col565 == 0) return 0; else return 1;
  } else if (colours == 4) {
    if (col565 == 0) return 0; else if (r>0 && g>0 && b>0) return 3; else if (r>0) return 2; else return 1;
  } else {
    return (r / entry[divisor][0]) * entry[factor][1] * entry[factor][2] +
    (g / entry[divisor][1]) * entry[factor][2] + (b / entry[divisor][2]);
  }
}

/*
  (posterise col565 colours)
  Converts a 16-bit colour col565 to an 8-bit colour index in the 
  specified number of colours by posterising it, and returns it.
*/
object *fn_posterise (object *args, object *env) {
  (void) env;
  int colours = checkinteger(second(args));
  if (colours < 0 || colours > 256) error("invalid number of colours", second(args));
  return number(Posterise(checkinteger(first(args)), colours));
}

/*
  PutNBits - adds n bits from the bitstring 'bits' to the bitstream in 'buf', writing blocks to the output stream when full.
  If n=0 resets the static variables. If n=-1 flushes the bitstream.
*/
void PutNBits (pfun_t pfun, int n, int bits) {
  static uint8_t nbuf = 0;     // Number of bits in buf
  static uint8_t blockptr = 0; // Index in Block[] array
  static uint32_t buf = 0;     // Bit buffer
  if (n > 0) {
    buf = bits<<nbuf | buf;
    nbuf = nbuf + n;
    while (nbuf >= 8) {
      Block[blockptr++] = buf & 0xff;
      buf = buf >> 8; nbuf = nbuf - 8;
      if (blockptr == 255) {
        pfun(blockptr);
        for (uint8_t i=0; i<blockptr; i++) pfun(Block[i]);
        blockptr = 0;
      }
    }
    return;
  } else if (n == -1) { // flush bitstream
    if (nbuf > 0) {
      Block[blockptr++] = buf & 0xff;
    }
    if (blockptr != 0) {
      pfun(blockptr);
      for (uint8_t i=0; i<blockptr; i++) pfun(Block[i]);
    }
    pfun(0); // terminator
  } else { // n == 0
    nbuf = 0; buf = 0; blockptr = 0;
  }
}

/*
  GetPixel - reads the display and returns a colour index for the point x,y for the specified number of colours.
*/
int GetPixel (object *fn, int start, int length, int xsize, int colours) {
  if (start < length) {
    if (fn == NULL) {
      #if defined(ARDUINO_WIO_TERMINAL) || defined(ARDUINO_M5STACK_TAB5)
      uint16_t col565 = tft.readPixel(start%xsize, start/xsize);
      #else
      uint16_t col565 = readpixel(start%xsize, start/xsize);
      #endif
      return Posterise(col565, colours);
    } else {
      object *list = cons(number(start%xsize), cons(number(start/xsize), (cons(number(colours), NULL))));
      object *result = apply(fn, list, NULL);
      int index = checkinteger(result);
      if (index < 0 || index >= colours) error("colour index out of range", result);
      return index;
    }
  } else {
    return -1;
  }
}

/*
  (encode-gif stream xsize ysize colours [function] [noclear])
  Encodes a GIF file by reading the display, and outputs the encoding with the specified number of colours to the stream.
  Alternatively a function fn(x y colours) can be provided; it is called for each point, and should return the colour index.
  Setting noclear to t can give a smaller file size for images with repetitive content.
*/
object *fn_encodegif (object *args, object *env) {
  (void) env;
  pfun_t pfun = pstreamfun(args);
  args = cdr(args);
  int xsize = checkinteger(first(args));
  int ysize = checkinteger(second(args));
  int colours = checkinteger(third(args));
  bool doclear = true;
  args = cdr(cddr(args));
  object *function = NULL;
  if (args != NULL) {
    function = first(args);
    args = cdr(args);
    if (args != NULL && first(args) != NULL) doclear = false;
  }
  int start = 0;
  int length = xsize * ysize;
  uint8_t colourbits = IntegerLength(colours - 1);
  if (colours < 0 || colours > 256) error("invalid number of colours", third(args));
  uint8_t codesize = colourbits<2 ? 2 : colourbits;
  int clr = 1<<codesize;
  int end = clr + 1;
  int free = end + 1;
  PutNBits(pfun, 0, 0); // clear nbuf, buf, and blockptr

  // Header
  const char *head = "GIF87a";
  for (int p=0; head[p]; p++) pfun(head[p]);

  // Screen descriptor
  pfun(xsize & 0xff); pfun(xsize >> 8); // Image width
  pfun(ysize & 0xff); pfun(ysize >> 8); // Image height
  pfun(0x80 | (colourbits - 1)<<4 | 0x00 | (colourbits - 1)); // Packed fields
  pfun(0); // Colour 0 = background
  pfun(0); // No odd aspect ratio

  // Global colour table
  EmitTable(pfun, colours);

  // Image descriptor
  pfun(0x2c);
  pfun(0); pfun(0); // Image top
  pfun(0); pfun(0); // Image left
  pfun(xsize & 0xff); pfun(xsize >> 8); // Image width
  pfun(ysize & 0xff); pfun(ysize >> 8); // Image height
  pfun(0); // Packed fields

  // Image data
  InitialiseTable(colours);
  pfun(codesize);
  PutNBits(pfun, IntegerLength(free - 1), clr);
  do {
    // Look up a sequence in translation table
    int ptr = GetPixel(function, start, length, xsize, colours);
    int second = GetPixel(function, start+1, length, xsize, colours);
    if (second == -1) {
      PutNBits(pfun, IntegerLength(free - 1), ptr);
    } else {
      int x = end + 1;
      while ((x != free) && (second != -1)) {
        if ((Table[x].ptr == ptr) && (Table[x].code == second)) {
          ptr = x;
          start++;
          second = GetPixel(function, start+1, length, xsize, colours);
        }
        x++;
      }
      PutNBits(pfun, IntegerLength(free - 1), ptr);
      if (free < TableSize) {
        Table[free].ptr = ptr;
        Table[free].code = GetPixel(function, start+1, length, xsize, colours);
        free++;
      }
    }
    start++;
    if ((free == TableSize) && doclear) {
      PutNBits(pfun, IntegerLength(free - 1), clr);
      free = end + 1;
    }
  } while (start < length);
  PutNBits(pfun, IntegerLength(free - 1), end);
  PutNBits(pfun, -1, 0); // Flush bitstream
  pfun(0x3b);
  return nil;
}

// Symbol names
const char stringdecodegif[] = "decode-gif";
const char stringencodegif[] = "encode-gif";
const char stringposterise[] = "posterise";

// Documentation strings
const char docdecodegif[] = "(decode-gif stream [function])\n"
"Decodes a GIF file from the stream and plots it.\n"
"Alternatively a function fn(x y colours) can be provided that plots the colour index at x,y.";
const char docencodegif[] = "(encode-gif stream xsize ysize colours [function] [noclear])\n"
"Encodes a GIF file by reading the display, and outputs the encoding with the specified number of colours to the stream.\n"
"Alternatively a function fn(x y colours) can be provided; it is called for each point, and should return the colour index.\n"
"Setting noclear to t can give a smaller file size for images with repetitive content.";
const char docposterise[] = "(posterise col565 colours)\n"
"Converts a 16-bit colour col565 to a colour index in the \n"
"specified number of colours by posterising it, and returns it.";

// Symbol lookup table
const tbl_entry_t lookup_table2[] = {
  { stringdecodegif, fn_decodegif, 0212, docdecodegif },
  { stringencodegif, fn_encodegif, 0246, docencodegif },
  { stringposterise, fn_posterise, 0222, docposterise },
};

// Table cross-reference functions - do not edit below this line

tbl_entry_t *tables[] = {lookup_table, lookup_table2};
const unsigned int tablesizes[] = { arraysize(lookup_table), arraysize(lookup_table2) };

const tbl_entry_t *table (int n) {
  return tables[n];
}

unsigned int tablesize (int n) {
  return tablesizes[n];
}