/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "common/c_math/linalg.h"

#include <assert.h>
#include <float.h>
#include <math.h>
#include <stddef.h>
#include <stdint.h>

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_vector.h>

#include "common/c_math/gsl_linalg_extra.h"
#include "common/c_math/linalg_common.h"

#if defined(__arm__)
// Turn off any error handling by the GSL library.  The standard way
// of doing this is with gsl_set_error_handler_off; however this
// method does not require bringing in more components of GSL.
void gsl_error(const char *reason, const char *file, int line, int gsl_errno) {
  (void)reason;
  (void)file;
  (void)line;
  (void)gsl_errno;
}
#endif  // defined(__arm__)

// Operations on vectors with any number of elements.

bool VecIsSize(const Vec *v, int32_t l) { return v != NULL && l == v->length; }

double *VecPtr(const Vec *v, int32_t i) {
  assert(0 <= i && i < v->length);
  return &v->d[i];
}

double VecGet(const Vec *v, int32_t i) {
  assert(0 <= i && i < v->length);
  return v->d[i];
}

// v = [0, 0, ... ]
const Vec *VecZero(Vec *v) {
  for (int32_t i = 0; i < v->length; ++i) {
    v->d[i] = 0.0;
  }
  return v;
}

// v_out = a*v + v_out
const Vec *VecAxpy(double a, const Vec *v, Vec *v_out) {
  assert(v->length == v_out->length);
  for (int32_t i = 0; i < v->length; ++i) {
    v_out->d[i] += a * v->d[i];
  }
  return v_out;
}

// Changes the length of a vector.  Returns true on success, false
// otherwise.
bool VecResize(int32_t length, Vec *v) {
  if (v->length == length) {
    return true;
  } else if (v->variable_len && 0 <= length && length <= v->max_length) {
    v->length = length;
    return true;
  }
  assert(false);
  return false;
}

// v_out = v_in
const Vec *VecCopy(const Vec *v_in, Vec *v_out) {
  VecResize(v_in->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = v_in->d[i];
  }
  return v_out;
}

// Copy the contents of an array in a Vec.
const Vec *VecInit(const double a_in[], int32_t length, Vec *v_out) {
  VecResize(length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = a_in[i];
  }
  return v_out;
}

// v_out = scale * v_in
const Vec *VecScale(const Vec *v_in, double scale, Vec *v_out) {
  VecResize(v_in->length, v_out);
  for (int32_t i = 0; i < v_in->length; ++i) {
    v_out->d[i] = scale * v_in->d[i];
  }
  return v_out;
}

// v_out = v0 + v1
const Vec *VecAdd(const Vec *v0, const Vec *v1, Vec *v_out) {
  assert(v0->length == v1->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = v0->d[i] + v1->d[i];
  }
  return v_out;
}

// v_out = v0 + v1 + v2
const Vec *VecAdd3(const Vec *v0, const Vec *v1, const Vec *v2, Vec *v_out) {
  assert(v0->length == v1->length && v0->length == v2->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = v0->d[i] + v1->d[i] + v2->d[i];
  }
  return v_out;
}

// v_out = v0 - v1
const Vec *VecSub(const Vec *v0, const Vec *v1, Vec *v_out) {
  assert(v0->length == v1->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = v0->d[i] - v1->d[i];
  }
  return v_out;
}

// v_out = c0 * v0 + c1 * v1
const Vec *VecLinComb(double c0, const Vec *v0, double c1, const Vec *v1,
                      Vec *v_out) {
  assert(v0->length == v1->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = c0 * v0->d[i] + c1 * v1->d[i];
  }
  return v_out;
}

// v_out = c0 * v0 + c1 * v1 + c2 * v2
const Vec *VecLinComb3(double c0, const Vec *v0, double c1, const Vec *v1,
                       double c2, const Vec *v2, Vec *v_out) {
  assert(v0->length == v1->length && v0->length == v2->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = c0 * v0->d[i] + c1 * v1->d[i] + c2 * v2->d[i];
  }
  return v_out;
}

// v_out = v0 .* v1
const Vec *VecMult(const Vec *v0, const Vec *v1, Vec *v_out) {
  assert(v0->length == v1->length);
  VecResize(v0->length, v_out);
  for (int32_t i = 0; i < v_out->length; ++i) {
    v_out->d[i] = v0->d[i] * v1->d[i];
  }
  return v_out;
}

// v0^T * v1
double VecDot(const Vec *v0, const Vec *v1) {
  assert(v0->length == v1->length);
  double tmp = 0.0;
  for (int32_t i = 0; i < v0->length; ++i) {
    tmp += v0->d[i] * v1->d[i];
  }
  return tmp;
}

// Computes the 2-norm of a vector.
//
// TODO: This does not yet handle overflow and underflow
// gracefully.
double VecNorm(const Vec *v) { return sqrt(VecNormSquared(v)); }

double VecNormBound(const Vec *v, double low) { return fmax(low, VecNorm(v)); }

double VecNormSquared(const Vec *v) { return VecDot(v, v); }

const Vec *VecNormalize(const Vec *v_in, Vec *v_out) {
  return VecScale(v_in, 1.0 / VecNormBound(v_in, 1e-9), v_out);
}

// Return the elements of v_in where inds != 0. The MATLAB logical indexing
// equivalent is: v_out = v_in(inds).
const Vec *VecSlice(const Vec *v_in, const int32_t inds[], Vec *v_out) {
  int32_t i_out = 0;
  for (int32_t i = 0; i < v_in->length; ++i) {
    if (inds[i]) {
      assert(i_out < v_out->max_length);
      // Don't use VecPtr here because we haven't set v_out->length yet.
      v_out->d[i_out] = VecGet(v_in, i);
      i_out++;
    }
  }

  // Do not resize v_out until the end of the function if you want to
  // be able to reuse and input as an output.
  VecResize(i_out, v_out);
  return v_out;
}

// Assigns elements of a larger vector with those from a smaller one. The
// MATLAB logical indexing equivalent is: v_out(inds) = v_in. The sum(inds != 0)
// must be equal to length(v_in). Warning: there is no check that
// length(inds) == length(v_out).
const Vec *VecSliceSet(const Vec *v_in, const int32_t inds[], Vec *v_out) {
  assert(v_in != v_out);

  int32_t i_in = 0;
  for (int32_t i = 0; i < v_out->length; ++i) {
    if (inds[i]) {
      *VecPtr(v_out, i) = VecGet(v_in, i_in);
      i_in++;
    }
  }

  // Assert that the sum(inds != 0) is equal to length(v_in).
  assert(v_in->length == i_in);

  return v_out;
}

// Operations on arbitrary matrices.

bool MatIsSize(const Mat *m, int32_t nr, int32_t nc) {
  return m != NULL && m->nr == nr && m->nc == nc;
}

double *MatPtr(const Mat *m, int32_t i, int32_t j) {
  assert(i < m->nr && j < m->nc);
  return &m->d[i * m->nc + j];
}

double MatGet(const Mat *m, int32_t i, int32_t j) {
  assert(i < m->nr && j < m->nc);
  return m->d[i * m->nc + j];
}

bool MatResize(int32_t nr, int32_t nc, Mat *m) {
  if (m->nr == nr && m->nc == nc) {
    return true;
  } else if (m->variable_dim && nr * nc <= m->max_size) {
    m->nr = nr;
    m->nc = nc;
    return true;
  }
  assert(false);
  return false;
}

// Copy the contents of an array into a Mat.
const Mat *MatInit(const double *d, int32_t nr, int32_t nc, Mat *m) {
  MatResize(nr, nc, m);
  for (int32_t i = 0; i < m->nr; ++i) {
    for (int32_t j = 0; j < m->nc; ++j) {
      *MatPtr(m, i, j) = d[i * m->nc + j];
    }
  }
  return m;
}

// m_out = scale * m_in
const Mat *MatScale(const Mat *m_in, double scale, Mat *m_out) {
  MatResize(m_in->nr, m_in->nc, m_out);
  for (int32_t i = 0; i < m_out->nr; ++i) {
    for (int32_t j = 0; j < m_out->nc; ++j) {
      *MatPtr(m_out, i, j) = scale * MatGet(m_in, i, j);
    }
  }
  return m_out;
}

const Mat *MatZero(Mat *m) {
  MatArrZero(m->nr, m->nc, m->d);
  return m;
}

const Mat *MatCopy(const Mat *a, Mat *b) {
  if (a != b) {  // No work to do if the Mat pointers are equal.
    MatResize(a->nr, a->nc, b);
    // This is still safe if the data pointers are equal.
    MatArrCopy(a->d, a->nr, a->nc, b->d);
  }
  return b;
}

// Copies the sub-matrix src(start_row + (1:num_rows), start_col + (1:num_cols))
// into dest(dest_row + (1:num_rows), dest_col + (1:num_cols)).
//
// It is not safe for src and dest to share any memory.
//
// Returns:
//   A pointer to dest.
const Mat *MatSubmatSet(const Mat *src, int32_t start_row, int32_t start_col,
                        int32_t num_rows, int32_t num_cols, int32_t dest_row,
                        int32_t dest_col, Mat *dest) {
  assert(src != dest && src->d != dest->d);
  assert(start_row >= 0 && start_col >= 0);
  assert(num_rows >= 0 && num_cols >= 0);
  assert(dest_row >= 0 && dest_col >= 0);
  // Make sure there is sufficient data and sufficient room.
  assert(src->nr >= start_row + num_rows && src->nc >= start_col + num_cols);
  assert(dest->nr >= dest_row + num_rows && dest->nc >= dest_col + num_cols);

  for (int32_t i = 0; i < num_rows; ++i) {
    for (int32_t j = 0; j < num_cols; ++j) {
      *MatPtr(dest, dest_row + i, dest_col + j) =
          MatGet(src, i + start_row, j + start_col);
    }
  }
  return dest;
}

const Mat *MatI(int32_t n, Mat *m) {
  MatResize(n, n, m);
  MatArrI(n, m->d);
  return m;
}

const Mat *MatMult(const Mat *m1, const Mat *m2, Mat *m_out) {
  assert(m1->nc == m2->nr);
  MatResize(m1->nr, m2->nc, m_out);
  MatArrMult(m1->d, m1->nr, m1->nc, m2->d, m2->nc, m_out->d);
  return m_out;
}

// Generalized matrix-vector multiply.
//
//   z = alpha*op(a)*x + beta*y.
//
// Args:
//   ta: Indicates if a should be transposed.
//   alpha, a, x, beta: See formula above.
//   y: Can be NULL if beta = 0.0.
//   z: Resulting vector.  It is safe to reuse y as z.
const Vec *MatVecGenMult(TransposeType ta, double alpha, const Mat *a,
                         const Vec *x, double beta, const Vec *y, Vec *z) {
  assert(a != NULL && x != NULL && z != NULL);
  assert((ta == kNoTrans ? a->nc : a->nr) == x->length);
  const int32_t zl = (ta == kNoTrans) ? a->nr : a->nc;
  assert(y == NULL || y->length == zl);
  assert(y != NULL || beta == 0.0);
  if (y == NULL) beta = 0.0;

  if (y == NULL || beta == 0.0) {
    VecResize(zl, z);
  } else {
    VecCopy(y, z);
  }

  MatArrGemv(ta, alpha, a->d, a->nr, a->nc, x->d, beta, z->d);
  return z;
}

// Generalized matrix-matrix multiply.
//
//   d = alpha*op(a)*op(b) + beta*c.
//
// Args:
//   ta: Indicates if a should be transposed.
//   tb: Indicates if b should be transposed.
//   alpha, a, b, beta: See above formula.
//   c: Can be NULL if beta = 0.0.
//   d: Resulting matrix.  It is safe to reuse c as d.
const Mat *MatGenMult(TransposeType ta, TransposeType tb, double alpha,
                      const Mat *a, const Mat *b, double beta, const Mat *c,
                      Mat *d) {
  assert(a != NULL && b != NULL && d != NULL);
  assert((ta == kNoTrans ? a->nc : a->nr) == (tb == kNoTrans ? b->nr : b->nc));
  const int32_t nr_d = (ta == kNoTrans ? a->nr : a->nc);
  const int32_t nc_d = (tb == kNoTrans ? b->nc : b->nr);
  assert(c == NULL || (c->nr == nr_d && c->nc == nc_d));
  assert(c != NULL || beta == 0.0);
  if (c == NULL) beta = 0.0;

  if (c == NULL || beta == 0.0)
    MatResize(nr_d, nc_d, d);
  else
    MatCopy(c, d);

  MatArrGemm(ta, tb, alpha, a->d, a->nr, a->nc, b->d, nc_d, beta, d->d);
  return d;
}

