/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   ANY COMMERCIAL USE FORBIDDEN!                                         *
 *   LICENSE:                                                              *
 *   This work is licensed under the Creative Commons                      *
 *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
 *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
 *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
 *   San Francisco, California, 94105, USA.                                *
 *                                                                         *
 *   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.                  *
 *                                                                         *
 *   $Log: invertmotornstep.cpp,v $
 *   Revision 1.25  2006/08/02 09:30:47  martius
 *   calcErrorFactor moved to invertmotorcontroller
 *
 *   Revision 1.24  2006/07/20 17:14:34  martius
 *   removed std namespace from matrix.h
 *   storable interface
 *   abstract model and invertablemodel as superclasses for networks
 *
 *   Revision 1.23  2006/07/18 15:17:16  martius
 *   buffer operations like delayed values and smoothed values moved to invertmotorcontroller
 *
 *   Revision 1.22  2006/07/18 14:48:02  martius
 *   new regularisation of g' and g''/g' and squashP moved to invertmotorcontroller
 *
 *   Revision 1.21  2006/07/14 12:23:58  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.19.6.16  2006/07/13 14:26:50  der
 *   error factor calculation fixed
 *
 *   Revision 1.19.6.15  2006/07/10 13:05:16  martius
 *   NON-COMMERICAL LICENSE added to controllers
 *
 *   Revision 1.19.6.14  2006/07/10 11:59:23  martius
 *   Matrixlib now in selforg
 *   no namespace std in header files
 *
 *   Revision 1.19.6.13  2006/06/25 21:56:07  martius
 *   configureable has name and revision
 *
 *   Revision 1.19.6.12  2006/03/29 15:12:05  martius
 *   damping for Model
 *
 *   Revision 1.19.6.11  2006/02/24 14:46:44  martius
 *   removed gR
 *
 *   Revision 1.19.6.10  2006/02/20 12:15:01  martius
 *   squashsize comes from invertcontroller and is configurable
 *
 *   Revision 1.19.6.9  2006/02/20 10:55:31  martius
 *   error function modifier for model and controller independent
 *
 *   Revision 1.19.6.8  2006/02/08 15:16:09  martius
 *   hack for teaching straight driving for nimm2
 *
 *   Revision 1.19.6.7  2006/01/31 15:30:23  martius
 *   model does not learn if too large error.
 *   S matrix is tweaked for special proactive test case
 *
 *   Revision 1.19.6.6  2006/01/18 16:49:34  martius
 *   new Update algo (simpler and hopefully better)
 *
 *   Revision 1.19.6.5  2006/01/17 16:58:40  martius
 *   loading and storing
 *
 *   Revision 1.19.6.4  2006/01/13 12:24:22  martius
 *   motor teaching included
 *
 *   Revision 1.19.6.3  2005/11/21 17:04:16  martius
 *   calcCandHUpdates is a separat function with delay option
 *   removed bug with x_smooth in learnController (now calcCandHUpdates)
 *    which was not using x values from different time steps
 *
 *   Revision 1.19.6.2  2005/11/16 11:24:26  martius
 *   moved to selforg
 *
 *   Revision 1.19.6.1  2005/11/14 15:38:17  martius
 *   moved to selforg
 *
 *   Revision 1.19  2005/11/04 14:40:43  martius
 *   someInternalParams typo in constructor fixed
 *
 *   Revision 1.18  2005/10/27 15:46:38  martius
 *   inspectable interface is expanded to structural information for network visualiser
 *
 *   Revision 1.17  2005/10/25 19:44:52  martius
 *   using fabs instead of abs
 *
 *   Revision 1.16  2005/10/21 16:02:49  martius
 *   added additional model matrix S (switch in contructor)
 *
 *   Revision 1.15  2005/10/21 13:05:29  martius
 *   norm dependend squashSize again
 *
 *   Revision 1.14  2005/10/21 11:55:13  martius
 *   id is now toId
 *   relativeE can be switched on
 *   uses an symmetric but exact formula for shift calcuation through neuron
 *   However g'/g'' stays -2g because implementation was making trouble
 *
 *   Revision 1.13  2005/10/06 17:08:07  martius
 *   switched to stl lists
 *   Bias noise
 *   correct error_factor for controller learning
 *
 *   Revision 1.12  2005/09/27 11:01:38  martius
 *   error_factor (from logaE or rootE) is calculated onces
 *
 *   Revision 1.11  2005/09/22 07:22:49  martius
 *   proper destruction
 *
 *   Revision 1.10  2005/09/21 08:32:38  fhesse
 *   norm of xsi (xsi^T*xsi) now plotted as internal parameter
 *
 *   Revision 1.9  2005/09/21 07:31:10  martius
 *   v does not go through g
 *
 *   Revision 1.8  2005/09/12 16:06:52  martius
 *   bug in H_update fixed
 *
 *   Revision 1.7  2005/09/11 18:45:15  martius
 *   adaptation of epsC relative to normC
 *
 *   Revision 1.6  2005/08/22 20:31:40  martius
 *   compile problem fixed
 *
 *   Revision 1.5  2005/07/29 08:38:45  martius
 *   adaptation of learnrate back to squashing related mode (as before)
 *
 *   Revision 1.4  2005/07/28 11:17:10  martius
 *   there is a switch for exporting just some of the internal parameters
 *   learningrate update is proportional matrix norm (not squash size as before)
 *
 *   Revision 1.3  2005/07/27 15:44:23  martius
 *   learning rate adaptation
 *
 *   Revision 1.2  2005/07/21 15:07:37  martius
 *   working version
 *   squashing is normalised
 *   learningRate adaptation prepared, but commented
 *
 *   Revision 1.1  2005/07/19 09:35:16  martius
 *   invert motor space controller that iterates
 *    over multiple time steps in past
 *
 *
 ***************************************************************************/

