/***************************************************************************
 *   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!                                         * 
 *                                                                         *
 *   $Log: invertnchannelcontroller.h,v $
 *   Revision 1.14.6.2  2006/01/18 16:48:35  martius
 *   stored and restore
 *
 *   Revision 1.14.6.1  2005/11/14 17:37:29  martius
 *   moved to selforg
 *
 *   Revision 1.14  2005/10/27 15:46:38  martius
 *   inspectable interface is expanded to structural information for network visualiser
 *
 *   Revision 1.13  2005/10/27 15:02:06  fhesse
 *   commercial use added
 *                                    *
 *                                                                         *
 ***************************************************************************/
#ifndef __INVERTNCHANNELCONTROLLER_H
#define __INVERTNCHANNELCONTROLLER_H

#include "invertcontroller.h"

#include <assert.h>
#include <math.h>

#include "matrix.h"
using namespace matrix;

/**
 * class for robot controller that uses the georg's matrixlib for 
 *  direct matrix inversion for n channels 
 * (simple one layer networks)
 * 
 * Implements standart parameters: eps, rho, mu, stepnumber4avg, stepnumber4delay
 */
class InvertNChannelController : public InvertController {

public:
  InvertNChannelController(int _buffersize, bool _update_only_1=false);
  virtual void init(int sensornumber, int motornumber);

  virtual ~InvertNChannelController(){}

  /// returns the name of the object (with version number)
  virtual paramkey getName() const {return name; } 
  /// returns the number of sensors the controller was initialised with or 0 if not initialised
  virtual int getSensorNumber() const { return number_channels; }
  /// returns the mumber of motors the controller was initialised with or 0 if not initialised
  virtual int getMotorNumber() const  { return number_channels; }

  /// performs one step (includes learning). 
  /// Calulates motor commands from sensor inputs.
  virtual void step(const sensor* , int number_sensors, motor* , int number_motors);


  /// performs one step without learning. Calulates motor commands from sensor inputs.
  virtual void stepNoLearning(const sensor* , int number_sensors, 
			      motor* , int number_motors);


  /** stores the controller values to a given file. */
  virtual bool store(const char* filename);
  /** loads the controller values from a given file. */
  virtual bool restore(const char* filename);  

  // inspectable interface
  virtual list<iparamkey> getInternalParamNames() const;
  virtual list<iparamval> getInternalParams() const;
  virtual list<ILayer> getStructuralLayers() const;
  virtual list<IConnection> getStructuralConnections() const;
  

protected:
  unsigned short number_channels;
  unsigned short buffersize;
  bool update_only_1;

  Matrix A; // Model Matrix
  Matrix C; // Controller Matrix
  Matrix h; // Controller Bias
  Matrix L; // Jacobi Matrix
  Matrix* x_buffer;
  Matrix* y_buffer;
  int t;
  paramkey name;
  

/*   virtual void iteration(double *column, */
/* 			 double dommy[NUMBER_CHANNELS][NUMBER_CHANNELS], */
/* 			 double *improvment); */

  virtual double calculateE(const Matrix& x_delay, const Matrix& y_delay);      

  /// learn values h,C
  virtual void learn(const Matrix& x_delay, const Matrix& y_delay);

  virtual void learnmodel( const Matrix& y_delay);

  /// calculate delayed values
  virtual Matrix calculateDelayedValues(const Matrix* buffer, 
					unsigned int number_steps_of_delay_);
  virtual Matrix calculateSmoothValues(const Matrix* buffer, 
				       unsigned int number_steps_for_averaging_);

  Matrix calculateControllerValues(const Matrix& x_smooth);
    
  // put new value in ring buffer
  void putInBuffer(Matrix* buffer, const Matrix& vec);

  /// neuron transfer function
  static double g(double z)
  {
    return tanh(z);
  };

  ///
  static double g_s(double z)
  {
    double k=tanh(z);
    return 1.0 - k*k;  //TODO:     return 1.0 - k*k;
    //    return 1.0 - tanh(z)*tanh(z);
  };



  /// squashing function, to protect against to large weight updates
  static double squash(double z)
  {
    return z < -0.1 ? -0.1 : ( z > 0.1 ? 0.1 : z );
    //return 0.1 * tanh(10.0 * z);
  };
};

#endif