const Mat *MatMult3(const Mat *m0, const Mat *m1, const Mat *m2, Mat *m_out) {
  assert(m0->nc == m1->nr && m1->nc == m2->nr);
  MatResize(m0->nr, m2->nc, m_out);
  MAT(m0->nr, m1->nc, m0m1);
  return MatMult(MatMult(m0, m1, &m0m1), m2, m_out);
}

const Vec *MatVecMult(const Mat *m, const Vec *v_in, Vec *v_out) {
  assert(m->nc == v_in->length);
  VecResize(m->nr, v_out);
  MatArrMult(m->d, m->nr, m->nc, v_in->d, 1, v_out->d);
  return v_out;
}

const Vec *MatTransVecMult(const Mat *m, const Vec *v_in, Vec *v_out) {
  assert(m->nr == v_in->length);
  VecResize(m->nc, v_out);
  MatArrGemm(kTrans, kNoTrans, 1.0, m->d, m->nr, m->nc, v_in->d, 1, 0.0,
             v_out->d);
  return v_out;
}

const Mat *MatTrans(const Mat *m, Mat *m_out) {
  MatResize(m->nc, m->nr, m_out);
  MatArrTrans(m->d, m->nr, m->nc, m_out->d);
  return m_out;
}

const Mat *MatAdd(const Mat *m0, const Mat *m1, Mat *m_out) {
  assert(m0->nr == m1->nr && m0->nc == m1->nc);
  MatResize(m0->nr, m0->nc, m_out);
  for (int32_t i = 0; i < m0->nr; ++i) {
    for (int32_t j = 0; j < m0->nc; ++j) {
      *MatPtr(m_out, i, j) = MatGet(m0, i, j) + MatGet(m1, i, j);
    }
  }
  return m_out;
}

const Mat *MatSub(const Mat *m0, const Mat *m1, Mat *m_out) {
  assert(m0->nr == m1->nr && m0->nc == m1->nc);
  MatResize(m0->nr, m0->nc, m_out);
  for (int32_t i = 0; i < m0->nr; ++i) {
    for (int32_t j = 0; j < m0->nc; ++j) {
      *MatPtr(m_out, i, j) = MatGet(m0, i, j) - MatGet(m1, i, j);
    }
  }
  return m_out;
}

void MatQrDecomp(const Mat *A, Mat *Q, Mat *R) {
  MatResize(A->nr, A->nc, R);
  double *Qd = NULL;
  if (Q != NULL) {
    MatResize(A->nr, A->nr, Q);
    Qd = Q->d;
  }
  MatArrQrDecomp(A->d, A->nr, A->nc, Qd, R->d);
}

// Calculates the "thin" singular value decomposition of an m x n
// matrix A, defined by:
//
//   A = U * S * V'
//
// where U is an m x n column-orthogonal matrix (i.e. U'*U = I), S is
// an n x n diagonal matrix with non-decreasing elements, and V is an
// n x n orthogonal matrix (i.e. V'*V = V*V' = I).  The "thin" aspect
// is equivalent to MATLAB's "economy" SVD.
//
// Args:
//   A: Input m x n matrix with m >= n.
//   U: Output m x n column-orthogonal matrix.
//   s: Output n length vector of the diagonal elements of S.
//   V: Output n x n orthogonal matrix.
//
// Returns:
//   kLinalgErrorNone on success and kLinalgErrorMaxIter if GSL's
//   solver reached its maximum number of iterations.
int32_t MatThinSvDecomp(const Mat *A, Mat *U, Vec *s, Mat *V) {
  assert(A != NULL && U != NULL && s != NULL && V != NULL);
  assert(A->nc <= VEC_MAX_ELEMENTS);
  assert(A->nr >= A->nc);

  MatCopy(A, U);
  VecResize(A->nc, s);
  MatResize(A->nc, A->nc, V);

  double work_data[VEC_MAX_ELEMENTS];
  gsl_matrix_view U_gsl =
      gsl_matrix_view_array(U->d, (size_t)U->nr, (size_t)U->nc);
  gsl_matrix_view V_gsl =
      gsl_matrix_view_array(V->d, (size_t)V->nr, (size_t)V->nc);
  gsl_vector_view s_gsl = gsl_vector_view_array(s->d, (size_t)s->length);
  gsl_vector_view work = gsl_vector_view_array(work_data, (size_t)s->length);
  int32_t ret = gsl_linalg_SV_decomp(&U_gsl.matrix, &V_gsl.matrix,
                                     &s_gsl.vector, &work.vector);
  // These are the only errors that gsl_linalg_SV_decomp should ever
  // return, assuming we're passing in matrices and vectors of the
  // appropriate sizes.
  assert(ret == GSL_SUCCESS || ret == GSL_EMAXITER);

  if (ret == GSL_EMAXITER) {
    assert(false);
    return kLinalgErrorMaxIter;
  }
  return kLinalgErrorNone;
}

// Calculates the rank of a matrix.  Performs a QR decomposition (with
// pivoting) of the input matrix and finds the number of diagonal
// elements of R with magnitude greater than a specified tolerance.
//
// Args:
//   A: Input matrix.
//   tol: Tolerance for checking linear independence of vectors.
//
// Returns:
//   Rank of the matrix.
int32_t MatRank(const Mat *A, double tol) {
  assert(A != NULL);
  assert(A->nr * A->nc <= MAT_MAX_ELEMENTS);
  assert(A->nr <= VEC_MAX_ELEMENTS && A->nc <= VEC_MAX_ELEMENTS);
  assert(tol >= DBL_EPSILON);

  int32_t min_dim = (A->nr < A->nc) ? A->nr : A->nc;
  if (min_dim == 0) return 0;

  double qr_data[MAT_MAX_ELEMENTS];
  double tau_data[VEC_MAX_ELEMENTS];
  double norm_data[VEC_MAX_ELEMENTS];
  size_t perm_data[VEC_MAX_ELEMENTS];
  int signum;
  gsl_matrix_const_view A_gsl =
      gsl_matrix_const_view_array(A->d, (size_t)A->nr, (size_t)A->nc);
  gsl_matrix_view QR =
      gsl_matrix_view_array(qr_data, (size_t)A->nr, (size_t)A->nc);
  gsl_vector_view tau = gsl_vector_view_array(tau_data, (size_t)min_dim);
  gsl_permutation p = {(size_t)A->nc, perm_data};
  gsl_vector_view norm = gsl_vector_view_array(norm_data, (size_t)A->nc);

  gsl_matrix_memcpy(&QR.matrix, &A_gsl.matrix);
  int32_t ret = gsl_linalg_QRPT_decomp(&QR.matrix, &tau.vector, &p, &signum,
                                       &norm.vector);
  assert(ret == GSL_SUCCESS);
  (void)ret;

  int32_t rank = 0;
  while (rank < min_dim) {
    if (fabs(gsl_matrix_get(&QR.matrix, (size_t)rank, (size_t)rank)) <= tol) {
      break;
    }
    rank++;
  }

  return rank;
}

// Solves A*x = b for x similar to x = A \ b.
int32_t MatVecLeftDivide(const Mat *A, const Vec *b, Vec *x) {
  assert(A != NULL && b != NULL && x != NULL);
  assert(b != x);
  assert(A->nr == b->length && A->nr > 0);
  VecResize(A->nc, x);
  if (A->nc == 0) return kLinalgErrorNone;

  size_t m = (size_t)A->nr, n = (size_t)A->nc;
  gsl_matrix_const_view A_gsl = gsl_matrix_const_view_array(A->d, m, n);
  gsl_matrix_const_view b_gsl = gsl_matrix_const_view_array(b->d, m, 1U);
  gsl_matrix_view x_gsl = gsl_matrix_view_array(x->d, n, 1U);
  int32_t ret =
      GslMatrixDivide(CblasLeft, &A_gsl.matrix, &b_gsl.matrix, &x_gsl.matrix);
  assert(ret == GSL_SUCCESS || ret == GSL_ESING);
  return (ret == GSL_ESING) ? kLinalgErrorSingularMat : kLinalgErrorNone;
}

// Solves x*A = b for x similar to x = b / A.
int32_t MatVecRightDivide(const Mat *A, const Vec *b, Vec *x) {
  assert(A != NULL && b != NULL && x != NULL);
  assert(b != x);
  assert(A->nc == b->length && A->nc > 0);
  VecResize(A->nr, x);
  if (A->nr == 0) return kLinalgErrorNone;

  size_t m = (size_t)A->nr, n = (size_t)A->nc;
  gsl_matrix_const_view A_gsl = gsl_matrix_const_view_array(A->d, m, n);
  gsl_matrix_const_view b_gsl = gsl_matrix_const_view_array(b->d, 1U, n);
  gsl_matrix_view x_gsl = gsl_matrix_view_array(x->d, 1U, m);
  int32_t ret =
      GslMatrixDivide(CblasRight, &A_gsl.matrix, &b_gsl.matrix, &x_gsl.matrix);
  assert(ret == GSL_SUCCESS || ret == GSL_ESING);
  return (ret == GSL_ESING) ? kLinalgErrorSingularMat : kLinalgErrorNone;
}

// Solves A*X = b for X similar to X = A \ B.
int32_t MatMatLeftDivide(const Mat *A, const Mat *B, Mat *X) {
  assert(A != NULL && B != NULL && X != NULL);
  assert(A != X && B != X);
  assert(A->nr == B->nr && A->nr > 0);
  MatResize(A->nc, B->nc, X);
  if (A->nc == 0) return kLinalgErrorNone;

  size_t m = (size_t)A->nr, n = (size_t)A->nc, k = (size_t)B->nc;
  gsl_matrix_const_view A_gsl = gsl_matrix_const_view_array(A->d, m, n);
  gsl_matrix_const_view B_gsl = gsl_matrix_const_view_array(B->d, m, k);
  gsl_matrix_view X_gsl = gsl_matrix_view_array(X->d, n, k);
  int32_t ret =
      GslMatrixDivide(CblasLeft, &A_gsl.matrix, &B_gsl.matrix, &X_gsl.matrix);
  assert(ret == GSL_SUCCESS || ret == GSL_ESING);
  return (ret == GSL_ESING) ? kLinalgErrorSingularMat : kLinalgErrorNone;
}

// Solves X*A = B for X similar to X = B / A.
int32_t MatMatRightDivide(const Mat *A, const Mat *B, Mat *X) {
  assert(A != NULL && B != NULL && X != NULL);
  assert(A != X && B != X);
  assert(A->nc == B->nc && A->nc > 0);
  MatResize(B->nr, A->nr, X);
  if (A->nr == 0) return kLinalgErrorNone;

  size_t m = (size_t)A->nr, n = (size_t)A->nc, k = (size_t)B->nr;
  gsl_matrix_const_view A_gsl = gsl_matrix_const_view_array(A->d, m, n);
  gsl_matrix_const_view B_gsl = gsl_matrix_const_view_array(B->d, k, n);
  gsl_matrix_view X_gsl = gsl_matrix_view_array(X->d, k, m);
  int32_t ret =
      GslMatrixDivide(CblasRight, &A_gsl.matrix, &B_gsl.matrix, &X_gsl.matrix);
  assert(ret == GSL_SUCCESS || ret == GSL_ESING);
  return (ret == GSL_ESING) ? kLinalgErrorSingularMat : kLinalgErrorNone;
}

bool MatIsUpperTriangular(const Mat *r) {
  return MatArrIsUpperTriangular(r->d, r->nr, r->nc);
}

bool MatIsLowerTriangular(const Mat *l) {
  return MatArrIsLowerTriangular(l->d, l->nr, l->nc);
}

bool MatHasNonnegDiag(const Mat *m) {
  assert(m != NULL);
  for (int32_t i = 0; i < m->nc && i < m->nr; ++i) {
    if (MatGet(m, i, i) < 0.0) return false;
  }
  return true;
}

// Calculates the matrix square root of P = A'*A + B'*B by calculating a QR
// decomposition, Q*R = [A ; B].  It is assumed that the matrix [A ; B] is full
// row rank (i.e. has linearly independent columns).
//
// Q is an orthogonal matrix and R "trapezoidal matrix", i.e. R = [sqrt_P ; 0]
// where sqrt_P is upper triangular.  Thus:
//   P = A'*A + B'*B = [A ; B]'*[A ; B] = [sqrT_P ; 0]'*Q'*Q*[sqrt_P ; 0].
//
// Args:
//   A: na-by-nc matrix.
//   B: nb-by-nc matrix.
//   sqrt_P: Upper-triangular matrix with positive diagonal elements such that
//     sqrt_P'*sqrt_P = P.
//
// Note: it is safe to reuse A or B as sqrt_P (assuming shapes match).
void MatSqrtSum(const Mat *A, const Mat *B, Mat *sqrt_P) {
  assert(A != NULL && B != NULL && sqrt_P != NULL);
  assert(A->nc == B->nc);
  assert(A->nr + B->nr >= A->nc);  // This is required for full row rank.
  MatResize(A->nc, A->nc, sqrt_P);

  // Define a temporary matrix, stack = [A ; B].
  MAT(A->nr + B->nr, A->nc, stack);
  MatSubmatSet(A, 0, 0, A->nr, A->nc, 0, 0, &stack);
  MatSubmatSet(B, 0, 0, B->nr, B->nc, A->nr, 0, &stack);
  MatQrDecomp(&stack, NULL, &stack);
  MatSubmatSet(&stack, 0, 0, A->nc, A->nc, 0, 0, sqrt_P);
  // Next, enforce a non-negative diagonal.
  for (int32_t i = 0; i < sqrt_P->nr; ++i) {
    if (MatGet(sqrt_P, i, i) < 0.0) {
      for (int32_t j = i; j < sqrt_P->nc; ++j) {
        *MatPtr(sqrt_P, i, j) *= -1.0;
      }
    }
  }
}

