#include "graph/bfs.h"
#include "graph/search.h"
#include <errno.h>
#include <stdlib.h>

static int
bfs_ (struct graph *g, struct graph *bt, struct state *vs, unsigned int ri)
{
  struct squeue sq;
  struct vertex *r, *n;
  struct state *rs, *ns;
  struct edge *e;
  unsigned int ni;
  int err;

  if (!(sq.q = calloc (g->nv, sizeof *sq.q)))
    return -errno;

  sq.f = sq.l = sq.q;
  *sq.f = &g->v[ri];
  vs[ri].c = RED;

  while (sq.f <= sq.l)
    {
      r = *sq.f;
      ri = vertex_id (g, r);
      rs = &vs[ri];
      e = r->adj;

      while (e && (n = e->to))
        {
          ni = vertex_id (g, n);
          ns = &vs[ni];

          if (ns->c == WHITE)
            {
              ns->c = RED;

              if ((err = add_arc (bt, ri, ni)))
                {
                  free (sq.q);
                  return err;
                }

              *(++sq.l) = n;
            }

          e = e->next;
        }

      rs->c = BLUE;
      ++sq.f;
    }

  free (sq.q);
  return 0;
}

struct graph *
bfs (struct graph *g)
{
  struct graph *bt;
  struct state *vs;
  unsigned int vi;

  if (!(bt = new_graph (g->nv)))
    goto err_new_graph;

  if (!(vs = calloc (g->nv, sizeof *vs)))
    goto err_calloc;

  for (vi = 0; vi < g->nv; ++vi)
    if (vs[vi].c == WHITE)
      if (bfs_ (g, bt, vs, vi))
        goto err_bfs_;

  free (vs);
  return bt;

err_bfs_:
  free (vs);
err_calloc:
  free_graph (bt);
err_new_graph:
  return NULL;
}