/* interpolation/cspline.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2004 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_vector.h>
#include "integ_eval.h"
#include <gsl/gsl_interp.h>

typedef struct
{
  double * c;
  double * g;
  double * diag;
  double * offdiag;
} cspline_state_t;

/* common initialization */
static void *
cspline_alloc (size_t size)
{
  cspline_state_t * state = (cspline_state_t *) malloc (sizeof (cspline_state_t));

  if (state == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
    }

  state->c = (double *) malloc (size * sizeof (double));

  if (state->c == NULL)
    {
      free (state);
      GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
    }

  state->g = (double *) malloc (size * sizeof (double));

  if (state->g == NULL)
    {
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for g", GSL_ENOMEM);
    }

  state->diag = (double *) malloc (size * sizeof (double));

  if (state->diag == NULL)
    {
      free (state->g);
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for diag", GSL_ENOMEM);
    }

  state->offdiag = (double *) malloc (size * sizeof (double));

  if (state->offdiag == NULL)
    {
      free (state->diag);
      free (state->g);
      free (state->c);
      free (state);
      GSL_ERROR_NULL("failed to allocate space for offdiag", GSL_ENOMEM);
    }

  return state;
}

/* natural spline calculation
 * see [Engeln-Mullges + Uhlig, p. 254]
 */
static int
cspline_init (void * vstate, const double xa[], const double ya[],
              size_t size)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  size_t i;
  size_t num_points = size;
  size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
  size_t sys_size = max_index - 1;    /* linear system is sys_size x sys_size */

  state->c[0] = 0.0;
  state->c[max_index] = 0.0;

  for (i = 0; i < sys_size; i++)
    {
      const double h_i   = xa[i + 1] - xa[i];
      const double h_ip1 = xa[i + 2] - xa[i + 1];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 -  ydiff_i * g_i);
    }

  if (sys_size == 1)
    {
      state->c[1] = state->g[0] / state->diag[0];
      return GSL_SUCCESS;
    }
  else
    {
      gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
      gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
      gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size - 1);
      gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);

      int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector,
                                                 &offdiag_vec.vector,
                                                 &g_vec.vector,
                                                 &solution_vec.vector);
      return status;
    }
}

/* periodic spline calculation
 * see [Engeln-Mullges + Uhlig, p. 256]
 */
static int
cspline_init_periodic (void * vstate, const double xa[], const double ya[],
                       size_t size)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  size_t i;
  size_t num_points = size;
  size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
  size_t sys_size = max_index;    /* linear system is sys_size x sys_size */

  if (sys_size == 2) {
    /* solve 2x2 system */

    const double h0 = xa[1] - xa[0];
    const double h1 = xa[2] - xa[1];

    const double A = 2.0*(h0 + h1);
    const double B = h0 + h1;
    double g[2];
    double det;

    g[0] = 3.0 * ((ya[2] - ya[1]) / h1 - (ya[1] - ya[0]) / h0);
    g[1] = 3.0 * ((ya[1] - ya[2]) / h0 - (ya[2] - ya[1]) / h1);

    det = 3.0 * (h0 + h1) * (h0 + h1);
    state->c[1] = ( A * g[0] - B * g[1])/det;
    state->c[2] = (-B * g[0] + A * g[1])/det;
    state->c[0] = state->c[2];

    return GSL_SUCCESS;
  } else {

    for (i = 0; i < sys_size-1; i++) {
      const double h_i       = xa[i + 1] - xa[i];
      const double h_ip1     = xa[i + 2] - xa[i + 1];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
    }

    i = sys_size - 1;

    {
      const double h_i       = xa[i + 1] - xa[i];
      const double h_ip1     = xa[1] - xa[0];
      const double ydiff_i   = ya[i + 1] - ya[i];
      const double ydiff_ip1 = ya[1] - ya[0];
      const double g_i = (h_i != 0.0) ? 1.0 / h_i : 0.0;
      const double g_ip1 = (h_ip1 != 0.0) ? 1.0 / h_ip1 : 0.0;
      state->offdiag[i] = h_ip1;
      state->diag[i] = 2.0 * (h_ip1 + h_i);
      state->g[i] = 3.0 * (ydiff_ip1 * g_ip1 - ydiff_i * g_i);
    }

    {
      gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
      gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
      gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size);
      gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);

      int status = gsl_linalg_solve_symm_cyc_tridiag(&diag_vec.vector,
                                                     &offdiag_vec.vector,
                                                     &g_vec.vector,
                                                     &solution_vec.vector);
      state->c[0] = state->c[max_index];

      return status;
    }
  }
}

static
void
cspline_free (void * vstate)
{
  cspline_state_t *state = (cspline_state_t *) vstate;

  free (state->c);
  free (state->g);
  free (state->diag);
  free (state->offdiag);
  free (state);
}

/* function for common coefficient determination
 */
static inline void
coeff_calc (const double c_array[], double dy, double dx, size_t index,
            double * b, double * c, double * d)
{
  const double c_i = c_array[index];
  const double c_ip1 = c_array[index + 1];
  *b = (dy / dx) - dx * (c_ip1 + 2.0 * c_i) / 3.0;
  *c = c_i;
  *d = (c_ip1 - c_i) / (3.0 * dx);
}

static
int
cspline_eval (const void * vstate,
              const double x_array[], const double y_array[], size_t size,
              double x,
              gsl_interp_accel * a,
              double *y)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *y = y_lo + delx * (b_i + delx * (c_i + delx * d_i));
      return GSL_SUCCESS;
    }
  else
    {
      *y = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_deriv (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    double x,
                    gsl_interp_accel * a,
                    double *dydx)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *dydx = b_i + delx * (2.0 * c_i + 3.0 * d_i * delx);
      return GSL_SUCCESS;
    }
  else
    {
      *dydx = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_deriv2 (const void * vstate,
                     const double x_array[], const double y_array[], size_t size,
                     double x,
                     gsl_interp_accel * a,
                     double * y_pp)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  double x_lo, x_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_hi = x_array[index + 1];
  x_lo = x_array[index];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      const double y_lo = y_array[index];
      const double y_hi = y_array[index + 1];
      const double dy = y_hi - y_lo;
      double delx = x - x_lo;
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
      *y_pp = 2.0 * c_i + 6.0 * d_i * delx;
      return GSL_SUCCESS;
    }
  else
    {
      *y_pp = 0.0;
      return GSL_EINVAL;
    }
}

static
int
cspline_eval_integ (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    gsl_interp_accel * acc,
                    double a, double b,
                    double * result)
{
  const cspline_state_t *state = (const cspline_state_t *) vstate;

  size_t i, index_a, index_b;

  if (acc != 0)
    {
      index_a = gsl_interp_accel_find (acc, x_array, size, a);
      index_b = gsl_interp_accel_find (acc, x_array, size, b);
    }
  else
    {
      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
    }

  *result = 0.0;

  /* interior intervals */
  for(i=index_a; i<=index_b; i++) {
    const double x_hi = x_array[i + 1];
    const double x_lo = x_array[i];
    const double y_lo = y_array[i];
    const double y_hi = y_array[i + 1];
    const double dx = x_hi - x_lo;
    const double dy = y_hi - y_lo;
    if(dx != 0.0) {
      double b_i, c_i, d_i;
      coeff_calc(state->c, dy, dx, i,  &b_i, &c_i, &d_i);

      if (i == index_a || i == index_b)
        {
          double x1 = (i == index_a) ? a : x_lo;
          double x2 = (i == index_b) ? b : x_hi;
          *result += integ_eval(y_lo, b_i, c_i, d_i, x_lo, x1, x2);
        }
      else
        {
          *result += dx * (y_lo + dx*(0.5*b_i + dx*(c_i/3.0 + 0.25*d_i*dx)));
        }
    }
    else {
      *result = 0.0;
      return GSL_EINVAL;
    }
  }

  return GSL_SUCCESS;
}

static const gsl_interp_type cspline_type =
{
  "cspline",
  3,
  &cspline_alloc,
  &cspline_init,
  &cspline_eval,
  &cspline_eval_deriv,
  &cspline_eval_deriv2,
  &cspline_eval_integ,
  &cspline_free
};

const gsl_interp_type * gsl_interp_cspline = &cspline_type;

static const gsl_interp_type cspline_periodic_type =
{
  "cspline-periodic",
  2,
  &cspline_alloc,
  &cspline_init_periodic,
  &cspline_eval,
  &cspline_eval_deriv,
  &cspline_eval_deriv2,
  &cspline_eval_integ,
  &cspline_free
};