int32_t MatMatBackSub(const Mat *R, const Mat *b, Mat *x) {
  assert(b->nr == R->nr && b->nc == x->nc);
  assert(x->nr == R->nc);
  return MatArrBackSub(R->d, R->nr, R->nc, b->d, b->nc, x->d);
}

int32_t MatMatForwardSub(const Mat *L, const Mat *b, Mat *x) {
  assert(b->nr == L->nr && b->nc == x->nc);
  assert(x->nr == L->nc);
  return MatArrForwardSub(L->d, L->nr, L->nc, b->d, b->nc, x->d);
}

int32_t MatVecBackSub(const Mat *R, const Vec *b, Vec *x) {
  assert(b->length == R->nr);
  assert(x->length == R->nc);
  return MatArrBackSub(R->d, R->nr, R->nc, b->d, 1, x->d);
}

int32_t MatVecForwardSub(const Mat *L, const Vec *b, Vec *x) {
  assert(b->length == L->nr);
  assert(x->length == L->nc);
  return MatArrForwardSub(L->d, L->nr, L->nc, b->d, 1, x->d);
}

// Returns the rows and columns of m where rows != 0 and cols != 0
const Mat *MatSlice(const Mat *m, const int32_t rows[], const int32_t cols[],
                    Mat *m_out) {
  int32_t nr_out = 0, nc_out = 0;
  if (rows == NULL) {
    nr_out = m->nr;
  } else {
    for (int32_t i = 0; i < m->nr; ++i)
      if (rows[i]) nr_out++;
  }
  if (cols == NULL) {
    nc_out = m->nc;
  } else {
    for (int32_t i = 0; i < m->nc; ++i)
      if (cols[i]) nc_out++;
  }

  if (m_out->variable_dim) {
    assert(nr_out * nc_out <= m_out->max_size);
  } else {
    assert(m_out->nr == nr_out && m_out->nc == nc_out);
  }

  int32_t i_out = 0;
  for (int32_t i = 0; i < m->nr; ++i) {
    if (rows == NULL || rows[i]) {
      int32_t j_out = 0;
      for (int32_t j = 0; j < m->nc; ++j) {
        if (cols == NULL || cols[j]) {
          // Don't use MatPtr here because we haven't set m_out->nc yet.
          m_out->d[i_out * nc_out + j_out] = MatGet(m, i, j);
          j_out++;
        }
      }
      i_out++;
    }
  }

  // Do not resize m until the end of the function if you want to be
  // able to reuse the input as the output.  Otherwise, the matrix
  // indexing in MatGet will be off.
  MatResize(nr_out, nc_out, m_out);
  return m_out;
}

const double *MatArrMult(const double *m1, int32_t nr1, int32_t nc1,
                         const double *m2, int32_t nc2, double *m_out) {
  assert(m_out != m1 && m_out != m2);
  return MatArrGemm(kNoTrans, kNoTrans, 1.0, m1, nr1, nc1, m2, nc2, 0.0, m_out);
}

// This is modeled after the BLAS function GEMV, which calculates:
//     y <-- alpha*op(A)*x + beta*y,
// where op either returns the matrix or its transpose.
//
// Args:
//   ta: Whether to transpose A.
//   alpha: See above formula.
//   a: Data for A.
//   nra: Number of rows of A without transpose applied.
//   nca: Number of columns of A without transpose applied.
//   x: Data for x.
//   beta: See above formula.
//   c: Data for y.
const double *MatArrGemv(TransposeType ta, double alpha, const double *a,
                         int32_t nra, int32_t nca, const double *x, double beta,
                         double *y) {
  assert(x != y);
  int32_t ly = ((ta == kNoTrans) ? nra : nca);
  int32_t lx = ((ta == kNoTrans) ? nca : nra);
  double aij, axj;
  for (int32_t i = 0; i < ly; ++i) {  // For each entry of y.
    axj = 0.0;
    for (int32_t j = 0; j < lx; ++j) {  // Walk down the row of op(A).
      aij = (ta == kNoTrans) ? a[i * nca + j] : a[j * nca + i];
      axj += aij * x[j];
    }
    if (beta == 0.0) {
      y[i] = alpha * axj;
    } else {
      y[i] = alpha * axj + beta * y[i];
    }
  }
  return y;
}

// This is modeled after the BLAS function GEMM, which calculates:
//     C <-- alpha*op(A)*op(B) + beta*C,
// where op either returns the matrix or its transpose.
//
// Args:
//   ta: Whether to transpose A.
//   tb: Whether to transpose B.
//   alpha: See above formula.
//   a: Data for A.
//   nra: Number of rows of A without transpose applied.
//   nca: Number of columns of A without transpose applied.
//   b: Data for B.
//   ncc: Number of columns of C.
//   beta: See above formula.
//   c: Data for C.
const double *MatArrGemm(TransposeType ta, TransposeType tb, double alpha,
                         const double *a, int32_t nra, int32_t nca,
                         const double *b, int32_t ncc, double beta, double *c) {
  assert(a != c && b != c);
  int32_t nrc = (ta == kNoTrans) ? nra : nca;
  int32_t nk = (ta == kNoTrans) ? nca : nra;
  double ABij, aik, bkj;
  for (int32_t i = 0; i < nrc; ++i) {
    for (int32_t j = 0; j < ncc; ++j) {
      ABij = 0.0;
      for (int32_t k = 0; k < nk; ++k) {
        aik = (ta == kNoTrans) ? a[i * nca + k] : a[k * nca + i];
        bkj = (tb == kNoTrans) ? b[k * ncc + j] : b[j * nk + k];
        ABij += aik * bkj;
      }
      if (beta == 0.0) {
        c[i * ncc + j] = alpha * ABij;
      } else {
        c[i * ncc + j] = alpha * ABij + beta * c[i * ncc + j];
      }
    }
  }
  return c;
}

const double *MatArrZero(int32_t nr, int32_t nc, double *m_out) {
  for (int32_t i = 0; i < nr; ++i) {
    for (int32_t j = 0; j < nc; ++j) {
      m_out[i * nc + j] = 0.0;
    }
  }
  return m_out;
}

void MatArrCopy(const double *d_in, int32_t nr, int32_t nc, double *d_out) {
  for (int32_t i = 0; i < nr; ++i) {
    for (int32_t j = 0; j < nc; ++j) {
      d_out[i * nc + j] = d_in[i * nc + j];
    }
  }
}

void MatArrTrans(const double *m_in, int32_t nr, int32_t nc, double *m_out) {
  assert(m_out != m_in && m_out != NULL);
  for (int32_t i = 0; i < nr; ++i) {
    for (int32_t j = 0; j < nc; ++j) {
      m_out[j * nr + i] = m_in[i * nc + j];
    }
  }
}

const double *MatArrI(int32_t nr, double *m_out) {
  MatArrZero(nr, nr, m_out);
  for (int32_t i = 0; i < nr; ++i) {
    m_out[i * nr + i] = 1.0;
  }
  return m_out;
}

// QR decomposition.  Decompose a matrix into orthogonal and
// upper-right-triangular components.
//
// You may reuse a as r, and you may skip calculating q by passing a
// NULL pointer.
void MatArrQrDecomp(const double *a, int32_t nr, int32_t nc, double *q,
                    double *r) {
  assert(q != a && r != q && r != NULL);
  assert(nr <= VEC_MAX_ELEMENTS && nc <= VEC_MAX_ELEMENTS);

  double tau_data[VEC_MAX_ELEMENTS];
  gsl_matrix_const_view A =
      gsl_matrix_const_view_array(a, (size_t)nr, (size_t)nc);
  gsl_matrix_view QR = gsl_matrix_view_array(r, (size_t)nr, (size_t)nc);
  gsl_vector_view tau =
      gsl_vector_view_array(tau_data, (nr < nc) ? (size_t)nr : (size_t)nc);
  gsl_matrix_memcpy(&QR.matrix, &A.matrix);
  gsl_linalg_QR_decomp(&QR.matrix, &tau.vector);

  if (q != NULL) {
    gsl_matrix_view Q = gsl_matrix_view_array(q, (size_t)nr, (size_t)nr);
    gsl_matrix_view R = gsl_matrix_view_array(r, (size_t)nr, (size_t)nc);
    gsl_linalg_QR_unpack(&QR.matrix, &tau.vector, &Q.matrix, &R.matrix);
  } else {
    for (int32_t i = 1; i < nr; ++i) {
      for (int32_t j = 0; j < i && j < nc; ++j) {
        r[i * nc + j] = 0.0;
      }
    }
  }
}

bool MatArrIsUpperTriangular(const double *r, int32_t nr, int32_t nc) {
  for (int32_t i = 1; i < nr; ++i) {
    for (int32_t j = 0; j < i && j < nc; ++j) {
      if (fabs(r[i * nc + j]) >= DBL_EPSILON) {
        return false;
      }
    }
  }
  return true;
}

bool MatArrIsLowerTriangular(const double *l, int32_t nr, int32_t nc) {
  for (int32_t i = 0; i < nr; ++i) {
    for (int32_t j = i + 1; j < nc; ++j) {
      if (fabs(l[i * nc + j]) >= DBL_EPSILON) {
        return false;
      }
    }
  }
  return true;
}

// Solves R*X = B using back substitution where R is an upper
// triangular matrix.  If the triangular matrix contains a zero
// diagonal element (i.e. the matrix is singular), we set the
// corresponding x element to zero.
//
// Returns kLinalgErrorNone on success and kLinalgErrorSingularMat
// if the matrix was singular.
//
// Note: Confirmed safe to reuse b as x.
int32_t MatArrBackSub(const double *r, int32_t nr, int32_t nc, const double *b,
                      int32_t nc_b, double *x) {
  assert(r != x);
  assert(nr >= nc);
  assert(nr >= 0 && nc >= 0 && nc_b >= 0);
  assert(MatArrIsUpperTriangular(r, nr, nc));
  (void)nr;

  int32_t err = kLinalgErrorNone;
  // Solve for the k-th column of x.
  for (int32_t k = 0; k < nc_b; ++k) {
    // Iterate over the rows of r.
    for (int32_t i = nc - 1; i >= 0; --i) {
      if (fabs(r[i * nc + i]) <= DBL_EPSILON) {
        err = kLinalgErrorSingularMat;
        x[i * nc_b + k] = 0.0;
      } else {
        double sum = 0.0;
        for (int32_t j = nc - 1; j > i; --j) {
          sum += x[j * nc_b + k] * r[i * nc + j];
        }
        x[i * nc_b + k] = (b[i * nc_b + k] - sum) / r[i * nc + i];
      }
    }
    // TODO: If nr > nc, then the bottom of r is all zeros.
    // Should we test if b has non-zero elements for these rows?
  }
  return err;
}

// Solves L*X = B using forward substitution where L is a lower
// triangular matrix.  If the triangular matrix contains a zero
// diagonal element (i.e. the matrix is singular), we set the
// corresponding x element to zero.
//
// Returns kLinalgErrorNone on success and kLinalgErrorSingularMat
// if the matrix is singular.
//
// Note: Confirmed safe to reuse b as x.
int32_t MatArrForwardSub(const double *l, int32_t nr, int32_t nc,
                         const double *b, int32_t nc_b, double *x) {
  assert(nr <= nc);
  assert(MatArrIsLowerTriangular(l, nr, nc));
  int32_t err = kLinalgErrorNone;
  for (int32_t k = 0; k < nc_b; ++k) {  // Solve for the k-th column of x.
    for (int32_t i = 0; i < nr; ++i) {  // Iterate over the rows of l.
      if (fabs(l[i * nc + i]) <= DBL_EPSILON) {
        err = kLinalgErrorSingularMat;
        x[i * nc_b + k] = 0.0;
      } else {
        double sum = 0.0;
        for (int32_t j = 0; j < i; ++j) {
          sum += x[j * nc_b + k] * l[i * nc + j];
        }
        x[i * nc_b + k] = (b[i * nc_b + k] - sum) / l[i * nc + i];
      }
    }
    for (int32_t i = nr; i < nc; ++i) {
      x[i * nc_b + k] = 0.0;
    }
  }
  return err;
}


