text:
#include "common/c_math/gsl_linalg_extra.h"

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

#include <gsl/gsl_blas.h>
#include <gsl/gsl_blas_types.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>

// TODO(kennyjensen): Remove these and allocate arrays in calling
// library.
#define MAX_VECTOR_SIZE (32U)
#define MAX_MATRIX_SIZE (16U * 16U)

// Solves triangular matrix equations of the form:
//
//   op(T) * X = B   or   X * op(T) = B
//
// using forward or back substitution.  For rectangular T
// (i.e. underdetermined or overdetermined systems), this only solves
// the square submatrix of T.  For underdetermined systems, the
// remaining elements of X are set to zeros.  If the triangular matrix
// T is singular, then this will only solve the part of the matrix up
// to the first zero diagonal element.
//
// Args:
//   side: Which side of the multiplication op(T) appears.
//   uplo: Whether T is upper or lower triangular.
//   transpose: Whether op(.) is a transpose or not.
//   T: Triangular matrix (m x n).
//   B: Right hand side matrix (m x k).
//   X: Solution matrix (n x k).
//
// Returns:
//   GSL_ESING if T is singular and GSL_SUCCESS otherwise.
int32_t GslTriangularSolve(CBLAS_SIDE_t side, CBLAS_UPLO_t uplo,
                           CBLAS_TRANSPOSE_t transpose, const gsl_matrix *T,
                           const gsl_matrix *B, gsl_matrix *X) {
  assert(T != NULL && B != NULL && X != NULL);
  assert(T != X && B != X);
  assert(side != CblasLeft ||
         (X->size2 == B->size2 &&
          (transpose == CblasNoTrans ? T->size1 : T->size2) == B->size1 &&
          (transpose == CblasNoTrans ? T->size2 : T->size1) == X->size1));
  assert(side != CblasRight ||
         (X->size1 == B->size1 &&
          (transpose == CblasNoTrans ? T->size1 : T->size2) == X->size2 &&
          (transpose == CblasNoTrans ? T->size2 : T->size1) == B->size2));

  int32_t err = GSL_SUCCESS;
  size_t small_dim = (T->size1 < T->size2) ? T->size1 : T->size2;

  gsl_matrix_set_zero(X);
  // Calling this "rank" here is assuming that the diagonal elements
  // of T are in descending order.
  size_t rank = small_dim;
  for (size_t i = 0U; i < small_dim; ++i) {
    if (fabs(gsl_matrix_get(T, i, i)) <= DBL_EPSILON) {
      rank = i;
      err = GSL_ESING;
      break;
    }
    gsl_vector_const_view row = gsl_matrix_const_row(B, i);
    gsl_matrix_set_row(X, i, &row.vector);
  }
  if (rank == 0U) return err;

  // Make a square sub-matrix of T that has all non-zero elements
  // along the diagonal and a reduced B matrix with the same number of
  // rows.
  gsl_matrix_const_view T_rank =
      gsl_matrix_const_submatrix(T, 0U, 0U, rank, rank);
  gsl_matrix_view X_rank = gsl_matrix_submatrix(X, 0U, 0U, rank, X->size2);

  // Solve the triangular system op(T) * X = B or X * op(T) = B.
  gsl_blas_dtrsm(side, uplo, transpose, CblasNonUnit, 1.0, &T_rank.matrix,
                 &X_rank.matrix);
  return err;
}

// Converts a trapezoidal matrix R with m < n to a pure upper
// triangular matrix with zeros outside of the square portion of the
// matrix using orthogonal transformations from the right:
//
//   R = [T, 0] * Z
//
// Here Z = Z_1 * Z_2 * ... * Z_m with Z_k = I - tau_k * v_k * v_k'
// where v_k is the Householder vector which cancels the
// non-triangular elements of the kth row of T.  This is similar to
// the DTZRZF function from LAPACK.
//
// Args:
//   R: Right trapezoidal matrix with m < n.  This function will
//       happily ignore non-zero components in the lower portion of
//       the R matrix; however these will also appear in the T matrix.
//   T: Upper triangular matrix output (m x n).  The "zero" portion of
//       the T matrix is used to store the Householder vectors that,
//       with tau, form Z.
//   tau: Vector of length m of Householder coefficients.
void GslTrapezoidalToTriangular(const gsl_matrix *R, gsl_matrix *T,
                                gsl_vector *tau) {
  assert(R != NULL && T != NULL && tau != NULL);
  assert(R->size1 == T->size1 && R->size2 == T->size2);
  assert(tau->size == T->size1);
  assert(R->size1 > 0U && R->size1 < R->size2);

  if (T != R) gsl_matrix_memcpy(T, R);

  size_t i_max = R->size1 - 1U;
  for (size_t k = 0U; k < R->size1; ++k) {
    size_t i = i_max - k;

    // hh is used to calculate the Householder vector; however, the
    // Household vector itself is only stored in hh1.
    gsl_vector_view hh = gsl_matrix_subrow(T, i, i_max, R->size2 - i_max);
    gsl_vector_view hh1 =
        gsl_matrix_subrow(T, i, R->size1, R->size2 - R->size1);

    // GSL assumes that the elements you want to cancel are adjacent
    // to the diagonal element, which isn't the case here, so we have
    // to swap elements around.
    gsl_vector_view row = gsl_matrix_row(T, i);
    gsl_vector_swap_elements(&row.vector, i, i_max);
    double tau_k = gsl_linalg_householder_transform(&hh.vector);
    gsl_vector_set(tau, k, tau_k);
    gsl_vector_swap_elements(&row.vector, i, i_max);

    // Apply Householder reflections to the right hand side of the
    // triangular matrix.  We don't apply the reflection to the ith
    // row itself because GSL's Householder transform sets the
    // diagonal element for us.  (Doing this with the
    // gsl_linalg_householder_mh function would require swapping
    // columns because of assumptions GSL makes about the diagonal
    // element being adjacent to the elements that the Householder
    // vector cancels.)
    for (size_t j = 0U; j < i; ++j) {
      gsl_vector_view T_jm =
          gsl_matrix_subrow(T, j, R->size1, R->size2 - R->size1);
      double alpha, T_ji = gsl_matrix_get(T, j, i);
      gsl_blas_ddot(&hh1.vector, &T_jm.vector, &alpha);
      alpha += T_ji;
      gsl_blas_daxpy(-tau_k * alpha, &hh1.vector, &T_jm.vector);
      gsl_matrix_set(T, j, i, -tau_k * alpha + T_ji);
    }
  }
}

// Applies the inverse of the Householder transformations output by
// GslTrapezoidalToTriangular to the matrix X.  More specifically, for
// the matrix Z defined by:
//
//   R = [T, 0] * Z
//
// this function applies Z' to the input matrix X.
//
// Args:
//   T: The R matrix of a QR decomposition after complete
//       orthogonalization.  The elements that should be zero from the
//       QR decomposition are ignored.  The elements that should be
//       zero from the complete orthogonalization must contain the
//       Householder vectors used in the orthogonalization.
//   tau: The factors to multiply the Householder vectors by which is
//       output by the GslTrapezoidalToTriangular function.
//   X: The input matrix to which we apply Z'.
//   Zt_X: Output matrix with Householder transformations applied  Z' * X.
void GslTrapezoidalToTriangularZTMat(const gsl_matrix *T, const gsl_vector *tau,
                                     const gsl_matrix *X, gsl_matrix *Zt_X) {
  assert(X != NULL && tau != NULL && Zt_X != NULL && T != NULL);
  assert(T != X && T != Zt_X);
  assert(T->size1 == tau->size && T->size2 == X->size1);
  assert(X->size1 == Zt_X->size1 && X->size2 == Zt_X->size2);
  assert(T->size1 <= T->size2);

  if (X != Zt_X) gsl_matrix_memcpy(Zt_X, X);

  for (size_t i = 0U; i < T->size1; ++i) {
    // Grab the Householder vector from the columns past the square
    // section of the T matrix.
    gsl_vector_const_view hh1 =
        gsl_matrix_const_subrow(T, i, T->size1, T->size2 - T->size1);

    // Apply Householder reflections to the left hand side of X.
    // (Doing this with the gsl_linalg_householder_hm function would
    // require swapping rows because of assumptions GSL makes about
    // the diagonal element being adjacent to the elements that the
    // Householder vector cancels.)
    //
    // TODO(kennyjensen): If we need this non-standard
    // gsl_linalg_householder_hm in other places, we should make a
    // helper function.
    double tau_k = gsl_vector_get(tau, T->size1 - i - 1U);
    for (size_t j = 0U; j < Zt_X->size2; ++j) {
      gsl_vector_view Zt_X_mj =
          gsl_matrix_subcolumn(Zt_X, j, T->size1, T->size2 - T->size1);
      double alpha, Zt_X_ij = gsl_matrix_get(Zt_X, i, j);
      gsl_blas_ddot(&hh1.vector, &Zt_X_mj.vector, &alpha);
      alpha += Zt_X_ij;
      gsl_blas_daxpy(-tau_k * alpha, &hh1.vector, &Zt_X_mj.vector);
      gsl_matrix_set(Zt_X, i, j, -tau_k * alpha + Zt_X_ij);
    }
  }
}

