
#include "multilayerffnn.h"
#include "controller_misc.h"

using namespace matrix;
using namespace std;

/// initialisation of the network with the given number of input and output units
void MultiLayerFFNN::init(unsigned int inputDim, unsigned  int outputDim) {
  assert(!initialised && layers.size() > 0);
  int last = layers.size() - 1;
  layers[last].size = outputDim; // adjust output dimension
  
  Matrix w(layers[0].size, inputDim);  
  w+=w.map(random_minusone_to_one) * 0.05;
  weights.push_back(w);  
  bias.push_back(Matrix(layers[0].size, 1));
  for(unsigned int i = 1; i < layers.size(); i++) {
    Matrix w(layers[i].size, layers[i-1].size);
    w+=w.map(random_minusone_to_one) * 0.05;
    weights.push_back(w);  
    bias.push_back(Matrix(layers[i].size, 1));
  }
  
  initialised = true;
}

/// passive processing of the input
const Matrix MultiLayerFFNN::process (const Matrix& input) const {
  assert(initialised);
  assert(weights.size() == layers.size());
  assert(bias.size() == layers.size());

  Matrix y = (weights[0] * input + bias[0]).map(layers[0].actfun);
  for(unsigned int i = 1; i < layers.size(); i++) {
    y = (weights[i] * y + bias[i]).map(layers[i].actfun);
  }  
  return y;
}

/// performs learning and returns the network output before learning
const Matrix MultiLayerFFNN::learn (const Matrix& input, 
				  const Matrix& nom_output, 
				  double learnRateFactor) {    
  assert(initialised);
  assert(initialised);
  assert(weights.size() == layers.size());
  assert(bias.size() == layers.size());

  int layernum  = layers.size();
  double epsilon         = eps*learnRateFactor;

  // calculate outputs (y's) and activations (z's), which are necessary for activation'()
  vector<Matrix> ys;
  vector<Matrix> zs;
  ys.resize(layernum);
  zs.resize(layernum);
  zs[0]     = weights[0] * input + bias[0];
  ys[0]     = zs[0].map(layers[0].actfun);    
  for(int i = 1; i < layernum; i++) {
    zs[i] = weights[i] * ys[i-1] + bias[i];
    ys[i] = zs[i].map(layers[i].actfun);
  }  

  // calculate weight updates
  Matrix delta;
  for( int i = layernum-1; i >= 0; i--) {    
    const Matrix& xsi = ( i == layernum-1 ) 
      ? nom_output - ys[layernum-1] 
      : (weights[i+1]^T) * delta;

    if(i!=0){ // all but first layer
      const Matrix& g_prime = zs[i].map(layers[i].dactfun);
      delta           = xsi.multrowwise(g_prime);    
      weights[i]     += delta * (ys[i-1]^T) * epsilon; 
      bias[i]        += delta * epsilon * layers[i].factor_bias;    
    }else{ // first layer sees real input (linear)
      weights[i]     += xsi * (input^T) * epsilon; 
      bias[i]        += xsi * epsilon * layers[i].factor_bias;      
    }
  }   
  
  return ys[layernum-1];
}

void MultiLayerFFNN::damp(double damping){
  unsigned int len = weights.size();
  for(unsigned int i = 0; i < len; i++) {    
    weights[i] *= (1-damping);
    bias[i]    *= (1-damping);
  }
}
