// After writing an optimiser for line drawing order by hand, and understanding
// the problem a bit better, I took it to ChatGPT to add some extra optimisations
// since the naive 'greedy' algorithm was failing on pathological cases which
// were far too common.  The ChatGPT code was quite buggy and needed a lot of
// tweaking, but overall it was on the right track.  Took about 2 hours to get
// it working as opposed to maybe 4 hours to have written it from scratch.

// This code reduces the movement between lines by roughly a third of the total
// movement for randomly positioned lines.  Real-world behaviour may differ, but
// at least for now it does appear to be an avenue worth following up on.

// The space requirements mean that this can only work on Pitrex, not plain Vectrex.
// The speed shouldn't be significantly worse than my earlier greedy algorithm
// version, but the drawing time ought to be noticably better.  I have yet to
// actually compare this against my earlier simpler code on real data.

// One optimisation I haven't looked at yet is to pick out the half dozen longest
// vectors and do the full brute-force optimisation on them before tacking
// what's left...  Those vectors are not going to be within the same cluster
// and may not be well optimised.  But leaving that for now until we have a
// real testbed on the Pitrex.

#include <stdio.h>
#include <stdlib.h>
#include <limits.h>

#define PRINT_STATS 1 // if defined, gather statistics on amount of beam movement saved...
#define SUPPRESS_IO 1 // if defined, turn off printf's to get more accurate
                      // timing to determine the effect on the frame rate.
                      // (Invoke this code on Linux using the "time" command...)

#define MAX_POINT 128
#define MAX_LINE 128
#define GRID_SIZE 32     // We do a tighter optimisation within a grid square

#define SCREEN_MIN -128
#define SCREEN_MAX 127
                         // and a less strict optimisation between clusters.
#define CLUSTER_DIM ((SCREEN_MAX - SCREEN_MIN + 1) / GRID_SIZE)  // 8x8 grid
#define MAX_CLUSTERS (CLUSTER_DIM * CLUSTER_DIM)
#define MAX_CLUSTER_LINE 32
                         // and if we fill a cluster, we just flush it and start again.

typedef struct {
  signed char x, y;
} Point;
Point point[MAX_POINT];

typedef struct {
  int p[2];  // indices into point[]
} Line;
Line line[MAX_LINE];
int next_line = 0;
Line *cluster_segment[MAX_CLUSTERS][MAX_CLUSTER_LINE];
int cluster_count[MAX_CLUSTERS];

typedef struct {
  int line_idx;
  int reversed;
} DrawOrder;
DrawOrder final_order[MAX_LINE];
int final_count = 0;

// These divides can be replaced with shifts as long as we're careful about signedness.
Point get_midpoint(Line S) {
  Point a = point[S.p[0]];
  Point b = point[S.p[1]];
  return (Point){(a.x + b.x) / 2.0, (a.y + b.y) / 2.0};
}

int get_cluster_id(Point p) {
  int cx = ((int)p.x - SCREEN_MIN) / GRID_SIZE;
  int cy = ((int)p.y - SCREEN_MIN) / GRID_SIZE;
  return cy * CLUSTER_DIM + cx;
}

int get_endpoint_index(int line_idx, int reversed, int side) {
  Line s = line[line_idx];
  return (reversed) ? (side ? s.p[0] : s.p[1]) : (side ? s.p[1] : s.p[0]);
}

// Cost of the move between two points.  I initially used the squared distance for
// efficiency, but because of the way the Vectrex actually performs moves, it proved
// sufficient just to use max(dx,dy) since the beam is moved by separate x and y
// plates, and the duration of the move is determined by the longer of the two distances.

static inline int iabs(int i) {
  if (i < 0) return -i;
  return i;
}

static inline int imax(int a, int b) {
  if (a > b) return a;
  return b;
}

static inline int iabsmax(int a, int b) {
  return imax(iabs(a), iabs(b));
}

// static inline unsigned int distance(Point a, Point b) { return (a.x - b.x)*(a.x - b.x) + (a.y - b.y)*(a.y - b.y); }
static inline unsigned int distance(Point a, Point b) { return iabsmax(a.x - b.x, a.y - b.y); }