#include "invertmotornstep.h"
using namespace matrix;
using namespace std;

InvertMotorNStep::InvertMotorNStep( const InvertMotorNStepConf& conf) 
  : InvertMotorController(conf.buffersize, "InvertMotorNStep", "$Id: invertmotornstep.cpp,v 1.25 2006/08/02 09:30:47 martius Exp $"), conf(conf) {

  BNoiseGen = 0;
};


InvertMotorNStep::~InvertMotorNStep(){
  if(x_buffer && y_buffer && eta_buffer){
    delete[] x_buffer;
    delete[] y_buffer;
    delete[] eta_buffer;
  }
  if(BNoiseGen) free(BNoiseGen);
}


void InvertMotorNStep::init(int sensornumber, int motornumber){

  number_motors  = motornumber;
  number_sensors = sensornumber;  
  
  //  z.set(number_motors,1);
  A.set(number_sensors, number_motors);
  if (conf.useS) S.set(number_sensors, number_sensors);
  C.set(number_motors,  number_sensors);
  H.set(number_motors,  1);
  B.set(number_sensors, 1);
  R.set(number_motors, number_motors);
  SmallID.set(number_motors, number_motors);
  SmallID.toId();
  SmallID *= 0.001;

  xsi.set(number_sensors,1);
  xsi_norm=0;
  xsi_norm_avg=0.2;
  pain=0;

  BNoiseGen = new WhiteUniformNoise();
  BNoiseGen->init(number_sensors);

  A.toId(); // set A to identity matrix;
  if (conf.useS) S.toId(); // set S to identity matrix;

  // initialise the C matrix with identity + noise (-conf.cNonDiag, conf.cNonDiag) scaled to cInit value
  C = ((C^0) + C.map(random_minusone_to_one) * conf.cNonDiag) * conf.cInit;   

  x_buffer = new Matrix[buffersize];
  y_buffer = new Matrix[buffersize];
  eta_buffer = new Matrix[buffersize];
  for (unsigned int k = 0; k < buffersize; k++) {
    x_buffer[k].set(number_sensors,1);
    y_buffer[k].set(number_motors,1);
    eta_buffer[k].set(number_motors,1);
  }
  y_teaching.set(number_motors, 1); 

  zero_eta.set(number_motors, 1); 
  initialised = true;
}


/// performs one step (includes learning). Calculates motor commands from sensor inputs.
void InvertMotorNStep::step(const sensor* x_, int number_sensors, 
			    motor* y_, int number_motors){
  fillBuffersAndControl(x_, number_sensors, y_, number_motors);
  if(t>buffersize){
    int delay = max(int(s4delay)-1,0);   
    calcXsi(delay);            // calculate the error (use delayed y values)
    calcEtaAndBufferIt(delay);  
    // learn controller with effective input/output
    learnController();
    
    // learn Model with actual sensors and with effective motors;
    learnModel(delay);
  }
  // update step counter
  t++;
};

/// performs one step without learning. Calulates motor commands from sensor inputs.
void InvertMotorNStep::stepNoLearning(const sensor* x, int number_sensors, 
				            motor*  y, int number_motors ){
  fillBuffersAndControl(x, number_sensors, y, number_motors);
  // update step counter
  t++;
};