#include "globals.h"
#include "peano.h"
#include <gsl/gsl_heapsort.h>

peanoKey Peano_Key ( const double x, const double y, const double z );
static void reorder_particles();

void Print_Int_Bits128 ( const peanoKey val )
{
    for ( int i = 127; i >= 0; i-- ) {

        printf ( "%llu", ( long long ) ( ( val & ( ( peanoKey ) 1 << i ) ) >> i ) );

        if ( i % 3 == 0 && i != 0 ) {
            printf ( "." );
        }
    }
    printf ( "\n" );
    fflush ( stdout );

    return ;
}

void Print_Int_Bits128r ( const peanoKey val )
{
    for ( int i = 127; i >= 0; i-- ) {

        printf ( "%llu", ( long long ) ( ( val & ( ( peanoKey ) 1 << i ) ) >> i ) );

        if ( i % 3 - 2 == 0 && i != 0 ) {
            printf ( "." );
        }
    }
    printf ( "\n" );
    fflush ( stdout );

    return ;
}

int compare_peanoKeys ( const void *a, const void *b )
{
    const peanoKey *x = ( const peanoKey * ) a;
    const peanoKey *y = ( const peanoKey * ) b;

    return ( int ) ( *x > *y ) - ( *x < *y );
}

static peanoKey *Keys = NULL;
static size_t *Idx = NULL;

void Sort_Particles_By_Peano_Key()
{
    if ( Keys == NULL ) {
        Keys = malloc ( Param.Npart * sizeof ( *Keys ) );
    } else {
        memset ( Keys, 0, Param.Npart * sizeof ( *Keys ) );
    }

    if ( Idx == NULL ) {
        Idx = malloc ( Param.Npart * sizeof ( *Idx ) );
    } else {
        memset ( Idx, 0, Param.Npart * sizeof ( *Idx ) );
    }

    #pragma omp parallel for
    for ( int ipart = 0; ipart < Param.Npart; ipart++ ) {

        double px = P[ipart].Pos[0] / Problem.Boxsize[0];
        double py = P[ipart].Pos[1] / Problem.Boxsize[1];
        double pz = P[ipart].Pos[2] / Problem.Boxsize[2];

        P[ipart].Key = Keys[ipart] = Peano_Key ( px, py, pz );
    }

    gsl_heapsort_index ( Idx, Keys, Param.Npart, sizeof ( *Keys ), &compare_peanoKeys );

    reorder_particles();

    return ;
}

static void reorder_particles()
{
    for ( int i = 0; i < Param.Npart; i++ ) {

        if ( Idx[i] == i ) {
            continue;
        }

        int dest = i;

        struct ParticleData Ptmp = P[i];
        struct GasParticleData Sphtmp = SphP[i];

        int src = Idx[i];

        for ( ;; ) {

            P[dest] = P[src];
            SphP[dest] = SphP[src];

            Idx[dest] = dest;

            dest = src;

            src = Idx[dest];

            if ( src == i ) {
                break;
            }
        }

        P[dest] = Ptmp;
        SphP[dest] = Sphtmp;

        Idx[dest] = dest;

    } // for i

    return ;
}

peanoKey Peano_Key ( const double x, const double y, const double z )
{
    Assert ( x >= 0 && x <= 1, "X coordinate of out range [0,1] have %g", x );
    Assert ( y >= 0 && y <= 1, "Y coordinate of out range [0,1] have %g", y );
    Assert ( z >= 0 && z <= 1, "Z coordinate of out range [0,1] have %g", z );

    const uint64_t m = 1UL << 63; // = 2^63;

    uint64_t X[3] = { y * m, z * m, x * m };

    /* Inverse undo */

    for ( uint64_t q = m; q > 1; q >>= 1 ) {

        uint64_t P = q - 1;

        if ( X[0] & q ) {
            X[0] ^= P;    // invert
        }

        for ( int i = 1; i < 3; i++ ) {

            if ( X[i] & q ) {

                X[0] ^= P; // invert

            } else {

                uint64_t t = ( X[0] ^ X[i] ) & P;

                X[0] ^= t;
                X[i] ^= t;

            } // exchange
        }
    }

    /* Gray encode (inverse of decode) */

    for ( int i = 1; i < 3; i++ ) {
        X[i] ^= X[i - 1];
    }

    uint64_t t = X[2];

    for ( int i = 1; i < 64; i <<= 1 ) {
        X[2] ^= X[2] >> i;
    }

    t ^= X[2];

    for ( int i = 1; i >= 0; i-- ) {
        X[i] ^= t;
    }

    /* branch free bit interleave of transpose array X into key */

    peanoKey key = 0;

    X[1] >>= 1;
    X[2] >>= 2;    // lowest bits not important

    for ( int i = 0; i < N_PEANO_TRIPLETS + 1; i++ ) {

        uint64_t col = ( ( X[0] & 0x8000000000000000 )
                         | ( X[1] & 0x4000000000000000 )
                         | ( X[2] & 0x2000000000000000 ) ) >> 61;

        key <<= 3;

        X[0] <<= 1;
        X[1] <<= 1;
        X[2] <<= 1;

        key |= col;
    }

    key <<= 2;

    return key;
}

/* This constructs the peano key with reversed triplet order. The order in the
 * triplets however is the same ! Also level zero is carried explicitely
 * to ease tree construction. */

peanoKey Reversed_Peano_Key ( const double x, const double y, const double z )
{
    Assert ( x >= 0 && x <= 1, "X coordinate of out range [0,1] have %g", x );
    Assert ( y >= 0 && y <= 1, "Y coordinate of out range [0,1] have %g", y );
    Assert ( z >= 0 && z <= 1, "Z coordinate of out range [0,1] have %g", z );

    const uint64_t m = 1UL << 63; // = 2^63;

    uint64_t X[3] = { y * m, z * m, x * m };

    /* Inverse undo */

    for ( uint64_t q = m; q > 1; q >>= 1 ) {

        uint64_t P = q - 1;

        if ( X[0] & q ) {
            X[0] ^= P;    // invert
        }

        for ( int i = 1; i < 3; i++ ) {

            if ( X[i] & q ) {

                X[0] ^= P; // invert

            } else {

                uint64_t t = ( X[0] ^ X[i] ) & P;

                X[0] ^= t;
                X[i] ^= t;

            } // exchange
        }
    }

    /* Gray encode (inverse of decode) */

    for ( int i = 1; i < 3; i++ ) {
        X[i] ^= X[i - 1];
    }

    uint64_t t = X[2];

    for ( int i = 1; i < 64; i <<= 1 ) {
        X[2] ^= X[2] >> i;
    }

    t ^= X[2];

    for ( int i = 1; i >= 0; i-- ) {
        X[i] ^= t;
    }

    /* branch free reversed (!) bit interleave of transpose array X into key */

    peanoKey key = 0;

    X[0] >>= 18;
    X[1] >>= 19;
    X[2] >>= 20;    // lowest bits not important

    for ( int i = 0; i < N_PEANO_TRIPLETS + 1; i++ ) {

        uint64_t col = ( ( X[0] & 0x4 ) | ( X[1] & 0x2 ) | ( X[2] & 0x1 ) );

        key <<= 3;

        key |= col;

        X[0] >>= 1;
        X[1] >>= 1;
        X[2] >>= 1;
    }

    key <<= 3; // include level 0

    return key;
}

void test_peanokey()
{
    const double box[3]  = { 1.0, 1, 1};
    double a[3] = { 0 };
    int order = 1;
    float delta = 1 / pow ( 2.0, order );
    int n = roundf ( 1 / delta );

    for ( int i = 0; i < n; i++ ) {
        for ( int j = 0; j < n; j++ ) {
            for ( int k = 0; k < n; k++ ) {

                a[0] = ( i + 0.5 ) * delta / box[0];
                a[1] = ( j + 0.5 ) * delta / box[1];
                a[2] = ( k + 0.5 ) * delta / box[2];

                peanoKey stdkey =  Peano_Key ( a[0], a[1], a[2] );

                printf ( "%g %g %g %llu  \n", a[0], a[1], a[2],
                         ( long long ) stdkey );

                Print_Int_Bits128 ( stdkey );

                printf ( "\n" );
            }
        }
    }

    return ;
}


/**
 * @copyright (c) 2017 King Abdullah University of Science and Technology (KAUST).
 *                     All rights reserved.
 **/
/**
 * @file codelet_zhagcm.c
 *
 *  HiCMA codelets kernel
 *  HiCMA is a software package provided by King Abdullah University of Science and Technology (KAUST)
 *
 * @version 0.1.1
 * @author Kadir Akbudak
 * @date 2018-11-08
 * @precisions normal z -> c d s
 **/
#include "morse.h"
#include "runtime/starpu/chameleon_starpu.h"
//#include "runtime/starpu/include/runtime_codelet_z.h"

#include "runtime/starpu/runtime_codelets.h"
ZCODELETS_HEADER(hagcm)

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>  //FIXME remove mallocs from this code
#include <sys/time.h>//FIXME for gettimeofday

#include "hicma.h"
#include "starsh.h"
#include "starsh-spatial.h"
#include "starsh-randtlr.h"
#ifdef MKL
  #include <mkl.h>
  #include <mkl_lapack.h>
  //#pragma message("MKL is used")
#else
  #ifdef ARMPL
    #include <armpl.h>
  #else
    #include <cblas.h>
  #endif
  #ifdef LAPACKE_UTILS
    #include <lapacke_utils.h>
  #endif
  #include <lapacke.h>
  //#pragma message("MKL is NOT used")
#endif

//#warning "An experimental feature is enabled!!!"
int steal_lrtile = 0; //non-zero values are for experimental reasons. Otherwise set to 0!!!!!

extern void _printmat(double * A, int m, int n, int ld);

void zhagcm( int m, int n, /*dimension of squareAD*/
        double *AU,
        double *AV,
        double *Ark,
        int ldu,
        int ldv,
        int tile_row_index, int tile_column_index,
        int maxrank, double tol,
        int A_mt)
{
    int ii = tile_row_index;
    int jj = tile_column_index;
    if(steal_lrtile == 1 && ii == jj){
        if(ii == A_mt-1) { // steal tile above
            ii = ii - 1;
        } else { // still tile below
            ii = ii + 1;
        }
    }
    int64_t i, j;
    int lda = ldu; //FIXME ASSUMPTION
    struct timeval tvalBefore, tvalAfter;  // removed comma
    gettimeofday (&tvalBefore, NULL);

    int shape[2];
    int rank = 0;
    int oversample = 10;
    double *work;
    int *iwork;
    STARSH_cluster *RC = HICMA_get_starsh_format()->row_cluster, *CC = RC;
    void *RD = RC->data, *CD = RD;
    double *AD;
    AD = malloc(sizeof(double) * lda * n);

    HICMA_get_starsh_format()->problem->kernel(m, n, RC->pivot+RC->start[ii], CC->pivot+CC->start[jj],
            RD, CD, AD, lda);
    int mn = m;
    int mn2 = maxrank+oversample;
    if(mn2 > mn)
        mn2 = mn;
    // Get size of temporary arrays
    size_t lwork = n, lwork_sdd = (4*mn2+7)*mn2;
    if(lwork_sdd > lwork)
        lwork = lwork_sdd;
    lwork += (size_t)mn2*(2*n+m+mn2+1);
    size_t liwork = 8*mn2;
    // Allocate temporary arrays
    //STARSH_MALLOC(iwork, liwork);
    iwork = malloc(sizeof(*iwork) * liwork);
    if(iwork == NULL) {
        fprintf(stderr, "%s %s %d:\t Allocation failed. No memory! liwork:%d", __FILE__, __func__, __LINE__, liwork);
        exit(-1);
    }
    //STARSH_MALLOC(work, lwork);
    work = malloc(sizeof(*work) * lwork);
    if(work == NULL) {
        fprintf(stderr, "%s %s %d:\t Allocation failed. No memory! lwork:%d", __FILE__, __func__, __LINE__, lwork);
        exit(-1);
    }
    starsh_dense_dlrrsdd(m, n, AD, lda, AU, ldu,  AV, ldv, &rank, maxrank, oversample, tol, work, lwork, iwork);

    Ark[0] = rank;

    free(work);
    free(iwork);
}
/**
 * HICMA_TASK_zhagcm - Generate compressed matrix from a problem determined according to current global setting of HiCMA library
 */
