#include "mat_mul.h"

#include <cstdlib>
#include <cstdio>
#include <pthread.h>

#define MAX_THREADS (40)

#define min(a,b) (((a) < (b)) ? (a) : (b))
#define max(a,b) (((a) < (b)) ? (b) : (a))
#define BLOCKSIZE 32
#define UNROLL    4

static float *A, *B, *C;
static int M, N, K;
static int num_threads;
static pthread_t threads[MAX_THREADS];

static void* mat_mul_thread(void *data) {
  int pid = (int)((unsigned long)data & 0xFFFFFFFFUL);

  int slice = M / num_threads;
  int start = pid * slice;
  int end = pid == num_threads - 1 ? M : (pid + 1) * slice;
  
  for (int kk = 0; kk < K; kk += BLOCKSIZE)
  {
    const int endK = min(kk + BLOCKSIZE, K);

    for (int i = start; i < end; ++i)
    {
      float *pA = &A[i * K];
      float *pC = &C[i * N];

      for (int k = kk; k < endK; ++k)
      {
        float Aik = pA[k];
        float *pB = &B[k * N];

        int j = 0;
        for (; j < N - UNROLL; j += UNROLL)
        {
          pC[j + 0] += Aik * pB[j + 0];
          pC[j + 1] += Aik * pB[j + 1];
          pC[j + 2] += Aik * pB[j + 2];
          pC[j + 3] += Aik * pB[j + 3];
        }
        for (; j < N; ++j)
        {
          pC[j] += Aik * B[k * N + j];
        }
      }
    }
  }

  return NULL;
}

void mat_mul(float *_A, float *_B, float *_C, int _M, int _N, int _K, int _num_threads) {
  A = _A, B = _B, C = _C;
  M = _M, N = _N, K = _K;
  num_threads = min(_num_threads, _M);

  for (int i = 0; i < num_threads; i++)
  {
    pthread_create(&threads[i], NULL, mat_mul_thread, (void*)(unsigned long)i);
  }

  for (int i = 0; i < num_threads; i++)
  {
    pthread_join(threads[i], NULL);
  }
}