// Left or right matrix divide.  Finds a solution, X, to the equations
// A*X = B or X*A = B for left and right divide respectively.  If the
// system is overdetermined or underdetermined, then this returns a
// least-squares solution that minimizes |A*X - B| or |X*A - B|,
// similar to MATLAB's "\" or "/" operators.  The solution is found
// using QR decomposition with column pivoting followed by back
// substitution.  For matrix left divide, the solution proceeds as:
//
//   A*X = B
//   Q*R*P' * X = B
//   R * P'*X = Q'*B
//
// For matrix right divide, the solution proceeds as:
//
//   X*A = B
//   A'*X' = B'
//   Q*R*P' * X' = B'
//   R * (X*P)' = Q'*B'
//
// Args:
//   side: CblasLeft or CblasRight for left or right divide.
//   A: Left hand side matrix.
//   B: Right hand side matrix.
//   X: Solution matrix.
//
// Returns:
//   GSL_ESING A is rank deficient and thus the R matrix is singular and
//   GSL_SUCCESS otherwise.
int32_t GslMatrixDivide(CBLAS_SIDE_t side, const gsl_matrix *A,
                        const gsl_matrix *B, gsl_matrix *X) {
  assert(A != NULL && B != NULL && X != NULL);
  assert(A != X && B != X);
  assert(side != CblasLeft || (A->size1 == B->size1 && A->size2 == X->size1 &&
                               X->size2 == B->size2));
  assert(side != CblasRight || (A->size2 == B->size2 && A->size1 == X->size2 &&
                                X->size1 == B->size1));
  assert(A->size1 <= MAX_VECTOR_SIZE && A->size2 <= MAX_VECTOR_SIZE);
  assert(A->size1 * A->size2 <= MAX_MATRIX_SIZE);
  assert(B->size1 * B->size2 <= MAX_MATRIX_SIZE);
  assert(X->size1 * X->size2 <= MAX_MATRIX_SIZE);

  int32_t err = GSL_SUCCESS;
  size_t m = (side == CblasLeft) ? A->size1 : A->size2;
  size_t n = (side == CblasLeft) ? A->size2 : A->size1;
  size_t k = (side == CblasLeft) ? B->size2 : B->size1;

  // Allocate workspace variables.
  double workspace[MAX_MATRIX_SIZE];
  double qr_data[MAX_MATRIX_SIZE];
  double tau_data[MAX_VECTOR_SIZE];
  size_t perm_data[MAX_VECTOR_SIZE];
  int signum;

  // Decompose op(A) into Q*R*P'.
  gsl_matrix_view QR = gsl_matrix_view_array(qr_data, m, n);
  gsl_vector_view tau = gsl_vector_view_array(tau_data, m < n ? m : n);
  gsl_permutation p = {n, perm_data};
  gsl_vector_view norm = gsl_vector_view_array(workspace, n);
  if (side == CblasLeft) {
    gsl_matrix_memcpy(&QR.matrix, A);
  } else {
    gsl_matrix_transpose_memcpy(&QR.matrix, A);
  }
  gsl_linalg_QRPT_decomp(&QR.matrix, &tau.vector, &p, &signum, &norm.vector);

  // Calculate Q'*op(B) matrix.
  gsl_matrix_view Qt_opB = gsl_matrix_view_array(workspace, m, k);
  if (side == CblasLeft) {
    gsl_matrix_memcpy(&Qt_opB.matrix, B);
  } else {
    gsl_matrix_transpose_memcpy(&Qt_opB.matrix, B);
  }
  gsl_linalg_QR_QTmat(&QR.matrix, &tau.vector, &Qt_opB.matrix);

  // Determine the rank of the R matrix.
  //
  // TODO(tobenkin): DGELSY uses a different iterative condition
  // number estimation technique to determine the rank of the
  // submatrix R(1:rank, 1:rank).  See http://www.netlib.org/lapack/
  // lapack-3.1.1/html/dgelsy.f.html#DGEQP3.269.
  double R_00 = fabs(gsl_matrix_get(&QR.matrix, 0U, 0U));
  uint32_t rank = 0U;
  if (R_00 > DBL_EPSILON) {
    for (rank = 1U; rank < m && rank < n; ++rank) {
      if (fabs(gsl_matrix_get(&QR.matrix, rank, rank)) < 1e-9 * R_00) {
        err = GSL_ESING;
        break;
      }
    }
  } else {
    gsl_matrix_set_zero(X);
    return GSL_ESING;
  }

  // Solve the triangular system R * Z * P' * op(X) = Q' * op(B) and
  // multiply the solution by Z' to get P' * op(X).
  gsl_matrix_view Z_Pt_opX = gsl_matrix_view_array(X->data, n, k);
  gsl_matrix_set_zero(&Z_Pt_opX.matrix);

  // For underdetermined or rank deficient matrices, use complete
  // orthogonalization to provide the minimal 2-norm solution.
  gsl_vector_view tau_trap = gsl_vector_view_array(tau_data, rank);
  gsl_matrix_view QR_trap = gsl_matrix_submatrix(&QR.matrix, 0U, 0U, rank, n);
  if (rank < n) {
    GslTrapezoidalToTriangular(&QR_trap.matrix, &QR_trap.matrix,
                               &tau_trap.vector);
  }

  // Solve the non-singular square triangular system.
  gsl_matrix_const_view QR_rank =
      gsl_matrix_const_submatrix(&QR.matrix, 0U, 0U, rank, rank);
  gsl_matrix_const_view Qt_opB_rank =
      gsl_matrix_const_submatrix(&Qt_opB.matrix, 0U, 0U, rank, k);
  gsl_matrix_view Z_Pt_opX_rank =
      gsl_matrix_submatrix(&Z_Pt_opX.matrix, 0U, 0U, rank, k);
  gsl_matrix_memcpy(&Z_Pt_opX_rank.matrix, &Qt_opB_rank.matrix);
  gsl_blas_dtrsm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0,
                 &QR_rank.matrix, &Z_Pt_opX_rank.matrix);

  // For full-rank square and overdetermined matrices, Z = I.
  if (rank < n) {
    GslTrapezoidalToTriangularZTMat(&QR_trap.matrix, &tau_trap.vector,
                                    &Z_Pt_opX.matrix, &Z_Pt_opX.matrix);
  }

  // Apply the permutation matrix to get op(X).
  for (uint32_t i = 0U; i < k; ++i) {
    gsl_vector_view col = gsl_matrix_column(&Z_Pt_opX.matrix, i);
    gsl_permute_vector_inverse(&p, &col.vector);
  }

  if (side == CblasRight) {
    // We can't transpose within the same memory, hence the
    // unfortunate extra memcpy.
    gsl_matrix_view X_copy = gsl_matrix_view_array(qr_data, k, n);
    gsl_matrix_transpose_memcpy(&X_copy.matrix, &Z_Pt_opX.matrix);
    gsl_matrix_memcpy(X, &X_copy.matrix);
  }

  return err;
}

max_stars_repo_name: OpenFAST/KiteFAST

max_issues_repo_name: OpenFAST/KiteFAST

max_forks_repo_name: OpenFAST/KiteFAST


text:
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_randist.h>

#include "allvars.h"
#include "proto.h"

void mcmc_conline_run()
{
  double aicc, chi2, aicc_best;
  double *theta_best_this, *theta_best_var_this;

  char fname_mcmc[100];

  aicc_best = DBL_MAX;
  theta_best_this = malloc(ntheta_max*sizeof(double));
  theta_best_var_this = malloc(ntheta_max*2*sizeof(double));

  printf("*******con-line mcmc\n");
  if(flag_mcmc==1)
  {
    fprintf(fp_results, "************AICC************\n");
    for(nc = nc_lim_low; nc<=nc_lim_up; nc++)
    {
      printf("nc = %d\n", nc);
      sprintf(fname_mcmc, "data/mcmc_%02d.txt", nc);
      mcmc_conline_init();
      mcmc_sampling(fname_mcmc, &probability_conline);
      mcmc_stats(fname_mcmc);
      aicc = cal_aicc();
      if(aicc < aicc_best)
      {
        aicc_best = aicc;
        nc_best = nc;
        memcpy(theta_best_this, theta_best, ntheta * sizeof(double));
        memcpy(theta_best_var_this, theta_best_var, ntheta * 2 * sizeof(double));
      }
      printf("aicc: %d %f\n", nc, aicc);
      fprintf(fp_results, "aicc: %d %f\n", nc, aicc);
    }
  }
  else
  {
    printf("reading par.txt\n");
    nc = nc_best = nc_lim_low;
    sprintf(fname_mcmc, "data/mcmc_%02d.txt", nc);
    mcmc_conline_init();
    read_input();
    memcpy(theta_best_this, theta_best, ntheta * sizeof(double));
    memcpy(theta_best_var_this, theta_best_var, ntheta * 2 * sizeof(double));
  }
  printf("*******finish mcmc\n");

  fprintf(fp_results, "************Best Estimate************\n");
  fprintf(fp_results, "best nc: %d\n", nc_best);
  printf("best nc: %d\n", nc_best);

  nc = nc_best;
  memcpy(theta_best, theta_best_this, ntheta * sizeof(double));
  memcpy(theta_best_var, theta_best_var_this, ntheta * 2 * sizeof(double));
  mcmc_conline_init(); // reset the grid of tau

  reconstruct_conline();
  transfer_function(theta_best);
  aicc = cal_aicc();
  chi2 = chi_square();
  line_convolution();
  fprintf(fp_results, "aicc: %f\n", aicc);
  fprintf(fp_results, "chi2: %f\n", chi2);

  free(theta_best_this);
  free(theta_best_var_this);
}