void InvertMotorNStep::fillBuffersAndControl(const sensor* x_, int number_sensors, 
					      motor* y_, int number_motors){
  assert((unsigned)number_sensors == this->number_sensors 
	 && (unsigned)number_motors == this->number_motors);
  
  Matrix x(number_sensors,1,x_);
  
  // put new input vector in ring buffer x_buffer
  putInBuffer(x_buffer, x);
  
  // averaging over the last s4avg values of x_buffer
  x_smooth = calculateSmoothValues(x_buffer, t < s4avg ? 1 : int(max(1.0,s4avg)));
  
  // calculate controller values based on smoothed input values
  Matrix y = calculateControllerValues(x_smooth);

  // put new output vector in ring buffer y_buffer
  putInBuffer(y_buffer, y);

  // convert y to motor* 
  y.convertToBuffer(y_, number_motors);
}


double regularizedInverse(double v){
  return 1/(fabs(v)+0.1);
}
  
/// calculates Eta which corrensponds to xsi_t which means eta_{t-1}
//  @param delay 0 for no delay and n>0 for n timesteps delay in the SML
void InvertMotorNStep::calcEtaAndBufferIt(int delay){ 
  // eta = A^-1 xsi (first shift in motor-space at current time) 
  //                
  //  we use pseudoinverse U=A^T A -> eta = U^-1 A^T xsi // TODO add 0.01*I
  Matrix eta = (A.multTM()^-1) * ( (A^T) * xsi ); 
  if(relativeE != 0) { // divide eta by |y|  == relative error
    const Matrix& y = y_buffer[t % buffersize];    
    eta = eta.multrowwise(y.map(regularizedInverse));
  }
  eta_buffer[(t-1)%buffersize] = eta; // Todo: work with the reference
}

/// calculates xsi for the current time step using the delayed y values
//  @param delay 0 for no delay and n>0 for n timesteps delay in the time loop 
void InvertMotorNStep::calcXsi(int delay){ 
  const Matrix& x = x_buffer[t % buffersize];
  const Matrix& y = y_buffer[(t - 1 - delay) % buffersize];
  if(conf.useS){
    Matrix x_tm1 = x_buffer[(t-1) % buffersize];
    x_tm1.val(0,0)=0; // FIXME: quick hack to get rid of the sensor value itself and just let the derivative in
    xsi = x -  (A * y + S * x_tm1 + B);        
  } else {
    xsi = x -  (A * y + B);        
  }
  xsi_norm = matrixNorm1(xsi);
}

/// learn values H,C
// This is the implementation uses a better formula for g^-1 using Mittelwertsatz
void InvertMotorNStep::learnController(){ 
  
  Matrix C_update(C.getM(), C.getN());
  Matrix H_update(H.getM(), H.getN());
  calcCandHUpdates(C_update, H_update, 0);
  if(conf.useTeaching){    
    calcCandHUpdatesTeaching(C_update, H_update, 0);
  }

  if(adaptRate>0){ // adapt learning rate   
    double norm = matrixNorm1(C); // + calcMatrixNorm(H);
    double update_norm = matrixNorm1(C_update) + matrixNorm1(H_update)*0.05;

    epsC = adapt(epsC, update_norm, norm*nomUpdate, adaptRate, adaptRate*10);  
    epsC = min( 5.0, epsC);
  }
  updateCandH(C_update, H_update, squashSize);  
};

