/***************************************************************************
 *   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: basiccontroller.cpp,v $
 *   Revision 1.2  2008/04/17 14:54:44  martius
 *   randomGen added, which is a random generator with long period and an
 *    internal state. Each Agent has an instance and passed it to the controller
 *    and the wiring. This is good for
 *   a) repeatability on agent basis,
 *   b) parallel execution as done in ode_robots
 *
 *   Revision 1.1  2008/02/28 07:46:01  der
 *   Most simple version of the controller with regularized inversion in sensor space.
 *
 *   Revision 1.1  2007/12/06 16:50:49  der
 *   new versions
 *
 *
 *
 ***************************************************************************/

#include "basiccontroller.h"
//#include "dercontroller.h"
#include "regularisation.h"
#include "invertmotornstep.h"
#include "invertmotorcontroller.h"
//#include "statistictools.h"
using namespace matrix;
using namespace std;

BasicController::BasicController( const BasicControllerConf& conf) 
  : InvertMotorController(conf.buffersize, "BasicController", "$Id: basiccontroller.cpp,v 1.2 2008/04/17 14:54:44 martius Exp $"), conf(conf) {
 
  //  assert(conf.model != NULL);

//   YNoiseGen = 0;
//   BNoiseGen = 0;
//   noiseB = 0;
//   managementInterval=10;
//   useTeaching=false;

  addParameterDef("dampC",&dampC,0);
};


BasicController::~BasicController(){
  if(x_buffer && y_buffer && eta_buffer){
    delete[] x_buffer;
    delete[] y_buffer;
    delete[] eta_buffer;
  }

//   if(BNoiseGen) delete BNoiseGen;
//   if(YNoiseGen) delete YNoiseGen;
}


void BasicController::init(int sensornumber, int motornumber, RandGen* randGen){
  number_motors  = motornumber;
  number_sensors = sensornumber;  
  assert(number_motors && number_sensors);
  if(!randGen) randGen = new RandGen(); // this gives a small memory leak


  ID.set(number_motors, number_motors);
  ID.toId();
  
   A.set(number_sensors,  number_motors); 
   A=A^0; //Initialisiert A als Einheitsmatrx 


   //   conf.model->init(number_motors, number_sensors, conf.modelInit);
  C.set(number_motors,  number_sensors);
  GSC.set(number_motors,  number_sensors);

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


  H.set(number_motors,  1);
  R.set(number_motors, number_motors);
  CCT_inv.set(number_motors, number_motors);
  CST.set(number_motors, number_sensors);

  xsi.set(number_sensors,1);
  squashSize = .05;

  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);
  }
   x_smooth.set(number_sensors,1);
   // x_smooth_long.set(number_sensors,1);

 
  eta.set(number_motors, 1); 
  initialised = true;
}
//*************** End init *******************

/// performs one step (includes learning). Calculates motor commands from sensor inputs.
void BasicController::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);   
    // learn Model with actual sensors and with effective motors,
    // calc xsi and A;
    learnModel(delay);
    // learn controller with effective input/output
    learnController(delay);
    
  }
  // update step counter
  t++;

};

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

void BasicController::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_);

   x = ((x * 0.9).map(g) )*(1/0.9);//TEST

  // Smoothing over time horizon of length s4avg
  x_smooth +=  ( x -  x_smooth )*(1.0/ s4avg); 
  
  // put new input vector in ring buffer x_buffer
  putInBuffer(x_buffer, x);
 

  // 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);
}


void BasicController::learnController(int delay){ 

  if ((t==buffersize+1) || ((t%19)==2))    ATA_inv = ((A^T) * A  + ID*0.1)^-1;

  Matrix eta = ATA_inv * ( (A^T) * xsi );

     // Diese erste Matrixinversion wäre zu  vermeiden mit:
     //  eta +=  (A^T) * (A*eta - xsi) * -0.1 /*(-epsA)*/ + eta * -0.001; 

 

  eta_buffer[(t-1)%buffersize] = eta; // Todo: work with the reference
  
  Matrix C_update(C.getM(), C.getN());
  Matrix H_update(H.getM(), H.getN());

  
 
  const Matrix& x  = x_buffer[(t-delay)%buffersize];
   
  
  const Matrix& z          = (C * x_smooth + H);
  const Matrix zpluseta = z + eta * .1; 
  const Matrix g_prime = zpluseta.map(g_derivative);
  //if ((t==buffersize+1) || ((t%8)==2)) 
  CST = C.multrowwise(g_prime); 
  CCT_inv = ( CST * (CST^T) +  ID * .1)^(-1);
  const Matrix mue = CCT_inv*eta; 
  const Matrix chi = mue.multrowwise(g_prime);
  const Matrix rho = mue.multrowwise((z.map(g)).multrowwise(CST*(CST^T)*mue))*-2;
  C_update = ( (  chi * (chi^T )* C) +  rho*(x^T)) * epsC;
  H_update =   rho  * epsC;
  //Hier Teaching 
  C_update += chi * (x^T) *teacher;// * epsC;
  H_update += chi * teacher;// * epsC *  .2; 


  // Controlling the learning parameters:  
     double Test_squashSize = squashSize/5.0;
  double u = calcMatrixNorm(C_update); 
  double q = calcMatrixNorm( C_update.mapP(&Test_squashSize, squash) );
 if (epsC>0)  epsC *= 1.03;
   if ( u*u > 1.01 *  q*q)      epsC *=0.5; 
     else { 
     C += C_update.mapP(&squashSize, squash) - C* dampC;
     double H_squashSize = squashSize*2.0;//TEST: H soll sich schneller bewegen können. 
     H += H_update.mapP(&H_squashSize, squash) - H * dampC; //Test; //Test - H*.001; 

        }
   
   // End  Controlling the learning parameters:  
};