void mcmc_conline_init()
{
  int i, j;

#ifdef JAVELIN  // only one-tophat is used in JAVELIN
  ntheta = 2 + 3 + 1;

  theta_range[0][0] = log(1.0e-6);  // sigma in DRW
  theta_range[0][1] = log(10.0);    //

  theta_range[1][0] = log(1.0e-2);  // tau in DRW
  theta_range[1][1] = log(1.0e5);

  theta_range[2][0] = (tau_lim_up - tau_lim_low)/1000.0;  // width of tophat
  theta_range[2][1] = (tau_lim_up - tau_lim_low)*10.0;

  theta_range[3][0] = 0.0;       // height of tophat
  theta_range[3][1] = 1.0e3;

  theta_range[4][0] = tau_lim_low;  //central value of tophat
  theta_range[4][1] = tau_lim_up;

  theta_range[5][0] = log(1.0e-5);  // systematic error
  theta_range[5][1] = log(1.0e6);

  i = 0;
  sigma_input[i++] = 0.01; // sigma
  sigma_input[i++] = 0.01; // taud
  sigma_input[i++] = 0.01; // width
  sigma_input[i++] = 0.1;  // fk
  sigma_input[i++] = 0.1;  // tauc
  sigma_input[i++] = 0.1;

  theta_input[0] = theta_best[0];
  theta_input[1] = theta_best[1];
  theta_input[2] = (tau_lim_up-tau_lim_low)/5.0;
  theta_input[3] = 1.0;
  theta_input[4] = (tau_lim_up-tau_lim_low)/2.0;
  theta_input[5] = log(10.0);

#else

  ntheta = nc + 3 + 1;
// grid of time lag
  for(i=0; i<nc; i++)
  {
    grid_tau[i] = (tau_lim_up-tau_lim_low)/(nc-1.0) * i + tau_lim_low;
  }

// limit range for parameters
  i=0;
  theta_range[i][0] = log(1.0e-6);
  theta_range[i++][1] = log(10.0);

  theta_range[i][0] = log(1.0);
  theta_range[i++][1] = log(1.0e4);

  theta_range[i][0] = (tau_lim_up - tau_lim_low)/(nc-1.0)/2.0;
  theta_range[i++][1] = (tau_lim_up - tau_lim_low)/(nc-1.0);
  for(j=0; j<nc; j++)
  {
    theta_range[i][0] = log(1.0e-10);
    theta_range[i++][1] = log(100.0);
  }

  theta_range[i][0] = log(1.0e-5);
  theta_range[i++][1] = log(1.0e6);

/* input step sizes for mcmc sampling */
  i = 0;
  sigma_input[i++] = 0.01; // sigma
  sigma_input[i++] = 0.01; // taud
  sigma_input[i++] = 0.01; // width
  for(j=0; j<nc; j++)
  {
    sigma_input[i++] = 0.1;  // fk
  }
  sigma_input[i++] = 0.1;    // systematic error

/* input values for parameters */
  theta_input[0] = theta_best[0];
  theta_input[1] = theta_best[1];
  theta_input[2] = (tau_lim_up-tau_lim_low)/(nc-1.0)/2.0;
  for(i=0; i<nc; i++)
  {
    theta_input[3+i] = log(1.0/nc);
  }
  theta_input[3 + nc] = log(10.0);

#endif

/* set if the parameters are fixed */
  for(i=0; i<ntheta; i++)
  {
    theta_fixed[i] = 0;
  }

/* for TOPHAT case, the width is fixed */
#ifdef TOPHAT
  theta_fixed[2] = 1;
#endif

}

/* theta represets sigma, tuad, and the parameters for transfer function */
double probability_conline(double *theta)
{
  double prob, prior, lndet_C, lndet_ICq, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info, sign_C, sign_Cq;

  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);

  nq = (flag_detrend + 1)*2;
  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

// cal q
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(Tmat2, Larr, nall_data*nq*sizeof(double));
  //printf("FFFF\n");
  //display_mat(Tmat1, 1, nall_data);
  multiply_mat_MN_inverseA(Tmat1, Tmat2, nall_data, nq); // Tmat2 = C^-1 * L

  multiply_mat_MN_transposeA(Larr, Tmat2, ICq, nq, nq, nall_data); // ICq = L^T*C^-1*L
  multiply_mat_MN_transposeA(Tmat2, Fall_data, ave, nq, 1, nall_data); // ave = L^T*C^-1*y
  memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ave, nq, 1); // (L^T*C^-1*L)^-1 * L^T*C^-1*y

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(ybuf, ysub, nall_data*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ybuf, nall_data, 1);

  prob = -0.5*cblas_ddot(nall_data, ysub, 1, ybuf, 1) / (sigma*sigma);
  //printf("%f\n", prob);
  if(prob > 0.0 )  // check if prob is positive
  {
    prob = -1.0e10;
    printf("prob >0!\n");
    return prob;
  }

  //memcpy(Tmat1, Cmat, n_data*nall_data*sizeof(double));
  lndet_C = lndet_mat3(Cmat, nall_data, &info, &sign_C) + 2.0*nall_data * log(sigma);
  if(info!=0|| sign_C==-1)
  {
    prob = -1.0e10;
    printf("lndet_C %f %d!\n", lndet_C, sign_C);
    return prob;
  }

  //memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  lndet_ICq = lndet_mat3(ICq, nq, &info, &sign_Cq) - 2.0*nq*log(sigma);
  if(info!=0 || sign_Cq==-1 )
  {
    prob = -1.0e10;
    printf("lndet_ICq!\n");
    return prob;
  }

  prob -= 0.5*(lndet_C + lndet_ICq);

  prior = 0.0;
// for sigmad
  if( theta[0] < theta_best_con[0])
  {
    prior += -0.5*pow(theta[0] - theta_best_con[0], 2.0)/pow(theta_best_var_con[0*2], 2.0);
  }
  else
  {
    prior += -0.5*pow(theta[0] - theta_best_con[0], 2.0)/pow(theta_best_var_con[0*2+1], 2.0);
  }
// for taud
  if( theta[1] < theta_best_con[1])
  {
    prior += -0.5*pow(theta[1] - theta_best_con[1], 2.0)/pow(theta_best_var_con[1*2], 2.0);
  }
  else
  {
    prior += -0.5*pow(theta[1] - theta_best_con[1], 2.0)/pow(theta_best_var_con[1*2+1], 2.0);
  }
//penalize very large tau or very small tau
/*  if(theta[1] > log(len_con) )
  {
    prior += (log(len_con) - theta[1])/fabs(log(cad_con));
  }
  if(theta[1] < log(cad_con))
  {
    prior += (theta[1] - log(cad_con) ) / fabs(log(cad_con));
  }*/

  prob += prior;
  return prob;
}

double cal_aicc()
{
  double prob, aic, aicc;
  int k, n;

  k =  nc + 4;
  n = nall_data;

  prob = probability_conline_aicc(theta_best);

  aic = 2.0*k - 2.0*prob;

  aicc = aic + 2.0*k*(k+1.0)/(n - k - 1.0);

  //printf("aicc: %d %f %f\n", nc, aicc, prob);
  return aicc;
}

double probability_conline_aicc(double *theta)
{
  double prob,  lndet_C, lndet_ICq, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info, sign_C, sign_Cq;

  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);

  nq = (flag_detrend + 1)*2;
  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

// cal q
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(Tmat2, Larr, nall_data*nq*sizeof(double));
  //printf("FFFF\n");
  //display_mat(Tmat1, 1, nall_data);
  multiply_mat_MN_inverseA(Tmat1, Tmat2, nall_data, nq); // Tmat2 = C^-1 * L

  multiply_mat_MN_transposeA(Larr, Tmat2, ICq, nq, nq, nall_data); // ICq = L^T*C^-1*L
  multiply_mat_MN_transposeA(Tmat2, Fall_data, ave, nq, 1, nall_data); // ave = L^T*C^-1*y
  memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ave, nq, 1); // (L^T*C^-1*L)^-1 * L^T*C^-1*y

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  memcpy(Tmat1, Cmat, nall_data*nall_data*sizeof(double));
  memcpy(ybuf, ysub, nall_data*sizeof(double));
  multiply_mat_MN_inverseA(Tmat1, ybuf, nall_data, 1);

  prob = -0.5*cblas_ddot(nall_data, ysub, 1, ybuf, 1) / (sigma*sigma);
  //printf("%f\n", prob);
  if(prob > 0.0 )  // check if prob is positive
  {
    prob = -1.0e10;
    printf("prob >0!\n");
    return prob;
  }

  //memcpy(Tmat1, Cmat, n_data*nall_data*sizeof(double));
  lndet_C = lndet_mat3(Cmat, nall_data, &info, &sign_C) + 2.0*nall_data * log(sigma);
  if(info!=0|| sign_C==-1)
  {
    prob = -1.0e10;
    printf("lndet_C %f %d!\n", lndet_C, sign_C);
    return prob;
  }

  //memcpy(Tmat1, ICq, nq*nq*sizeof(double));
  lndet_ICq = lndet_mat3(ICq, nq, &info, &sign_Cq) - 2.0*nq*log(sigma);
  if(info!=0 || sign_Cq==-1 )
  {
    prob = -1.0e10;
    printf("lndet_ICq!\n");
    return prob;
  }

  prob -= 0.5*(lndet_C + lndet_ICq);

  return prob;
}

double chi_square()
{
  double chi2, sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *ysub, *yrec, *yrec_err, *yave;
  int i, nq, info;

  taud = exp(theta_best[1]);
  sigma = exp(theta_best[0]) * sqrt(taud/2.0);

  nq = 2*(flag_detrend + 1);

  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  ysub = ave + nq;
  yrec = ysub + nall_data;
  yrec_err = yrec + nall_data;
  yave = yrec_err + nall_data;

  set_covar_mat(theta_best);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

  memcpy(ICmat, Cmat, nall_data*nall_data*sizeof(double));
  inverse_mat(ICmat, nall_data, &info);

  multiply_mat_MN(ICmat, Larr, Tmat1, nall_data, nq, nall_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, nall_data);

  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, nall_data, nall_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, nall_data, nq);
  multiply_mat_MN(Tmat2, Fall_data, ave, nq, 1, nall_data);

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  multiply_matvec(ICmat, ysub, nall_data, ybuf);

  multiply_matvec_MN(Smat, nall_data, nall_data, ybuf, yrec);
  for(i=0; i<nall_data; i++)yrec[i] += yave[i];

  multiply_mat_MN(Smat, ICmat, Tmat1, nall_data, nall_data, nall_data);
  multiply_mat_MN_transposeB(Tmat1, Smat, Tmat2, nall_data, nall_data, nall_data);

  multiply_mat_MN(Tmat1, Larr, Tmat3, nall_data, nq, nall_data);
  for(i=0; i<nall_data*nq; i++)Tmat3[i] -= Larr[i];
  multiply_mat_MN(Tmat3, Cq, Tmat1, nall_data, nq, nq);
  multiply_mat_MN_transposeB(Tmat1, Tmat3, Tmat4, nall_data, nall_data, nq);

  for(i=0; i<nall_data; i++)
  {
    yrec_err[i] = sigma*sqrt((Smat[i*nall_data+i] - Tmat2[i*nall_data+i] + Tmat4[i*nall_data+i]));
  }

  FILE *fp;
  fp = fopen("data/tmp.txt", "w");
  for(i=0; i<ncon_data; i++)
  {
    fprintf(fp, "%f %f %f\n", Tcon_data[i], yrec[i]*scale_con, yrec_err[i]*scale_con);
  }
  fprintf(fp, "\n");
  for(i=ncon_data; i<nall_data; i++)
  {
    fprintf(fp, "%f %f %f\n", Tline_data[i-ncon_data], yrec[i]*scale_line, yrec_err[i]*scale_line);
  }
  fclose(fp);

  chi2 = 0.0;
  for(i=0; i<ncon_data; i++)chi2 += pow(Fcon_data[i] - yrec[i], 2)
                                /( pow(Fcerrs_data[i], 2) );
  for(i=ncon_data; i<nall_data; i++) chi2 += pow(Fline_data[i-ncon_data] - yrec[i], 2)
                                /( pow(Flerrs_data[i-ncon_data], 2) );
  printf("chi2: %f\n", chi2/(nall_data-ntheta));
  return chi2/(nall_data - ntheta);
}