/// calculates the Update for C and H 
// @param y_delay timesteps to delay the y-values.  (usually 0)
//  Please note that the delayed values are NOT used for the error calculation 
//  (this is done in calcXsi())
void InvertMotorNStep::calcCandHUpdates(Matrix& C_update, Matrix& H_update, int y_delay){ 
  assert( steps + y_delay < buffersize);
  // Matrix& eta = zero_eta;
  // Matrix v_old = (eta_buffer[t%buffersize]).map(g);
  Matrix v = zero_eta;
  for(unsigned int s = 1; s <= steps; s++){
    const Matrix& eta = eta_buffer[(t-s)%buffersize].map(g);
    
    //const Matrix& x      = A * z.map(g) + xsi; 
    //    z                    = R * z.map(g) + H + C*xsi; // z is a dynamic itself (not successful)
    const Matrix& x          = x_buffer[(t-s-y_delay)%buffersize];
    const Matrix& z          = C * x + H;
    const Matrix shift       = (eta + v).map(g);
    const Matrix g_prime     = Matrix::map2(g_s, z, shift);
    const Matrix g_prime_inv = g_prime.map(one_over);
    const Matrix g_2p_div_p  = Matrix::map2(g_2s_div_s, z, shift);
    
    const Matrix zeta = shift.multrowwise(g_prime_inv); // G'(Z)^-1 * (eta+v)
    R                 = C * A;
    const Matrix chi  = ((R.multMT()+SmallID)^-1) * zeta;
    v                 = (R^T) * chi;
    const Matrix rho  = g_2p_div_p.multrowwise(chi.multrowwise(zeta)) * -1;
    
    
    // calculate updates of H,C
    // delta C += eps * (zeta * v^T * A^T + beta * x^T)    
    //  q_prime_inv.elementProduct is the correction of negled terms 
    //   (LL^T)^-1 from learning rule, which drive the system out of saturation. (important)
    // double q_prime_invers = q_prime_inv.elementProduct();
    C_update += ( chi*(v^T)*(A^T) - rho*(x^T) ) * epsC;
    H_update += rho * -epsC;
  }

  // we are just using the last shift here! Is this of any problem.
  double error_factor = calcErrorFactor(v, (logaE & 1) !=0, (rootE & 1) !=0); 
  C_update *= error_factor;
  H_update *= error_factor;
};

/// calculates the Update for C and H using the teaching signal
//  @param y_delay timesteps to delay the y-values.  (usually 0)
//  Please note that the delayed values are NOT used for the error calculation 
//  (this is done in calcXsi())
void InvertMotorNStep::calcCandHUpdatesTeaching(Matrix& C_update, Matrix& H_update, int y_delay){ 
  assert( steps + y_delay < buffersize);
  const Matrix& y = y_buffer[(t)% buffersize]; // eventuell t-1
  const Matrix eta = y_teaching - y;
  
  const Matrix& x = x_buffer[(t)%buffersize];
  const Matrix z  = C * x + H; 
  const Matrix g_          = z.map(g);
  const Matrix g_prime     = Matrix::map2(g_s, z, eta);
  const Matrix g_prime_inv = g_prime.map(one_over);
  const Matrix g_2p_div_p  = Matrix::map2(g_2s_div_s, z, eta);
  R = C * A;
  const Matrix zeta = eta.multrowwise(g_prime_inv);
  const Matrix chi = (R.multMT()^-1) * zeta;
  const Matrix v   = ((R + SmallID)^-1) * zeta;
  
  const Matrix rho = chi.multrowwise(zeta).multrowwise(g_2p_div_p) * -1;
  C_update = (chi * (v^T) * (A^T) + (chi*teacher - rho)*(x^T)) * epsC * 0.1;
  H_update = (chi*teacher - rho) * epsC * 0.1;
    
  //  we use error_factor of 1 for teaching
  //  C_update *= error_factor; 
  //  H_update *= error_factor;  
};


void InvertMotorNStep::updateCandH(const Matrix& C_update, const Matrix& H_update, double _squashSize){
  C += C_update.mapP(&_squashSize, squash);
  double H_squashSize = _squashSize*10;
  H += H_update.mapP(&H_squashSize, squash);  
}


// normal delta rule (xsi is assumed to be already up to date)
void InvertMotorNStep::learnModel(int delay){
  const Matrix& y = y_buffer[(t - 1 - delay) % buffersize];
  xsi_norm_avg = xsi_norm_avg*0.999 + xsi_norm*0.001; // calc longterm average

  if(xsi_norm > 5*xsi_norm_avg){
    pain= 1; //xsi_norm/ xsi_norm_avg/5;
  } else {
    pain = 0; //pain > 1 ? pain*0.9: 0;  
    //    double error_factor = calcErrorFactor(xsi, true, false); // logarithmic error  
    double error_factor = calcErrorFactor(xsi, (logaE & 2) != 0, (rootE & 2) != 0); // 
    Matrix A_update;  
    Matrix S_update;
    Matrix B_update;
    A_update=(( xsi*(y^T) ) * (epsA * error_factor)); 
    
    if(conf.useS){
      Matrix x_tm1 = x_buffer[(t - 1) % buffersize];
      for(int i; i < number_sensors; i++){
	if(i%2==0)
	  x_tm1.val(i,0)=0; // FIXME: quick hack to get rid of the sensor value itself and just let the derivative in
      }
      S_update=(( xsi*(x_tm1^T) ) * (epsA * error_factor)); 
    }
    
    Matrix B_noise  = noiseMatrix(B.getM(),B.getN(), *BNoiseGen, -noiseB, noiseB); // noise for bias
    B_update=( xsi * (epsA * factorB * error_factor)) + B_noise;
  
    if(adaptRate!=0){
      double normA = matrixNorm1(A);
      epsA=adaptMinMax(epsA, matrixNorm1(A_update) , normA/2500, normA/250, adaptRate, adaptRate*5);
      epsA = min( 2.0, epsA);  
    }
    
    A += A_update.mapP(&squashSize, squash);
    B += B_update.mapP(&squashSize, squash);
    
    if(dampA){
      A *= 1 - dampA * epsA;
      B *= 1 - dampA * epsA * factorB;
    }

    if(conf.useS) S += S_update.mapP(&squashSize, squash);
  }
};