const gsl_interp_type * gsl_interp_cspline_periodic = &cspline_periodic_type;


// my_gsl.h is included in bayesMLR.c
// Contains wrapper funtions
// References:
// https://www.gnu.org/software/gsl/manual/html_node/Matrices.html
// https://github.com/Blei-Lab/diln/blob/master/gsl_wrapper.c
#include <stdio.h>             // standard input/output
#include <stdlib.h>            // malloc
#include <math.h>              // fabs, sqrt, etc.
#include <time.h>              // time / set seed
#include <gsl/gsl_rng.h>       // GNU Scientific Library
#include <gsl/gsl_cdf.h>       // GNU Scientific Library
#include <gsl/gsl_randist.h>   // GNU Scientific Library
#include <gsl/gsl_linalg.h>    // GNU Scientific Library
#include <gsl/gsl_matrix.h>    // GNU Scientific Library
#include <gsl/gsl_statistics.h>// GNU Scientific Library
#include <gsl/gsl_blas.h>      // GSL Basic Linear Algebra
#define pi 3.14159265358979323846

// Reading Matrices ////////////////////////////////////////////////
int countFileRows(char* filename) {
  // Count number of rows in file "filename"
  FILE *fp;
  int count = 0;  // Line counter (result)
  char c;  // To store a character read from file

  fp = fopen(filename, "r");
  // Extract characters from file and store in character c
  if (fp) { // check file existence
  for (c = getc(fp); c != EOF; c = getc(fp))
      if (c == '\n') // Increment count if this character is newline
          count = count + 1;
  }
  fclose(fp);

  return count;
}

int countFileCols(char* filename, char delim) {
  // Count number of columns in file "filename" (with delimiter delim)
  FILE *fp = fopen(filename, "r");
  char c;
  int count = 0;

  while( c=fgetc(fp) ) {
    if(c == EOF) {
      break; /* break if end of file */
    } else if (c == delim) {
      count += 1; /* otherwise add one to the count */
    } else if (c == '\n') {
      count += 1;
      break;
    }
  }
  fclose(fp);

  return count;
}

void read_csv(char* filename, gsl_matrix* m) {
  // Read a csv into matrix m
  FILE* f = fopen(filename, "r");
  gsl_matrix_fscanf(f, m);
  fclose(f);
}

// Printing Matrix Info ////////////////////////////
void print_vector(gsl_matrix* v, char* filename) {
  FILE* f;
  if (filename == "") {
    f = stdout;
    printf("\n");
  } else {
    f = fopen(filename, "w");
  }

  gsl_vector_fprintf(f,v,"%g");
}

void print_matrix(gsl_matrix* m, char* filename) {
  // print matrix to filename or stdout if filename == ""
  FILE* f;
  if (filename == "") {
    f = stdout;
    printf("\n");
  } else {
    f = fopen(filename, "w");
  }
  int n = m->size1;
  int k = m->size2;

  for (int i = 0; i < n; i++) {
    for (int j = 0; j < k; j++) {
      fprintf (f, "%g\t", gsl_matrix_get (m, i, j));
    }
    fprintf (f, "\n");
  }

  if (filename == "") {
    printf("\n");
  } else {
    fclose(f);
  }
}

void print_matrix_dims(gsl_matrix* m) {
  // print dimensions of matrix m
  printf("%d%s%d\n",m->size1,"x",m->size2);
}

// Access Matrix Elements: //////////////////////////////////
void mat_sub(gsl_matrix* X, size_t r1, size_t r2, size_t c1, size_t c2, gsl_matrix* X_new) {
  gsl_matrix_view x = gsl_matrix_submatrix(X,r1,c1,r2-r1+1,c2-c1+1);
  gsl_matrix_memcpy(X_new,&x.matrix);
}

// Random Number Functions: /////////////////////////////////
double runif() {
  // Returns a random number b/w 0 & 1
  return (double) rand() / (double) RAND_MAX;
}

void mvrnorm(gsl_vector* m, gsl_matrix* cholS, gsl_rng* r, gsl_vector* out) {
  // vector addition, vector prod
  // m + cholS * e

  int n = m->size;
  double tmp;
  gsl_vector* e = gsl_vector_alloc(n);

  for (int i=0; i<n; i++) {
    tmp = 0;
    for (int j=0; j<n; j++) {
      if (i==0) gsl_vector_set(e,j,gsl_ran_gaussian(r,1.0));
      tmp += gsl_matrix_get(cholS,i,j) * gsl_vector_get(e,j);
    }
    gsl_vector_set(out,i,gsl_vector_get(m,i) + tmp);
  }

  gsl_vector_free(e);
}

// Linear Algebra Wrappers //////////////////////////////////
// mm: matrix-matrix
// mv: matrix-vector
// ms: matrix-scalar
//  d: double real
// ge: general matrix
void mm_prod(gsl_matrix* A, gsl_matrix* B, double a, double b, gsl_matrix* out) {
}

void mm_add(gsl_matrix* A, gsl_matrix* B, double a, double b, gsl_matrix* out) {
}

void mv_prod(gsl_matrix* A, gsl_vector* x, double a, gsl_vector* out) {
  gsl_blas_dgemv(CblasNoTrans,a,A,x,0.0,out);
}

void vv_add(gsl_vector* x, gsl_vector* y, gsl_vector *out) {
  // sum two vectors
  if (x->size != y->size)
    error("Error in vv_add(): input vectors must have the same dimensionality");
  else if (x->size != out->size || y->size != out->size)
    error("Error in vv_add(): output vector has incorrect dimensions");

  double tmp;
  int n = x->size;

  for (int i=0; i<n; i++) {
    tmp = gsl_vector_get(x,i) + gsl_vector_get(y,i);
    gsl_vector_set(out,i,tmp);
  }
}

double vv_prod(gsl_vector* x, gsl_vector* y) {
  // dot product
  if (x->size != y->size)
    error("Error in vv_prod(): input vectors must have the same dimensionality");

  double out = 0;
  int n = x->size;

  for (int i=0; i<n; i++) {
    out += gsl_vector_get(x,i) * gsl_vector_get(y,i);
  }

  return out;
}

void mat_inv(gsl_matrix * X, gsl_matrix * invX) {
// invert the matrix X
    int signum;
    gsl_matrix * LU = gsl_matrix_calloc(X->size1,X->size2);
    gsl_permutation * p = gsl_permutation_calloc(X->size1);
    gsl_matrix_memcpy(LU,X);

    gsl_linalg_LU_decomp (LU,p,&signum);
    gsl_linalg_LU_invert(LU,p,invX);

    gsl_matrix_free(LU);
    gsl_permutation_free(p);
}

void xxi_m(gsl_matrix* X, gsl_matrix* xxi) {
  int n = X->size1;
  int k = X->size2;
  double dot;
  gsl_matrix* xx = gsl_matrix_alloc(k,k);

  for (int i=0; i<k; i++) {
    for (int j=0; j<=i; j++) {
      dot = 0;
      for (int z=0; z<n; z++) {
        dot += gsl_matrix_get(X,z,i) * gsl_matrix_get(X,z,j);
      }
      gsl_matrix_set(xx,j,i,dot);
      gsl_matrix_set(xx,i,j,dot);
    }
  }

  mat_inv(xx,xxi);
  gsl_matrix_free(xx);
}