// Calculates Xsi and A  and learns the model

void BasicController::learnModel(int delay){
  const Matrix& x = x_buffer[t % buffersize];
  const Matrix& y = y_buffer[(t - 1 - delay) % buffersize];//TEST

  xsi = x - A * y;   
      
  A += xsi * (y^T) * epsA - A * dampA *epsA ; 

};

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

};


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

Matrix BasicController::calcDerivatives(const matrix::Matrix* buffer,int delay){  
  const Matrix& xt    = buffer[(t-delay)%buffersize];
  const Matrix& xtm1  = buffer[(t-delay-1)%buffersize];
  const Matrix& xtm2  = buffer[(t-delay-2)%buffersize];
  return ((xt - xtm1) * 5).above((xt - xtm1*2 + xtm2)*10);
}


/** stores the controller values to a given file. */
bool BasicController::store(FILE* f) const {  
  // save matrix values
  C.store(f);
  H.store(f);
  if(conf.useS)  S.store(f);
  conf.model->store(f);
  Configurable::print(f,0);
  return true;
}

/** loads the controller values from a given file. */
bool BasicController::restore(FILE* f){
  // save matrix values
  C.restore(f);
  H.restore(f);
  if(conf.useS)  S.restore(f);
  conf.model->restore(f);
  Configurable::parse(f);
  t=0; // set time to zero to ensure proper filling of buffers
  return true;
}


bool BasicController::setParam(const paramkey& key, paramval val){
  if(key == "epsA") {
    //   cout << "MODEL: " << conf.model->getParam("eps") << endl;
    // conf.model->setParam("eps",val);
  }
  return InvertMotorController::setParam(key, val);
}


list<Inspectable::iparamkey> BasicController::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 += storeMatrixFieldNames(y_teaching, "yteach");
  }
   keylist += storeVectorFieldNames(H, "H");
    keylist += storeVectorFieldNames(eta, "eta");
     keylist += storeVectorFieldNames(xsi, "xsi");
     //  keylist += storeVectorFieldNames(x_smooth_long, "v_smooth");
  keylist += string("weighting");
  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> BasicController::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 += y_teaching.convertToList();
  }
    l += H.convertToList();
  l += eta.convertToList();
  l += xsi.convertToList();
  //  l += x_smooth_long.convertToList();//TEST
  l += weighting;
  l += epsA;
  l += epsC;
   l += xsi.elementSum();
    l += xsi_norm;
  l += xsi_norm_avg;
  l += pain;
  return l;
}

list<Inspectable::ILayer> BasicController::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> BasicController::getStructuralConnections() const {
  list<Inspectable::IConnection> l;
  l+=IConnection("C", "x", "y");
  l+=IConnection("A", "y", "xP");
  return l;
}

//double clip095(double x){
// return clip(x,-0.95,0.95); 
//}

// void BasicController::setMotorTeachingSignal(const motor* teaching, int len){
//   assert(len == number_motors);  
//   y_teaching.set(len, 1, teaching);
//   //  y_teaching.toMap(clip095); //TODO where is clip 
//   useTeaching=true;
// }

// void BasicController::setSensorTeachingSignal(const sensor* teaching, int len){
//   assert(len == number_sensors);  
//   Matrix x_teaching(len,1,teaching);
//   // calculate the y_teaching, that belongs to the distal teaching value by the inverse model.
//   // y_teaching = (A.multTM()^(-1)) *  ((A^T) * x_teaching) *0.000000000; //TEST
//   //y_teaching.toMap(clip095); //TODO
//   useTeaching=true;
// }

double BasicController::calcMatrixNorm(const Matrix& m){
  return m.map(fabs).elementSum() / (m.getM() * m.getN());
}