void HICMA_TASK_zhagcm( const MORSE_option_t *options,
                        int m, int n,
                        const MORSE_desc_t *AUV,
                        const MORSE_desc_t *Ark,
                        int Am, int An,
                        int ldu,
                        int ldv,
                        int maxrank, double tol,
                        int A_mt
                        )
{
    struct starpu_codelet *codelet = &cl_zhagcm;
    //void (*callback)(void*) = options->profiling ? cl_zhagcm_callback : NULL;
    void (*callback)(void*) = NULL;
    int nAUV = AUV->nb;

    MORSE_BEGIN_ACCESS_DECLARATION;
    MORSE_ACCESS_W(AUV, Am, An);
    MORSE_ACCESS_W(Ark, Am, An);
    MORSE_END_ACCESS_DECLARATION;

    //printf("%s:%d: Am:%d An:%d lda:%d bigM:%d m0:%d n0:%d\n ", __FILE__, __LINE__, Am, An, lda, bigM, m0, n0);
        //printf("%s %d: Am:%d An:%d ADm:%d ADn:%d ptr:%p\n", __func__, __LINE__, Am, An, ADm, ADn, ptr);
    starpu_insert_task(
            starpu_mpi_codelet(codelet),
            STARPU_VALUE,    &m,                      sizeof(int),
            STARPU_VALUE,    &n,                      sizeof(int),
            STARPU_VALUE,    &nAUV,                      sizeof(int),
            STARPU_W,         RTBLKADDR(AUV, double, Am, An),
            STARPU_W,         RTBLKADDR(Ark, double, Am, An),
            STARPU_VALUE,  &ldu,                      sizeof(int),
            STARPU_VALUE,  &ldv,                      sizeof(int),
            STARPU_VALUE,   &Am,                      sizeof(int),
            STARPU_VALUE,   &An,                      sizeof(int),
            STARPU_VALUE,   &maxrank,                      sizeof(int),
            STARPU_VALUE,   &tol,                      sizeof(double),
            STARPU_VALUE,   &A_mt,                      sizeof(int),
            STARPU_PRIORITY,    options->priority,
            STARPU_CALLBACK,    callback,
#if defined(CHAMELEON_CODELETS_HAVE_NAME)
            STARPU_NAME, "zhagcm",
#endif
            0);
}

/*   cl_zhagcm_cpu_func - Generate a tile for random matrix. */

#if !defined(CHAMELEON_SIMULATION)
static void cl_zhagcm_cpu_func(void *descr[], void *cl_arg)
{
    int m;
    int n;
    int nAUV;
    double *AUV;
    double *Ark;
    int ldu;
    int ldv;
    int tile_row_index;
    int tile_column_index;
    int maxrank;
    double tol;
    int A_mt;

    AUV = (double *)STARPU_MATRIX_GET_PTR(descr[0]);
    Ark = (double *)STARPU_MATRIX_GET_PTR(descr[1]);

    starpu_codelet_unpack_args(cl_arg, &m, &n, &nAUV, &ldu, &ldv, &tile_row_index, &tile_column_index, &maxrank, &tol, &A_mt);

    double *AU = AUV;
    int nAU = nAUV/2;
    assert(ldu == ldv);
    size_t nelm_AU = (size_t)ldu * (size_t)nAU;
    double *AV = &(AUV[nelm_AU]);

    //printf("(%d,%d)%d %s %d %d\n", m0/m,n0/n,MORSE_My_Mpi_Rank(), __func__, __LINE__, AD == Dense);
    zhagcm( m, n,
            AU,
            AV,
            Ark,
            ldu,
            ldv,
            tile_row_index, tile_column_index,
            maxrank, tol, A_mt
            );
}
#endif /* !defined(CHAMELEON_SIMULATION) */

/*
 * Codelet definition
 */
CODELETS_CPU(zhagcm, 2, cl_zhagcm_cpu_func)


 // MIT License

 // Copyright (c) [2017] [Vinay Yuvashankar]

 // Permission is hereby granted, free of charge, to any person obtaining a copy
 // of this software and associated documentation files (the "Software"), to deal
 // in the Software without restriction, including without limitation the rights
 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 // copies of the Software, and to permit persons to whom the Software is
 // furnished to do so, subject to the following conditions:

 // The above copyright notice and this permission notice shall be included in all
 // copies or substantial portions of the Software.

 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.

/** \file ERSP.cc

	\brief This file contains all of the function required to generate the Event Related Spectral Pertubation of EEG signals.
*/
#include "wavelet.h"
#include <gsl/gsl_statistics.h>

#define NORMALIZATION_FACTOR 0.375402913609157562

int ERSP (double * raw_data, double* scales, const int sampling_frequency, const int n,
	const int J, int const trials, const int padding_type,
	double * output)
{
	int i, j, x;

	//Calculate the necessary constants for the Continuous Wavelet Transform.
    const int    PADDED_SIZE = CalculatePaddingSize(n, padding_type);
    const int    m           = PRE_EVENT_TIME * sampling_frequency;
    const double dw          = (2 * M_PI * sampling_frequency)/(PADDED_SIZE); //NOT IN RAD/SEC in Hz

    fftw_init_threads();
    #pragma omp parallel private(i, j, x) shared(raw_data, output, scales) default(none)
    {
    	//Array Inits
    	double * pre_stimulus, *wavelet_out, *baseline_out;
    	fftw_complex *data_in, *fft_data, *filter_convolution, *fftw_result;
    	fftw_plan plan_forward, plan_backward;

	    //Memory Allocations
	    wavelet_out  = (double*) malloc( n * J * sizeof(double) );
	    baseline_out = (double*) malloc( n * J * sizeof(double) );
	    pre_stimulus = (double*) malloc( m     * sizeof(double) );

	    //FFTW Memory Allocations
	    data_in            = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );
		fft_data           = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );
		filter_convolution = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );
		fftw_result        = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * PADDED_SIZE );

		#pragma omp critical (make_plan)
		{
			fftw_plan_with_nthreads(1);
			if (fftw_import_wisdom_from_filename("FFTW_Plan.wise") == 0)
			{
				printf("No FFTW Plan, using an unoptimized method\n");
				plan_forward  = fftw_plan_dft_1d(PADDED_SIZE, data_in,            fft_data,    FFTW_FORWARD,  FFTW_ESTIMATE);
				plan_backward = fftw_plan_dft_1d(PADDED_SIZE, filter_convolution, fftw_result, FFTW_BACKWARD, FFTW_ESTIMATE);
			}
			else
			{
				plan_forward  = fftw_plan_dft_1d(PADDED_SIZE, data_in,            fft_data,    FFTW_FORWARD,  FFTW_EXHAUSTIVE);
				plan_backward = fftw_plan_dft_1d(PADDED_SIZE, filter_convolution, fftw_result, FFTW_BACKWARD, FFTW_EXHAUSTIVE);
			}
		}

		/*Begin ERSP*/
		#pragma omp for
		for ( x = 0; x < trials; ++x)
		{
			memset(data_in,            0.0, sizeof( fftw_complex ) * PADDED_SIZE);
			memset(fft_data,           0.0, sizeof( fftw_complex ) * PADDED_SIZE);
			memset(filter_convolution, 0.0, sizeof( fftw_complex ) * PADDED_SIZE);
			memset(fftw_result,        0.0, sizeof( fftw_complex ) * PADDED_SIZE);

			/*Begin Wavelet Analysis*/
			PopulateDataArray(raw_data, n, x,
							  PADDED_SIZE, padding_type, data_in);
			fftw_execute(plan_forward);

			for (i = 0; i < J; ++i)
			{
				FrequencyMultiply(fft_data, PADDED_SIZE, scales[i], dw,
								  filter_convolution);

				//Take the inverse FFT and store it in fftw_result
				fftw_execute(plan_backward);

				//Calculate the power and store it in result this may need to be changed to accomodate for phase
				for (j = 0; j < n; ++j)
				{
					wavelet_out[i * n + j] = MAGNITUDE(fftw_result[j][0], fftw_result[j][1]) / (NORMALIZATION_FACTOR * sqrt(scales[i]) );
				}
			}
			/*End Wavelet Analysis*/

			//Remove the baseline
			RemoveBaseline(pre_stimulus, wavelet_out,
						   n, J, m,
						   baseline_out);

			for ( i = 0; i < n * J; ++i)
			{
				output[i] += fabs(baseline_out[i]);
			}
		}

		#pragma omp for simd
		for ( i = 0; i < n * J; ++i)
		{
			output[i] /= trials;
		}
		/*End ERSP*/

		//Sanitation Engineering
		fftw_destroy_plan(plan_forward); fftw_destroy_plan(plan_backward);
		fftw_free(data_in); fftw_free(fft_data); fftw_free(filter_convolution); fftw_free(fftw_result);
		free(pre_stimulus); free(baseline_out); free(wavelet_out);

    }/*End of OpenMP*/
	return(0);
}

int RemoveBaseline(double* pre_stimulus, double* pre_baseline_array,
	const int n, const int J, const int m,
	double* output)
{
	double value;
	double mean, sDeviation;
	const int stride = 1;
	int i, j;

	for ( i = 0; i < J; ++i)
	{
		//Copy the pre trial results from each frequency block into pre_stimulus.
		for ( j = 0; j < m; ++j)
		{
			pre_stimulus[j] = pre_baseline_array[i * n + j];
		}

		//Calculate mean and standard deviation
		mean       = gsl_stats_mean(pre_stimulus, stride, m);
    	sDeviation = gsl_stats_sd_m(pre_stimulus, stride, m, mean);

    	//Remove the Baseline
	    for ( j = 0; j < n; ++j)
	    {
	    	value = pre_baseline_array[i * n + j] * pre_baseline_array[i * n + j];
	        output[i * n + j] = (fabs(value) - mean) / sDeviation;
	        // output[i * n + j] = ( pre_baseline_array[i * n + j] - mean ) / sDeviation;
	    }
	}

	return(0);
}

int Generate_FFTW_Wisdom(int padded_size)
{
	int success_flag = 1;

	//Array Inits
	fftw_complex *data_in, *fft_data, *filter_convolution, *fftw_result;
	fftw_plan plan_forward, plan_backward;

	fftw_init_threads();

	//FFTW Memory Allocations
    data_in            = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * padded_size );
	fft_data           = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * padded_size );
	filter_convolution = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * padded_size );
	fftw_result        = (fftw_complex *) fftw_malloc( sizeof( fftw_complex ) * padded_size );

	//FFTW Planning
	fftw_plan_with_nthreads(1);
	printf("Generating an Exhaustive FFTW Plan\n");
	plan_forward  = fftw_plan_dft_1d(padded_size, data_in,            fft_data,    FFTW_FORWARD,  FFTW_EXHAUSTIVE);
	plan_backward = fftw_plan_dft_1d(padded_size, filter_convolution, fftw_result, FFTW_BACKWARD, FFTW_EXHAUSTIVE);

	printf("Writing FFTW plan to FFTW_Plan.wise\n");
	if (fftw_export_wisdom_to_filename("FFTW_Plan.wise") != 0)
	{
		success_flag = 0;
	}

	fftw_destroy_plan(plan_forward); fftw_destroy_plan(plan_backward);
	fftw_free(data_in); fftw_free(fft_data); fftw_free(filter_convolution); fftw_free(fftw_result);

	return(success_flag);
}

int FrequencyMultiply(fftw_complex* fft_data,
	const int data_size, const double scale, const double dw,
	fftw_complex* filter_convolution)
{
	int j;
	double value;
	//Compute the Fourier Morlet at 0 and N/2
	double norm = sqrt(scale);
	value = CompleteFourierMorlet(0.0, scale, norm);

	filter_convolution[0][0] = (fft_data[0][0]/data_size) * value;
	filter_convolution[0][1] = (fft_data[0][1]/data_size) * value;

	filter_convolution[data_size/2][0] = 0.0;
	filter_convolution[data_size/2][1] = 0.0;

	//Compute the Fourier Morlet Convolution in between
	for (j = 1; j < data_size/2 - 1; ++j)
	{
		value = CompleteFourierMorlet( j * dw , scale, norm);
		filter_convolution[j][0] = (fft_data[j][0]/data_size) * value;
		filter_convolution[j][1] = (fft_data[j][1]/data_size) * value;

		filter_convolution[data_size- j][0] = 0.0;
		filter_convolution[data_size- j][1] = 0.0;
	}

	return(0);
}

