/***************************************************************************
 *   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_gnu.hpp,v $
 *   Revision 1.2  2005/10/27 15:02:06  fhesse
 *   commercial use added
 *                              *
 *                                                                         *
 ***************************************************************************/

AbstractController* getController_Gnu(int sensornumber, int motornumber, 
				      int param1/*=0*/, double param2/*=0*/)
{
  if(param1 < 2) param1 = 2;
  return new InvertNChannelController_Gnu(sensornumber, param1);
}


InvertNChannelController_Gnu::InvertNChannelController_Gnu(int channels, int _buffersize)
  :InvertNChannelController(channels, _buffersize),
   plotlog("c.dat"),plotlog_x("x.dat"),plotlog_a("a.dat")
{

  // add channels to gnuplot 
  for (int i=0; i<number_channels; i++){
    plotlog.addChannel("h["+intToStr(i)+"]");
    plotlog_x.addChannel("x["+intToStr(i)+"]");
    plotlog_x.addChannel("y["+intToStr(i)+"]");
    for (int j=0; j<number_channels; j++){
      plotlog.addChannel("c["+intToStr(i)+"]["+intToStr(j)+"]");
      plotlog_a.addChannel("a["+intToStr(i)+"]["+intToStr(j)+"]");
    }
  }
  plotlog.command("set zeroaxis");
  //plotlog.print_names_list();   //print nameslist in logfiles
  
  plotlog_x.command("set zeroaxis");
  //plotlog_x.print_names_list();
  
  plotlog_a.command("set zeroaxis");
  //plotlog_a.print_names_list();

};


/// plots internal values
void InvertNChannelController_Gnu::plot(const sensor* x, const motor* y){

  // put value in buffer for plotting/writing
  for (int i=0; i<number_channels; i++){
    plotlog.putData("h["+intToStr(i)+"]",h.val(i,0));
    plotlog_x.putData("x["+intToStr(i)+"]",x[i]);
    plotlog_x.putData("y["+intToStr(i)+"]",y[i]);
    for (int j=0; j<number_channels; j++){
      plotlog.putData("c["+intToStr(i)+"]["+intToStr(j)+"]",C.val(i,j));
      plotlog_a.putData("a["+intToStr(i)+"]["+intToStr(j)+"]",A.val(i,j));
    }
  }
  
  t++;
  if (t%100) return;  // plot/write only ervery 100th step

  plotlog.plot();     // plot values in window
  //plotlog.print();    // write values to logfile

  plotlog_x.plot();
  //plotlog_x.print();

  plotlog_a.plot();
  //plotlog_a.print();
    

};


/// performs one step (includes learning). Calulates motor commands from sensor inputs.
void InvertNChannelController_Gnu::step(const sensor* x_, int number_sensors, 
					motor* y_, int number_motors){
  InvertNChannelController::step(x_, number_sensors, y_, number_motors);
  t--;

  plot(x_, y_);

  t++;
};


/// performs one step without learning. Calulates motor commands from sensor inputs.
void InvertNChannelController_Gnu::stepNoLearning(const sensor* x_, int number_sensors, 
						  motor* y_, int number_motors){
  InvertNChannelController::stepNoLearning(x_, number_sensors, y_, number_motors);
  t--;
  plot(x_, y_);
  t++;
};
  
 
/// converts integer to string
std::string InvertNChannelController_Gnu::intToStr(int value)
{
  std::ostringstream stream;
  stream << value;
  return stream.str();
};





  
