#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdarg.h>
#include <assert.h>
#include <math.h>

#include "draw.h"

#define abs(x) ((x) < 0 ? -(x) : (x))

Color framebuffer[WIDTH * HEIGHT];

void to_hsv(u8 r, u8 g, u8 b, u8* h, u8* s, u8* v) {
  int min = r < g ? (r < b ? r : b) : (g < b ? g : b);
  int max = r > g ? (r > b ? r : b) : (g > b ? g : b);

  *h = 0;
  *s = 0;
  *v = max;

  if (*v != 0) {
    *s = 255 * ((long)max - min) / *v;
    if (*s != 0) {
      if (max == r) *h = 0 + 43 * (g - b) / (max - min);
      else if (max == g) *h = 85 + 43 * (b - r) / (max - min);
      else *h = 171 + 43 * (r - g) / (max - min);
    } else {
      *h = 0;
    }
  }
}

void from_hsv(u8 h, u8 s, u8 v, u8* r, u8* g, u8* b) {
  int region = h / 43;
  int remainder = (h - (region * 43)) * 6;

  int p = (v * (255 - s)) >> 8;
  int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
  int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;

  switch (region) {
    case 0: *r = v; *g = t; *b = p; break;
    case 1: *r = q; *g = v; *b = p; break;
    case 2: *r = p; *g = v; *b = t; break;
    case 3: *r = p; *g = q; *b = v; break;
    case 4: *r = t; *g = p; *b = v; break;
    default: *r = v; *g = p; *b = q; break;
  }
}

void clear_screen() {
  memset(framebuffer, 0, WIDTH * HEIGHT * sizeof(Color));
}

void draw_point(i32 x, i32 y, Color color) {
  if (x >= 0 && x < WIDTH && y >= 0 && y < HEIGHT) framebuffer[y * WIDTH + x] = color;
}

void fill_rect(i32 x, i32 y, i32 width, i32 height, Color color) {
  if (x < 0) {
    width += x;
    x = 0;
  }
  if (y < 0) {
    height += y;
    y = 0;
  }
  if (x + width > WIDTH) width = WIDTH - x;
  if (y + height > HEIGHT) height = HEIGHT - y;

  for (i32 j = y; j < y + height; ++j) {
    Color* pixel = &framebuffer[j * WIDTH + x];
    for (i32 i = 0; i < width; ++i) *(pixel++) = color;
  }
}

void draw_rect(i32 x, i32 y, i32 width, i32 height, Color color) {
  horizontal_line(x, x + width, y, color);
  for (i32 i = 1; i < height - 1; i++) {
    draw_point(x, y + i, color);
    draw_point(x + width - 1, y + i, color);
  }
  horizontal_line(x, x + width, y + height - 1, color);
}

void blit(i32 x, i32 y, i32 x_source, i32 y_source, i32 width, i32 height, Color* source, i32 source_width, i32 source_height) {
  if (x_source < 0) {
    width += x_source;
    x_source = 0;
  }
  if (y_source < 0) {
    height += y_source;
    y_source = 0;
  }
  if (x_source + width > source_width) width = source_width - x_source;
  if (y_source + height > source_height) height = source_height - y_source;
  if (x < 0) {
    width += x;
    x_source -= x;
    x = 0;
  }
  if (y < 0) {
    height += y;
    y_source -= y;
    y = 0;
  }
  if (x + width >= WIDTH) width = WIDTH - x; // WARNING: x might be larger than WIDTH, resulting in negative width
  if (y + height >= HEIGHT) height = HEIGHT - y;
  for (int j = 0; j < height; j++) {
    int offset = (y + j) * WIDTH + x;
    int source_offset = (y_source + j) * source_width + x_source;
    if (width > 0) memcpy(framebuffer + offset, source + source_offset, width * sizeof(Color));
  }
}

