/*

  Benchmark the `matchBud' function against an implementation in C.

  The graph implementation here is based upon the one used by plantri.

*/

#include "bench_match_bud.h"
#define mod(r,m) (((r)<0) ? ((r)%(m) + (m)) : ((r)%(m)))
#define min(X,Y) (((X) < (Y)) ? (X) : (Y))

static void cloneEdge(EDGE *src, EDGE *dest, node nOffset, edgeIndex eOffset);
static node addIncomingBud(GRAPH *gr, edgeIndex e);
static void printQueue(DEQUEUE *dq);

/*
  ddl already has the root node.

  ddr is _not_ touched (ddl is assumed to be mid-way through the
  merging process).

  Procedure to follow:

  * Merge graphs

  * Add incoming bud bd to root node after merged ddr.

  * Use merged graphs to adjust right queues.

  * Match as many ddl->outer to ddr->inner

  * Merge remaining queues

  * If any outgoing buds remaining, match to bd.

*/
void
mergeBlackSub(DANGD *ddl, DANGD *ddr) {
  printf("merge!\n");
  printf("testing: %d\n", (ddl -> graph -> firstEdge)[NULL_NODE]);
  int i;

  node nOffset;
  edgeIndex eOffset;

  GRAPH *gr = ddl -> graph;

  printf("(alias) Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start);

  appendGraph(ddl -> graph, ddr -> graph, &nOffset, &eOffset);

  printf("%d %d\n", nOffset, eOffset);

  printf("%d %d\n", gr -> nextNode, gr -> nextEdge);

  printf("(after append) Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start);

  printf("appended: %d %d\n", nOffset, eOffset);
  /* Will have a root node as it's constructed to have one. */
  node bd = addIncomingBud(gr, (gr -> firstEdge)[ddl -> rootNode]);
  printf("bud added: %d\n", bd);
  printf("Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start);

  printf("ddr -> outBuds\n");
  printQueue(ddr -> outBuds);

  node orv[ddr -> outBuds -> qLen];
  printf("Created array for blank queue\n");
  DEQUEUE *or = malloc(sizeof(DEQUEUE));
  printf("Created pointer for blank queue\n");
  or -> q = orv;
  printf("Created blank queue?\n");
  cloneQueue(ddr -> outBuds, or, nOffset);
  printf("cloned ddr -> outBuds\n");

  printQueue(or);

  printf("(clone out queue) Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start);


  /* printf("ddr -> inBuds\n"); */
  /* printQueue(ddr -> inBuds); */
  /* node irv[ddr -> inBuds -> qLen]; */
  /* DEQUEUE *ir = malloc(sizeof(DEQUEUE)); */
  /* ir -> q = irv; */
  /* cloneQueue(ddr -> inBuds, ir, nOffset); */
  /* printf("cloned ddr -> inBuds\n"); */
  /* printQueue(ir); */

  /* int qm = min(queueSize(ddl -> outBuds), queueSize(ir)); */
  /* node ob, ib; */

  /* printf("(pre-match) Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start); */

  /* for (i = 0; i < qm; i++) { */
  /*   ob = dequeueF(ddl -> outBuds); */
  /*   ib = dequeueF(ir); */
  /*   printf("ob = %d, ib = %d\n", ob, ib); */

  /*   printf("Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start); */

  /*   matchBud(gr, ob, ib); */
  /* } */


  /* printf("(matched) Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start); */

  /* appendQueue(ddl -> inBuds, ir); */
  /* prependQueue(ddl -> outBuds, or); */

  /* if (queueSize(ddl -> outBuds) > 0) { */
  /*   ob = dequeueF(ddl -> outBuds); */
  /*   printf("have outoing bud! %d\n", ob); */
  /*   matchBud(gr, ob, bd); */
  /* } else { */
  /*   enqueueE(ddl -> inBuds, bd); */
  /* } */

  free(or);
  //free(ir);

  printf("C is done\n");

  printf("Last edge from: %d\n", (gr -> edges)[gr -> numEdges - 1].start);

}

void
cloneDANGD(DANGD *src, DANGD *dest) {
  dest -> rootNode = src -> rootNode;

  cloneGraph(src -> graph, dest -> graph);

  cloneQueue(src -> outBuds, dest -> outBuds, 0);

  cloneQueue(src -> inBuds, dest -> inBuds, 0);
}

/* This is done manually rather than using addEdge. */
static node
addIncomingBud(GRAPH *gr, edgeIndex fE) {
  node n = gr -> nextNode;
  gr -> nextNode = n + 1;

  edgeIndex e = gr -> nextEdge;
  edgeIndex ei = e + 1;
  gr -> nextEdge = ei + 1;

  edgeIndex *ns = gr -> firstEdge;
  EDGE *es = gr -> edges;

  node f = es[fE].start;

  es[e].start = f;
  es[e].end = n;
  es[e].prev = es[fE].prev;
  es[e].next = fE;
  es[e].invers = ei;
  es[es[fE].prev].next = e;
  es[fE].prev = e;

  es[ei].start = n;
  es[ei].end = f;
  es[ei].prev = ei; /* single edge! */
  es[ei].next = ei;
  es[ei].invers = e;

  ns[f] = e;
  ns[n] = ei;

  return n;
}

void
matchBud(GRAPH *gr, node ob, node ib) {
  edgeIndex *ns = gr -> firstEdge;
  EDGE *es = gr -> edges;

  edgeIndex oe = es[ns[ob]].invers;
  edgeIndex oeBefore = es[oe].next;

  edgeIndex ie = es[ns[ib]].invers;
  edgeIndex ieBefore = es[ie].next;

  deleteNode(gr, ob);
  deleteNode(gr, ib);
  addEdge(gr, oeBefore, ieBefore);
}

/*
   Assumes the node is in the graph.

   This isn't really correct, as the node is still in the array of
   edgeIndexes, but the alternative would be to re-allocate the array.

 */
void
deleteNode(GRAPH *gr, node n) {
  EDGE *es = gr -> edges;
  edgeIndex *ns = gr -> firstEdge;

  edgeIndex thisEdge = ns[n];

  if (thisEdge < 0) {
    return;
  }

  edgeIndex nextEdge = es[thisEdge].next;

  while (thisEdge != nextEdge) {

    deleteEdge(gr, thisEdge);

    thisEdge = nextEdge;
    nextEdge = es[thisEdge].next;
  }

  /* Last edge to take care of. */
  deleteEdge(gr, thisEdge);

  /* Can't actually "delete" it, so set a nonsensical first edge. */
  ns[n] = NULL_EDGE;

}

/*
  Unlike the equivalent Haskell version, this assumes you want the
  half-edges added before the provided half-edges, rather than
  allowing the caller to choose.
*/
edgeIndex
addEdge(GRAPH *gr, edgeIndex fE, edgeIndex tE) {
  edgeIndex e = gr -> nextEdge;
  edgeIndex ei = e + 1;
  gr -> nextEdge = ei + 1;

  edgeIndex *ns = gr -> firstEdge;
  EDGE *es = gr -> edges;

  node from = es[fE].start;
  node to = es[tE].start;

  es[e].start = from;
  es[e].end = to;
  es[e].prev = es[fE].prev;
  es[e].next = fE;
  es[e].invers = ei;
  es[es[fE].prev].next = e;
  es[fE].prev = e;

  es[ei].start = to;
  es[ei].end = from;
  es[ei].prev = es[tE].prev;
  es[ei].next = tE;
  es[ei].invers = e;
  es[es[tE].prev].next = ei;
  es[tE].prev = ei;

  ns[from] = e;
  ns[to] = ei;

  return e;
}

/*
   Assumes the edge is in the graph.

   This isn't really correct, as the edge is still in the array of
   edges, but the alternative would be to re-allocate the array.

 */
void
deleteEdge(GRAPH *gr, edgeIndex e) {
  EDGE *es = gr -> edges;
  edgeIndex *ns = gr -> firstEdge;

  node from = es[e].start;
  node to = es[e].end;

  es[es[e].prev].next = es[e].next;
  es[es[e].next].prev = es[e].prev;

  if (ns[from] == e) {
    if (es[e].next == e) {
      // deg == 1
      ns[from] = EPHEMERAL_EDGE;
    } else {
      ns[from] = es[e].next;
    }
  }

  edgeIndex ei = es[e].invers;

  es[es[ei].prev].next = es[ei].next;
  es[es[ei].next].prev = es[ei].prev;

  if (ns[to] == ei) {
    if (es[ei].next == ei) {
      // deg == 1
      ns[to] = EPHEMERAL_EDGE;
    } else {
      ns[to] = es[ei].next;
    }
  }

  // To "delete" it, we set nonsensical nodes and edges.
  es[e].start = es[e].end = es[ei].start = es[ei].end = NULL_NODE;
  es[e].prev = es[e].next = es[e].invers = NULL_EDGE;
  es[ei].prev = es[ei].next = es[ei].invers = NULL_EDGE;
}

void
initGraph(int numNodes, int numEdges, GRAPH *gr) {
  int c;

  gr = malloc(sizeof(GRAPH));

  gr -> numNodes = numNodes;

  gr -> nextNode = 0;

  edgeIndex ns[numNodes];

  for (c = 0; c < numNodes; c++) {
    ns[c] = NULL_EDGE;
  }

  gr -> firstEdge = ns;

  gr -> numEdges = numEdges;

  gr -> nextEdge = 0;

  EDGE es[numEdges];

  for (c = 0; c < numNodes; c++) {
    es[c].start = NULL_NODE;
    es[c].end = NULL_NODE;
    es[c].prev = NULL_EDGE;
    es[c].next = NULL_EDGE;
    es[c].invers = NULL_EDGE;
  }

  gr -> edges = es;
}

/*
void
test(DANGD *dd, GRAPH *gr) {
  //gr = malloc(sizeof(GRAPH));

  gr -> firstEdge = malloc((dd -> graph -> numNodes) * sizeof(edgeIndex));

  gr -> edges = malloc((dd -> graph -> numEdges) * sizeof(EDGE));

  cloneGraph(dd -> graph, gr);

  deleteNode(gr, dd -> outBud);
}
*/

static void
cloneEdge(EDGE *src, EDGE *dest, node nOffset, edgeIndex eOffset) {
  dest -> start = src -> start + nOffset;
  dest -> end = src -> end + nOffset;
  dest -> prev = src -> prev + eOffset;
  dest -> next = src -> next + eOffset;
  dest -> invers = src -> invers + eOffset;
}

/* Assumes both inputs are the same size and have sufficient room. */
void
mergeGraphs(GRAPH *gr1, GRAPH *gr2, GRAPH *dest, node *nOffset, edgeIndex *eOffset) {
  int i;

  int ord1 = gr1 -> nextNode;
  int ord2 = gr2 -> nextNode;
  int ord = ord1 + ord2;
  dest -> numNodes = gr1 -> numNodes;
  dest -> nextNode = ord - 1;
  *nOffset = ord1;

  int sz1 = gr1 -> nextEdge;
  int sz2 = gr2 -> nextEdge;
  int sz = sz1 + sz2;
  dest -> numEdges = gr2 -> numEdges;
  dest -> nextNode = sz - 1;
  *eOffset = sz1;

  for (i = 0; i < ord1; i++) {
    (dest -> firstEdge)[i] = (gr1 -> firstEdge)[i]; // No offset
  }

  for (i = ord1; i < ord; i++) {
    (dest -> firstEdge)[i] = (gr2 -> firstEdge)[i-ord1] + *eOffset;
  }

  for (i = 0; i < sz1; sz++) {
    cloneEdge(gr1 -> edges + i, dest -> edges + i, 0, 0); // No offset
  }

  for (i = sz1; i < sz; i++) {
    cloneEdge(gr2 -> edges + i - sz1, dest -> edges + i, *nOffset, *eOffset);
  }

}

/* Assumes first graph has sufficient room. */
void
appendGraph(GRAPH *gr1, GRAPH *gr2, node *nOffset, edgeIndex *eOffset) {
  int i;

  printf("I haz an 'i'!\n");

  int ord1 = gr1 -> nextNode;
  printf("ord1 = %d\n",ord1);
  int ord2 = gr2 -> nextNode;
  printf("ord2 = %d\n",ord2);
  int ord = ord1 + ord2;
  printf("ord = %d\n",ord);
  gr1 -> nextNode = ord;
  printf("setting gr1 -> nextNode\n");
  *nOffset = ord1;


  printf("specified node vals\n");

  int sz1 = gr1 -> nextEdge;
  printf("sz1 = %d\n", sz1);
  int sz2 = gr2 -> nextEdge;
  int sz = sz1 + sz2;
  gr1 -> nextEdge = sz;
  *eOffset = sz1;

  printf("specified edge vals\n");

  for (i = 0; i < ord2; i++) {
    (gr1 -> firstEdge)[i + ord1] = (gr2 -> firstEdge)[i] + sz1;
  }

  printf("nodes cloned!\n");

  for (i = 0; i < sz2; i++) {
    cloneEdge(gr2 -> edges + i, gr1 -> edges + i + sz1, ord1, sz1);
  }

  printf("edges cloned!\n");

  printf("Last edge from: %d\n", (gr1 -> edges)[gr1 -> numEdges - 1].start);
}

/* Either ldd -> inBuds or rdd -> outBuds should be empty. */
void
mergeDangDs(DANGD *ldd, DANGD *rdd, DANGD *res) {
  node *nOffset;
  edgeIndex *eOffset;
  mergeGraphs(ldd -> graph, rdd -> graph, res -> graph, nOffset, eOffset);
  /* Note the different order! */
  mergeQueues(rdd -> outBuds, ldd -> outBuds, res -> outBuds);
  mergeQueues(ldd -> inBuds, rdd -> inBuds, res -> inBuds);
}

void
cloneGraph(GRAPH *src, GRAPH *dest) {
  int i;

  dest -> numNodes = src -> numNodes;
  dest -> nextNode = src -> nextNode;

  for (i = 0; i < src -> numNodes; i++) {
    (dest -> firstEdge)[i] = (src -> firstEdge)[i];
  }


  dest -> numEdges = src -> numEdges;
  dest -> nextEdge = src -> nextEdge;

  for (i = 0; i < src -> numEdges; i++) {
    cloneEdge(src -> edges + i, dest -> edges + i, 0, 0); /* no offset */
  }

}

/*

   Simplified double-ended queue functions; assumes:

   * Always have empty spot when queueing.

   * Once we empty a queue, we never want to put anything back.

   * We have elements in the queue before we dequeue.

*/

void
enqueueF(DEQUEUE *dq, node n) {
  int i = mod((dq -> startInd) - 1, dq -> qLen);

  (dq -> q)[i] = n;

  dq -> startInd = i;
}

void
enqueueE(DEQUEUE *dq, node n) {
  int i = mod((dq -> endInd) + 1, dq -> qLen);

  (dq -> q)[i] = n;

  dq -> endInd = i;
}

node
dequeueF(DEQUEUE *dq) {
  int i = dq -> startInd;

  node n = (dq -> q)[i];

  (dq -> q)[i] = NULL_NODE;

  dq -> startInd = mod(i + 1, dq -> qLen);

  return n;
}


node
dequeueE(DEQUEUE *dq) {
  int i = dq -> endInd;

  node n = (dq -> q)[i];

  (dq -> q)[i] = NULL_NODE;

  dq -> endInd = mod(i - 1, dq -> qLen);

  return n;
}

int
queueSize(DEQUEUE *dq) {
  return mod((dq -> endInd) - (dq -> startInd) + 1, dq -> qLen);
}

void
appendQueue(DEQUEUE *q1, DEQUEUE *q2) {
  int len2 = queueSize(q2);
  int start2 = q2 -> startInd;
  int totLen = q2 -> qLen;

  int i;

  for (i = 0; i < len2; i++) {
    enqueueE(q1,(q2 -> q)[mod(i + start2, totLen)]);
  }

}

void
prependQueue(DEQUEUE *q1, DEQUEUE *q2) {
  int len2 = queueSize(q2);
  int end2 = q2 -> endInd;
  int totLen = q2 -> qLen;

  int i;

  for (i = 0; i < len2; i++) {
    enqueueF(q1,(q2 -> q)[mod(end2 - i, totLen)]);
  }
}

void
mergeQueues(DEQUEUE *q1, DEQUEUE *q2, DEQUEUE *dest) {
  int len1 = queueSize(q1);
  int len2 = queueSize(q2);
  int len = len1 + len2;

  int totLen = q1 -> qLen; // same for both

  dest -> qLen = totLen;
  dest -> startInd = 0; // q1 -> startInd;
  dest -> endInd = len - 1; // -1 for 0-indexed
  //mod((dest -> startInd) + len, totLen);

  int i;

  for (i = 0; i < len1; i++) {
    (dest -> q)[i] = (q1 -> q)[mod(i + (q1 -> startInd),totLen)];
  }

  for (i = len1; i < len; i++) {
    (dest -> q)[i] = (q2 -> q)[mod(i + (q2 -> startInd),totLen)];
  }

  for (i = len; i < totLen; i++) {
    (dest -> q)[i] = NULL_NODE;
  }
}

void cloneQueue(DEQUEUE *src, DEQUEUE *dest, node offset) {
  int len = src -> qLen;

  dest -> qLen = len;
  dest -> startInd = src -> startInd;
  dest -> endInd = src -> endInd;

  int i;
  node n;

  for (i = 0; i < len; i++) {
    n = (src -> q)[i];

    (dest -> q)[i] = (n == NULL_NODE) ? NULL_NODE : (n + offset);
  }
}

static void
printQueue(DEQUEUE *dq) {
  printf("Printing double-ended queue:\n");
  printf("Queue size: %d\n", queueSize(dq));
  printf("Queue storage size: %d\n", dq -> qLen);
  printf("Queue start: %d\n", dq -> startInd);
  printf("Queue end: %d\n", dq -> endInd);

  int i;

  for (i = 0; i < dq -> qLen; i++) {
    printf("\t%d : %d\n", i, (dq -> q)[i]);
  }
}