/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                         *
 *   $Log: main.cpp,v $
 *   Revision 1.2.4.4  2006/03/29 15:10:22  martius
 *   *** empty log message ***
 *
 *   Revision 1.2.4.3  2006/02/24 14:43:51  martius
 *   keys
 *
 *   Revision 1.2.4.2  2005/12/29 16:44:54  martius
 *   moved to osg
 *
 *   Revision 1.2.4.1  2005/11/15 12:29:59  martius
 *   new selforg structure and OdeAgent, OdeRobot ...
 *
 *   Revision 1.2  2005/11/09 13:41:25  martius
 *   GPL'ised
 *
 ***************************************************************************/


#include <selforg/noisegenerator.h>

#include "simulation.h"
#include "odeagent.h"
#include <selforg/one2onewiring.h>
#include <selforg/derivativewiring.h>
#include "playground.h"
#include "passivesphere.h"

#include <selforg/deprivation.h>
#include <selforg/invertnchannelcontroller.h>
#include <selforg/invertmotorspace.h>
#include <selforg/invertmotornstep.h>
#include <selforg/sinecontroller.h>

#include "schlangeservo2.h"

list<PlotOption> plotoptions;

using namespace lpzrobots;

int zeit =0;
Matrix turnMotor(const Matrix& _dont_care){  
  Matrix y(_dont_care.getM(),1);
  for(int i=0; i< y.getM(); i++){
    y.val(i,0) = pow(-1.0,i)*sin(zeit/100.0)*0.9;
  }
  zeit++;
  return y;
}

Deprivation *controller;

class ThisSim : public Simulation {
public:
	

  /// start() is called at the start and should create all the object (obstacles, agents...).
  virtual void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global){
    setCameraHomePos(Pos(-10.7854, -7.41751, 5.92078),  Pos(-50.6311, -5.00218, 0));

    //****************/
    SchlangeConf conf = Schlange::getDefaultConf();
    conf.motorPower=0.2;
    conf.frictionJoint=0.01;
    conf.segmNumber=6; 
    //     conf.jointLimit=conf.jointLimit*3;
    SchlangeServo2* schlange1 = 
      new SchlangeServo2 ( odeHandle, osgHandle.changeColor(Color(0.8, 0.3, 0.5)),
			  conf, "S1");
    ((OdeRobot*)schlange1)->place(Pos(0,0,3)); 

    //AbstractController *controller = new InvertNChannelController(100/*,true*/);  
    //  AbstractController *controller = new InvertMotorSpace(100/*,true*/);  
    //    AbstractController *controller = new InvertMotorNStep();  
    //    controller = new Deprivation(turnMotor);  
    AbstractController *controller = new SineController();  
  
    AbstractWiring* wiring = new One2OneWiring(new ColorUniformNoise(0.1));
    //   DerivativeWiringConf c = DerivativeWiring::getDefaultConf();
    //   c.useId=true;
    //   c.useFirstD=true;
    //   // c.useSecondD=true;
    //   c.derivativeScale=10;
    //   AbstractWiring* wiring = new DerivativeWiring(c, new ColorUniformNoise(0.1));
    OdeAgent* agent = new OdeAgent(plotoptions);
    agent->init(controller, schlange1, wiring);
    global.agents.push_back(agent);
    global.configs.push_back(controller);
    global.configs.push_back(schlange1);
  
 
    global.odeConfig.setParam("controlinterval",1);
    global.odeConfig.setParam("gravity", 0.0); 

    controller->setParam("steps",2);
    controller->setParam("epsC",0.0005);
    controller->setParam("epsA",0.01);
    controller->setParam("adaptrate",0);
    //    controller->setParam("rootE",3);

    // controller->setParam("desens",0.0);
    //   controller->setParam("s4delay",1.0);
    //   controller->setParam("s4avg",1.0);
    
    //   controller->setParam("factorB",0.0);
    //   controller->setParam("zetaupdate",0.1);

    Primitive* head = schlange1->getMainPrimitive();
    Joint* j = new BallJoint(head, global.environment, head->getPosition());
    j->init(odeHandle, osgHandle);

    showParams(global.configs);
  }

  //Funktion die eingegebene Befehle/kommandos verarbeitet
  virtual bool command (const OdeHandle&, const OsgHandle&, GlobalData& globalData, int key, bool down)
  {
    if (!down) return false;    
    bool handled = false;
    switch ( key )
      {
      case 't' : 	
	controller->setExternalControlMode(!controller->getExternalControlMode());
	printf("Control Mode: %i\n", controller->getExternalControlMode());
	handled = true; break;	
      case 's' :
	controller->store("test") && printf("Controller stored\n");
	handled = true; break;	
      case 'l' :
	controller->restore("test") && printf("Controller loaded\n");
	handled = true; break;	
      }
    fflush(stdout);
    return handled;
  }


  virtual void bindingDescription(osg::ApplicationUsage & au) const {
    au.addKeyboardMouseBinding("Deprivation: t","toggle mode (straight moving/controller)");
    au.addKeyboardMouseBinding("Simulation: s","store");
    au.addKeyboardMouseBinding("Simulation: l","load");
  }


};

void printUsage(const char* progname){
  printf("Usage: %s [-g] [-l]\n\t-g\tuse guilogger\n\t-l\tuse guilogger with logfile", progname);
}

int main (int argc, char **argv)
{  
  if(contains(argv, argc, "-g")) plotoptions.push_back(PlotOption(GuiLogger));
  if(contains(argv, argc, "-f")) plotoptions.push_back(PlotOption(GuiLogger_File, Controller, 10));
  if(contains(argv, argc, "-h")) printUsage(argv[0]);

  ThisSim sim;
  return sim.run(argc, argv) ? 0 : 1;
}


 
  
