void *block_reverse (void *arg)
{
  args *my_arg = (args *) arg;
  int idx_p = my_arg->thread_idx,
      p = my_arg->p,
      n = my_arg->n,
      m = my_arg->m,
      s = my_arg->s,
      l = my_arg->l;
  double *mtx = my_arg->mtx,
         *ans = my_arg->ans;
  char *fname = my_arg->sourse;
  double t_time = 0., time = 0., resid_time = 0.;
  static bool err_flag = false;
  static pthread_mutex_t mutex_err = PTHREAD_MUTEX_INITIALIZER;

  cpu_set_t cpu;
  CPU_ZERO (&cpu);
  CPU_SET (idx_p, &cpu);
  pthread_setaffinity_np (pthread_self(), sizeof (cpu), &cpu);

  for (int i = idx_p; i < l; i += p)
    {
      clean (mtx + i * m * n, m, n);
      clean (ans + i * m * n, m, n);
    }
  if (s && idx_p == l % p)
    {
      clean (mtx + l * m * n, s, n);
      clean (ans + l * m * n, s, n);
    }
  /*
  int i, j, shift, x = n * m, y = m * m;
  for (i = 0; i < l; i++)
    {
      shift = (i + p - 1 - idx_p) / p;
      for (j = idx_p + num_of_threads * shift; j < l; j += p)
        {
          ptr = mtx + j * x + i * y;
          clean (ptr, m, m);
        }
      if (j == l)
        {
          ptr = mtx + l * x + i * s * m;
          clean (ptr, s, m);
        }
      shift = (i + p - idx_p) / p;
      for (j = idx_p + p * shift; j < l; j += p)
        {
          ptr = mtx + i * x + j * y;
          clean (ptr, m, m);
        }
      if (j == l)
        {
          ptr = mtx + i * x + l * y;
          clean (ptr, s, m);
        }
    }
  */
  reduce (p);
  if (fname)
    {
      if (idx_p == 0)
        {
          int ret = read_mtx (mtx, n, m, s, l, fname);
          if (ret < 0)
            {
              my_arg->err = -100;
              switch (ret)
                {
                  case -1:
                    fprintf (stdout, "Cannot open %s!\n", fname);
                    break;
                  case -2:
                    fprintf (stdout, "Cannot read %s!\n", fname);
                    break;
                  case -3:
                    fprintf (stdout, "%s is empty!\n", fname);
                    break;
                  case -4:
                    fprintf (stdout, "Not enough data in %s!\n", fname);
                    break;
                  default:
                    fprintf (stdout, "Error %d in %s!\n", ret, fname);
                }
              return 0;
            }
        }
      reduce (p);
    }
  else
    {
      if (idx_p == 0)
        init_mtx (mtx, n, m, s, l, init);
      reduce (p);
    }
  if (idx_p == 0)
    {
      fprintf (stdout, "Matrix A:\n");
      print_mtx (stdout, mtx, n, m, s, l);
    }
  reduce (p);
  static double norm;
  if (idx_p == 0)
    norm = norm_matrix (mtx, n, m, s, l);
  reduce (p);
  my_arg->norm *= norm;

  if (idx_p == 0)
    time = get_time();

  t_time = get_cpu_time();
  LU_decomp (my_arg);
  for (int i = idx_p; i < l; i += p)
    copy (ans + i * m * n, mtx + i * m * n, m, n);
  if (s && idx_p == l % p)
    copy (ans + l * m * n, mtx + l * m * n, s, n);
  reduce (p);
  fprintf (stdout, "decomp: [%d] = %.3f\n", idx_p, get_cpu_time() - t_time);
  my_arg->time += (get_cpu_time() - t_time);
  t_time = get_cpu_time();
  reverse_L (my_arg);
  fprintf (stdout, "rev_L: [%d] = %.3f\n", idx_p, get_cpu_time() - t_time);
  my_arg->time += (get_cpu_time() - t_time);
  t_time = get_cpu_time();
  reverse_U (my_arg);
  fprintf (stdout, "rev_U: [%d] = %.3f\n", idx_p, get_cpu_time() - t_time);
  my_arg->time += (get_cpu_time() - t_time);
  t_time = get_cpu_time();
  mult_rev_UL (my_arg);
  fprintf (stdout, "mult_UL: [%d] = %.3f\n", idx_p, get_cpu_time() - t_time);
  my_arg->time += (get_cpu_time() - t_time);

  if (idx_p == 0)
    time = get_time() - time;

  if (my_arg->err != 0)
    {
      pthread_mutex_lock (&mutex_err);
      err_flag = true;
      pthread_mutex_unlock (&mutex_err);
    }
  reduce (p);
  if (!err_flag)
    {
      if (idx_p == 0)
        {
          //time = my_arg->time;
          fprintf (stdout, "\nMatrix A^(-1):\n");
          print_mtx (stdout, ans, n, m, s, l);
          fprintf (stdout, "Elapsed: %.2f\n", time);
        }
      if (!(n > 6000 && p == 1))
        {
          if (idx_p == 0)
            {
              if (fname)
                read_mtx (mtx, n, m, s, l, fname);
              else
                init_mtx (mtx, n, m, s, l, init);
            }
          reduce (p);
          resid_time = get_time();
          block_residual (my_arg);
          reduce (p);
          resid_time = get_time() - resid_time;
          if (idx_p == 0)
            {
              fprintf (stdout, "Residual = %e\n", my_arg->norm);
              fprintf (stdout, "Elapsed for residual: %.2f\n", resid_time);
            }
        }
    }
  reduce (p);
  return 0;
}