int PopulateDataArray(double* input_data, const int data_size, const int trial_number,
	const int padded_size, const int padding_type,
	fftw_complex* output_data)
{
	const double ramp = 2.0/data_size; // = 1.0/ n / 2
	double gain;
	int i;

	int output_counter = 0;
	int input_counter = 0;

	switch(padding_type)
	{
		case 0: //No Padding what so ever
			//populate the FFTW data vector.
			for (i = 0; i < data_size; ++i)
		    {
		    	output_data[i][0] = input_data[trial_number * data_size + i];
		    	output_data[i][1] = 0.0;
		    }
		    break;

		case 1: //Zero - Padding
			//populate the FFTW data vector.
			for (i = 0; i < data_size; ++i)
		    {
		    	output_data[i][0] = input_data[trial_number * data_size + i];
		    	output_data[i][1] = 0.0;
		    }

		    //Horse the rest of the data vector to zero just in case
		    for (i = data_size; i < padded_size; ++i)
		    {
		    	output_data[i][0] = 0.0;
		    	output_data[i][1] = 0.0;
		    }
		    break;

		case 2: //Duplicate array and ramp up and ramp down output
			for (i = 0; i < data_size; ++i)
		    {
		    	output_data[i][0] = input_data[trial_number * data_size + i];
		    	output_data[i][1] = 0.0;
		    }

		    for (i = 0; i < (int) (0.5* data_size); ++i)
		    {
		    	output_counter = data_size + i;
		    	input_counter = (int) (trial_number * data_size + (0.5*data_size) + i);
		    	gain = i * ramp;

		    	output_data[output_counter][0] = (1.0 - gain) * input_data[input_counter];
		    	output_data[output_counter][1] = 0.0;

		    	output_data[output_counter + (int) (0.5 * data_size)][0] = gain * input_data[trial_number * data_size + i];
		    	output_data[output_counter + (int) (0.5 * data_size)][1] = 0.0;
		    }
			break;

		default: //Return just the array no padding.
			//populate the FFTW data vector.
			for (i = 0; i < data_size; ++i)
		    {
		    	output_data[i][0] = input_data[trial_number * data_size + i];
		    	output_data[i][1] = 0.0;
		    }
		    break;
	}
    return(padding_type);
}


#ifndef LINALG_H_GUARD
#define LINALG_H_GUARD

#include "coneOS.h"
#include <cblas.h>
#include <math.h>

void setAsScaledArray(double *x, const double * a,const double b,int len);
void scaleArray(double * a,const double b,int len);
double innerProd(const double * x, const double * y, int len);
double calcNorm(const double * v,int len);
double calcNormInf(const double *a, int l);
double calcNormSq(const double * v,int len);
void addScaledArray(double * a, const double * b, int n, const double sc);
double calcNormDiff(const double *a,const double *b, int l);
double calcNormInfDiff(const double *a, const double *b, int l);
void accumByAtrans(Data * d, const double *x, double *y);
void accumByA(Data * d, const double *x, double *y);
#endif


#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics_double.h>
#include <utilities.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_image_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_pixel_array.h"
#include "tz_stack_bwmorph.h"
#include "tz_dimage_lib.h"
#include "tz_darray.h"
#include "tz_u8array.h"
#include "tz_stack_objlabel.h"
#include "tz_voxel_linked_list.h"
#include "tz_voxel_graphics.h"
#include "tz_dmatrix.h"
#include "tz_stack_sampling.h"
#include "tz_stack_draw.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[])
{
  char neuron_name[100];
  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  char file_path[100];
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);

  Stack *mask = Read_Stack(file_path);

  //sprintf(file_path, "../data/%s/seeds.pa", neuron_name);
  //Pixel_Array *pa = Pixel_Array_Read(file_path);
  //double *pa_array = (double *) pa->array;
  //double max_dist = gsl_stats_max(pa_array, 1, pa->size);

  sprintf(file_path, "../data/%s/seeds.ar", neuron_name);
  int length;
  double *pa_array = darray_read(file_path, &length);
  gsl_sort(pa_array, 1, length);

  //double mean_dist = gsl_stats_median_from_sorted_data(pa_array, 1, length);
  double mean_dist = darray_mean(pa_array, length);
  double std_dist = sqrt(gsl_stats_variance(pa_array, 1, length));

  /* for fly neuron data, a good eroding size is around 6*/
  int erode_size = (int) (mean_dist + std_dist + 0.5);
  //int erode_size = 6;

  //int erode_size = (int)
  //  round(gsl_stats_quantile_from_sorted_data(pa_array, 1, length, 0.85));
  //int erode_size = 6;

  //darray_write("../data/test.bn",pa_array, length);

  printf("%d\n", erode_size);

  int dilate_size = erode_size * 2;

  Struct_Element *se = Make_Ball_Se(erode_size);
  Stack *erode_mask = Stack_Erode_Fast(mask, NULL, se);

  Free_Se(se);

  se = Make_Ball_Se(dilate_size);
  Stack *dilate_mask = Stack_Dilate(erode_mask, NULL, se);

  /*
  Stack *erode_region = Copy_Stack(erode_mask);
  int region_start = 2;
  int nobj = Stack_Label_Objects_N(erode_region, NULL, 1, region_start, 26);
  double *dist = darray_malloc(nobj);
  int i;
  for (i = 0; i < nobj; i++) {
    dist[i] = 0.0;
  }

  sprintf(file_path, "../data/%s/erode_region.tif", neuron_name);
  Write_Stack(file_path, erode_region);

  sprintf(file_path, "../data/%s/seeds.tif", neuron_name);
  Stack *seed_stack = Read_Stack(file_path);
  Voxel_List *list = Stack_To_Voxel_List(seed_stack);
  Kill_Stack(seed_stack);
  Pixel_Array *region_pa = Voxel_List_Sampling(erode_region, list);
  uint8 *region_array = (uint8 *) region_pa->array;
  //  u8array_print2(region_array, region_pa->size, 1);

  dist -= region_start;
  for (i = 0; i < region_pa->size; i++) {
    if (region_array[i] >= region_start) {
      if (dist[region_array[i]] <  pa_array[i]) {
	dist[region_array[i]] = pa_array[i];
      }
    }
  }
  dist += region_start;

  darray_print2(dist, nobj, 1);

  Stack *dilate_mask = Copy_Stack(erode_mask);
  Stack *tmp_dilate = Copy_Stack(erode_mask);
  for (i = 0; i < nobj; i++) {
    se = Make_Ball_Se((int) (dist[i] + 0.5));
    Stack_Dilate_Rm(erode_mask, tmp_dilate, se,
		    erode_region, region_start + i);
    Stack_Or(dilate_mask, tmp_dilate, dilate_mask);
    Free_Se(se);
  }

  Kill_Stack(tmp_dilate);
  */

  Stack_And(mask, dilate_mask, dilate_mask);

  sprintf(file_path, "../data/%s/blobmask.tif", neuron_name);
  Write_Stack(file_path, dilate_mask);
  printf("%s created.\n", file_path);

  sprintf(file_path, "../data/%s/large_seeds.tif", neuron_name);
  Stack *seeds = Read_Stack(file_path);
  Stack *blob = dilate_mask;
  Stack_Not(blob, blob);
  Stack_And(seeds, blob, seeds);

  Kill_Stack(mask);
  Kill_Stack(dilate_mask);
  Kill_Stack(erode_mask);

  sprintf(file_path, "../data/%s.tif", neuron_name);
  Stack *signal = Read_Stack(file_path);
  Stack *canvas = Translate_Stack(signal, COLOR, 0);
  Stack_Draw_Voxel_Mc(canvas, seeds, 4, 255, 0, 0);
  sprintf(file_path, "../data/%s/seedimg.tif", neuron_name);
  Write_Stack(file_path, canvas);
  printf("%s (show where tracing seeds are located) created.\n", file_path);

  return 0;
}


/**
 *
 * @file example_dpotrf.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example of Cholesky factorization
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:20 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_dmain.h"

static int check_factorization(int, double*, double*, int, int);

//int IONE=1;
//int ISEED[4] = {0,0,0,1};   /* initial seed for dlarnv() */

static void GENMAT_SYM_FULL(int m, double *A)
{
	srand48(time(NULL));

	int j;
	for (j = 0; j < m; ++j ) {
		int i;
		for( i = j; i < m; ++i ) {
			double dran = drand48();
			A[j*m+i] = A[i*m+j] = dran;
		}
  	}
	for(j = 0; j < m; ++j)
		A[j*m+j] += 10 * m;
}

int testing_dpotrf(int argc, char **argv)
{

	if (argc != 2){
		fprintf(stderr,"POTRF: N LDA\n");
		return -1;
	}

    int N     = atoi(argv[0]);
    int LDA   = atoi(argv[1]);
    int info_factorization;

	PLASMA_Set(PLASMA_TILE_SIZE, 388);
    double *A1   = (double *)malloc(LDA*N*sizeof(double));
	#pragma omp register ([LDA*N]A1)
    double *A2   = (double *)malloc(LDA*N*sizeof(double));
	#pragma omp register ([LDA*N]A2)

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)){
        printf("Out of Memory \n ");
        return 0;
    }

    /* Initialize A1 and A2 for Symmetric Positive Matrix */
	GENMAT_SYM_FULL(N, A1);
	int i;
	for(i = 0; i < N*LDA; ++i){
		A2[i] = A1[i];
	}

    /* Plasma routines */
    PLASMA_dpotrf(PlasmaUpper, N, A2, LDA);

    /* Check the factorization */
    info_factorization = check_factorization( N, A1, A2, LDA, PlasmaUpper);

    if ( info_factorization != 0 )
       printf("-- Error in DPOTRF example ! \n");
    else
       printf("-- Run of DPOTRF example successful ! \n");

    free(A1); free(A2);

    return 0;
}

static int check_factorization(int N, double *A1, double *A2, int LDA, int uplo)
{
    double Anorm, Rnorm;
    double alpha;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Residual = (double *)malloc(N*N*sizeof(double));
    double *L1       = (double *)malloc(N*N*sizeof(double));
    double *L2       = (double *)malloc(N*N*sizeof(double));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(double));
    memset((void*)L2, 0, N*N*sizeof(double));

    alpha= 1.0;

    LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, N, Residual, N, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

int timing_dpotrf(int argc, char **argv)
{

    int N;
    int LDA;
	int bs;
	int rep;

	double start, end;
	double elapsed;
	FILE *log;
	int num_threads;

	if (argc != 4 ) {
		fprintf(stderr,"DPOTRF n lda bs rep\n");
		exit(1);
	}

	sscanf(argv[0], "%d", &N);
	sscanf(argv[1], "%d", &LDA);
	sscanf(argv[2], "%d", &bs);
	sscanf(argv[3], "%d", &rep);

    double *A = malloc(LDA*N*sizeof(double));
	#pragma omp register ([LDA*N]A)

    /* Check if unable to allocate memory */
    if (!A){
        printf("Out of Memory \n ");
        return 0;
    }

    /* Plasma Initialize */
	PLASMA_Set( PLASMA_TILE_SIZE, bs);

	GENMAT_SYM_FULL(N, A);

	elapsed = 0.0;
	int i;
	for ( i = 0; i < rep; i++ ) {
		start = gtime();
		PLASMA_dpotrf(PlasmaUpper, N, A, LDA);
		end = gtime();
		elapsed += end - start;
	}

	num_threads = omp_get_max_threads();
	dump_info("plasma_dpotrf.log", num_threads, elapsed, rep);

    free(A);

    return 0;

}


// run with OPENBLAS_NUM_THREADS=1 and OMP_NUM_THREADS=n
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <cblas.h>
#include <omp.h>
#include <pthread.h>
#define MIN_SIZE 5
#define MAX_SIZE 60
#define NB_SIZE 10

// number of loop for a 1x1 matrix. Lower it if the test is
// too slow on you computer.
#define NLOOP 2e7

typedef struct {
    int matrix_size;
    int n_loop;
    void (* bench_func)();
    void (* blas_func)();
    void * (* create_matrix)(int size);
} BenchParam;

void * s_create_matrix(int size) {
    float * r = malloc(size * sizeof(double));
    int i;
    for(i = 0; i < size; i++)
        r[i] = 1e3 * i / size;
    return r;
}

void * c_create_matrix(int size) {
    float * r = malloc(size * 2 * sizeof(double));
    int i;
    for(i = 0; i < 2 * size; i++)
        r[i] = 1e3 * i / size;
    return r;
}

void * z_create_matrix(int size) {
    double * r = malloc(size * 2 * sizeof(double));
    int i;
    for(i = 0; i < 2 * size; i++)
        r[i] = 1e3 * i / size;
    return r;
}

void * d_create_matrix(int size) {
    double * r = malloc(size * sizeof(double));
    int i;
    for(i = 0; i < size; i++)
        r[i] = 1e3 * i / size;
    return r;
}

void trmv_bench(BenchParam * param)
{
    int i, n;
    int size = param->matrix_size;
    n = param->n_loop / size;
    int one = 1;
    void * A = param->create_matrix(size * size);
    void * y = param->create_matrix(size);
    for(i = 0; i < n; i++) {
        param->blas_func("U", "N", "N", &size, A, &size, y, &one);
    }
    free(A);
    free(y);
}

void gemv_bench(BenchParam * param)
{
    int i, n;
    int size = param->matrix_size;
    n = param->n_loop / size;
    double v = 1.01;
    int one = 1;
    void * A = param->create_matrix(size * size);
    void * y = param->create_matrix(size);
    for(i = 0; i < n; i++) {
        param->blas_func("N", &size, &size, &v, A, &size, y, &one, &v, y, &one);
    }
    free(A);
    free(y);
}

