#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <pthread.h>

#define N 1000
#define P 4
#define NMTX (P * P)
#define EDGEPROB 0.005

char G[N][N];
char visited[N];
int Q[N];
int F, B;
int C[P][2];

pthread_mutex_t Qmtx;
pthread_mutex_t Vmtx[NMTX];
pthread_barrier_t Lbar;

void setup ( )
{
   int i, j;

   printf("+++ Master thread calling setup\n");

   for (i=0; i<N; ++i) {
      G[i][i] = 0;
      for (j=0; j<i; ++j)
         G[i][j] = G[j][i] = (((double)rand() / (double)RAND_MAX) <= EDGEPROB) ? 1 : 0;
   }
   printf("    Adjacency matrix created\n");

   Q[0] = 0; F = B = 0;
   printf("    Q initialized\n");

   visited[0] = 1; for (i=0; i<N; ++i) visited[i] = 0;
   printf("    visited[] initialized\n");

   pthread_mutex_init(&Qmtx, NULL);
   pthread_mutex_trylock(&Qmtx);
   pthread_mutex_unlock(&Qmtx);
   printf("    Mutex for Q initialized\n");

   for (i=0; i<NMTX; ++i) {
      pthread_mutex_init(Vmtx+i, NULL);
      pthread_mutex_trylock(Vmtx + i);
      pthread_mutex_unlock(Vmtx + i);
   }
   printf("    %d mutexes for visited[] initialized\n", NMTX);

   pthread_barrier_init(&Lbar, NULL, P+1);
   printf("    Loop-sync barrier initialized\n");
}

void printgraph ( )
{
   int i, j;

   printf("\n+++ Input graph\n");
   for (i=0; i<N; ++i) {
      printf("    %-4d ->", i);
      for (j=0; j<N; ++j) {
         if (G[i][j]) printf(" %4d", j);
      }
      printf("\n");
   }
   printf("\n");
}

void *tmain ( void *arg )
{
   int tno;
   pthread_t tid;
   int start, end, u, v, qpos;
   int mtxno;
   int *bfsparent, *bfschild, nadd;

   tno = *((int *)arg);
   tid = pthread_self();
   printf("+++ Worker thread %d with ID %lu starting\n", tno, tid);

   bfsparent = (int *)malloc(N * sizeof(int));
   bfschild = (int *)malloc(N * sizeof(int));

   pthread_barrier_wait(&Lbar);

   while (1) {
      pthread_barrier_wait(&Lbar);
      start = C[tno][0]; end = C[tno][1];
      printf("--- Worker %d gets Q chunk [%-4d,%4d]\n", tno, start, end);
      usleep(1000);
      if (start < 0) {
         pthread_barrier_wait(&Lbar);
         break;
      }
      nadd = 0;
      for (qpos=start; qpos<=end; ++qpos) {
         u = Q[qpos];
         for (v=0; v<N; ++v) {
            if (G[u][v] == 1) {
               mtxno = v % NMTX;
               pthread_mutex_lock(Vmtx + mtxno);
               if (!visited[v]) {
                  visited[v] = 1;
                  bfsparent[nadd] = u;
                  bfschild[nadd] = v;
                  ++nadd;
               }
               pthread_mutex_unlock(Vmtx + mtxno);
            }
         }
      }
      pthread_mutex_lock(&Qmtx);
      for (u=0; u<nadd; ++u) {
         Q[++B] = bfschild[u];
         printf("    Worker thread %d adds the BFS link (%4d,%4d), and enqueues Q[%4d] = %4d\n",
                     tno, bfsparent[u], bfschild[u], B, Q[B]);
      }
      pthread_mutex_unlock(&Qmtx);
      pthread_barrier_wait(&Lbar);
   }

   free(bfsparent);
   free(bfschild);

   printf("+++ Worker thread %d with ID %lu going to finish\n", tno, tid);
   pthread_exit(NULL);
}

void createworkers ( pthread_t tid[], int tno[] )
{
   int i;

   for (i=0; i<P; ++i) {
      tno[i] = i;
      pthread_create(tid+i,NULL,tmain,(void *)(tno+i));
   }
}

void coordinate ()
{
   int i, start, end, chunksize, l;

   pthread_barrier_wait(&Lbar);

   l = 1;
   while (1) {
      // sleep(1);
      if (F > B) {
         printf("\n+++ All reachable nodes are visited. Current Q position is [%4d,%4d].\n\n", F, B);
         for (i=0; i<P; ++i) { C[i][0] = -1; C[i][1] = 0; }
         pthread_barrier_wait(&Lbar);
         pthread_barrier_wait(&Lbar);
         break;
      }
      printf("\n+++ Level %d starts. Current Q position is [%4d,%4d].\n", l, F, B);
      chunksize = (B - F + P) / P;
      start = F;
      for (i=0; i<P; ++i) {
         end = start + chunksize - 1;
         if (end > B) end = B;
         C[i][0] = start;
         C[i][1] = end;
         start = end + 1;
      }
      F = B + 1;
      pthread_barrier_wait(&Lbar);
      pthread_barrier_wait(&Lbar);
      printf("+++ Level %d ends\n", l);
      ++l;
   }
}

void waitforworkers ( pthread_t tid[] )
{
   int i;

   usleep(1000);
   printf("\n+++ Master thread waits for workers to terminate\n");
   for (i=0; i<P; ++i) {
      pthread_join(tid[i], NULL);
      printf("    Worker thread %d with ID %lu terminated\n", i, tid[i]);
   }
}

void windup ()
{
   int i;

   printf("\n+++ Master thread winds up\n");
   pthread_mutex_destroy(&Qmtx);
   for (i=0; i<NMTX; ++i) pthread_mutex_destroy(Vmtx + i);
   pthread_barrier_destroy(&Lbar);
}

int main ()
{
   pthread_t tid[P];
   int tno[P];

   srand((unsigned int)time(NULL));

   setup();
   printgraph();
   createworkers(tid,tno);
   coordinate();
   waitforworkers(tid);
   windup();
}