void blit_masked(i32 x, i32 y, i32 x_source, i32 y_source, i32 width, i32 height, Color mask, Color* source, i32 source_width, i32 source_height) {
  if (x_source < 0) {
    width += x_source;
    x_source = 0;
  }
  if (y_source < 0) {
    height += y_source;
    y_source = 0;
  }
  if (x_source + width > source_width) width = source_width - x_source;
  if (y_source + height > source_height) height = source_height - y_source;
  if (x < 0) {
    width += x;
    x_source -= x;
    x = 0;
  }
  if (y < 0) {
    height += y;
    y_source -= y;
    y = 0;
  }
  if (x + width >= WIDTH) width = WIDTH - x; // WARNING: x might be larger than WIDTH, resulting in negative width
  if (y + height >= HEIGHT) height = HEIGHT - y;
  for (int j = 0; j < height; j++) {
    int offset = (y + j) * WIDTH + x;
    int source_offset = (y_source + j) * source_width + x_source;
    for (int i = 0; i < width; i++) {
      if (source[source_offset + i] != mask) framebuffer[offset + i] = source[source_offset + i];
    }
  }
}

void draw_line(i32 x0, i32 y0, i32 x1, i32 y1, Color color) {
  i32 dx =  abs(x1-x0);
  i32 sx = x0 < x1 ? 1 : -1;
  i32 dy = -abs(y1-y0);
  i32 sy = y0<y1 ? 1 : -1;
  i32 err = dx + dy;  /* error value e_xy */
  while (1) {   /* loop */
    draw_point(x0, y0, color);
    if (x0 == x1 && y0 == y1) break;
    i32 e2 = 2*err;
    if (e2 >= dy) { /* e_xy+e_x > 0 */
      err += dy;
      x0 += sx;
    }
    if (e2 <= dx) { /* e_xy+e_y < 0 */
      err += dx;
      y0 += sy;
    }
  }
}

void draw_circle(i32 xm, i32 ym, i32 r, Color color) {
   i32 x = -r, y = 0, err = 2 - 2 * r; /* II. Quadrant */
   do {
      draw_point(xm - x, ym + y, color); /*   I. Quadrant */
      draw_point(xm - y, ym - x, color); /*  II. Quadrant */
      draw_point(xm + x, ym - y, color); /* III. Quadrant */
      draw_point(xm + y, ym + x, color); /*  IV. Quadrant */
      r = err;
      if (r <= y) err += ++y * 2 + 1;           /* e_xy+e_y < 0 */
      if (r > x || err > y) err += ++x * 2 + 1; /* e_xy+e_x > 0 or no 2nd y-step */
   } while (x < 0);
}

void horizontal_line(i32 x1, i32 x2, i32 y, Color color) {
  if (x1 >= WIDTH || x2 < 0 || y < 0 || y >= HEIGHT) return;
  if (x1 < 0) x1 = 0;
  if (x2 >= WIDTH) x2 = WIDTH;
  int offset = y * WIDTH;
  while (x1 < x2) {
    framebuffer[offset + x1] = color;
    x1 ++;
  }
}

void horizontal_line_wrapped(i32 x1, i32 x2, i32 y, Color color) {
  if (y < 0 || y >= HEIGHT) return;
  int offset = y * WIDTH;
  while (x1 < x2) {
    framebuffer[offset + x1 % WIDTH] = color;
    x1 ++;
  }
}

void fill_circle(i32 xm, i32 ym, i32 r, Color color) {
   i32 x = -r, y = 0, err = 2 - 2 * r; /* II. Quadrant */
   do {
      horizontal_line(xm + x, xm - x, ym - y, color);
      horizontal_line(xm + x, xm - x, ym + y, color);
      r = err;
      if (r <= y) err += ++y * 2 + 1;           /* e_xy+e_y < 0 */
      if (r > x || err > y) err += ++x * 2 + 1; /* e_xy+e_x > 0 or no 2nd y-step */
   } while (x < 0);
}

void copy(int n, float *points, float* result) {
  for (int i = 0; i < n; i++) result[i] = points[i];
}

