/* 
   This is C-language implementation of the mex-function mlp_cgd.
   It can be compiled by using the mex-command from the Matlab 
   command line:  mex mlp_cgd.c
  
   - Study the gateway function and try to understand what
     is done in different stages of mexFunction.
   - The purpose of this mex-function is to compute value of the
     error function and its gradient. It performs exactly the same
     operations as the function mlp_cgd.m from Exercise 9&10. 
   - value of the functional is stored in f and the
     gradient in fg.
     */

#include "mex.h"
#include <math.h>

void fillve (double *v, int n, double val)
{
  int i;
  
  for (i=0; i<n; i++) {
    *(v+i) = val;
  }
  return;
}

void vec_cpy (double *x, double *y, int n)
{
  int i;
  
  for (i=0; i<n; i++) {
    *(y+i) = *(x+i);
  }
  return;
}

/* sigmoid_der

   Applies the sigmoid function and its derivative to the given 
   vector ol. The results are returned in vectors ol and dl.
   */
void sigmoid_der (double *ol, double *dl, double *k, int n, int nk)
{
  int i;
  double t;
  
  if (nk==1) {
    for (i=0; i<n; i++) {
      t = *(k) * *(ol + i);
      *(ol+i) = 1.e0/(1.e0 + exp(-t));
      *(dl+i) = *(k) * *(ol+i) * (1.e0 - *(ol+i));
    }
  }
  else if (nk==n) {
    for (i=0; i<n; i++) {
      t = *(k+i) * *(ol + i);
      *(ol+i) = 1.e0/(1.e0 + exp(-t));
      *(dl+i) = *(k+i) * *(ol+i) * (1.e0 - *(ol+i));
    }
  }
  return;
}

/* ext_matvec

   Multiplies the matrix w with the extension of the vector x.
   Returns the result in y. This function is used by mlp_out.
   */
void ext_matvec (double *w, double *x, double *y, int rows, int cols)
{
  int i, j, count=0, c_row, c_col = 0;
  double t;
  
  vec_cpy (w, y, rows);
  
  for (j=0; j<cols; j++) {
    c_row = 0;
    for (i=0; i<rows; i++) {
      *(y+c_row) += *(w+rows+count) * *(x+c_col);
      c_row++;
      count++;
    }
    c_col++;
  }
  return;
}

/* ext_outprod

   Perform an extended version of the outer product of the vectors
   v1 and v2. The result is added to the matrix mtx.
   */
void ext_outprod (double *mtx, double *v1, double *v2,
                  int n1, int n2, double sc)
{
  int i, j, count;
  
  for (i=0; i<n1; i++) {
    *(mtx+i) += sc * *(v1+i);
  }
  
  count = n1;
  for (j=0; j<n2; j++) {
    for (i=0; i<n1; i++) {
      *(mtx+count) += sc * *(v1+i) * *(v2+j);
      count++;
    }
  }
  return;
}

/* diag_mul

   Multiplies vector v with a diagonal matrix whose diagonal elements
   are in vector d. Result is given in v.
   */
void diag_mul (double *d, double *v, int n)
{
  int i;
  
  for (i=0; i<n; i++) {
    *(v+i) = *(d+i) * *(v+i);
  }
  return;
}

/* transmul

   Multiplies vector v with the transpose of the matrix w.
   Result is given wv.
   */
void transmul (double *w, double *v, double *wv, int nr, int nc)
{ 
  int i, j;
  double tmp;
  
  for (j=0; j<nc; j++) {
    tmp = 0.e0;
    for (i=0; i<nr; i++) {
      tmp += *(w+j*nr+i) * *(v+i);
    }
    *(wv+j) = tmp;
  }
  return;
}

/* mlp_out

   Computes the output of an MLP network.
   */
void mlp_out (double *o, double *o1, double *d1, double *x, 
	      double *v, double *k, int n0, int n1, int n2, int nk)
{  
  double *w1, *w2;
  w1 = v;
  w2 = v + (n0+1)*n1;
  
  ext_matvec (w1, x, o1, n1, n0);
  sigmoid_der (o1, d1, k, n1, nk);
  ext_matvec (w2, o1, o, n2, n1);
  return;
}

/* The gateway function of the mex-function mlp_cgd. */
void mexFunction( int nlhs, mxArray *plhs[],
		  int nrhs, const mxArray *prhs[] )
{
  int N, cols, v_cols, v_rows;
  double *v, *k, *x, *y, *f, *fg, *tmp, *o, *o1, *d1, *e;
  double np, c;
  int i, j, l, n, m, n0, n1, n2, nk;
  
  /* Check for proper number of arguments */
  if (nrhs != 5) {
    mexErrMsgTxt("Five inputs in the form (v,k,np,x,y) required.");}
  else if (nlhs > 2) {
    mexErrMsgTxt("At most two outputs in the form [f,fg] allowed.");
  }
  
  /* Grab out the inputs v=prhs[0], k=prhs[1], np=prhs[2],
     x=prhs[3], y=prhs[4]. */
  v = mxGetPr(prhs[0]);
  v_cols = mxGetN(prhs[0]);
  v_rows = mxGetM(prhs[0]);
  if (v_cols != 1) {
    mexErrMsgTxt("Input v must be a column vector.");}
  
  np = mxGetScalar(prhs[2]);
  n1 = (int)np;
  
  k = mxGetPr(prhs[1]);
  if (mxGetN(prhs[1]) != 1) {
    mexErrMsgTxt("Input k must be a column vector.");}
  nk = mxGetM(prhs[1]);
  if (nk != 1 && nk != n1) {
    mexErrMsgTxt("Input k has invalid length.");}
  
  x = mxGetPr(prhs[3]);
  N = mxGetN(prhs[3]);
  n0 = mxGetM(prhs[3]);
  
  y = mxGetPr(prhs[4]);
  cols = mxGetN(prhs[4]);
  n2 = mxGetM(prhs[4]);
  if (N != cols) {
    mexErrMsgTxt("Inputs x and y have incompatible sizes.");}
  if (n1*(n0+1) + n2*(n1+1) != v_rows) {
    mexErrMsgTxt("Inputs v and x and y have incompatible sizes.");}
  
  /* Create the outputs f and fg. */
  plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
  f = mxGetPr(plhs[0]);
  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleMatrix(v_rows,v_cols,mxREAL);
    fg = mxGetPr(plhs[1]);
  }

  o   = mxCalloc(n2,sizeof(double));
  e   = mxCalloc(n2,sizeof(double));
  o1  = mxCalloc(n1,sizeof(double));
  d1  = mxCalloc(n1,sizeof(double));
  tmp = mxCalloc(n1,sizeof(double));
  
  /* Call the C subroutines. */
  c    = 1.e0/N;
  *(f) = 0.e0;
  
  if (nlhs > 1) {
    fillve(fg,v_rows,0.e0);
  }
  
  for (n=0; n<N; n++) {
    mlp_out(o,o1,d1,x+n*n0,v,k,n0,n1,n2,nk);
    
    for (i=0; i<n2; i++) {
      *(e+i) = *(o+i) - *(y+n*n2+i);
      *(f)  += *(e+i) * *(e+i);
    }
    
    if (nlhs > 1) {
      transmul(v+n1*(n0+1)+n2,e,tmp,n2,n1);
      diag_mul(d1,tmp,n1);
      ext_outprod(fg,tmp,x+n*n0,n1,n0,c);
      ext_outprod(fg+n1*(n0+1),e,o1,n2,n1,c);
    }
  }
  
  *(f) = 0.5e0*c * *(f);
}