void LU_decomp (args *my_arg)
{
  double *mtx = my_arg->mtx;
  int n = my_arg->n,
      m = my_arg->m,
      s = my_arg->s,
      l = my_arg->l;
  int idx_p = my_arg->thread_idx,
      p = my_arg->p;
  double *ptr = mtx, *ptr1 = mtx, *ptr2 = mtx, *diag = mtx;
  double *buf = new double [m * m];
  double *sum = new double [m * m];
  double *rev_diag = new double [m * m];

  copy (rev_diag, diag, m, m);
  if (mtx_reverse (rev_diag, buf, m, my_arg->norm) < 0)
    {
      my_arg->err = -1;
    }
  copy (rev_diag, buf, m, m);
  /*
  if (idx_p == 0)
    {
      for (int i = 1; i < l; i++)
        {
          ptr += m * m;
          mtx_prod (rev_diag, ptr, buf, m, m, m);
          copy (ptr, buf, m, m);
        }
      if (s)
        {
          ptr += m * m;
          mtx_prod (rev_diag, ptr, buf, m, m, s);
          copy (ptr, buf, m, s);
        }
    }
  */
  for (int i = 1 + idx_p; i < l; i += p)
    {
      ptr = mtx + m * m * i;
      mtx_prod (rev_diag, ptr, buf, m, m, m);
      copy (ptr, buf, m, m);
    }
  if (s && idx_p == 0)
    {
      ptr = mtx + l * m * m;
      mtx_prod (rev_diag, ptr, buf, m, m, s);
      copy (ptr, buf, m, s);
    }
  //
  reduce (p);
  diag += m * n + m * m;
  clean (buf, m, m);
  for (int i = 1 ; i < l ; i++)
    {
      int new_i = i + idx_p;
      int board_thread = (l - i) % p;
      ptr2 = mtx + new_i * m * n;
      for (int j = new_i ; j < l ; j += p)
        {
          clean (sum, m, m);
          ptr1 = mtx + i * m * m;
          for (int r = 0, k = 0 ; k < i ; r += m * m, k++)
            {
              mtx_prod (ptr2 + r, ptr1, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr1 += m * n;
            }
          mtx_dif (ptr2 + i * m * m, sum, ptr2 + i * m * m, m, m);
          ptr2 += p * m * n;
        }
      if (s && idx_p == board_thread)
        {
          clean (sum, s, m);
          ptr1 = mtx + i * m * m;
          for (int r = 0, k = 0 ; k < i ; r += m * s, k++)
            {
              mtx_prod (ptr2 + r, ptr1, buf, s, m, m);
              mtx_add (sum, buf, sum, s, m);
              ptr1 += m * n;
            }
          mtx_dif (ptr2 + i * m * s, sum, ptr2 + i * m * s, s, m);
        }
      reduce (p);
      copy (rev_diag, diag, m, m);
      if (mtx_reverse (rev_diag, buf, m, my_arg->norm) < 0)
         {
           my_arg->err = -1;
           break;
         }
      copy (rev_diag, buf, m, m);
      ptr2 = mtx + i * m * n;
      for (int k = new_i + 1, r = (new_i + 1) * m * m; k < l ; k += p, r += m * m * p)
        {
          clean (sum, m, m);
          ptr1 = mtx;
          for (int j = 0, row_i = 0 ; j < i ; j++, row_i += m * m)
            {
              mtx_prod (ptr2 + row_i, ptr1 + r, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr1 += m * n;
            }
          mtx_dif (ptr2 + r, sum, buf, m, m);
          mtx_prod (rev_diag, buf, ptr2 + r, m, m, m);
        }
      if (s && idx_p == board_thread)
        {
          clean (sum, m, s);
          ptr1 = mtx;
          for (int r = 0, k = 0 ; k < i ; r += m * m, k++)
            {
              mtx_prod (ptr2 + r, ptr1 + l * m * m, buf, m, m, s);
              mtx_add (sum, buf, sum, m, s);
              ptr1 += m * n;
            }
          mtx_dif (ptr2 + l * m * m, sum, buf, m, s);
          mtx_prod (rev_diag, buf, ptr2 + l * m * m, m, m, s);
        }
      diag += m * n + m * m;
      reduce (p);
    }
  if (s && idx_p == l % p)
    {
      ptr2 = mtx + l * m * n;
      clean (sum, s, s);
      ptr1 = mtx + l * m * m;
      for (int r = 0, k = 0 ; k < l ; r += m * s, k++)
        {
          mtx_prod (ptr2 + r, ptr1, buf, s, m, s);
          mtx_add (sum, buf, sum, s, s);
          ptr1 += m * n;
        }
      mtx_dif (ptr2 + l * m * s, sum, ptr2 + l * m * s, s, s);
    }
  delete [] buf;
  delete [] sum;
  delete [] rev_diag;
  reduce (p);
  return;
}

void reverse_L (args *my_arg)
{
  double *mtx = my_arg->mtx,
         *ans = my_arg->ans;
  int n = my_arg->n,
      m = my_arg->m,
      s = my_arg->s,
      l = my_arg->l;
  int idx_p = my_arg->thread_idx,
      p = my_arg->p;
  double *diag = mtx, *ptr1 = mtx, *ptr2 = mtx;
  double *rev_diag, *buf, *sum;
  buf = new double [m * m];
  sum = new double [m * m];
  rev_diag = new double [m * m];
  double *whole_row  = new double [m * n];
  for (int i = 0 ; i < l ; i++)
    {
      copy (rev_diag, diag, m, m);
      if (mtx_reverse (rev_diag, buf, m, my_arg->norm) < 0)
        {
          my_arg->err = -1;
        }
      if (idx_p == i % p)
        {
          copy (diag, buf, m, m);
        }
      reduce (p);
      copy (whole_row, ans, m, n);
      for (int j = idx_p, k = idx_p * m * m; j < i; j += p, k += m * m * p)
        {
          clean (sum, m, m);
          ptr2 = mtx + j * m * n;
          for (int cnt = j, r = j * m * m; cnt < i; cnt++, r += m * m)
            {
              mtx_prod (whole_row + r, ptr2 + k, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr2 += m * n;
            }
          clean (ptr1 + k, m, m);
          mtx_dif (ptr1 + k, sum, buf, m, m);
          mtx_prod (diag, buf, ptr1 + k, m, m, m);
        }
      ptr1 += m * n;
      ans += m * n;
      diag += m * n + m * m;
      reduce (p);
    }
  diag = mtx + l * (m * n + m * s);
  if (s)
    {
      copy (rev_diag, diag, s, s);
      if (mtx_reverse (rev_diag, buf, s, my_arg->norm) < 0)
        {
          my_arg->err = -1;
        }
      if (idx_p == l % p)
        {
          copy (diag, buf, s, s);
        }
      reduce (p);
      for (int j = idx_p, k = idx_p * m * m; j < l; j += p, k += m * m * p)
        {
          clean (sum, m, m);
          ptr2 = mtx + j * m * n;
          for (int cnt = j, r = j * m * s; cnt < l; cnt++, r += m * s)
            {
              mtx_prod (ans + r, ptr2 + k, buf, s, m, m);
              mtx_add (sum, buf, sum, s, m);
              ptr2 += m * n;
            }
          clean (ptr1 + j * m * s, s, m);
          mtx_dif (ptr1 + j * m * s, sum, buf, s, m);
          mtx_prod (diag, buf, ptr1 + j * m * s, s, s, m);
        }
    }
  delete [] buf;
  delete [] sum;
  delete [] rev_diag;
  delete [] whole_row;
  reduce (p);
  return;
}

void reverse_U (args *my_arg)
{
  double *mtx = my_arg->mtx,
         *ans = my_arg->ans;
  int n = my_arg->n,
      m = my_arg->m,
      s = my_arg->s,
      l = my_arg->l;
  int idx_p = my_arg->thread_idx,
      p = my_arg->p;
  double *ptr1 = mtx, *ptr2 = mtx;
  double *buf, *sum;
  buf = new double [m * m];
  sum = new double [m * m];
  for (int i = idx_p ; i < l ; i += p)
    {
      ptr1 = mtx + i * m * n;
      for (int j = i + 1, k = (i + 1) * m * m; j < l; j++, k += m * m)
        {
          clean (sum, m, m);
          ptr2 = ans + i * m * n;
          mtx_add (sum, ptr2 + k, sum, m, m);
          ptr2 += m * n;
          for (int cnt = i + 1, r = (i + 1) * m * m; cnt < j; cnt++, r += m * m)
            {
              mtx_prod (ptr1 + r, ptr2 + k, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr2 += m * n;
            }
          clean (ptr1 + k, m, m);
          mtx_dif (ptr1 + k, sum, ptr1 + k, m, m);
        }
      if (s)
        {
          clean (sum, m, s);
          ptr2 = ans + i * m * n;
          mtx_add (sum, ptr2 + l * m * m, sum, m, s);
          ptr2 += m * n;
          for (int cnt = i + 1, r = (i + 1) * m * m; cnt < l; cnt++, r += m * m)
            {
              mtx_prod (ptr1 + r, ptr2 + l * m * m, buf, m, m, s);
              mtx_add (sum, buf, sum, m, s);
              ptr2 += m * n;
            }
          clean (ptr1 + l * m * m, m, s);
          mtx_dif (ptr1 + l * m * m, sum, ptr1 + l * m * m, m, s);
        }
    }
  delete [] buf;
  delete [] sum;
  reduce (p);
  return;
}

void mult_rev_UL (args *my_arg)
{
  double *mtx = my_arg->mtx,
         *ans = my_arg->ans;
  int n = my_arg->n,
      m = my_arg->m,
      s = my_arg->s,
      l = my_arg->l;
  int idx_p = my_arg->thread_idx,
      p = my_arg->p;
  double *ptr_L = mtx, *ptr_U = mtx, *ptr_buf = ans;
  double *sum, *buf;
  buf = new double [m * m];
  sum = new double [m * m];
  for (int i = 0, row_i = 0; i < l; i++, row_i += m * n)
    {
      for (int j = idx_p, k = idx_p * m * m; j <= i; j += p, k += m * m * p)
        {
          clean (sum, m, m);
          ptr_L = mtx + (i + 1) * m * n;
          for (int cnt = i + 1, r = (i + 1) * m * m; cnt < l; cnt++, r += m * m)
            {
              mtx_prod (ptr_U + r, ptr_L + k, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr_L += m * n;
            }
          if (s)
            {
              mtx_prod (ptr_U + l * m * m, ptr_L + j * m * s, buf, m, s, m);
              mtx_add (sum, buf, sum, m, m);
            }
          mtx_add (mtx + row_i + k, sum, ptr_buf + k, m, m);
        }
      for (int j = i + 1 + idx_p, k = (i + 1 + idx_p) * m * m; j < l; j += p, k += m * m * p)
        {
          clean (sum, m, m);
          ptr_L = mtx + j * m * n;
          for (int cnt = j, r = k; cnt < l; cnt++, r += m * m)
            {
              mtx_prod (ptr_U + r, ptr_L + k, buf, m, m, m);
              mtx_add (sum, buf, sum, m, m);
              ptr_L += m * n;
            }
          if (s)
            {
              mtx_prod (ptr_U + l * m * m, ptr_L + j * m * s, buf, m, s, m);
              mtx_add (sum, buf, sum, m, m);
              ptr_L += m * n;
            }
          copy (ptr_buf + k, sum, m, m);
        }
      if (s && idx_p == i % p)
        {
          clean (sum, m, m);
          mtx_prod (ptr_U + l * m * m, mtx + l * m * n + l * m * s, buf, m, s, s);
          mtx_add (sum, buf, ptr_buf + l * m * m, m, s);
        }
      ptr_U += m * n;
      ptr_buf += m * n;
      reduce (p);
    }
  if (s && idx_p == l % p)
    {
      ptr_L = mtx + l * m * n;
      for (int j = 0, k = 0; j < l; j++, k += s * m)
        copy (ptr_buf + k, ptr_L + k, s, m);
      copy (ptr_buf + l * s * m, ptr_L + l * s * m, s, s);
    }
  delete [] buf;
  delete [] sum;
  reduce (p);
  return;
}