void set_covar_mat(double *theta)
{
  double t1, t2, nerr, syserr;
  double taud, sigma;
  int i, j;

  //alpha = 1.0;
  taud = exp(theta[1]);
  sigma = exp(theta[0]) * sqrt(taud/2.0);
  syserr = exp(theta[ntheta-1]);

// first con-con
  for(i=0; i<ncon_data; i++)
  {
    t1 = Tcon_data[i];
    for(j=0; j<i; j++)
    {
      t2 = Tcon_data[j];
      Smat[i*nall_data+j] = Smat[j*nall_data+i] = exp( - fabs(t2-t1)/taud );
      Nmat[i*nall_data+j] = Nmat[j*nall_data+i] = 0.0;
    }
    nerr = Fcerrs_data[i];
    Nmat[i*nall_data+i] = (nerr*nerr + syserr*syserr)/(sigma*sigma);
    Smat[i*nall_data+i] = 1.0;
  }

// then con-line and line-con
  for(i=0; i<ncon_data; i++)
  {
    t1 = Tcon_data[i];
    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      Smat[ i*nall_data + (ncon_data+j)] = Smat[(j+ncon_data)*nall_data + i] =  Slc(t1, t2, theta);
      Nmat[ i*nall_data + (ncon_data+j)] = Nmat[(j+ncon_data)*nall_data + i] = 0.0;
    }
  }

/*// then line-con
  for(i=0; i<nline_data; i++)
  {
    t1 = Tline_data[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      Smat[ (i+ncon_data)*n_data + j] = Slc(t2, t1, theta);
      Nmat[ (i+ncon_data)*n_data + j] = 0.0;
    }
  }  */

// then line-line
  for(i=0; i<nline_data; i++)
  {
    t1 = Tline_data[i];
    for(j=0; j<i; j++)
    {
      t2 = Tline_data[j];
      Smat[ (i+ncon_data)*nall_data + (ncon_data+j)] = Smat[ (j+ncon_data)*nall_data + (ncon_data+i)] = Sll(t1, t2, theta);
      Nmat[ (i+ncon_data)*nall_data + (ncon_data+j)] = Nmat[ (j+ncon_data)*nall_data + (ncon_data+i)] = 0.0;
    }
    nerr = Flerrs_data[i];
    Nmat[ (i+ncon_data)*nall_data + (ncon_data+i) ] = (nerr * nerr + syserr*syserr)/(sigma*sigma);
    Smat[ (i+ncon_data)*nall_data + (ncon_data+i) ] = Sll(t1, t1, theta);
  }

}

void set_covar_Umat(double *theta)
{
  double t1, t2, taud;
  int i, j;

  taud = exp(theta[1]);
  //sigma = exp(theta[0]) * sqrt(taud/2.0);

  for(i=0; i<ncon; i++)
  {
    t1 = Tcon[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      USmat[i*nall_data+j] = exp (- fabs(t1-t2) / taud );
    }

    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      USmat[i*nall_data + (ncon_data+j)] = Slc(t1, t2, theta);
    }
  }

  for(i=0; i<nline; i++)
  {
    t1 = Tline[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      USmat[ (i+ncon)*nall_data + j] = Slc(t2, t1, theta);
    }

    for(j=0; j<nline_data; j++)
    {
      t2 = Tline_data[j];
      USmat[ (i+ncon)*nall_data + (ncon_data+j)] = Sll(t1, t2, theta);
    }
  }
  return;
}

void set_covar_Amat(double *theta)
{
  double t1, t2, taud;
  int i, j;

  taud = exp(theta[1]);
  //sigma = exp(theta[0]) * sqrt(taud/2.0);

  for(i=0; i<ncon; i++)
  {
    t1 = Tcon[i];
    for(j=0; j<ncon; j++)
    {
      t2 = Tcon[j];
      ASmat[i*nall+j] = exp (- fabs(t1-t2) / taud );
    }

    for(j=0; j<nline; j++)
    {
      t2 = Tline[j];
      ASmat[i*nall + (ncon+j)] = Slc(t1, t2, theta);
    }
  }

  for(i=0; i<nline; i++)
  {
    t1 = Tline[i];
    for(j=0; j<ncon; j++)
    {
      t2 = Tcon[j];
      ASmat[ (i+ncon)*nall + j] = Slc(t2, t1, theta);
    }

    for(j=0; j<nline; j++)
    {
      t2 = Tline[j];
      ASmat[ (i+ncon)*nall + (ncon+j)] = Sll(t1, t2, theta);
    }
  }
  return;
}

double Slc(double tcon, double tline, double *theta)
{
  double Dt, DT, w, tauk, fk, Sk, Stot, taud;
  int i;

  Dt = tline - tcon;
  taud = exp(theta[1]);
  w = theta[2];

  Stot = 0.0;
#ifdef TOPHAT
  for(i=0; i<nc; i++)
  {
    tauk = grid_tau[i]; //theta[3+nc+i];
    fk = exp(theta[3+i]);
    DT = Dt - tauk;
    if(DT < -w)
    {
      Sk = exp( (DT + w) / taud ) - exp( (DT - w)/taud);
    }
    else if(DT < w)
    {
      Sk = 2.0 - exp(- (DT + w) / taud) - exp( (DT - w)/taud);
    }
    else
    {
      Sk = exp( - (DT - w)/taud) - exp ( - (DT + w)/taud);
    }
    Stot += fk * Sk;
  }
  Stot *= (taud/2.0/w);
#elif defined JAVELIN
  tauk = theta[4]; //theta[3+nc+i];
  fk = theta[3];
  DT = Dt - tauk;
  if(DT < -w)
  {
    Sk = exp( (DT + w) / taud ) - exp( (DT - w)/taud);
  }
  else if(DT < w)
  {
    Sk = 2.0 - exp(- (DT + w) / taud) - exp( (DT - w)/taud);
  }
  else
  {
    Sk = exp( - (DT - w)/taud) - exp ( - (DT + w)/taud);
  }
  Stot = fk * Sk;
  Stot *= (taud/2.0/w);
#else
  for(i=0; i<nc; i++)
  {
    tauk = grid_tau[i]; //theta[3+nc+i];
    fk = exp(theta[3+i]);
    DT = Dt - tauk;
    Sk = exp(-DT/taud) * erfc( -(DT/w - w/taud)/sqrt(2.0) )
        +exp( DT/taud) * erfc(  (DT/w + w/taud)/sqrt(2.0) );

    Stot += Sk * fk;
  }
  Stot *= 1.0/2.0 * exp(w*w/2.0/taud/taud);
#endif
  return Stot;
}