void scale(int n, float* points, float factorX, float factorY) {
  for (int i = 0; i < n; i += 2) {
    points[i] *= factorX;
    points[i + 1] *= factorY;
  }
}

void translate(int n, float* points, float x, float y) {
  for (int i = 0; i < n; i += 2) {
    points[i] += x;
    points[i + 1] += y;
  }
}

void rotate(int n, float* points, float angle) {
  float c = cos(angle);
  float s = sin(angle);
  for (int i = 0; i < n; i += 2) {
    float x = c * points[i] - s * points[i + 1];
    float y = s * points[i] + c * points[i + 1];
    points[i] = x;
    points[i + 1] = y;
  }
}

void draw(int n, float* points, int close, Color color) {
  for (int i = 0; i < n - 2; i += 2) {
    draw_line(points[i], points[i + 1], points[i + 2], points[i + 3], color);
  }
  if (close) {
    draw_line(points[n - 2], points[n - 1], points[0], points[1], color);
  }
}

void fill(int n, float* points, int wrap, Color color) {
  int polyCorners = n >> 1;
  int nodes, nodeX[n], pixelY, i, j, swap;

  // determine bounding segment on Y axis
  int minY = HEIGHT, maxY = 0;
  for (int i = 0; i < n; i += 2) {
    if (points[i + 1] < minY) minY = points[i + 1];
    if (points[i + 1] > maxY) maxY = points[i + 1];
  }

  if (minY < 0) minY = 0;
  if (maxY >= HEIGHT) maxY = HEIGHT - 1;

  for (pixelY = minY; pixelY <= maxY; pixelY++) {

    //  Build a list of nodes.
    nodes = 0; j = polyCorners - 1;
    for (i = 0; i < polyCorners; i++) {
      int ii = i << 1;
      int jj = j << 1;
      if (points[ii + 1] < pixelY && points[jj + 1] >= pixelY || points[jj + 1] < pixelY && points[ii + 1] >= pixelY) {
        nodeX[nodes++] = (int) (points[ii] + (pixelY - points[ii + 1]) / (points[jj + 1] - points[ii + 1]) *(points[jj] - points[ii])); 
      }
      j = i; 
    }

    //  Sort the nodes, via a simple "Bubble" sort.
    i = 0;
    while (i < nodes - 1) {
      if (nodeX[i] > nodeX[i + 1]) {
        swap = nodeX[i]; nodeX[i] = nodeX[i + 1]; nodeX[i + 1] = swap; if (i) i--; 
      } else {
        i++; 
      }
    }

    //  Fill the pixels between node pairs.
    for (i = 0; i < nodes; i += 2) {
      if (wrap) {
        horizontal_line(nodeX[i] - wrap, nodeX[i + 1] - wrap, pixelY, color);
        horizontal_line(nodeX[i], nodeX[i + 1], pixelY, color);
      }
      else
      if   (nodeX[i] >= WIDTH) break;
      if   (nodeX[i + 1] > 0 ) {
        if (nodeX[i] < 0 ) nodeX[i] = 0;
        if (nodeX[i + 1] >= WIDTH ) nodeX[i + 1] = WIDTH - 1;
        horizontal_line(nodeX[i], nodeX[i + 1], pixelY, color);
      }
    }
  }
}

int point_in_polygon(int n, float* points, float x, float y) {
  int i, j = n - 2 ;
  int oddNodes = 0;

  for (i = 0; i < n; i += 2) {
    if ((points[i + 1] < y && points[j + 1] >= y || points[j + 1] < y && points[i + 1] >= y) && (points[i] <= x || points[j] <= x)) {
      oddNodes ^= (points[i] + (y - points[i + 1]) / (points[j + 1] - points[i + 1]) * (points[j] - points[i]) < x); 
    }
    j = i; 
  }

  return oddNodes; 
}