void ger_bench(BenchParam * param) {
    int i, n;
    int size = param->matrix_size;
    n = param->n_loop / size;
    double v = 1.01;
    int one = 1;
    void * A = param->create_matrix(size * size);
    void * y = param->create_matrix(size);
    for(i = 0; i < n; i++) {
        param->blas_func(&size, &size, &v, y, &one, y, &one, A, &size);
    }
    free(A);
    free(y);
}

#ifndef _WIN32
void * pthread_func_wrapper(void * param) {
    ((BenchParam *)param)->bench_func(param);
    pthread_exit(NULL);
}
#endif

#define NB_TESTS 5
void * TESTS[4 * NB_TESTS] = {
    trmv_bench, ztrmv_, z_create_matrix, "ztrmv",
    gemv_bench, dgemv_, d_create_matrix, "dgemv",
    gemv_bench, zgemv_, z_create_matrix, "zgemv",
    ger_bench, dger_, d_create_matrix, "dger",
    ger_bench, zgerc_, z_create_matrix, "zgerc",
};

inline static double delta_time(struct timespec tick) {
    struct timespec tock;
	clock_gettime(CLOCK_MONOTONIC, &tock);
	return (tock.tv_sec - tick.tv_sec) + (tock.tv_nsec - tick.tv_nsec) / 1e9;
}

double pthread_bench(BenchParam * param, int nb_threads)
{
#ifdef _WIN32
    return 0;
#else
    BenchParam threaded_param = *param;
    pthread_t threads[nb_threads];
    int t, rc;
    struct timespec tick;
    threaded_param.n_loop /= nb_threads;
    clock_gettime(CLOCK_MONOTONIC, &tick);
    for(t=0; t<nb_threads; t++){
        rc = pthread_create(&threads[t], NULL, pthread_func_wrapper, &threaded_param);
        if (rc){
            printf("ERROR; return code from pthread_create() is %d\n", rc);
            exit(-1);
        }
    }
    for(t=0; t<nb_threads; t++){
        pthread_join(threads[t], NULL);
    }
	return delta_time(tick);
#endif
}

double seq_bench(BenchParam * param) {
    struct timespec tick;
    clock_gettime(CLOCK_MONOTONIC, &tick);
    param->bench_func(param);
    return delta_time(tick);
}

double omp_bench(BenchParam * param) {
    BenchParam threaded_param = *param;
    struct timespec tick;
    int t;
    int nb_threads = omp_get_max_threads();
    threaded_param.n_loop /= nb_threads;
    clock_gettime(CLOCK_MONOTONIC, &tick);
    #pragma omp parallel for
    for(t = 0; t < nb_threads; t ++){
        param->bench_func(&threaded_param);
    }
    return delta_time(tick);
}

int main(int argc, char * argv[]) {
    double inc_factor = exp(log((double)MAX_SIZE / MIN_SIZE) / NB_SIZE);
    BenchParam param;
    int test_id;
    printf ("Running on %d threads\n", omp_get_max_threads());
    for(test_id = 0; test_id < NB_TESTS; test_id ++) {
        double size = MIN_SIZE;
        param.bench_func = TESTS[test_id * 4];
        param.blas_func = TESTS[test_id * 4 + 1];
        param.create_matrix = TESTS[test_id * 4 + 2];
        printf("\nBenchmark of %s\n", (char*)TESTS[test_id * 4 + 3]);
        param.n_loop = NLOOP;
        while(size <= MAX_SIZE) {
            param.matrix_size = (int)(size + 0.5);
            double seq_time = seq_bench(&param);
            double omp_time = omp_bench(&param);
            double pthread_time = pthread_bench(&param, omp_get_max_threads());
            printf("matrix size %d, sequential %gs, openmp %gs, speedup %g, "
                   "pthread %gs, speedup %g\n",
                   param.matrix_size, seq_time,
                   omp_time, seq_time / omp_time,
                   pthread_time, seq_time / pthread_time);
            size *= inc_factor;
        }
    }
    return(0);
}


#pragma once
#include <gsl/gsl_vector.h>
#include "BooleanNodes.h"
#include "BooleanDAG.h"
#include "NodeVisitor.h"
#include "VarStore.h"
#include <map>
#include "FloatSupport.h"
#include <glpk.h>

#include <iostream>
#include <fstream>

using namespace std;

class Line { // m(x - x0) + d < 0 (x0 is fixed for a particular evaluator, hence not stored in the line)
public:
	double d;
	gsl_vector* m;
	Line(double _d, gsl_vector* _m): d(_d), m(_m) {}
	~Line(void) {
		delete m;
	}

	double getOffset(const gsl_vector* x0) { // m.x0 - d
		double r = 0.0;
		for (int i = 0; i < x0->size; i++) {
			r += gsl_vector_get(x0, i) * gsl_vector_get(m, i);
		}
		return r - d;
	}

	double getCoeff(int i) {
		return gsl_vector_get(m, i);
	}

	double getDist() {
		double r = 0;
		for (int i = 0; i < m->size; i++) {
			r += gsl_vector_get(m, i) * gsl_vector_get(m, i);
		}
		return d/sqrt(r);
	}

	string print() {
		stringstream str;
		str << "<";
		for (int i = 0; i < m->size; i++) {
			str << gsl_vector_get(m, i) << " ";
		}
		str << d << ">";
		return str.str();
	}
};

class Region { // Intersection of lines
public:
	vector<Line*> lines;
	int size() {
		return lines.size();
	}
	Line* get(int i) {
		return lines[i];
	}
	void addLine(Line* l) {
		if (find(lines.begin(), lines.end(), l) == lines.end()) {
			lines.push_back(l);
		}
	}

	static Region* intersect(Region* r1, Region* r2) {
		Region* r = new Region();
		for (int i = 0; i < r1->size(); i++) {
			r->addLine(r1->get(i));
		}
		for (int i = 0; i < r2->size(); i++) {
			r->addLine(r2->get(i));
		}
		return r;
	}

	static Region* copy(Region* r1) {
		Region* r = new Region();
		for (int i = 0; i < r1->size(); i++) {
			r->addLine(r1->get(i));
		}
		return r;
	}

	double getMaxDist() {
		double d = - numeric_limits<double>::max();
		for (int i = 0; i < lines.size(); i++) {
			if (lines[i]->getDist() > d) {
				d = lines[i]->getDist();
			}
		}
		return d;
	}

	gsl_vector* getMaxGrad() {
		double d = - numeric_limits<double>::max();
		gsl_vector* g;
		for (int i = 0; i < lines.size(); i++) {
			if (lines[i]->d > d) {
				d = lines[i]->d;
				g = lines[i]->m;
			}
		}
		return g;

	}

	~Region(void) {
		for (int i = 0; i < lines.size(); i++) {
			delete lines[i];
		}
	}

	string print() {
		stringstream str;
		str << "{";
		for (int i = 0; i < lines.size(); i++) {
			str << lines[i]->print() << " ";
		}
		str << "}";
		return str.str();
	}

};

class Value {
public:
	double val;
	gsl_vector* grad;
	Value(double v, gsl_vector* g): val(v), grad(g) {}
	~Value(void) {
		delete grad;
	}

	double getOffset(const gsl_vector* x0) { // d - m.x0
		double r = 0.0;
		for (int i = 0; i < x0->size; i++) {
			r += gsl_vector_get(x0, i) * gsl_vector_get(grad, i);
		}
		return val - r;
	}

	double getCoeff(int i) {
		return gsl_vector_get(grad, i);
	}

	static Value* add(Value* v1, Value* v2);
	static Value* mult(Value* v1, Value* v2);
	static Value* div(Value* v1, Value* v2);
	static Value* neg(Value* v1);
	static Value* copy(Value* v1);
	static Value* arctan(Value* v1);
	static Value* tan(Value* v1);
	static Value* cos(Value* v1);
	static Value* sin(Value* v1);
	static Value* sqrt(Value* v1);
	static Value* square(Value* v1);

	string print() {
		stringstream str;
		str << "<";
		for (int i = 0; i < grad->size; i++) {
			str << gsl_vector_get(grad, i) << " ";
		}
		str << val << ">";
		return str.str();
	}
};

class RegionValuePair {
public:
	Region* r;
	Value* v;
	RegionValuePair(Region* _r, Value* _v): r(_r), v(_v) {}

	string print() {
		stringstream str;
		str << r->print() << " -> " << v->print();
		return str.str();
	}
};

typedef vector<RegionValuePair*> ValuesList;

class LP {
	glp_prob* lp;
	int ia[1+1000], ja[1+1000];
	double ar[1+1000];
	double LOWBND = -32.0; // TODO: don;t hardcode
	double HIGHBND = 32.0;
	double PRECISION = 1e-5;

public:
	double optimize(RegionValuePair* rv, const gsl_vector* x0) {
		lp = glp_create_prob();
		glp_set_obj_dir(lp, GLP_MIN);
		int numrows = rv->r->size();
		if (numrows > 0) {
			glp_add_rows(lp, numrows);
			for (int i = 0; i < numrows; i++) {
				glp_set_row_name(lp, (i+1), "r" + (i+1));
				double c = rv->r->lines[i]->getOffset(x0);
				glp_set_row_bnds(lp, (i+1), GLP_UP, 0.0, c - PRECISION);
			}
		}
		int numcols = x0->size;
		glp_add_cols(lp, numcols);
		for (int i = 0; i < numcols; i++) {
			glp_set_col_name(lp, (i+1), "x" + (i+1));
			glp_set_col_bnds(lp, (i+1), GLP_DB, LOWBND, HIGHBND);
			glp_set_obj_coef(lp, (i+1), rv->v->getCoeff(i));
		}
		int counter = 1;
		for (int i = 0; i < numrows; i++) {
			for (int j = 0; j < numcols; j++) {
				ia[counter] = (i+1);
				ja[counter] = (j+1);
				ar[counter] = rv->r->lines[i]->getCoeff(j);
				counter++;
			}
		}
		Assert(counter-1 <= 1000, "Out of bounds");

		glp_load_matrix(lp, counter-1, ia, ja, ar);
		glp_simplex(lp, NULL);
		int status = glp_get_prim_stat(lp);
		cout << "status: " << status << endl;
		if (status == GLP_FEAS) {
			double z = glp_get_obj_val(lp);
			cout << "z = " << z;
			for (int i = 0; i < numcols; i++) {
				cout << " x" << i << " = " << glp_get_col_prim(lp, (i+1));
			}
			glp_delete_prob(lp);
			return z + rv->v->getOffset(x0);
		} else {
			return 1000;
		}

	}

	bool check(Region* r, const gsl_vector* x0) {
		int numrows = r->size();
		if (numrows == 0) return true;

		lp = glp_create_prob();
		glp_set_obj_dir(lp, GLP_MIN);
		glp_add_rows(lp, numrows);
		for (int i = 0; i < numrows; i++) {
			stringstream str;
			str << "r" << (i+1);
			glp_set_row_name(lp, (i+1), str.str().c_str());
			double c = r->lines[i]->getOffset(x0);
			glp_set_row_bnds(lp, (i+1), GLP_UP, 0.0, c-PRECISION);
		}
		int numcols = x0->size;
		glp_add_cols(lp, numcols);
		for (int i = 0; i < numcols; i++) {
			stringstream str;
			str << "x" << (i+1);
			glp_set_col_name(lp, (i+1), str.str().c_str());
			glp_set_col_bnds(lp, (i+1), GLP_DB, LOWBND, HIGHBND);
			glp_set_obj_coef(lp, (i+1), 0.0);
		}
		int counter = 1;
		for (int i = 0; i < numrows; i++) {
			for (int j = 0; j < numcols; j++) {
				ia[counter] = (i+1);
				ja[counter] = (j+1);
				ar[counter] = r->lines[i]->getCoeff(j);
				counter++;
			}
		}

		glp_load_matrix(lp, counter-1, ia, ja, ar);
		glp_smcp param;
		glp_init_smcp(&param);
		param.msg_lev = GLP_MSG_ERR;
		glp_simplex(lp, &param);
		int status = glp_get_prim_stat(lp);
		glp_delete_prob(lp);
		if (status == GLP_FEAS) {
			return true;
		} else {
			return false;
		}
	}

};

class GlobalEvaluator: NodeVisitor
{
	FloatManager& floats;
	BooleanDAG& bdag;
  map<string, int> floatCtrls; // Maps float ctrl names to indices with grad vectors
  int nctrls; // number of float ctrls
	gsl_vector* ctrls; // Ctrl values
	vector<ValuesList> values; // Keeps track of values for each node for different regions
	map<int, int> inputValues; // Maps node id to values set by the SAT solver
	double error = 0.0;
	gsl_vector* errorGrad;

	int DEFAULT_INP = -1;
	static gsl_vector* tmp;
	LP* lp;