int next_permutation(int *a, int n) {
  int i = n - 2;
  while (i >= 0 && a[i] >= a[i + 1]) i--;
  if (i < 0) return 0;
  int j = n - 1;
  while (a[j] <= a[i]) j--;
  int tmp = a[i]; a[i] = a[j]; a[j] = tmp;
  for (int p = i + 1, q = n - 1; p < q; p++, q--) {
    tmp = a[p]; a[p] = a[q]; a[q] = tmp;
  }
  return 1;
}


#define MAX_SMALL_CLUSTER 6

void brute_force_cluster_order(Line **segs, int count, DrawOrder *best_order) {
  int perm[MAX_SMALL_CLUSTER];
  int best_perm[MAX_SMALL_CLUSTER];
  int best_dirs[MAX_SMALL_CLUSTER];
  int min_total = 0x7FFFFFFF;

  for (int i = 0; i < count; ++i) perm[i] = i;

  int total_perms = 1;
  for (int i = 2; i <= count; ++i) total_perms *= i;

  do {
    int dir_mask_limit = 1 << count;
    for (int mask = 0; mask < dir_mask_limit; ++mask) {
      int total_dist = 0;
      for (int i = 0; i < count - 1; ++i) {
        int s1 = perm[i];
        int s2 = perm[i + 1];
        int rev1 = (mask >> s1) & 1;
        int rev2 = (mask >> s2) & 1;

        Point p1 = point[get_endpoint_index(segs[s1] - line, rev1, 1)];
        Point p2 = point[get_endpoint_index(segs[s2] - line, rev2, 0)];
        total_dist += distance(p1, p2);
      }

      if (total_dist < min_total) {
        min_total = total_dist;
        for (int j = 0; j < count; ++j) {
          best_perm[j] = perm[j];
          best_dirs[j] = (mask >> perm[j]) & 1;
        }
      }
    }
  } while (next_permutation(perm, count));

  for (int i = 0; i < count; ++i) {
    best_order[i].line_idx = segs[best_perm[i]] - line;
    best_order[i].reversed = best_dirs[i];
  }
}

void optimise_within_a_cluster(Line **segs, int count, DrawOrder *order) {
  int used[MAX_CLUSTER_LINE] = {0};
  int current = 0, dir = 0;

  if (count <= MAX_SMALL_CLUSTER /* Pick threshold by inspection... */) {
    // This optimisation brings the total pen-up movement distance down from 90254 to 85738 (vs unoptimised 138579)
    // It remains to be seen if the gain is worth the extra overhead.
    brute_force_cluster_order(segs, count, order);    // brute-force all permutations and directions
    return;
  }
  
  // Otherwise use a greedy algorithm:
  used[0] = 1; order[0] = (DrawOrder){.line_idx = segs[0] - line, .reversed = 0};

  for (int step = 1; step < count; step++) {
    int min_dist = 65536;
    int next = -1, next_dir = 0;
    Point cur_end = point[get_endpoint_index(segs[current] - line, dir, 1)];

    for (int j = 0; j < count; j++) {
      if (used[j]) continue;
      for (int rev = 0; rev <= 1; rev++) {
        Point start = point[get_endpoint_index(segs[j] - line, rev, 0)];
        int d = distance(cur_end, start);
        if (d < min_dist) {
          min_dist = d; next = j; next_dir = rev;
        }
      }
    }

    order[step] = (DrawOrder){.line_idx = segs[next] - line, .reversed = next_dir};
    used[next] = 1; current = next; dir = next_dir;
  }
}

// No longer needed because we're now using a more accurate cost metric than the diagonal distance:
//
//int isqrt(int x) {
//  int x1, s, g0, g1;
//
//  if (x <= 1) return x;
//  s = 1; x1 = x - 1;
//  if (x1 > 255)   {s = s + 4; x1 = x1 >> 8;}
//  if (x1 > 15)    {s = s + 2; x1 = x1 >> 4;}
//  if (x1 > 3)     {s = s + 1;}
//
//  g0 = 1 << s;                // g0 = 2**s.
//  g1 = (g0 + (x >> s)) >> 1;  // g1 = (g0 + x/g0)/2.
//
//  while (g1 < g0) {           // Do while approximations strictly decrease.
//    g0 = g1; g1 = (g0 + (x/g0)) >> 1;
//  }
//  return g0;
//}