int line_intersect(float* l1, float* l2) {
  float Ax = l1[0], Ay = l1[1], Bx = l1[2], By = l1[3];
  float Cx = l2[0], Cy = l2[1], Dx = l2[2], Dy = l2[3];

  float distAB, theCos, theSin, newX, ABpos;

  //  Fail if either line segment is zero-length.
  if (Ax == Bx && Ay == By || Cx == Dx && Cy == Dy) return 0;

  //  Fail if the segments share an end-point.
  if (Ax == Cx && Ay == Cy || Bx == Cx && By == Cy ||  Ax == Dx && Ay == Dy || Bx == Dx && By == Dy) return 0;

  //  (1) Translate the system so that point A is on the origin.
  Bx -= Ax; By -= Ay;
  Cx -= Ax; Cy -= Ay;
  Dx -= Ax; Dy -= Ay;

  //  Discover the length of segment A-B.
  distAB = sqrt(Bx * Bx + By * By);

  //  (2) Rotate the system so that point B is on the positive X axis.
  theCos = Bx / distAB;
  theSin = By / distAB;
  newX = Cx * theCos + Cy * theSin;
  Cy = Cy * theCos - Cx * theSin;
  Cx = newX;
  newX = Dx * theCos + Dy * theSin;
  Dy = Dy * theCos - Dx * theSin; 
  Dx = newX;

  //  Fail if segment C-D doesn't cross line A-B.
  if (Cy < 0 && Dy < 0 || Cy >= 0 && Dy >= 0) return 0;

  //  (3) Discover the position of the intersection point along line A-B.
  ABpos = Dx + (Cx - Dx) * Dy / (Dy - Cy);

  //  Fail if segment C-D crosses line A-B outside of segment A-B.
  if (ABpos < 0 || ABpos > distAB) return 0;

  //  (4) Apply the discovered position to line A-B in the original coordinate system.
  //*X = Ax + ABpos * theCos;
  //*Y = Ay + ABpos * theSin;

  //  Success.
  return 1;
} 

int polygon_intersect(int n, float* a, int m, float* b) {
  for (int i = 0; i < n; i+=2) {
    for (int j = 0; j < m; j += 2) {
      if (line_intersect(a + i, b + j)) return 1;
    }
  }
  if (point_in_polygon(n, a, b[0], b[1])) return 1;
  if (point_in_polygon(m, b, a[0], a[1])) return 1;
  return 0;
}

font_t* load_font(char* glyphs, int font_data_size) {
  printf("load_font: not implemented\n");
  return NULL;
}

void draw_text(font_t* font, i32 x, i32 y, Color color, char* text) {
  while(*text) {
    int start = 0, end = font->num_glyphs - 1;
    int current = 0;
    glyph_t glyph = font->glyphs[0];
    while (current < 4 && start <= end) {
      int middle = (start + end) / 2;
      //printf("  %c <> %c ? %d %d=%d+%d/2\n", text[current], glyph.text[current], current, middle, start, end);
      glyph = font->glyphs[middle];
      if (glyph.text[current] > text[current]) end = middle - 1;
      else if (glyph.text[current] < text[current]) start = middle + 1;
      else break;
      //else current++;
    }
    //printf("%s %s\n", text, glyph.text);
    
    int offset = glyph.offset;
    for (int j = 0; j < glyph.height; j++) {
      for (int i = 0; i < glyph.width; i++) {
        int mask = 1 << i;
        if (font->pixels[offset] & mask) framebuffer[x + i + (j + y) * WIDTH] = color;
      }
      offset++;
    }

    x += glyph.width;
    text += current + 1;
  }
}

void draw_printf(font_t* font, i32 x, i32 y, Color color, char* format, ...) {
  char* buffer = NULL;
  va_list list;
  va_start(list, format);
  int result = vasprintf(&buffer, format, list);
  if (result > -1) {
    draw_text(font, x, y, color, buffer);
  }
  if (buffer) free(buffer);
}

// todo: triangle, blit, roto-scale, etc.