int my_gsl_linalg_cholesky_decomp (gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i,j,k;
      int status = 0;

      /* Do the first 2 rows explicitly.  It is simple, and faster.  And
       * one can return if the matrix has only 1 or 2 rows.
       */

      double A_00 = gsl_matrix_get (A, 0, 0);

      double L_00 = sqrt(A_00);

      if (A_00 <= 0)
        {
          status = GSL_EDOM ;
        }

      gsl_matrix_set (A, 0, 0, L_00);

      if (M > 1)
        {
          double A_10 = gsl_matrix_get (A, 1, 0);
          double A_11 = gsl_matrix_get (A, 1, 1);

          double L_10 = A_10 / L_00;
          double diag = A_11 - L_10 * L_10;
          double L_11 = sqrt(diag);

          if (diag <= 0)
            {
              status = GSL_EDOM;
            }

          gsl_matrix_set (A, 1, 0, L_10);
          gsl_matrix_set (A, 1, 1, L_11);
        }

      for (k = 2; k < M; k++)
        {
          double A_kk = gsl_matrix_get (A, k, k);

          for (i = 0; i < k; i++)
            {
              double sum = 0;

              double A_ki = gsl_matrix_get (A, k, i);
              double A_ii = gsl_matrix_get (A, i, i);

              gsl_vector_view ci = gsl_matrix_row (A, i);
              gsl_vector_view ck = gsl_matrix_row (A, k);

              if (i > 0) {
                gsl_vector_view di = gsl_vector_subvector(&ci.vector, 0, i);
                gsl_vector_view dk = gsl_vector_subvector(&ck.vector, 0, i);

                gsl_blas_ddot (&di.vector, &dk.vector, &sum);
              }

              A_ki = (A_ki - sum) / A_ii;
              gsl_matrix_set (A, k, i, A_ki);
            }

          {
            gsl_vector_view ck = gsl_matrix_row (A, k);
            gsl_vector_view dk = gsl_vector_subvector (&ck.vector, 0, k);

            double sum = gsl_blas_dnrm2 (&dk.vector);
            double diag = A_kk - sum * sum;

            double L_kk = sqrt(diag);

            if (diag <= 0)
              {
                status = GSL_EDOM;
              }

            gsl_matrix_set (A, k, k, L_kk);
          }
        }

      /* Now copy the transposed lower triangle to the upper triangle,
       * the diagonal is common.
       */

      //for (i = 1; i < M; i++)
      //  {
      //    for (j = 0; j < i; j++)
      //      {
      //        double A_ij = 0;//gsl_matrix_get (A, i, j);
      //        gsl_matrix_set (A, j, i, A_ij);
      //      }
      //  }

      if (status == GSL_EDOM)
        {
          GSL_ERROR ("matrix must be positive definite", GSL_EDOM);
        }

      return GSL_SUCCESS;
    }
}

void chol(gsl_matrix* X, gsl_matrix* cholS) {
  int n = X->size1;
  gsl_matrix_memcpy(cholS,X);
  my_gsl_linalg_cholesky_decomp(cholS);

  // lower tri stored in cholS.
  // So: mvrnorm <- function(M,S,n=nrow(S)) M + chol(S) %*% rnorm(n)
}


/* randist/logarithmic.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Logarithmic distribution

   prob(n) =   p^n / (n log(1/(1-p)) for n = 1, 2, 3, ...

   We use Kemp's second accelerated generator, from Luc Devroye's book
   on "Non-Uniform Random Variate Generation", Springer */

unsigned int
gsl_ran_logarithmic (const gsl_rng * r, const double p)
{
  double c = log (1-p) ;

  double v = gsl_rng_uniform_pos (r);

  if (v >= p)
    {
      return 1 ;
    }
  else
    {
      double u = gsl_rng_uniform_pos (r);
      double q = 1 - exp (c * u);

      if (v <= q*q)
	{
	  double x = 1 + log(v)/log(q) ;
	  return x ;
	}
      else if (v <= q)
	{
	  return 2;
	}
      else
	{
	  return 1 ;
	}
    }
}

double
gsl_ran_logarithmic_pdf (const unsigned int k, const double p)
{
  if (k == 0)
    {
      return 0 ;
    }
  else
    {
      double P = pow(p, (double)k) / (double) k / log(1/(1-p)) ;
      return P;
    }
}


/* integration/qcheb.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>

/* This function computes the 12-th order and 24-th order Chebyshev
   approximations to f(x) on [a,b] */