#ifdef PRINT_STATS
static int output_dist = 0;
static int output_cx = 0, output_cy = 0;
#endif

void draw_line(Line S) {
  int fromx, fromy, tox, toy;
  fromx = point[S.p[0]].x; fromy = point[S.p[0]].y; tox = point[S.p[1]].x; toy = point[S.p[1]].y;
#ifndef SUPPRESS_IO
  printf("draw_line(%d, %d,  %d, %d);\n",
         fromx, fromy, tox, toy);
#endif
#ifdef PRINT_STATS
  //output_dist += isqrt((fromx-output_cx)*(fromx-output_cx) + (fromy-output_cy)*(fromy-output_cy));
  output_dist += iabsmax(fromx-output_cx, fromy-output_cy);
  output_cx = tox; output_cy = toy;
#endif
}

void flush_vectors() {
  for (int i = 0; i < next_line; i++) {
    Point mid = get_midpoint(line[i]);
    int cid = get_cluster_id(mid);
    if (cluster_count[cid] < MAX_CLUSTER_LINE) {
      cluster_segment[cid][cluster_count[cid]++] = &line[i];
    }
  }
  for (int cid = 0; cid < MAX_CLUSTERS; cid++) {
    if (cluster_count[cid] == 0) continue;
    DrawOrder local_order[MAX_CLUSTER_LINE];
    optimise_within_a_cluster(cluster_segment[cid], cluster_count[cid], local_order);
    for (int i = 0; i < cluster_count[cid]; i++) {
      final_order[final_count++] = local_order[i];
    }
  }
  for (int i = 0; i < final_count; i++) {
    DrawOrder order = final_order[i];
    Line S;
    S.p[0] = get_endpoint_index(order.line_idx, order.reversed, 0);
    S.p[1] = get_endpoint_index(order.line_idx, order.reversed, 1);
    draw_line(S);
  }
  final_count = 0;  // Reset buffer
}

#ifdef PRINT_STATS
static int input_dist = 0;
#endif

void buffered_draw_line(Point p[2]) {
  static int next_point = 0;

#ifdef PRINT_STATS
  static int input_cx = 0, input_cy = 0;
  int fromx, fromy, tox, toy;
  fromx = p[0].x; fromy = p[0].y; tox = p[1].x; toy = p[1].y;
  // input_dist += isqrt((fromx-input_cx)*(fromx-input_cx) + (fromy-input_cy)*(fromy-input_cy));
  input_dist += iabsmax(fromx-input_cx, fromy-input_cy);
  input_cx = tox; input_cy = toy;
#endif
  
  if (next_point + 2 >= MAX_POINT || next_line >= MAX_LINE) {
    flush_vectors();
#ifndef SUPPRESS_IO
    printf("// Next batch...\n");
#endif
    for (int i = 0; i < MAX_CLUSTERS; i++) cluster_count[i] = 0;
    next_point = 0; next_line = 0; final_count = 0;
  }

  point[next_point] = p[0]; point[next_point+1] = p[1];
  line[next_line++] = (Line){{next_point, next_point + 1}};
  next_point += 2;
}

int main(int argc, char **argv) {
  for (int i = 0; i < 300; i++) {
    Point p[2];
    p[0] = (Point){((unsigned int)random() & 255) - 128, ((unsigned int)random() & 255) - 128};
    p[1] = (Point){((unsigned int)random() & 255) - 128, ((unsigned int)random() & 255) - 128};
    buffered_draw_line(p);
  }
  flush_vectors();

#ifdef PRINT_STATS
  // Changing the distance metric from hypotenuse to max(abs(dx), abs(dy)) did not make a significant difference
  // to the results (abt 37% improvement over unoptimised vs 39%) so the speed up from using the simpler
  // metric was worth doing.
  // Initial experiments suggest that 200 to 300 vectors should comfortably be able to be reordered in the alloted time,
  // which is far more than a game is likely to be drawing within a frame.
  printf("Unoptimised total move distance: %d\n", input_dist);
  printf("  Optimised total move distance: %d\n", output_dist);
  printf("%d%% movement saved.\n", (input_dist-output_dist)*100/input_dist);
#endif

  exit(EXIT_SUCCESS);
  return EXIT_FAILURE;
}