/// calculate controller outputs 
/// @param x_smooth smoothed sensors Matrix(number_channels,1) 
Matrix InvertMotorNStep::calculateControllerValues(const Matrix& x_smooth){
  return (C*x_smooth+H).map(g); 
};


void InvertMotorNStep::getLastMotors(motor* motors, int len){
  const Matrix& y = y_buffer[t%buffersize];
  y.convertToBuffer(motors, len);
}


/** stores the controller values to a given file. */
bool InvertMotorNStep::store(FILE* f) const {  
  // save matrix values
  C.store(f);
  H.store(f);
  A.store(f);
  B.store(f);
  S.store(f);
  // TODO: restore parameters!
  return true;
}

/** loads the controller values from a given file. */
bool InvertMotorNStep::restore(FILE* f){
  // save matrix values
  C.restore(f);
  H.restore(f);
  B.restore(f);
  S.restore(f);
  // TODO: restore parameters!
  t=0; // set time to zero to ensure proper filling of buffers
  return true;
}

list<Inspectable::iparamkey> InvertMotorNStep::getInternalParamNames() const {
  list<iparamkey> keylist;
  if(conf.someInternalParams){
    keylist += store4x4AndDiagonalFieldNames(A, "A");
    if(conf.useS) keylist += store4x4AndDiagonalFieldNames(S, "S");
    keylist += store4x4AndDiagonalFieldNames(C, "C");
    keylist += store4x4AndDiagonalFieldNames(R, "R");    
  }else{
    keylist += storeMatrixFieldNames(A, "A");
    if(conf.useS) keylist += storeMatrixFieldNames(S, "S");
    keylist += storeMatrixFieldNames(C, "C");
    keylist += storeMatrixFieldNames(R, "R");
  }
  keylist += storeVectorFieldNames(H, "H");
  keylist += storeVectorFieldNames(B, "B");
  keylist += string("epsA");
  keylist += string("epsC");
  keylist += string("xsi");
  keylist += string("xsi_norm");
  keylist += string("xsi_norm_avg");
  keylist += string("pain");
  return keylist; 
}

list<Inspectable::iparamval> InvertMotorNStep::getInternalParams() const {
  list<iparamval> l;
  if(conf.someInternalParams){
    l += store4x4AndDiagonal(A);
    if(conf.useS) l += store4x4AndDiagonal(S);
    l += store4x4AndDiagonal(C);
    l += store4x4AndDiagonal(R);    
  }else{
    l += A.convertToList();
    if(conf.useS) l += S.convertToList();
    l += C.convertToList();
    l += R.convertToList();
  }
  l += H.convertToList();
  l += B.convertToList();
  l += epsA;
  l += epsC;
  l += xsi.elementSum();
  l += xsi_norm;
  l += xsi_norm_avg;
  l += pain;
  return l;
}

list<Inspectable::ILayer> InvertMotorNStep::getStructuralLayers() const {
  list<Inspectable::ILayer> l;
  l+=ILayer("x","", number_sensors, 0, "Sensors");
  l+=ILayer("y","H", number_motors, 1, "Motors");
  l+=ILayer("xP","B", number_sensors, 2, "Prediction");
  return l;
}

list<Inspectable::IConnection> InvertMotorNStep::getStructuralConnections() const {
  list<Inspectable::IConnection> l;
  l+=IConnection("C", "x", "y");
  l+=IConnection("A", "y", "xP");
  if(conf.useS) l+=IConnection("S", "x", "xP");
  return l;
}

void InvertMotorNStep::setTeachingMode(bool onOff){
  conf.useTeaching=onOff;
}

bool InvertMotorNStep::getTeachingMode(){
  return conf.useTeaching;
}

void InvertMotorNStep::setMotorTeachingSignal(const motor* teaching, int len){
  assert(len == number_motors);
  y_teaching.set(len, 1, teaching);
}