void
gsl_integration_qcheb (gsl_function * f, double a, double b, double *cheb12, double *cheb24)
{
  size_t i;
  double fval[25], v[12];

  /* These are the values of cos(pi*k/24) for k=1..11 needed for the
     Chebyshev expansion of f(x) */

  const double x[11] = { 0.9914448613738104,
                         0.9659258262890683,
                         0.9238795325112868,
                         0.8660254037844386,
                         0.7933533402912352,
                         0.7071067811865475,
                         0.6087614290087206,
                         0.5000000000000000,
                         0.3826834323650898,
                         0.2588190451025208,
                         0.1305261922200516 };

  const double center = 0.5 * (b + a);
  const double half_length =  0.5 * (b - a);

  fval[0] = 0.5 * GSL_FN_EVAL (f, b);
  fval[12] = GSL_FN_EVAL (f, center);
  fval[24] = 0.5 * GSL_FN_EVAL (f, a);

  for (i = 1; i < 12; i++)
    {
      const size_t j = 24 - i;
      const double u = half_length * x[i-1];
      fval[i] = GSL_FN_EVAL(f, center + u);
      fval[j] = GSL_FN_EVAL(f, center - u);
    }

  for (i = 0; i < 12; i++)
    {
      const size_t j = 24 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  {
    const double alam1 = v[0] - v[8];
    const double alam2 = x[5] * (v[2] - v[6] - v[10]);

    cheb12[3] = alam1 + alam2;
    cheb12[9] = alam1 - alam2;
  }

  {
    const double alam1 = v[1] - v[7] - v[9];
    const double alam2 = v[3] - v[5] - v[11];
    {
      const double alam = x[2] * alam1 + x[8] * alam2;

      cheb24[3] = cheb12[3] + alam;
      cheb24[21] = cheb12[3] - alam;
    }

    {
      const double alam = x[8] * alam1 - x[2] * alam2;
      cheb24[9] = cheb12[9] + alam;
      cheb24[15] = cheb12[9] - alam;
    }
  }

  {
    const double part1 = x[3] * v[4];
    const double part2 = x[7] * v[8];
    const double part3 = x[5] * v[6];

    {
      const double alam1 = v[0] + part1 + part2;
      const double alam2 = x[1] * v[2] + part3 + x[9] * v[10];

      cheb12[1] = alam1 + alam2;
      cheb12[11] = alam1 - alam2;
    }

    {
      const double alam1 = v[0] - part1 + part2;
      const double alam2 = x[9] * v[2] - part3 + x[1] * v[10];
      cheb12[5] = alam1 + alam2;
      cheb12[7] = alam1 - alam2;
    }
  }

  {
    const double alam = (x[0] * v[1] + x[2] * v[3] + x[4] * v[5]
                + x[6] * v[7] + x[8] * v[9] + x[10] * v[11]);
    cheb24[1] = cheb12[1] + alam;
    cheb24[23] = cheb12[1] - alam;
  }

  {
    const double alam = (x[10] * v[1] - x[8] * v[3] + x[6] * v[5]
                - x[4] * v[7] + x[2] * v[9] - x[0] * v[11]);
    cheb24[11] = cheb12[11] + alam;
    cheb24[13] = cheb12[11] - alam;
  }

  {
    const double alam = (x[4] * v[1] - x[8] * v[3] - x[0] * v[5]
                - x[10] * v[7] + x[2] * v[9] + x[6] * v[11]);
    cheb24[5] = cheb12[5] + alam;
    cheb24[19] = cheb12[5] - alam;
  }

  {
    const double alam = (x[6] * v[1] - x[2] * v[3] - x[10] * v[5]
                + x[0] * v[7] - x[8] * v[9] - x[4] * v[11]);
    cheb24[7] = cheb12[7] + alam;
    cheb24[17] = cheb12[7] - alam;
  }

  for (i = 0; i < 6; i++)
    {
      const size_t j = 12 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  {
    const double alam1 = v[0] + x[7] * v[4];
    const double alam2 = x[3] * v[2];

    cheb12[2] = alam1 + alam2;
    cheb12[10] = alam1 - alam2;
  }

  cheb12[6] = v[0] - v[4];

  {
    const double alam = x[1] * v[1] + x[5] * v[3] + x[9] * v[5];
    cheb24[2] = cheb12[2] + alam;
    cheb24[22] = cheb12[2] - alam;
  }

  {
    const double alam = x[5] * (v[1] - v[3] - v[5]);
    cheb24[6] = cheb12[6] + alam;
    cheb24[18] = cheb12[6] - alam;
  }

  {
    const double alam = x[9] * v[1] - x[5] * v[3] + x[1] * v[5];
    cheb24[10] = cheb12[10] + alam;
    cheb24[14] = cheb12[10] - alam;
  }

  for (i = 0; i < 3; i++)
    {
      const size_t j = 6 - i;
      v[i] = fval[i] - fval[j];
      fval[i] = fval[i] + fval[j];
    }

  cheb12[4] = v[0] + x[7] * v[2];
  cheb12[8] = fval[0] - x[7] * fval[2];

  {
    const double alam = x[3] * v[1];
    cheb24[4] = cheb12[4] + alam;
    cheb24[20] = cheb12[4] - alam;
  }

  {
    const double alam = x[7] * fval[1] - fval[3];
    cheb24[8] = cheb12[8] + alam;
    cheb24[16] = cheb12[8] - alam;
  }

  cheb12[0] = fval[0] + fval[2];

  {
    const double alam = fval[1] + fval[3];
    cheb24[0] = cheb12[0] + alam;
    cheb24[24] = cheb12[0] - alam;
  }

  cheb12[12] = v[0] - v[2];
  cheb24[12] = cheb12[12];

  for (i = 1; i < 12; i++)
    {
      cheb12[i] *= 1.0 / 6.0;
    }

  cheb12[0] *= 1.0 / 12.0;
  cheb12[12] *= 1.0 / 12.0;

  for (i = 1; i < 24; i++)
    {
      cheb24[i] *= 1.0 / 12.0;
    }

  cheb24[0] *= 1.0 / 24.0;
  cheb24[24] *= 1.0 / 24.0;
}


/*
 * ParticleFilter.h
 *
 *  Created on: Mar 21, 2012
 *      Author: nair
 */

#ifndef PARTICLEFILTER_H_
#define PARTICLEFILTER_H_

#include <gsl/gsl_rng.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

namespace tracking1
{

class ParticleFilterParams
{
public:
    ParticleFilterParams();
    ~ParticleFilterParams();

    void parse(std::string filename);

public:

    int NUMBER_OF_PARTICLES;

    /* standard deviations for gaussian sampling in transition model */
    float TRANS_X_STD;
    float TRANS_Y_STD;
    float TRANS_SCALE_STD;

    /* autoregressive dynamics parameters for transition model */
    float DYNAMICS_A1;
    float DYNAMICS_A2;
    float DYNAMICS_B0;

    double LAMBDA;

    bool DYNAMIC_MODEL_CV;
};

struct Particle
{
    float x;          /**< current x coordinate */
    float y;          /**< current y coordinate */
    float s;          /**< current z coordinate */
    float xp;         /**< previous x coordinate */
    float yp;         /**< previous y coordinate */
    float sp;         /**< previous z coordinate */
    float x0;         /**< original x coordinate */
    float y0;         /**< original y coordinate */
    float s0;			/**< original z coordinate */
    float x_velocity; /**< current x_velocity coordinate */
    float y_velocity; /**< current y_velocity coordinate */
    float s_velocity; /**< current z_velocity coordinate */

    float w;          /**< weight */
};

class ParticleFilter
{
public:

    ParticleFilter();
    ~ParticleFilter();

    void initialize(const cv::Vec3d& initPose);

    void predict();

    void predict_with_simple_model();

    void predict_with_cv_model();

    void correct();

    void resizeParticleSet(int n);
    void setParticleWeight(int id, float w);

    double getMaxLikelihood()
    {
        double max=0.0;
        for(int i=0;i<mParams.NUMBER_OF_PARTICLES;++i)
        {
            if(max<mParticles[i].w)
            {
                max=mParticles[i].w;
            }
        }
        return max;
    }

    double getAvgLikelihood()
    {
        double avg=0.0;
        for(int i=0;i<mParams.NUMBER_OF_PARTICLES;++i)
        {

            avg+=mParticles[i].w;
        }
        return avg/mParams.NUMBER_OF_PARTICLES;
    }

    int nOfParticles()
    {
        return mParams.NUMBER_OF_PARTICLES;
    }

    static int particle_cmp(const void* p1,const void* p2 )
    {
        Particle* _p1 = (Particle*)p1;
        Particle* _p2 = (Particle*)p2;

        if( _p1->w > _p2->w )
            return -1;
        if( _p1->w < _p2->w )
            return 1;
        return 0;
    }

    void getParticlePose(int id, cv::Vec3d& pose);

    void getOutputPose(cv::Vec3d& pose);

    void getOutputAvgPose(cv::Vec3d& pose);

    void getOutputVelocity(cv::Vec3d& vel);

    double getLambda()
    {
        return mParams.LAMBDA;
    }

private:

    void normalize();

    void resample();

private:

    ParticleFilterParams mParams;

    Particle* mParticles;
    cv::Vec3d mOutputPose;

    Eigen::MatrixXd A_Matrix;
    Eigen::MatrixXd G_Matrix;
    Eigen::MatrixXd W_Matrix;

    Eigen::MatrixXd State_Matrix;
    Eigen::MatrixXd State_Prev_Matrix;

    gsl_rng* rng;

};

}
#endif /* PARTICLEFILTER_H_ */


/*
 * Assignment 4, Question 5 matvec implementation in C using MPI.
 *
 * Copyright (C) 2015, Jonathan Gillett (100437638)
 * All rights reserved.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <mpi.h>

// Minimum of two values
#define min(a, b) (((a) < (b)) ? (a) : (b))
// Define process 0 as MASTER
#define MASTER 0
// Message tags
#define REQUEST 1
#define REPLY   2

// Master process, divides up the matrix into rows for computation by slaves
void master(int n_proc, int n_rows, int n_cols) {
    // 0. Allocate space for arrays as needed to compute y = A*x
    double *x = (double *) malloc(n_cols*sizeof(double));
    double *y = (double *) malloc(n_rows*sizeof(double));
    double *z = (double *) malloc(n_rows*sizeof(double));
    double *A = (double *) malloc(n_rows*n_cols*sizeof(double));
    double *sub = (double *) malloc(n_cols*sizeof(double));
    double ans = 0.0;
    int n_sent = 0, row = 0, sender = 0;
    MPI_Status status;

    // 1. Define data A (n_rows x n_cols matrix) and x (n_cols x 1 vector)
    for (int i = 0; i < n_cols; ++i) {
        x[i] = i+1;
    }
    for (int i = 0; i < n_rows; ++i) {
        for (int j = 0; j < n_cols; ++j) {
            A[(i*n_cols)+j] = i+1;
        }
    }

    // 2. Broadcast vector x to all processes
    MPI_Bcast(x, n_cols, MPI_DOUBLE, MASTER, MPI_COMM_WORLD);

    // 3. Send first rows to other processes
    for (int i = 1; i < min(n_rows+1, n_proc); ++i) {
    	memcpy((void *) sub, (void *) &A[n_sent*n_cols], n_cols * sizeof(double));
        MPI_Send(sub, n_cols, MPI_DOUBLE, i, n_sent, MPI_COMM_WORLD);
        n_sent++;
    }

    // Now loop over remaining rows, receiving dot products computed
    for (int i = 0; i < n_rows; ++i) {
        // Receive a computed inner product from one of the processes
        MPI_Recv(&ans, 1, MPI_DOUBLE, MPI_ANY_SOURCE, MPI_ANY_TAG,
                 MPI_COMM_WORLD, &status);
        sender = status.MPI_SOURCE;
        row = status.MPI_TAG;
        y[row] = ans;

        // Either send another row back to sender or send final y=Ax
        // along with a tag to signal completion.
        if (n_sent < n_rows) {
        	memcpy((void *) sub, (void *) &A[n_sent*n_cols], n_cols * sizeof(double));
            MPI_Send(sub, n_cols, MPI_DOUBLE, sender, n_sent, MPI_COMM_WORLD);
            n_sent++;
        }
        // Inform sender that there is no more work
        else {
            MPI_Send(y, n_rows, MPI_DOUBLE, sender, n_rows+1, MPI_COMM_WORLD);
        }
    }

    printf("Finished!\n");
    // Calculate the norm to verify ||y-Ax|| is correct
    cblas_dgemv(CblasRowMajor, CblasNoTrans, n_rows, n_cols, 1.0, A, n_cols,
	      		x, 1, 0.0, z, 1);
    for (int i = 0; i < n_rows; ++i) {
    	y[i] -= z[i];
    }
    printf("Check: ||y-Ax|| = %f\n", cblas_dnrm2(n_rows, y, 1));

    free(x);
    free(y);
    free(z);
    free(A);
    free(sub);
}

// Slave process computes inner product and returns result to master process
void slave(int proc_id, int n_rows, int n_cols) {
    // Allocate arrays for receiving and sending rows & dot products
    double *x = (double *) malloc(n_cols*sizeof(double));
    double *my_row = (double *) malloc(n_cols*sizeof(double));
    double ans = 0.0;
    int row = 0;
    MPI_Status status;

    // 1. Receive vector x from master process
    MPI_Bcast(x, n_cols, MPI_DOUBLE, MASTER, MPI_COMM_WORLD);

    // Receive row to calculate from master process
    MPI_Recv(my_row, n_cols, MPI_DOUBLE, MASTER, MPI_ANY_TAG,
             MPI_COMM_WORLD, &status);
    row = status.MPI_TAG;

    while (row < n_rows) {
        // Compute inner product and return result to master process
        ans = cblas_ddot(n_cols, my_row, 1, x, 1);
        MPI_Send(&ans, 1, MPI_DOUBLE, MASTER, row, MPI_COMM_WORLD);
        // Receive new row from master
        MPI_Recv(my_row, n_cols, MPI_DOUBLE, MASTER, MPI_ANY_TAG,
                 MPI_COMM_WORLD, &status);
        row = status.MPI_TAG;
    }
}

int main(int argc, char *argv[]) {
    int    proc_id;       // Process ID number
    int    n_proc;        // Number of processes
    int    n_rows = 1000; // Number of rows
    int    n_cols = 1000; // Number of columns

    // Initialize MPI
    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &n_proc);
    MPI_Comm_rank(MPI_COMM_WORLD, &proc_id);

    if (n_proc == 1) {
        puts("Error: requires two or more processes (no work will be done).");
        MPI_Finalize();
        exit(1);
    }

    if (proc_id == MASTER) {
        master(n_proc, n_rows, n_cols);
    }
    else {
        slave(proc_id, n_rows, n_cols);
    }

    MPI_Finalize();
    return 0;
}


#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <cblas.h>
#include <string.h> // memcpy
#include <math.h>

#include "cgraph.h"
#include "cg_operation.h"
#include "cg_types.h"
#include "cg_variables.h"
#include "cg_errors.h"
#include "cg_constants.h"
#include "cg_enums.h"
#include "cg_factory.h"
#include "cg_math.h"

#if cg_float == float
#define cblas_dcopy cblas_scopy
#define cblas_dscal cblas_sscal
#define cblas_dgemm cblas_sgemm
#define cblas_ddot cblas_sdot
#define cblas_dgemv cblas_sgemv
#endif

int selectContext() {
    return 0;
}

void copyDataToHost(CGResultNode* res){}

/*
 * *********************
 * Multiplication
 * *********************
 */

/*
 * d.d
 */
CGResultNode* mulDD(CGDouble* M, CGDouble* V, CGraph* graph, CGNode* parentNode){
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = M->value * V->value;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * d.V == V.d
 */
CGResultNode* mulDV(CGDouble* a, CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = V->len;
    Y->data = y;

    if(a->value != 0){
        cblas_dcopy(V->len, V->data, 1, y, 1);
        cblas_dscal(V->len, a->value, y, 1);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * d.M == M.d
 */
CGResultNode* mulDM(CGDouble* a, CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->cols*M->rows;
    cg_float value = a->value;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = y;

    if(a->value != 0){
        cblas_dcopy(size, M->data, 1, y, 1);
        cblas_dscal(size, a->value, y, 1);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M*v
 * broadcasting
 */
CGResultNode* mulMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    if(M->cols != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Add M(%"PRIu64", %"PRIu64") by V(%"PRIu64")", M->rows, M->cols, V->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M->cols*M->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M->data[i] * V->data[(i)%V->len];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * Mv
 */
/*
CGResultNode* mulMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
	if(M->cols != V->len){
		char msg[MAX_ERR_FMT_LEN];
		snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Add M(%"PRIu64", %"PRIu64") by V(%"PRIu64")", M->rows, M->cols, V->len);
		return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
	}

	uint64_t size = M->cols*M->rows;
	cg_float* res = calloc(size, sizeof(cg_float));

	CGMatrix* Y = calloc(1, sizeof(CGMatrix));
	Y->rows = M->rows;
	Y->cols = M->cols;
	Y->data = res;

	uint64_t i = 0;

	for(;i<size;i++){
		res[i] = M->data[i] * V->data[(i)%V->len];
	}

	CGResultNode* result = calloc(1, sizeof(CGResultNode));
	result->type = CGVT_MATRIX;
	result->value = Y;

	return result;
}
*/

/*
 * M*N
 * broadcasting
 */
CGResultNode* mulMM(CGMatrix* M1, CGMatrix* M2, CGraph* graph, CGNode* parentNode){
    if((M1->rows != M2->rows) && (M1->cols != M2->cols)){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot multiply M(%"PRIu64", %"PRIu64") by N(%"PRIu64", %"PRIu64")", M1->rows, M1->cols, M2->rows, M2->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M1->cols*M1->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M1->rows;
    Y->cols = M1->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M1->data[i] * M2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M.v
 */
CGResultNode* dotMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    if(M->cols != 1){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot calculate M(%"PRIu64", %"PRIu64") DOT V(%"PRIu64")", M->rows, M->cols, V->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    cg_float* y = calloc(M->rows*V->len, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows= M->rows;
    Y->cols  = V->len;
    Y->data = y;

    /*
    cblas_dgemv(CblasRowMajor,
            CblasNoTrans, M->rows, M->cols, 1.0, M->data,
            M->cols, V->data, 1, 0.0, Y->data, 1);
    */

    cblas_dgemm(CblasRowMajor,
                CblasNoTrans, CblasNoTrans, M->rows, V->len, M->cols,
                1.0, M->data, M->cols, V->data, V->len, 0, y, Y->cols);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * v.M
 */
CGResultNode* dotVM(CGVector* V, CGMatrix* M, CGraph* graph, CGNode* parentNode){
    if(M->rows != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot calculate V(%"PRIu64") DOT M(%"PRIu64", %"PRIu64")", V->len, M->rows, M->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    cg_float* y = calloc(M->cols, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = M->cols;
    Y->data = y;

    int m = 1;
    int n = M->cols;
    int k = V->len;

    cblas_dgemm(CblasRowMajor,
                CblasNoTrans, CblasNoTrans, m, n, k, 1, V->data, k, M->data, n, 0, Y->data, n);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M.N
 */
CGResultNode* dotMM(CGMatrix* M, CGMatrix* N, CGraph* graph, CGNode* parentNode){
    if(M->cols != N->rows){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot DOT M(%"PRIu64", %"PRIu64") by N(%"PRIu64", %"PRIu64")", M->rows, M->cols, N->rows, N->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    cg_float* z = calloc(M->rows*N->cols, sizeof(cg_float));

    CGMatrix* Z = calloc(1, sizeof(CGMatrix));
    Z->rows = M->rows;
    Z->cols = N->cols;

    cblas_dgemm(CblasRowMajor,
                CblasNoTrans, CblasNoTrans, M->rows, N->cols, M->cols,
                1.0, M->data, M->cols, N->data, N->cols, 0, z, Z->cols);

    Z->data = z;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Z;

    return result;
}

/*
 * V.V
 */
CGResultNode* crossVV(CGVector* V1, CGVector* V2, CGraph* graph, CGNode* parentNode){
    if(V1->len != V2->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot compute Cross Product of V(%"PRIu64") by V(%"PRIu64")", V1->len, V2->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = V1->len;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = V1->len;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = V1->data[i] * V2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Dot Product
 * *********************
 */

/*
 * V.V
 */
CGResultNode* dotVV(CGVector* V1, CGVector* V2, CGraph* graph, CGNode* parentNode){
    if(V1->len != V2->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot compute Dot Product of V(%"PRIu64") by V(%"PRIu64")", V1->len, V2->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = V1->len;
    cg_float res = 0;

    CGDouble* Y = calloc(1, sizeof(CGDouble));

    Y->value = cblas_ddot(V1->len, V1->data, 1, V2->data, 1);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Division
 * *********************
 */

/*
 *  d/d
 */
CGResultNode* divDD(CGDouble* D1, CGDouble* D2, CGraph* graph, CGNode* parentNode){
    if(D2->value == 0.0){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Diving by zero D/(D=0)");
        return returnResultError(graph, CGET_DIVIDE_BY_ZERO, parentNode, msg);
    }

    cg_float res = D1->value / D2->value;

    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = res;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 *  V/d
 */
CGResultNode* divVD(CGVector* V, CGDouble* D, CGraph* graph, CGNode* parentNode){
    if(D->value == 0.0){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Diving by zero V/(D=0)");
        return returnResultError(graph, CGET_DIVIDE_BY_ZERO, parentNode, msg);
    }

    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    cblas_dcopy(V->len, V->data, 1, res, 1);
    cblas_dscal(V->len, 1.0/D->value, res, 1);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 *  V/V
 */
CGResultNode* divVV(CGVector* V, CGVector* D, CGraph* graph, CGNode* parentNode){
    if(V->len != D->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot calculate  V(%"PRIu64") DIV V(%"PRIu64")", V->len, D->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    cg_float* res = calloc(V->len, sizeof(cg_float));

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;
    for(;i<V->len;i++){
        res[i] = V->data[i]/D->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 *  d/V element-wise
 */
CGResultNode* divDV(CGDouble* D, CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        res[i] = value / V->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 *  M/d
 */
CGResultNode* divMD(CGMatrix* M, CGDouble* D, CGraph* graph, CGNode* parentNode){
    if(D->value == 0.0){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Diving by zero M/(D=0)");
        return returnResultError(graph, CGET_DIVIDE_BY_ZERO, parentNode, msg);
    }

    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    cblas_dcopy(size, M->data, 1, res, 1);
    cblas_dscal(size, 1.0/D->value, res, 1);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M/v
 */
CGResultNode* divMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    if(M->rows != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot calculate  M(%"PRIu64", %"PRIu64") DIV V(%"PRIu64")", M->rows, M->cols, V->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(; i < size;i++)
    {
        res[i] = M->data[i] / V->data[i/M->cols];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 *  d/M element-wise
 */
CGResultNode* divDM(CGDouble* D, CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = value / M->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Addition
 * *********************
 */

/*
 * d+d
 */
CGResultNode* addDD(CGDouble* D1, CGDouble* D2, CGraph* graph, CGNode* parentNode){
    cg_float res = D1->value + D2->value;

    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = res;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * V+d
 */
CGResultNode* addVD(CGVector* V, CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        res[i] = V->data[i] + value;
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M+d
 */
CGResultNode* addMD(CGMatrix* M, CGDouble* D, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M->data[i] + value;
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * V+V
 */
CGResultNode* addVV(CGVector* V1, CGVector* V2, CGraph* graph, CGNode* parentNode){
    if(V1->len != V2->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Add V(%"PRIu64") by V(%"PRIu64")", V1->len, V2->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = V1->len;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = V1->len;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = V1->data[i] + V2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M+V
 */
CGResultNode* addMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    if(M->cols != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Add M(%"PRIu64", %"PRIu64") by V(%"PRIu64")", M->rows, M->cols, V->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M->cols*M->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M->data[i] + V->data[(i)%V->len];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M+M
 */
CGResultNode* addMM(CGMatrix* M1, CGMatrix* M2, CGraph* graph, CGNode* parentNode){
    if((M1->rows != M2->rows) && (M1->cols != M2->cols)){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot add M(%"PRIu64", %"PRIu64") by N(%"PRIu64", %"PRIu64")", M1->rows, M1->cols, M2->rows, M2->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M1->cols*M1->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M1->rows;
    Y->cols = M1->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M1->data[i] + M2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Substraction
 * *********************
 */

/*
 * d-d
 */
CGResultNode* subDD(CGDouble* D1, CGDouble* D2, CGraph* graph, CGNode* parentNode){
    cg_float res = D1->value - D2->value;

    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = res;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * V-d
 */
CGResultNode* subVD(CGVector* V, CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        res[i] = V->data[i] - value;
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * d-V
 */
CGResultNode* subDV(CGDouble* D, CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        res[i] = value - V->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M-d
 */
CGResultNode* subMD(CGMatrix* M, CGDouble* D, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M->data[i] - value;
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * D-M
 */
CGResultNode* subDM(CGDouble* D, CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = value - M->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * V-V
 */
CGResultNode* subVV(CGVector* V1, CGVector* V2, CGraph* graph, CGNode* parentNode){
    if(V1->len != V2->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Substract V(%"PRIu64") by V(%"PRIu64")", V1->len, V2->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = V1->len;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = V1->len;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = V1->data[i] - V2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M-M
 */
CGResultNode* subMM(CGMatrix* M1, CGMatrix* M2, CGraph* graph, CGNode* parentNode){
    if((M1->rows != M2->rows) && (M1->cols != M2->cols)){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Substract M(%"PRIu64", %"PRIu64") by N(%"PRIu64", %"PRIu64")", M1->rows, M1->cols, M2->rows, M2->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M1->cols*M1->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M1->rows;
    Y->cols = M1->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M1->data[i] - M2->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M-V
 */
CGResultNode* subMV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    if(M->rows != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Substract M(%"PRIu64", %"PRIu64") by V(%"PRIu64")", M->rows, M->cols, V->len);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M->cols*M->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = M->data[i] - V->data[(i)/M->cols];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * V-M
 */
CGResultNode* subVM(CGVector* V, CGMatrix* M, CGraph* graph, CGNode* parentNode){

    if(M->rows != V->len){
        char msg[MAX_ERR_FMT_LEN];
        snprintf(msg, MAX_ERR_FMT_LEN, "Cannot Substract V(%"PRIu64") by M(%"PRIu64", %"PRIu64")", V->len, M->rows, M->cols);
        return returnResultError(graph, CGET_INCOMPATIBLE_DIMENTIONS_EXCEPTION, parentNode, msg);
    }

    uint64_t size = M->cols*M->rows;
    cg_float* res = calloc(size, sizeof(cg_float));

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = V->data[(i)/M->cols] - M->data[i];
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Power
 * *********************
 */

/*
 * d^d
 */
CGResultNode* powDD(CGDouble* D1, CGDouble* D2, CGraph* graph, CGNode* parentNode){
    cg_float res = pow(D1->value, D2->value);

    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = res;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * V^d
 */
CGResultNode* powVD(CGVector* V, CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float* res = calloc(V->len, sizeof(cg_float));
    cg_float value = D->value;

    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = res;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        res[i] = pow(V->data[i], value);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * M^d
 */
CGResultNode* powMD(CGMatrix* M, CGDouble* D, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;
    cg_float* res = calloc(size, sizeof(cg_float));
    cg_float value = D->value;

    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->rows = M->rows;
    Y->cols = M->cols;
    Y->data = res;

    uint64_t i = 0;

    for(;i<size;i++){
        res[i] = pow(M->data[i], value);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Transpose Multiplication
 * *********************
 */
/*
 * M^T.v
 */
CGResultNode* mulMtV(CGMatrix* M, CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->len = V->len;
    Y->data = y;

    cblas_dgemv(CblasRowMajor,
                CblasTrans, M->rows, M->cols, 1.0, M->data,
                V->len, V->data, 1, 0.0, Y->data, 1);

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * exp(d)
 */
CGResultNode* expD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = exp(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * exp(v)
 */
CGResultNode* expV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = exp(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * exp(M)
 */
CGResultNode* expM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = exp(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * log(d)
 */
CGResultNode* logD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = log(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * log(V)
 */
CGResultNode* logV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = log(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * exp(M)
 */
CGResultNode* logM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = log(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Sin
 * *********************
 */

/*
 * sin(d)
 */
CGResultNode* sinD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = sin(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * sin(V)
 */
CGResultNode* sinV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = sin(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * sin(M)
 */
CGResultNode* sinM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = sin(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * cos
 * *********************
 */

/*
 * cos(d)
 */
CGResultNode* cosD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = cos(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * cos(V)
 */
CGResultNode* cosV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = cos(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * cos(M)
 */
CGResultNode* cosM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = cos(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * tan
 * *********************
 */

/*
 * tan(d)
 */
CGResultNode* tanD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = tan(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * tan(V)
 */
CGResultNode* tanV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = tan(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * tan(M)
 */
CGResultNode* tanM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = tan(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * tanh
 * *********************
 */

/*
 * tanh(d)
 */
CGResultNode* tanhD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = tanh(D->value);
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * tanh(V)
 */
CGResultNode* tanhV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    CGVector* Y = calloc(1, sizeof(CGVector));
    Y->data = y;
    Y->len = V->len;

    uint64_t i = 0;

    for(;i<V->len;i++){
        y[i] = tanh(V->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = Y;

    return result;
}

/*
 * tanh(M)
 */
CGResultNode* tanhM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->rows;
    Y->cols = M->cols;

    uint64_t i = 0;

    for(;i<size;i++){
        y[i] = tanh(M->data[i]);
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * Invert
 * *********************
 */

// TODO

/*
 * *********************
 * Transpose
 * *********************
 */

/*
 * D^t
 */
CGResultNode* transposeD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    CGDouble* R = calloc(1, sizeof(CGDouble));
    R->value = D->value;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = R;

    return result;
}

/*
 * V^t
 */
CGResultNode* transposeV(CGVector* V, CGraph* graph, CGNode* parentNode){
    uint64_t size = V->len;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = V->len;
    Y->cols = 1;
    Y->data = y;

    uint64_t i = 0;
    uint64_t j = 0;

    memcpy(Y->data, V->data, V->len*sizeof(cg_float));

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * M^t
 */
CGResultNode* transposeM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t size = M->rows*M->cols;

    cg_float* y = calloc(size, sizeof(cg_float));
    CGMatrix* Y = calloc(1, sizeof(CGMatrix));
    Y->data = y;
    Y->rows = M->cols;
    Y->cols = M->rows;
    Y->data = y;

    uint64_t i = 0;
    uint64_t j = 0;

    for(;i<M->rows;i++){
        for(j = 0;j<M->cols;j++){
            y[j*Y->cols+i] = M->data[i*M->cols +j];
        }
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_MATRIX;
    result->value = Y;

    return result;
}

/*
 * *********************
 * SUM
 * *********************
 */

/*
 * sum(D)
 */
CGResultNode* sumD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float y = D->value;
    CGDouble* Y = calloc(1, sizeof(CGDouble));
    Y->value = y;

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * sum(V)
 */
CGResultNode* sumV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float y = 0;
    CGDouble* Y = calloc(1, sizeof(CGDouble));

    uint64_t i = 0;

    for(;i<V->len;i++){
        y += V->data[i];
    }

    Y->value = y;
    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_DOUBLE;
    result->value = Y;

    return result;
}

/*
 * TODO:
 * sum(M, axis=axis)
 */

CGResultNode* sumM(CGMatrix* M, CGraph* graph, CGNode* parentNode, uint8_t axis){
    CGVector* V = calloc(1, sizeof(CGVector));

    if(axis == 0){
        uint64_t len = M->cols;
        cg_float* y = calloc(len, sizeof(cg_float));

        V->data = y;
        V->len = len;

        uint64_t i = 0;

        for(;i<M->rows*M->cols;i++){
            y[i%len] += M->data[i];
        }
    }
    else {
        uint64_t len = M->rows;
        cg_float* y = calloc(len, sizeof(cg_float));

        V->data = y;
        V->len = len;

        uint64_t i = 0;

        for(;i<M->rows*M->cols;i++){
            y[i/M->cols] += M->data[i];
        }
    }

    CGResultNode* result = calloc(1, sizeof(CGResultNode));
    result->type = CGVT_VECTOR;
    result->value = V;

    return result;
}

CGResultNode* max(CGNode* X, CGraph* graph){
    CGResultNode* res = computeCGNode(graph, X->axop->uhs);

    switch(res->type){
        case CGVT_DOUBLE:
        {
            cg_float y = 0;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            Y->value = ((CGDouble*)res->value)->value;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_VECTOR:
        {
            CGVector* v = (CGVector*)res->value;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            uint64_t i = 1;
            cg_float m = v->data[0];

            for (;i < v->len; i++){
                if(v->data[i] > m)
                    m = v->data[i];
            }

            Y->value = m;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_MATRIX: {
            CGMatrix* M = (CGMatrix*)res->value;
            CGVector* V = calloc(1, sizeof(CGVector));

            if(X->axop->axis == 0){
                uint64_t len = M->cols;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->cols;i++){
                    y[i] = M->data[i];
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(y[i%len] < M->data[i])
                        y[i%len] = M->data[i];
                }
            }
            else {
                uint64_t len = M->rows;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows;i++){
                    y[i] = M->data[i*M->cols];
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(y[i/M->cols] < M->data[i])
                        y[i/M->cols] = M->data[i];
                }
            }

            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_VECTOR;
            result->value = V;

            return result;

        }
    }
}

CGResultNode* min(CGNode* X, CGraph* graph){
    CGResultNode* res = computeCGNode(graph, X->axop->uhs);

    switch(res->type){
        case CGVT_DOUBLE:
        {
            cg_float y = 0;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            Y->value = ((CGDouble*)res->value)->value;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_VECTOR:
        {
            CGVector* v = (CGVector*)res->value;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            uint64_t i = 1;
            cg_float m = v->data[0];

            for (;i < v->len; i++){
                if(v->data[i] > m)
                    m = v->data[i];
            }

            Y->value = m;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_MATRIX: {
            CGMatrix* M = (CGMatrix*)res->value;
            CGVector* V = calloc(1, sizeof(CGVector));

            if(X->axop->axis == 0){
                uint64_t len = M->cols;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->cols;i++){
                    y[i] = M->data[i];
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(y[i%len] > M->data[i])
                        y[i%len] = M->data[i];
                }
            }
            else {
                uint64_t len = M->rows;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows;i++){
                    y[i] = M->data[i*M->cols];
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(y[i/M->cols] > M->data[i])
                        y[i/M->cols] = M->data[i];
                }
            }

            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_VECTOR;
            result->value = V;

            return result;

        }
    }
}

CGResultNode* argmax(CGNode* X, CGraph* graph){
    CGResultNode* res = computeCGNode(graph, X->axop->uhs);

    switch(res->type){
        case CGVT_DOUBLE:
        {
            cg_float y = 0;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            Y->value = ((CGDouble*)res->value)->value;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_VECTOR:
        {
            CGVector* v = (CGVector*)res->value;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            uint64_t i = 1;
            uint64_t maxIdx = 0;

            for (;i < v->len; i++){
                if(v->data[i] > v->data[maxIdx])
                    maxIdx = i;
            }

            Y->value = maxIdx;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_MATRIX: {
            CGMatrix* M = (CGMatrix*)res->value;
            CGVector* V = calloc(1, sizeof(CGVector));

            if(X->axop->axis == 0){
                uint64_t len = M->cols;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->cols;i++){
                    y[i] = i;
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(M->data[(uint64_t)y[i%len]] < M->data[i])
                        y[i%len] = i;
                }

                // transform vector representation into matrix
                for(i=0;i<M->cols;i++){
                    y[i] = ((int)y[i])/M->cols;
                }
            }
            else {
                uint64_t len = M->rows;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows;i++){
                    y[i] = i*M->cols;
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(M->data[(uint64_t)y[i/M->cols]] < M->data[i])
                        y[i/M->cols] = i;
                }

                // transform vector representation into matrix
                for(i=0;i<M->rows;i++){
                    y[i] = ((int)y[i])%M->cols;
                }
            }

            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_VECTOR;
            result->value = V;

            return result;

        }
    }
}

CGResultNode* argmin(CGNode* X, CGraph* graph){
    CGResultNode* res = computeCGNode(graph, X->axop->uhs);

    switch(res->type){
        case CGVT_DOUBLE:
        {
            cg_float y = 0;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            Y->value = 0;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_VECTOR:
        {
            CGVector* v = (CGVector*)res->value;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            uint64_t i = 1;
            uint64_t minIdx = 0;

            for (;i < v->len; i++){
                if(v->data[i] > v->data[minIdx])
                    minIdx = (cg_float)i;
            }

            Y->value = minIdx;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_MATRIX: {
            CGMatrix* M = (CGMatrix*)res->value;
            CGVector* V = calloc(1, sizeof(CGVector));

            if(X->axop->axis == 0){
                uint64_t len = M->cols;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->cols;i++){
                    y[i] = i;
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(M->data[(uint64_t)y[i%len]] > M->data[i])
                        y[i%len] = (cg_float)i;
                }

                // transform vector representation into matrix
                for(i=0;i<M->cols;i++){
                    y[i] = ((int)y[i])/M->cols;
                }
            }
            else {
                uint64_t len = M->rows;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows;i++){
                    y[i] = i*M->cols;
                }

                for(i=0;i<M->rows*M->cols;i++){
                    if(M->data[(uint64_t)y[i/M->cols]] > M->data[i])
                        y[i/M->cols] = (cg_float)i;
                }

                // transform vector representation into matrix
                for(i=0;i<M->rows;i++){
                    y[i] = ((int)y[i])%M->cols;
                }
            }

            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_VECTOR;
            result->value = V;

            return result;

        }
    }
}

CGResultNode* mean(CGNode* X, CGraph* graph){
    CGResultNode* res = computeCGNode(graph, X->axop->uhs);

    switch(res->type){
        case CGVT_DOUBLE:
        {
            cg_float y = 0;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            Y->value = ((CGDouble*)res->value)->value;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_VECTOR:
        {
            CGVector* v = (CGVector*)res->value;
            CGDouble* Y = calloc(1, sizeof(CGDouble));

            uint64_t i = 0;
            cg_float m = v->data[0];

            for (;i < v->len; i++){
                m += v->data[i];
            }

            m /= v->len;

            Y->value = m;
            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_DOUBLE;
            result->value = Y;

            return result;
        }

        case CGVT_MATRIX: {
            CGMatrix* M = (CGMatrix*)res->value;
            CGVector* V = calloc(1, sizeof(CGVector));

            if(X->axop->axis == 0){
                uint64_t len = M->cols;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows*M->cols;i++){
                    y[i%len] += M->data[i];
                }

                for(i=0;i<M->cols;i++){
                    y[i] /= M->rows;
                }
            }
            else {
                uint64_t len = M->rows;
                cg_float* y = calloc(len, sizeof(cg_float));

                V->data = y;
                V->len = len;

                uint64_t i = 0;

                for(;i<M->rows*M->cols;i++){
                    y[i/M->cols] += M->data[i];
                }

                for(i=0;i<M->rows;i++){
                    y[i] /= M->cols;
                }
            }

            CGResultNode* result = calloc(1, sizeof(CGResultNode));
            result->type = CGVT_VECTOR;
            result->value = V;

            return result;

        }
    }
}

/*
 * relu(d)
 */
CGResultNode* reluD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float val = D->value;
    return makeDoubleResultNode(__cg_relu(val));
}

/*
 * relu(V)
 */
CGResultNode* reluV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    __cg_map_array(V->data, y, V->len, __cg_relu);
    return makeVectorResultNode(V->len, y);
}

/*
 * relu(M)
 */
CGResultNode* reluM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t len = M->cols*M->rows;
    cg_float* y = calloc(len, sizeof(cg_float));
    __cg_map_array(M->data, y, len, __cg_relu);
    return makeMatrixResultNode(M->rows, M->cols, y);
}

/*
 * softplus(d)
 */
CGResultNode* softplusD(CGDouble* D, CGraph* graph, CGNode* parentNode){
    cg_float val = D->value;
    return makeDoubleResultNode(__cg_softplus(val));
}

/*
 * softplus(V)
 */
CGResultNode* softplusV(CGVector* V, CGraph* graph, CGNode* parentNode){
    cg_float* y = calloc(V->len, sizeof(cg_float));
    __cg_map_array(V->data, y, V->len, __cg_softplus);
    return makeVectorResultNode(V->len, y);
}

/*
 * softplus(M)
 */
CGResultNode* softplusM(CGMatrix* M, CGraph* graph, CGNode* parentNode){
    uint64_t len = M->cols*M->rows;
    cg_float* y = calloc(len, sizeof(cg_float));
    __cg_map_array(M->data, y, len, __cg_softplus);
    return makeMatrixResultNode(M->rows, M->cols, y);
}

#if cg_float == float
#undef cblas_dcopy
#undef cblas_dscal
#undef cblas_dgemm
#undef cblas_ddot
#undef cblas_dgemv
#endif


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

#define NCols 5
#define Nrows 4

float A[] = { 8, 4, 7, 3, 5, 1, 1, 3, 2, 1, 2, 3, 2, 0, 1, 1 , 2, 3, 4, 1};

float x[] = { -1, 2, -1, 1, 2 };

float y[Nrows];
float alpha = 1.0, beta = 0.0;
char tbuf[1024];
int main() {
    int i, j;

    // Print original matrix

    // y = Ax
    cblas_sgemv(CblasRowMajor, CblasNoTrans, Nrows, NCols, alpha, A, NCols, x, 1, beta, y, 1);
    // Print resulting vector
    for (j = 0; j < Nrows; j++) {
        printf(" %f\n", y[j]);
    }

    cblas_sgemv(CblasColMajor, CblasNoTrans, Nrows, NCols, alpha, A, Nrows, x, 1, beta, y, 1);
    // Print resulting vector
    for (j = 0; j < Nrows; j++) {
        printf(" %f\n", y[j]);
    }

    return 0;
}


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch, M. Oliveira

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2, or (at your option)
 any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 02110-1301, USA.

*/

#include <config.h>

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

#include <gsl/gsl_sf.h>
#include <gsl/gsl_combination.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "string_f.h"

#include <fortran_types.h>

/* Numerical threshold for oct_bessel_k0 and oct_bessel_k1 */
#define  BESSEL_K_THRES  1.0e2

/* ---------------------- Interface to GSL functions ------------------------ */

/* Special Functions */
double FC_FUNC_(oct_gamma, OCT_GAMMA)
     (const double *x)
{
  return gsl_sf_gamma(*x);
}

double FC_FUNC_(oct_incomplete_gamma, OCT_INCOMPLETE_GAMMA)
     (const double *a, const double *x)
{
  return gsl_sf_gamma_inc_Q(*a, *x);
}

double FC_FUNC_(oct_sph_bessel, OCT_SPH_BESSEL)
     (const fint *l, const double*x)
{
  return gsl_sf_bessel_jl(*l, *x);
}

double FC_FUNC_(oct_bessel, OCT_BESSEL)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_Jn(*n, *x);
}

double FC_FUNC_(oct_bessel_in, OCT_BESSEL_IN)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_In(*n, *x);
}

double FC_FUNC_(oct_bessel_j0, OCT_BESSEL_J0)
     (const double *x)
{
  return gsl_sf_bessel_J0(*x);
}

double FC_FUNC_(oct_bessel_j1, OCT_BESSEL_J1)
     (const double *x)
{
  return gsl_sf_bessel_J1(*x);
}

double FC_FUNC_(oct_bessel_k0, OCT_BESSEL_K0)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K0(*x);
  }
}

double FC_FUNC_(oct_bessel_k1, OCT_BESSEL_K1)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K1(*x);
  }
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erfc, OCT_ERFC)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  0.0;
  if(*x < -20.0) return  2.0;
  /* otherwise call gsl */
  return gsl_sf_erfc(*x);
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erf, OCT_ERF)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  1.0;
  if(*x < -20.0) return -1.0;
  /* otherwise call gsl */
  return gsl_sf_erf(*x);
}

double FC_FUNC_(oct_legendre_sphplm, OCT_LEGENDRE_SPHPLM)
     (const fint *l, const int *m, const double *x)
{
  return gsl_sf_legendre_sphPlm(*l, *m, *x);
}

/* generalized Laguerre polynomials */
double FC_FUNC_(oct_sf_laguerre_n, OCT_SF_LAGUERRE_N)
     (const int *n, const double *a, const double *x)
{
  return gsl_sf_laguerre_n(*n, *a, *x);
}

/* Combinations */

void FC_FUNC_(oct_combination_init, OCT_COMBINATION_INIT)
     (gsl_combination **c, const fint *n, const fint *k)
{
  *c = gsl_combination_calloc (*n, *k);
}

void FC_FUNC_(oct_combination_next, OCT_COMBINATION_NEXT)
     (gsl_combination **c, fint *next)
{
  *next = gsl_combination_next (((gsl_combination *)(*c)));
}

void FC_FUNC_(oct_get_combination, OCT_GET_COMBINATION)
     (gsl_combination **c, fint *comb)
{
  int i;
  for (i=0;i< ((gsl_combination *)(*c))->k; i++) {
    comb[i]=(fint)((gsl_combination *)(*c))->data[i];
  }
}

void FC_FUNC_(oct_combination_end, OCT_COMBINATION_END)
     (gsl_combination **c)
{
  gsl_combination_free (((gsl_combination *)(*c)));
}

/* Random Number Generation */
void FC_FUNC_(oct_ran_init, OCT_RAN_INIT)
     (gsl_rng **r)
{
  gsl_rng_env_setup();
  *r = gsl_rng_alloc(gsl_rng_default);
}

void FC_FUNC_(oct_ran_end, OCT_RAN_END)
     (gsl_rng **r)
{
  gsl_rng_free(*r);
}

/* Random Number Distributions */
double FC_FUNC_(oct_ran_gaussian, OCT_RAN_GAUSSIAN)
     (const gsl_rng **r, const double *sigma)
{
  return gsl_ran_gaussian(*r, *sigma);
}

double FC_FUNC_(oct_ran_flat, OCT_RAN_FLAT)
     (const gsl_rng **r, const double *a, const double *b)
{
  return gsl_ran_flat(*r, *a, *b);
}

void FC_FUNC_(oct_strerror, OCT_STRERROR)
     (const fint *err, STR_F_TYPE res STR_ARG1)
{
  const char *c;

  c = gsl_strerror(*err);
  TO_F_STR1(c, res);
}