	int MAX_REGIONS = 16;
	int ctr = 0;
public:
  GlobalEvaluator(BooleanDAG& bdag_p, FloatManager& _floats, const map<string, int>& floatCtrls_p);
  ~GlobalEvaluator(void);

  virtual void visit( SRC_node& node );
  virtual void visit( DST_node& node );
  virtual void visit( CTRL_node& node );
  virtual void visit( PLUS_node& node );
  virtual void visit( TIMES_node& node );
  virtual void visit( ARRACC_node& node );
  virtual void visit( DIV_node& node );
  virtual void visit( MOD_node& node );
  virtual void visit( NEG_node& node );
  virtual void visit( CONST_node& node );
  virtual void visit( LT_node& node );
  virtual void visit( EQ_node& node );
	virtual void visit( AND_node& node );
	virtual void visit( OR_node& node );
	virtual void visit( NOT_node& node );
  virtual void visit( ARRASS_node& node );
  virtual void visit( UFUN_node& node );
  virtual void visit( TUPLE_R_node& node );
	virtual void visit( ASSERT_node& node );

  double run(const gsl_vector* ctrls_p, map<int, int>& inputValues_p, gsl_vector* errorGrad_p);
	void truncate(ValuesList& vl, const gsl_vector* x0, int numRegions);
	void truncate(vector<Region*>& vl, const gsl_vector* x0, int numRegions);
	void truncateUnfeasibleRegions(ValuesList& vl, const gsl_vector* x0);
	void truncateUnfeasibleRegions(vector<Region*>& vl, const gsl_vector* x0);
	void truncateFarRegions(ValuesList& vl, const gsl_vector* x0, int numRegions);
	void truncateFarRegions(vector<Region*>& vl, const gsl_vector* x0, int numRegions);

	void setvalues(bool_node& bn, ValuesList& v) {
		values[bn.id] = v;
	}

	ValuesList& getvalues(bool_node& bn) {
		return values[bn.id];
	}

	ValuesList& getvalues(bool_node* bn) {
		return getvalues(*bn);
	}

	void print() {
    for (int i = 0; i < bdag.size(); i++) {
      cout << bdag[i]->lprint() << endl;
		}
  }

	bool isFloat(bool_node& bn) {
		return (bn.getOtype() == OutType::FLOAT);
	}

	bool isFloat(bool_node* bn) {
		return (bn->getOtype() == OutType::FLOAT);
	}

	int getInputValue(bool_node& bn) {
		if (inputValues.find(bn.id) != inputValues.end()) {
			int val = inputValues[bn.id];
			Assert(val == 0 || val == 1, "NYI: Integer values");
			return val;
		} else {
			return DEFAULT_INP;
		}
	}

	int getInputValue(bool_node* bn) {
		return getInputValue(*bn);
	}

	static gsl_vector* default_grad(int nctrls) {
		gsl_vector* g = gsl_vector_alloc(nctrls);
		for (int i = 0; i < nctrls; i++) {
			gsl_vector_set(g, i, 0);
		}
		return g;
	}

	static gsl_vector* add_grad(gsl_vector* g1, gsl_vector* g2);
	static gsl_vector* mult_grad(double v1, double v2, gsl_vector* g1, gsl_vector* g2);
	static gsl_vector* div_grad(double v1, double v2, gsl_vector* g1, gsl_vector* g2);
	static gsl_vector* neg_grad(gsl_vector* g1);
	static gsl_vector* copy_grad(gsl_vector* g1);
	static gsl_vector* sub_grad(gsl_vector* g1, gsl_vector* g2);
	static gsl_vector* arctan_grad(double v1, gsl_vector* g1);
	static gsl_vector* tan_grad(double v1, gsl_vector* g1);
	static gsl_vector* cos_grad(double v1, gsl_vector* g1);
	static gsl_vector* sin_grad(double v1, gsl_vector* g1);
	static gsl_vector* sqrt_grad(double v1, gsl_vector* g1);
	static gsl_vector* square_grad(double v1, gsl_vector* g1);

};


/**
 *
 * @file potrf.h
 *
 * @copyright 2009-2014 The University of Tennessee and The University of
 *                      Tennessee Research Foundation. All rights reserved.
 * @copyright 2012-2018 Bordeaux INP, CNRS (LaBRI UMR 5800), Inria,
 *                      Univ. Bordeaux. All rights reserved.
 *
 ***
 *
 * @brief Chameleon step2 example header
 *
 * @version 1.1.0
 * @author Florent Pruvost
 * @date 2019-02-06
 *
 */
#ifndef _potrf_h_
#define _potrf_h_

/* Common include for all steps of the tutorial */

/* Specific includes for potrf */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <errno.h>
#include <assert.h>
#include <stdarg.h>
#include <pthread.h>
#include <al4san.h>
#include "runtime/al4san_runtime.h"
#include "control/al4san_descriptor.h"
#include <al4san/timer.h>
//#include <lapacke.h>
#include <mkl.h>
#include <sys/time.h>
#include <sys/times.h>
#if defined( _WIN32 ) || defined( _WIN64 )
#include <windows.h>
#else  /* Non-Windows */
#include <unistd.h>
#include <sys/resource.h>
#endif

/* Common functions for all steps of the tutorial */
static void get_thread_count(int *thrdnbr) {
#if defined WIN32 || defined WIN64
    sscanf( getenv( "NUMBER_OF_PROCESSORS" ), "%d", thrdnbr );
#else
    *thrdnbr = sysconf(_SC_NPROCESSORS_ONLN);
#endif
}

/* define complexity of algorithms - see Lawn 41 page 120 */
#define FMULS_POTRF(__n) ((double)(__n) * (((1. / 6.) * (double)(__n) + 0.5) * (double)(__n) + (1. / 3.)))
#define FADDS_POTRF(__n) ((double)(__n) * (((1. / 6.) * (double)(__n)      ) * (double)(__n) - (1. / 6.)))
#define FMULS_TRSM(__m, __n) (0.5 * (double)(__n) * (double)(__m) * ((double)(__m)+1.))
#define FADDS_TRSM(__m, __n) (0.5 * (double)(__n) * (double)(__m) * ((double)(__m)-1.))

/*
    Cholesky Task Headers
*/

void Task_dpotrf( const AL4SAN_option_t *options,
                        al4san_uplo_t uplo, int n, int nb,
                        const AL4SAN_desc_t *A, int Am, int An, int lda,
                        int iinfo );
void Task_dgemm( const AL4SAN_option_t *options,
                        al4san_trans_t transA, al4san_trans_t transB,
                        int m, int n, int k, int nb,
                        double alpha, const AL4SAN_desc_t *A, int Am, int An, int lda,
                        const AL4SAN_desc_t *B, int Bm, int Bn, int ldb,
                        double beta, const AL4SAN_desc_t *C, int Cm, int Cn, int ldc );
void TASK_dplgsy( const AL4SAN_option_t *options,
                        double bump, int m, int n, AL4SAN_desc_t *A, int Am, int An, int lda,
                        int bigM, int m0, int n0, unsigned long long int seed );
void Task_dsyrk( const AL4SAN_option_t *options,
                       al4san_uplo_t uplo, al4san_trans_t trans,
                       int n, int k, int nb,
                       double alpha, const AL4SAN_desc_t *A, int Am, int An, int lda,
                       double beta, const AL4SAN_desc_t *C, int Cm, int Cn, int ldc );
void Task_dtrsm( const AL4SAN_option_t *options,
                       al4san_side_t side, al4san_uplo_t uplo, al4san_trans_t transA, al4san_diag_t diag,
                       int m, int n, int nb,
                       double alpha, const AL4SAN_desc_t *A, int Am, int An, int lda,
                       const AL4SAN_desc_t *B, int Bm, int Bn, int ldb );
void CORE_dplgsy( double bump, int m, int n, double *A, int lda,
                  int bigM, int m0, int n0, unsigned long long int seed );

int dplgsy_Tile( double bump, al4san_uplo_t uplo,
                           AL4SAN_desc_t *A,
                           unsigned long long int seed );

int dplgsy_Tile_Async( double  bump,
                                 al4san_uplo_t            uplo,
                                 AL4SAN_desc_t           *A,
                                 unsigned long long int seed,
                                 AL4SAN_sequence_t    *sequence,
                                 AL4SAN_request_t     *request );

void pdplgsy( double bump, al4san_uplo_t uplo, AL4SAN_desc_t *A,
                    unsigned long long int seed,
                    AL4SAN_sequence_t *sequence, AL4SAN_request_t *request );

extern char runtime[20];
/* Integer parameters for step2 */
enum iparam_step2 {
    IPARAM_THRDNBR,        /* Number of cores                            */
    IPARAM_GPUS,        /* Number of gpus                            */
    IPARAM_N,              /* Number of columns of the matrix            */
    IPARAM_NRHS,           /* Number of RHS                              */
    IPARAM_NB,           /* Number of NB                              */
    IPARAM_P,           /* Number of P                              */
    IPARAM_Q,           /* Number of Q                              */
    /* End */
    IPARAM_SIZEOF
};

/* Specific routines used in step2.c main program */

/**
 * Initialize integer parameters
 */
static void init_iparam(int iparam[IPARAM_SIZEOF]){
    iparam[IPARAM_THRDNBR       ] = -1;
    iparam[IPARAM_GPUS          ] = -1;
    iparam[IPARAM_N             ] = 500;
    iparam[IPARAM_NRHS          ] = 1;
    iparam[IPARAM_NB            ] = 10;
    iparam[IPARAM_P            ] = 1;
    iparam[IPARAM_Q            ] = 1;

}

/**
 * Print how to use the program
 */
static void show_help(char *prog_name) {
    printf( "Usage:\n%s [options]\n\n", prog_name );
    printf( "Options are:\n"
            "  --help           Show this help\n"
            "\n"
            "  --n=X            dimension (N). (default: 500)\n"
            "  --nrhs=X         number of RHS. (default: 1)\n"
            "  --nb=X         number of RHS. (default: 10)\n"
            "\n"
            "  --threads=X      Number of CPU workers (default: _SC_NPROCESSORS_ONLN)\n"
            "\n");
}

static int startswith(const char *s, const char *prefix) {
    size_t n = strlen( prefix );
    if (strncmp( s, prefix, n ))
        return 0;
    return 1;
}

/**
 * Read arguments following step2 program call
 */
static void read_args(int argc, char *argv[], int *iparam){
    int i;
    for (i = 1; i < argc && argv[i]; ++i) {
        if ( startswith( argv[i], "--help") || startswith( argv[i], "-help") ||
             startswith( argv[i], "--h") || startswith( argv[i], "-h") ) {
            show_help( argv[0] );
            exit(0);
        } else if (startswith( argv[i], "--n=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_N]) );
        } else if (startswith( argv[i], "--nrhs=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_NRHS]) );
        } else if (startswith( argv[i], "--nb=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_NB]) );
        } else if (startswith( argv[i], "--threads=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_THRDNBR]) );
        } else if (startswith( argv[i], "--gpus=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_GPUS]) );
        }else if (startswith( argv[i], "--p=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_P]) );
        } else if (startswith( argv[i], "--q=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_Q]) );
        } else if (startswith( argv[i], "--runtime=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%s", runtime);
        } else {
            fprintf( stderr, "Unknown option: %s\n", argv[i] );
        }
    }
}

/**
 * Print a header message to summarize main parameters
 */
static void print_header(char *prog_name, int * iparam) {
#if defined(CHAMELEON_SIMULATION)
    double    eps = 0.;
#else
    double    eps = LAPACKE_dlamch_work( 'e' );
#endif
  if(AL4SAN_My_Mpi_Rank()==0){

    printf( "#\n"
            "# AL4SAN %d.%d.%d, %s\n"
            "# Nb threads: %d\n"
            "# Nb gpus:    %d\n"
            "# Nb Prows:    %d\n"
            "# Nb Qcols:    %d\n"
            "# N:          %d\n"
            "# NB:         %d\n"
            "# eps:        %e\n"
            "# Activated runtime: %s\n"
            "#\n",
            AL4SAN_VERSION_MAJOR,
            AL4SAN_VERSION_MINOR,
            AL4SAN_VERSION_MICRO,
            prog_name,
            iparam[IPARAM_THRDNBR],
            iparam[IPARAM_GPUS],
            iparam[IPARAM_P],
            iparam[IPARAM_Q],
            iparam[IPARAM_N],
            iparam[IPARAM_NB],
            eps,
            runtime );

    printf( "#      M       N  K/NRHS      NB  seconds   Gflop/s\n");
    printf( "#%7d %7d %7d %7d ", iparam[IPARAM_N], iparam[IPARAM_N], iparam[IPARAM_NRHS], iparam[IPARAM_NB]);
    fflush( stdout );
}
    return;
}

#endif /* _step2_h_ */