double Sll(double ti, double tj, double *theta)
{
  double Dt, DT, w, tauk, fk, taum, fm, Skm, Stot, taud;
  int k, m;

  Dt = ti - tj;
  taud = exp(theta[1]);
  w = theta[2];

  Stot = 0.0;
#ifdef TOPHAT
  for(k=0; k<nc; k++)
  {
    tauk = grid_tau[k]; //theta[3+nc+k];
    fk = exp(theta[3+k]);
    for(m=0; m<nc; m++)
    {
      taum = grid_tau[m]; //theta[3+nc+m];
      fm = exp(theta[3+m]);

      DT = Dt - (tauk-taum);

      if(DT < -2.0*w)
      {
        Skm = exp((DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud) - 2.0*exp(DT/taud);
      }
      else if(DT < 0.0)
      {
        Skm = exp(-(DT + 2.0*w)/taud)  + exp((DT - 2.0*w)/taud)  - 2.0*exp(DT/taud) + 2.0*(DT+2.0*w)/taud;
      }
      else if(DT < 2.0*w)
      {
        Skm = exp(-(DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud)  - 2.0*exp(-DT/taud) - 2.0*(DT-2.0*w)/taud;
      }
      else
      {
        Skm = exp( -(DT + 2.0*w)/taud) + exp(-(DT - 2.0*w)/taud) - 2.0*exp(-DT/taud);
      }
      Stot += fk*fm*Skm;
    }
  }
  Stot *= (taud*taud/4.0/w/w);

#elif defined JAVELIN
  for(k=0; k<1; k++)
  {
    tauk = theta[3+k+1]; //theta[3+nc+k];
    fk = theta[3+k];
    for(m=0; m<1; m++)
    {
      taum = theta[3+m+1]; //theta[3+nc+m];
      fm = theta[3+m];

      DT = Dt - (tauk-taum);

      if(DT < -2.0*w)
      {
        Skm = exp((DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud) - 2.0*exp(DT/taud);
      }
      else if(DT < 0.0)
      {
        Skm = exp(-(DT + 2.0*w)/taud)  + exp((DT - 2.0*w)/taud)  - 2.0*exp(DT/taud) + 2.0*(DT+2.0*w)/taud;
      }
      else if(DT < 2.0*w)
      {
        Skm = exp(-(DT + 2.0*w)/taud) + exp((DT - 2.0*w)/taud)  - 2.0*exp(-DT/taud) - 2.0*(DT-2.0*w)/taud;
      }
      else
      {
        Skm = exp( -(DT + 2.0*w)/taud) + exp(-(DT - 2.0*w)/taud) - 2.0*exp(-DT/taud);
      }
      Stot += fk*fm*Skm;
    }
  }
  Stot *= (taud*taud/4.0/w/w);
#else
  for(k=0; k<nc; k++)
  {
    tauk = grid_tau[k]; //theta[3+nc+k];
    fk = exp(theta[3+k]);
    for(m=0; m<nc; m++)
    {
      taum = grid_tau[m]; //theta[3+nc+m];
      fm = exp(theta[3+m]);
      DT = Dt - (tauk-taum);
      Skm = ( exp(-DT/taud) * erfc( -DT/2.0/w + w/taud )
            +  exp( DT/taud) * erfc(  DT/2.0/w + w/taud ) );
      Stot += Skm * fm*fk;
    }
  }
  Stot *= 1.0/2.0 * exp( w*w/taud/taud);
#endif
  return Stot;
}

int reconstruct_conline()
{
  FILE *frec;
  double *Larr, *Larr_rec, *ave, *yave, *yave_rec, *ysub, *ybuf, *yrec, *yrec_err, *Cq, *ICq;
  double *Tmp1, *Tmp2, *Tmp3, *Tmp4;
  double sigma, taud;
  int i, nq, info;

  nq = 2*(flag_detrend + 1);

  Larr = array_malloc(nall_data * nq);
  Larr_rec = array_malloc(nall * nq);
  ybuf = array_malloc(nall_data);
  ysub = array_malloc(nall_data);
  yave = array_malloc(nall_data);
  yave_rec = array_malloc(nall);
  yrec = array_malloc(nall);
  yrec_err = array_malloc(nall);
  ave = array_malloc(nq);
  Cq = array_malloc(nq*nq);
  ICq = array_malloc(nq*nq);

  Tmp1 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp2 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp3 = array_malloc(max(nall,nall_data)*max(nall,nall_data));
  Tmp4 = array_malloc(max(nall,nall_data)*max(nall,nall_data));

  taud = exp(theta_best[1]);
  sigma = exp(theta_best[0]) * sqrt(taud/2.0);
  set_covar_mat(theta_best);
  set_covar_Umat(theta_best);
  set_covar_Amat(theta_best);

  for(i=0;i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }
  memcpy(ICmat, Cmat, nall_data*nall_data*sizeof(double));
  inverse_mat(ICmat, nall_data, &info);

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

  for(i=0; i<ncon; i++)
  {
    if(flag_detrend==0)
    {
      Larr_rec[i*nq+0] = 1.0;
      Larr_rec[i*nq+1] = 0.0;
    }
    else
    {
      Larr_rec[i*nq + 0] = 1.0;
      Larr_rec[i*nq + 1] = Tcon[i];
      Larr_rec[i*nq + 2] = 0.0;
      Larr_rec[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline; i++)
  {
    if(flag_detrend==0)
    {
      Larr_rec[(i+ncon)*nq+0] = 0.0;
      Larr_rec[(i+ncon)*nq+1] = 1.0;
    }
    else
    {
      Larr_rec[(i+ncon)*nq + 0] = 0.0;
      Larr_rec[(i+ncon)*nq + 1] = 0.0;
      Larr_rec[(i+ncon)*nq + 2] = 1.0;
      Larr_rec[(i+ncon)*nq + 3] = Tline[i];
    }
  }

  multiply_mat_MN(ICmat, Larr, Tmat1, nall_data, nq, nall_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, nall_data);
  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, nall_data, nall_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, nall_data, nq);
  multiply_mat_MN(Tmat2, Fall_data, ave, nq, 1, nall_data);

  if(flag_detrend==0)
  {
    printf("ave: %e %e\n", ave[0]*scale_con, ave[1]*scale_line);
    fprintf(fp_results, "ave: %e %e\n", ave[0]*scale_con, ave[1]*scale_line);
  }
  else
  {
    printf("ave_line: %e %e\n", ave[2]*scale_line, ave[3]*scale_line);
    printf("ave_con: %e %e\n", ave[0]*scale_con, ave[1]*scale_con);
    fprintf(fp_results, "ave_con: %e %e\n", ave[0]*scale_con, ave[1]*scale_con);
    fprintf(fp_results, "ave_line: %e %e\n", ave[2]*scale_line, ave[3]*scale_line);
  }

  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  for(i=0; i<nall_data; i++)ysub[i] = Fall_data[i] - yave[i];
  multiply_matvec(ICmat, ysub, nall_data, ybuf);

  multiply_matvec_MN(USmat, nall, nall_data, ybuf, yrec);
  multiply_mat_MN(Larr_rec, ave, yave_rec, nall, 1, nq);

/* store the reconstructed, mean-substracted contionum */
  memcpy(Fcon, yrec, ncon*sizeof(double));

  for(i=0; i<nall; i++)yrec[i] += yave_rec[i];

  // get errors

  multiply_mat_MN(USmat, ICmat, Tmp1, nall, nall_data, nall_data);
  multiply_mat_MN_transposeB(Tmp1, USmat, Tmp2, nall, nall, nall_data);

  multiply_mat_MN(Tmp1, Larr, Tmp3, nall, nq, nall_data);
  for(i=0; i<nall*nq; i++)Tmp3[i] -= Larr_rec[i];
  multiply_mat_MN(Tmp3, Cq, Tmp1, nall, nq, nq);
  multiply_mat_MN_transposeB(Tmp1, Tmp3, Tmp4, nall, nall, nq);

  for(i=0; i<nall; i++)
  {
    yrec_err[i] = sigma * sqrt(ASmat[i*nall+i] - Tmp2[i*nall+i] + Tmp4[i*nall+i]);
  }

/* store the error */
  for(i=0; i<ncon; i++)
  {
    Fcerrs[i] = sigma * sqrt(ASmat[i*nall+i] - Tmp2[i*nall+i]);
  }

  frec = fopen("data/sall_con.txt", "w");
  for(i=0; i<ncon; i++)
  {
    fprintf(frec, "%f %e %e\n", Tcon[i], yrec[i]*scale_con, yrec_err[i]*scale_con);
  }
  fclose(frec);
  frec = fopen("data/sall_line.txt", "w");
  for(i=ncon; i<nall; i++)
  {
    fprintf(frec, "%f %e %e\n", Tline[i-ncon], yrec[i]*scale_line, yrec_err[i]*scale_line);
  }
  fclose(frec);
  free(Tmp1);
  free(Tmp2);
  free(Tmp3);
  free(Tmp4);
  return 0;
}

ext: c

max_stars_repo_name: LiyrAstroph/MICA

max_issues_repo_name: LiyrAstroph/MICA

max_forks_repo_name: LiyrAstroph/MICA


text:
//
// Created by jonathan on 1/5/18.
//

#ifndef CIMPLE_CIMPLE_MPC_COMPUTATION_H
#define CIMPLE_CIMPLE_MPC_COMPUTATION_H

#include <stddef.h>
#include <math.h>
#include "cimple_system.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include "cimple_polytope_library.h"

/**
 * @brief Set up weight matrices for the quadratic problem
 * @param P
 * @param q
 * @param L
 * @param M
 * @param now
 * @param s_dyn
 * @param f_cost
 * @param time_horizon
 * @return
 */
polytope *set_cost_function(gsl_matrix *P,
                            gsl_vector *q,
                            gsl_matrix *L,
                            gsl_vector *M,
                            current_state *now,
                            system_dynamics *s_dyn,
                            cost_function *f_cost,
                            size_t time_horizon);

/**
 * @brief Set up GUROBI environment and solve qp
 * @param low_u
 * @param low_cost
 * @param P
 * @param q
 * @param opt_constraints
 * @param time_horizon
 * @param n
 */
void compute_optimal_control_qp(gsl_matrix *low_u,
                                double *low_cost,
                                gsl_matrix *P,
                                gsl_vector* q,
                                polytope *opt_constraints,
                                size_t time_horizon,
                                size_t n);

/**
 * @brief Calculate (optimal) input that will be applied to take plant from current state (now) to target_abs_state.
 *
 * Global function to compute continuous control input for discrete transition.
 *
 * Computes continuous control input sequence which takes the plant:
 *
 *      - from now
 *      - to (chebyshev center of) target_abs_state
 *      => partitions given by discrete dynamics (d_dyn)
 *
 * Control input is calculated such that it minimizes:
 *
 *      f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x + distance_error_weight * |xc - x(N)|_{ord}
 *      with xc == chebyshev center of target_abs_state
 *
 *    Notes
 *    =====
 *    1. The same horizon length as in reachability analysis
 *    should be used in order to guarantee feasibility.
 *
 *    2. If the closed loop algorithm has been used
 *    to compute reachability the input needs to be
 *    recalculated for each time step
 *    (with decreasing horizon length).
 *
 *    In this case only u(0) should be used as
 *    a control signal and u(1) ... u(N-1) discarded.
 *
 *    3. The "conservative" calculation makes sure that
 *    the plant remains inside the convex hull of the
 *    starting region during execution, i.e.::
 *
 *    x(1), x(2) ...  x(N-1) are in starting region.
 *
 *    If the original proposition preserving partition
 *    is not convex, then safety cannot be guaranteed.
 *
 * @param low_u row k contains the control input: u(k) dim[N x m]
 * @param now initial continuous state
 * @param d_dyn discrete abstraction of system
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param target_abs_state index of target region in discrete dynamics (d_dyn)
 * @param f_cost cost func matrices: f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x + distance_error_weight *|xc - x(N)|_{ord}
 */
void get_input (gsl_matrix *u,
                current_state * now,
                discrete_dynamics *d_dyn,
                system_dynamics *s_dyn,
                int target_abs_state,
                cost_function * f_cost,
                size_t current_time_horizon,
                polytope **polytope_list_backup);

/**
 * @brief Calculates (optimal) input to reach desired state (P3) from current state (now) through convex optimization
 *
 * Calculate the sequence low_u such that:
 *
 *      - x(t+1) = A x(t) + B u(t) + K
 *      - x(k) in P1 for k = 0,...,N-1 (if closed loop == 'true')
 *      - x(N) in P3
 *      - [u(k); x(k)] always obey s_dyn.Uset
 *
 * Actual optimality is compared in get_input()
 *
 *
 * @param low_u currently optimal calculated input to target region (input to beat)
 * @param now current state
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param P1 current polytope (or hull of region) the system is in
 * @param P3 a polytope from the target region
 * @param ord ordinance of the norm that should be minimized ord in {1, 2, INFINITY} (currently only '2' is possible)
 * @param time_horizon
 * @param f_cost predefined cost functions |Rx|_{ord} + |Qu|_{ord} + r'x + mid_weight * |xc - x(N)|_{ord}
 * @param low_cost cost associate to low_u
 */
void search_better_path(gsl_matrix *low_u,
                        current_state *now,
                        system_dynamics *s_dyn,
                        polytope *P1,
                        polytope *P3,
                        int ord,
                        size_t time_horizon,
                        cost_function * f_cost,
                        double* low_cost,
                        polytope **polytope_list_backup,
                        size_t total_time);

/**
 * @brief Compute a polytope that constraints the system over the next N time steps to fullfill the GR(1) specifications
 *
 * @param L_full empty matrix when passed in, left side of constraint polytope at the end
 * @param M_full empty vector when passed in, right side of constraint polytope at the end
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param list_polytopes list of N+1 polytopes in which the systems needs to be in to reach new desired state at time N
 * @param N time horizon
 *
 * Compute the components of the polytope:
 *
 *      L [x(0)' u(0)' ... u(N-1)']' <= M
 *
 * which stacks the following constraints:
 *
 *      - x(t+1) = A x(t) + B u(t) + E d(t)
 *      - [u(k); x(k)] in s_dyn.Uset for all k => system obeys predefined constraints on how inputs may behave
 *
 * [L_full; M_full] polytope intersection of required and allowed polytopes
 *
 */
polytope * set_path_constraints(current_state * now,
                                system_dynamics * s_dyn,
                                polytope **list_polytopes,
                                size_t N);

#endif //CIMPLE_CIMPLE_MPC_COMPUTATION_H

ext: h

max_stars_repo_name: shaesaert/TuLiPXML

max_issues_repo_name: shaesaert/TuLiPXML

max_forks_repo_name: shaesaert/TuLiPXML


text:
#include "poisson.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

static gsl_rng* poisson_rng_g = NULL;

void poisson_init(void)
{
	const gsl_rng_type * T;

	/* create a generator chosen by the
	environment variable GSL_RNG_TYPE */

	gsl_rng_env_setup();

	T = gsl_rng_default;
	poisson_rng_g = gsl_rng_alloc (T);
}

void poisson_fini(void)
{
	gsl_rng_free (poisson_rng_g);
}

unsigned int poisson(double mu)
{
	return gsl_ran_poisson (poisson_rng_g, mu);
}


text:
//This does CELL (~soma) stage of peephole LSTM (long short-term memory) model.
//This requires each neuron to have 4 input time series, Xc, Xi, Xf, Xo,
//where Xc is the usual (or "cellular") input and Xi, Xf, Xo the inputs for the input, forget, output gates.
//Xc, Xi, Xf, Xo are the output of separate linear IN stages (weights and baises).
//In this version, these are stacked into one matrix
//For dim=0: X = [Xc; Xi; Xf; Xo]; for dim=1: X = [Xc Xi Xf Xo].

//For dim=0, I[:,t] = sig{Xi[:,t] + Ui*C[:,t-1]}
//           F[:,t] = sig{Xf[:,t] + Uf*C[:,t-1]}
//           O[:,t] = sig{Xo[:,t] + Uo*C[:,t-1]}
//           C[:,t] = F[:,t].*C[:,t-1] + I[:,t].*sig{Xc[t,:]}
//           Y[:,t] = tanh{O[:,t].*C[:,t]}
//with sizes Xc, Xi, Xf, Xo: N x T
//               Ui, Uf, Uo: N x N
//                        Y: N x T
//
//For dim=1, I[t,:] = sig{Xi[t,:] + C[t-1,:]*Ui}
//           F[t,:] = sig{Xf[t,:] + C[t-1,:]*Uf}
//           O[t,:] = sig{Xo[t,:] + C[t-1,:]*Uo}
//           C[t,:] = F[t,:].*C[t-1,:] + I[t,:].*sig{Xc[t,:]}
//           Y[t,:] = tanh{O[t,:].*C[t,:]}
//with sizes Xc, Xi, Xf, Xo: T x N
//               Ui, Uf, Uo: N x N
//                        Y: T x N
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//I is the input gate, F is the forget gate, O is the output gate,
//C is the "cell input activation vector",
//H is an intermediate (hidden) vector (sometimes called the "cell state vector"),
//Uc, Ui, Uf, Uo are NxN matrices, and Y is the final output (sometimes called the "hidden state vector").

//The tanh is omitted here!! (To allow trying other nonlinearities.)

//Note that, the neurons of a layer are independent only if Uc, Ui, Uf, Uo are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifdef I
    #undef I
#endif

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int lstm_peephole_s (float *Y, const float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm_peephole_d (double *Y, const double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm_peephole_inplace_s (float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm_peephole_inplace_d (double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm_peephole_s (float *Y, const float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    float *C, *I, *F, *O;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                Y[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                Y[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                Y[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                Y[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_d (double *Y, const double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    double *C, *I, *F, *O;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                Y[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                Y[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                Y[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                Y[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_inplace_s (float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    float *C, *I, *F, *O;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                X[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                X[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                X[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                X[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_inplace_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_inplace_d (double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    double *C, *I, *F, *O;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                X[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                X[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                X[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                X[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_inplace_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


text:
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"
#include "cblas.h"

void
cblas_sswap (const int N, float *X, const int incX, float *Y, const int incY)
{
#define BASE float
#include "source_swap_r.h"
#undef BASE
}


text:
#define _XOPEN_SOURCE 500
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <sys/time.h>
#include <getopt.h>
#include <pthread.h>
#include <cblas.h>
#include <omp.h>

#define WALLTIME(t) ((double)(t).tv_sec + 1e-6 * (double)(t).tv_usec)
#define min(x,y) (((x) < (y)) ? (x) : (y))

void options ( int argc, char **argv );

const char *usage =
"\t-k <int>\n"
"\t-m <int>\n"
"\t-n <int>\n"
"\t-t <int>\n";

//Global variables and definitions
int num_threads = 4;

double
    *A, *B,
    *C_serial, *C_openmp, *C_pthreads, *C_blas,
    alpha = 1.0, beta = 0.0;

int
    m = 1024, n = 1024, k = 1024;

void print_matrices()
{
	size_t i, j;
	printf (" Top left corner of matrix A: \n");
	for (i=0; i<min(m,6); i++) {
		for (j=0; j<min(k,6); j++) {
			printf ("%12.0f", A[j+i*k]);
		}
		printf ("\n");
	}

	printf ("\n Top left corner of matrix B: \n");
	for (i=0; i<min(k,6); i++) {
		for (j=0; j<min(n,6); j++) {
			printf ("%12.0f", B[j+i*n]);
		}
		printf ("\n");
	}

	printf ("\n Top left corner of matrix C: \n");
	for (i=0; i<min(m,6); i++) {
		for (j=0; j<min(n,6); j++) {
			printf ("%12.5G", C_serial[j+i*n]);
		}
		printf ("\n");
	}
}

//TODO: Pthreads - function

//TODO end

int
main ( int argc, char **argv )
{
    options ( argc, argv );
    A = (double *)malloc( m*k*sizeof( double ) );
    B = (double *)malloc( k*n*sizeof( double ) );
    C_serial = (double *)malloc( m*n*sizeof( double ) );
    C_openmp = (double *)malloc( m*n*sizeof( double ) );
    C_pthreads = (double *)malloc( m*n*sizeof( double ) );
    C_blas = (double *)malloc( m*n*sizeof( double ) );

    int i, j;

    /* Initialize with dummy data */
    for ( i = 0; i < (m*k); i++ )
        A[i] = (double)(i+1);
    for ( i = 0; i < (k*n); i++ )
        B[i] = (double)(-i-1);
    for ( i = 0; i < (m*n); i++ ){
        C_serial[i] = 0.0;
        C_openmp [i] = 0.0;
        C_pthreads[i] = 0.0;
        C_blas[i] = 0.0;
    }

    struct timeval start, end;

    double total_time_serial = 0;
    double total_time_openmp = 0;
    double total_time_pthreads = 0;
    double total_time_blas = 0;

    /*
     * DGEMM (Double-precision GEneral Matrix Multiply)
     * Example: A general multiplication between two matricies A and B, accumulating in C.
     */
    // A has m rows and k columns
    // B has k rows and n columns
    // C has m rows and n columns
    double sum = 0;
    int p;

    gettimeofday( &start, NULL );
    for (i = 0; i < m; i++) {
    	for (j = 0; j < n; j++) {
    	    sum = 0;
    	    for (p = 0; p < k; p++) {
                sum = sum + A[i * k + p] * B[p * n + j];
    	    }
    	    C_serial[i * n + j] = sum;
        }
    }
    gettimeofday ( &end, NULL );
    total_time_serial = (WALLTIME(end)-WALLTIME(start));
    //print_matrices();

//TODO: OpenMP
    gettimeofday( &start, NULL );
    for (i = 0; i < m; i++) {
    	for (j = 0; j < n; j++) {
    	    sum = 0;
    	    for (p = 0; p < k; p++) {
                sum = sum + A[i * k + p] * B[p * n + j];
    	    }
    	    C_openmp[i * n + j] = sum;
    	}
    }
    gettimeofday ( &end, NULL );
    total_time_openmp = (WALLTIME(end)-WALLTIME(start));
//TODO end

//TODO: Pthreads - spawn threads
    gettimeofday( &start, NULL );

    gettimeofday ( &end, NULL );
    total_time_pthreads = (WALLTIME(end)-WALLTIME(start));
//TODO end

    gettimeofday( &start, NULL );
    // Documentation for GEMM:
    // https://software.intel.com/content/www/us/en/develop/documentation/onemkl-developer-reference-c/top/blas-and-sparse-blas-routines/blas-routines/blas-level-3-routines/cblas-gemm.html
    cblas_dgemm(
    	CblasRowMajor, 	// Formating of the matrix, RowMajor or ColMajor
    	CblasNoTrans, 	// The form of Matrix A
    	CblasNoTrans, 	// The form of Matrix B
    	m,		// Number of rows in A and in C
    	n, 		// Number of columns in B and C
    	k, 		// Number of columns in A and rows of B
    	alpha, 	  	// Scalar alpha of multiplication with A
    	A, 		// The A matrix
    	k, 		// Leading dimension of A, k if CblasRowMajor and NoTrans
    	B, 		// The B matrix
    	n, 		// Leading dimension of B, n if CblasRowMajor and NoTrans
    	beta,         	// Scalar beta of multiplication with C
    	C_blas, 		// The C matrix, this is also the output matrix
    	n		// Leading dimension of C, n if CblasRowMajor
    );
    gettimeofday ( &end, NULL );
    //print_matrices();
    total_time_blas = (WALLTIME(end)-WALLTIME(start));

    bool openmp_correct = true;
    bool pthreads_correct = true;
    bool blas_correct = true;

    for(i = 0; i < m; i++){
        for(j = 0; j < n; j++){
            if(C_openmp[i*n+j] != C_serial[i*n+j])
                openmp_correct = false;
            if(C_pthreads[i*n+j] != C_serial[i*n+j])
                pthreads_correct = false;
            if(C_blas[i*n+j] != C_serial[i*n+j])
                blas_correct = false;
        }
    }

    printf ( "Manual\t\t\tTime:\t%es\n", total_time_serial );

    printf("OpenMP:\t  %s\t", openmp_correct ? "CORRECT" : "INCORRECT");
    printf ( "Time:\t%es\t", total_time_openmp );
    printf ( "Speedup: %.2lfx\n", total_time_serial/total_time_openmp );

    printf("Pthreads: %s\t", pthreads_correct ? "CORRECT" : "INCORRECT");
    printf ( "Time:\t%es\t", total_time_pthreads );
    printf ( "Speedup: %.2lfx\n", total_time_serial/total_time_pthreads );

    printf("BLAS:\t  %s\t", blas_correct ? "CORRECT" : "INCORRECT");
    printf ( "Time:\t%es\t", total_time_blas );
    printf ( "Speedup: %.2lfx\n", total_time_serial/total_time_blas );

    free ( A );
    free ( B );
    free ( C_serial );
    free ( C_openmp );
    free ( C_pthreads );
    free ( C_blas );

    exit ( EXIT_SUCCESS );
}

void
options ( int argc, char **argv )
{
    int o;
    while ( (o = getopt(argc,argv,"k:m:n:t:h")) != -1 )
    switch ( o )
    {
        case 'k': k = strtol ( optarg, NULL, 10 ); break;
        case 'm': m = strtol ( optarg, NULL, 10 ); break;
        case 'n': n = strtol ( optarg, NULL, 10 ); break;
        case 't': num_threads = strtol ( optarg, NULL, 10 ); break;
        case 'h':
            fprintf ( stderr, "%s", usage );
            exit ( EXIT_FAILURE );
            break;
    }
}

max_stars_repo_name: jorgstei/Datateknologi

max_issues_repo_name: jorgstei/Datateknologi

max_forks_repo_name: jorgstei/Datateknologi


text:
#include <gsl/gsl_matrix.h>
#include "cimple_safe_mode.h"

/**
 * Adds a node to the beginning of the list
 */
void push_burn_node(burn_graph_node ** head,
                    abstract_state *state)
{

    burn_graph_node * new_node;
    new_node = malloc(sizeof(burn_graph_node));

    new_node->state = state;
    new_node->next = *head;
    *head = new_node;
};

/**
 * Remove a node from the beginning of the list
 */
void pop_burn_node(burn_graph_node ** head)
{

    burn_graph_node * next_node = NULL;

    next_node = (*head)->next;
    free(*head);
    *head = next_node;

};

/**
 * Delete and free memory of all nodes of a list
 */
void clear_burn_list(burn_graph_node **head)
{
    while (head !=NULL){
        pop_burn_node(head);
    }
};

/**
 * Given a set of abstract states (associated with the transitions going out from a state, where the function was called)
 * it finds the shortest distance to a state with an invariant set.
 */
abstract_state* fastest_burn(abstract_state **transitions,
                             int transitions_count)
{
    //Initialize
    abstract_state* fastest = transitions[0];
    int distance = transitions[0]->distance_invariant_set;

    //Check that there are more than 1 transition to evaluate, else trivial
    if(transitions_count >1){

        //Compare transitions
        for(int i = 1; i<transitions_count; i++){
            if(transitions[i]->distance_invariant_set<distance){
                distance = transitions[i]->distance_invariant_set;
                fastest = transitions[i];
            }
        }

    }

    return fastest;
}

/**
 * Given two polytopes, computes one step backwards from second polytope towards first polytope.
 */
polytope* previous_polytope(polytope *P1,
                            polytope *P2,
                            system_dynamics *s_dyn)
{

    size_t n = s_dyn->A->size2;  // State space dimension;
    size_t m = s_dyn->B->size2;  // Input space dimension;
    polytope *scaled_W_set = polytope_linear_transform(s_dyn->W_set, s_dyn->E); // multiplication: EW

    polytope * robust_P1 = polytope_pontryagin(P1, scaled_W_set);
    polytope * robust_P2 = polytope_pontryagin(P2, scaled_W_set);

    size_t sum_dim = robust_P1->H->size1+robust_P2->H->size1;

    polytope *precedent_polytope = polytope_alloc(sum_dim+s_dyn->U_set->H->size1, n+m);

    // FOR precedent_polytope G
    /*
     *     |P1_G|
     * G = |P2_G|
     *     |  0 |
     */
    gsl_vector_set_zero(precedent_polytope->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(precedent_polytope->G, 0, robust_P1->G->size);
    gsl_vector_memcpy(&G_P1.vector, robust_P1->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(precedent_polytope->G, robust_P1->G->size,robust_P2->G->size);
    gsl_vector_memcpy(&G_P2.vector, robust_P2->G);
//    gsl_vector * P2_HdotK = gsl_vector_alloc(robust_P2->G->size);
//    gsl_blas_dgemv(CblasNoTrans,1.0, robust_P2->H, s_dyn->K, 0.0, P2_HdotK);
//    gsl_vector_sub(&G_P2.vector, P2_HdotK);
//    //Clean up!
//    gsl_vector_free(P2_HdotK);

    // FOR Dist
    /*
     *         |  0   |
     * Dist =  |P2_H.E|
     *         |  0   |
     */
//    gsl_matrix *Dist = gsl_matrix_alloc(sum_dim+s_dyn->U_set->H->size1, p);
//    gsl_matrix_set_zero(Dist);
//    gsl_matrix_view Dist_P2 = gsl_matrix_submatrix(Dist, P1->H->size1, 0, P2->H->size1, Dist->size2);
//    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, P2->H, s_dyn->E, 0.0, &Dist_P2.matrix);

    // FOR precedent_polytope H
    /*
     *  H = |H1   0 |  OR  H = |H1    0 |
     *      |H2A H2B|          |H2A  H2B|
     *      | 0  HU |          |   HU   |
     */
    gsl_matrix_set_zero(precedent_polytope->H);

    gsl_matrix_view H_P1 = gsl_matrix_submatrix(precedent_polytope->H, 0, 0, robust_P1->H->size1, n);
    gsl_matrix_memcpy(&H_P1.matrix, robust_P1->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, 0, robust_P2->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, n, robust_P2->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->B, 0.0, &H_P2_2.matrix);

    if (s_dyn->U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, n, s_dyn->U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,s_dyn->U_set->H);
    } else if (s_dyn->U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, 0, s_dyn->U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, s_dyn->U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }

    polytope *return_polytope = polytope_projection(precedent_polytope, n);

    //Clean up!
//    gsl_matrix_free(Dist);
//    gsl_vector_free(D_hat);
    polytope_free(precedent_polytope);

    return return_polytope;
};

/**
 * Computes N-1 polytopes the system has to transition to go from origin to target in N time steps
 */
polytope ** compute_path(polytope *origin,
                         polytope *target,
                         system_dynamics *s_dyn,
                         int N)
{

    //Initialize
    polytope **path = malloc(sizeof(polytope) * N);

    //Instead of just pointing path[N] = target, the memory is copied,
    // thus when the target polytope is freed somewhere the path stays complete
    path[N] = polytope_alloc(target->H->size1,target->H->size2);
    gsl_matrix_memcpy(path[N]->H,target->H);

    path[0] = polytope_alloc(origin->H->size1,origin->H->size2);
    gsl_matrix_memcpy(path[0]->H,origin->H);

    //Compute intermediate steps
    for(int j = N-1; j>0;j--){
        path[j]=previous_polytope(path[0], path[j+1], s_dyn);
    }

    return path;
};

polytope *pre_alpha(polytope *R_i,
                    gsl_matrix *A,
                    gsl_matrix *B,
                    polytope *W_set,
                    polytope *U_set,
                    polytope *scaled_unit_cube)
{
    size_t n = A->size2;
    size_t m = B->size2;
    polytope *W_set_scaled = polytope_minkowski(W_set, scaled_unit_cube);
    polytope *R_i_scaled = polytope_pontryagin(R_i, W_set_scaled);

    polytope *P0 = polytope_alloc(R_i_scaled->H->size1+U_set->H->size1, n+m);
    gsl_matrix_set_zero(P0->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(P0->H, 0, 0, R_i_scaled->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(P0->H, 0, n, R_i_scaled->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, B, 0.0, &H_P2_2.matrix);
    if (U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, n, U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,U_set->H);
    } else if (U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, 0, U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }
    gsl_vector_set_zero(P0->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(P0->G, 0, R_i_scaled->G->size);
    gsl_vector_memcpy(&G_P1.vector, R_i_scaled->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(P0->G, R_i_scaled->G->size,U_set->G->size);
    gsl_vector_memcpy(&G_P2.vector, U_set->G);

    polytope *pre_R_1 = polytope_projection(P0, n);
    polytope_free(P0);
    return pre_R_1;
}
polytope * compute_invariant_set(polytope* X,
                                 gsl_matrix *A,
                                 gsl_matrix *B,
                                 polytope *W_set,
                                 polytope *U_set,
                                 double alpha)
{
    polytope * scaled_unit_cube = polytope_scaled_unit_cube(alpha, (int)X->H->size2);

    polytope *R_i = polytope_alloc(X->H->size1,X->H->size2);
    gsl_matrix_memcpy(R_i->H,X->H);
    gsl_vector_memcpy(R_i->G,X->G);
    polytope *pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
    polytope *R_i_plus = polytope_unite_inequalities(pre_R_i, X);
    dd_ErrorType err = dd_NoError;
    dd_WriteMatrix(stdout,dd_CopyGenerators(polytope_to_cdd(R_i_plus,&err)));

    polytope *R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    while(!(polytope_is_subset(R_i, R_scaled))){
        polytope_free(pre_R_i);
        polytope_free(R_scaled);
        polytope_free(R_i);
        R_i = R_i_plus;
        pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
        R_i_plus = polytope_unite_inequalities(pre_R_i, X);
        R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    }
    polytope_free(pre_R_i);
    polytope_free(R_i_plus);
    polytope_free(R_scaled);
    return R_i;

};

bool has_transition(abstract_state *state,
                    abstract_state *transition)
{
    bool transition_found = false;
    for(int i = 0; i<state->transitions_in_count; i++){
        if(state->transitions_in[i] == transition){
            transition_found = true;
        }
    }

    return transition_found;
};
/**
 * 1) Runs through all abstract states,
 * 2) checks if invariant set is contained
 * 3) sets path for all cells in those states containing an invariant set if possible
 */
burn_graph_node *set_invariant_sets(discrete_dynamics *d_dyn,
                                    system_dynamics *s_dyn)
{

    //Initialize graph
    burn_graph_node * head_node = NULL;
    head_node = malloc(sizeof(burn_graph_node));
    head_node->state = NULL;
    head_node->next = NULL;
    int N = (int)d_dyn->time_horizon;
    double unit_cube_scale = 1;
    //Create graph by
    // 1) running through all abstract_states and
    // 2) check whether it contains an invariant set
    for(int i = 0; i< d_dyn->abstract_states_count; i++){
        bool found_set = false;
        for(int j=0; j<d_dyn->abstract_states_set[i]->cells_count; j++){
            //Try to compute invariant set
            polytope *invariant_set = compute_invariant_set(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                            s_dyn->A,
                                                            s_dyn->B,
                                                            s_dyn->W_set,
                                                            s_dyn->U_set,
                                                            unit_cube_scale);

            //if invariant set is found
            if(invariant_set != NULL){
                found_set = true;
                //Set invariant_set cell to A cell containing an invariant set
                //(may be more than one invariant set!!)
                d_dyn->abstract_states_set[i]->invariant_set = d_dyn->abstract_states_set[i]->cells[j];
                //Compute path for rest of cell to invariant set (may be just a subset of that polytope)
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                      invariant_set,
                                                                      s_dyn,
                                                                      N);
            }else{
                //Set NULL because later safe_mode==NULL cells will be picked out
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = NULL;
            }
        }

        //If abstract state contains at least one invariant set it is appended to the graph
        if(found_set){
            //Check whether state has a transition to itself
            if(has_transition(d_dyn->abstract_states_set[i], d_dyn->abstract_states_set[i])){
                //if yes compute the safe_mode path for the cells that do not contain an invariant set
                for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
                    if(d_dyn->abstract_states_set[i]->cells[j]->safe_mode == NULL){
                        d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                              d_dyn->abstract_states_set[i]->invariant_set->polytope_description,
                                                                              s_dyn,
                                                                              N);
                    }
                }
            }
            //Append state to graph
            if (head_node->state == NULL) {
                head_node->state = d_dyn->abstract_states_set[i];
            }else{
                push_burn_node(&head_node,d_dyn->abstract_states_set[i]);
            }
        }
    }
    return head_node;

};

/**
 * Gets list of states containing invariant sets.
 * Burns through transition system, setting all other safe mode paths.
 */
//void burning_method(burn_graph_node *seeds,
//                    discrete_dynamics *d_dyn,
//                    system_dynamics *s_dyn)
//{
//
//    //Initialize needed variables
//    bool graph_burnt = false;
//    int N = (int)d_dyn->time_horizon;
//    burn_graph_node *current_burning = seeds;
//    int burning_round = 0;
//
//
//    while(!graph_burnt){
//        burning_round +=1;
//        burn_graph_node *next_burning = current_burning;
//        while (current_burning != NULL) {
//            if(current_burning->state->distance_invariant_set == INFINITY){
//                for(int i = 0; i<current_burning->state->cells_count;i++){
//                    if(current_burning->state->cells[i]->safe_mode == NULL){
//                        current_burning->state->cells[i]->safe_mode = compute_path(current_burning->state->cells[i]->polytope_description,
//                                                                                current_burning->state->next_state->convex_hull,
//                                                                                s_dyn,
//                                                                                N);
//                    }
//
//                }
//                current_burning->state->distance_invariant_set = burning_round;
//            }
//            current_burning = current_burning->next;
//        }
//        while(next_burning != NULL){
//            for(int i = 0;i<next_burning->state->transitions_in_count;i++){
//                if(next_burning->state->transitions_in[i]->distance_invariant_set == NULL){
//                    push_burn_node(&current_burning,next_burning->state->transitions_in[i]);
//                }
//            }
//            next_burning = next_burning->next;
//        }
//        if(current_burning == NULL){
//            graph_burnt = true;
//        }
//    }
//    while (seeds !=NULL){
//        if(seeds->state->next_state != seeds->state){
//            abstract_state* fastest = fastest_burn(seeds->state->transitions_out,seeds->state->transitions_out_count);
//            for(int i = 0; i< seeds->state->cells_count; i++){
//                if(seeds->state->cells[i]->safe_mode == NULL){
//                    seeds->state->cells[i]->safe_mode = compute_path(seeds->state->cells[i]->polytope_description, fastest->convex_hull,s_dyn,N);
//                }
//            }
//        }
//        seeds = seeds->next;
//    }
//};

/**
 * Set the safe_mode path for every cell in every abstract_state:
 * 1) Compute invariant sets
 * 2) Find path towards invariant sets for other abstract_states
 */
//void compute_safe_mode(discrete_dynamics *d_dyn,
//                       system_dynamics *s_dyn)
//{
//
//    burn_graph_node * invariant_sets = set_invariant_sets(d_dyn, s_dyn);
//    burning_method(invariant_sets, d_dyn, s_dyn);
//    free(invariant_sets);
//};

//gsl_vector *one_step_input(polytope *A, polytope *B)
//{
//    gsl_vector *u;
//    return u;
//};

//abstract_state *find_current_state(gsl_vector *x,
//                                   discrete_dynamics *d_dyn)
//{
//    abstract_state *now;
//    return now;
//};
//
//void *apply_safe_mode(current_state *now,
//                      discrete_dynamics *d_dyn,
//                      system_dynamics *s_dyn,
//                      void *arg)
//{
//
//    //find correct cell
//    cell * current_cell = NULL;
//    int N = (int)d_dyn->time_horizon;
//    int found = 0;
//    int i;
//    for(i = 0; i < d_dyn->abstract_states_set[now->current_abs_state]->cells_count; i++){
//        found = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->cells[i]->polytope_description, now->x);
//        if(found){
//            current_cell = d_dyn->abstract_states_set[now->current_abs_state]->cells[i];
//            break;
//        }
//    }
//
//    //if not found check all other abstract_states_set all other cells
//    if(current_cell ==NULL){
//        for(i = 0; i<d_dyn->abstract_states_count; i++){
//            for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
//                found = polytope_check_state(d_dyn->abstract_states_set[i]->cells[j]->polytope_description, now->x);
//                if(found){
//                    now->current_abs_state = i;
//                    current_cell = d_dyn->abstract_states_set[i]->cells[j];
//                }
//            }
//        }
//    }
//    int safe_mode_state = 0;
//    found = 0;
//    for(i = N; i>0; i--){
//        found = polytope_check_state(current_cell->safe_mode[i], now->x);
//        if(found){
//            safe_mode_state = i;
//            break;
//        }
//    }
//    pthread_mutex_t *mx = arg;
//
//    //while ! in invariant set
//    while(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set == NULL && !needQuit(mx)){
//        //find polytope of safe_mode
//        int check = polytope_check_state(current_cell->safe_mode[safe_mode_state], now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//
//        //linear combination to next polytope in line
//        gsl_vector *u = one_step_input(current_cell->safe_mode[safe_mode_state],
//                                               current_cell->safe_mode[safe_mode_state+1]);
//        gsl_vector *w = gsl_vector_alloc(s_dyn->E->size2);
//        simulate_disturbance(w, 0, 0.01);
//        apply_control(now->x, u, s_dyn->A, s_dyn->B, s_dyn->E, w, (size_t)safe_mode_state);
//
//    }
//
//    //while not interrupted stay in invariant set
//
//    while( !needQuit(mx) ) {
//        int check = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//        one_step_input(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description);
//    }
//    return NULL;
//};


text:
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <stdio.h>
#include <stdlib.h>

#pragma comment(lib,"gsl")

int main(int argc, char** argv)
{
    gsl_complex a,b;
    GSL_SET_COMPLEX(&a,3,4);//a=3+4i
    GSL_SET_COMPLEX(&b,6,8);//b=6+8i
    gsl_complex c = gsl_complex_add(a,b);
    printf("a+b\treal : %f image : %f\n",c.dat[0],c.dat[1]);
    c = gsl_complex_sub(a,b);
    printf("a-b\treal : %f image : %f\n",c.dat[0],c.dat[1]);
    c = gsl_complex_mul(a,b);
    printf("a*b\treal : %f image : %f\n",c.dat[0],c.dat[1]);
    c = gsl_complex_div(a,b);
    printf("a/b\treal : %f image : %f\n",c.dat[0],c.dat[1]);
    // system("PAUSE");
    return 0;
}