//-----------------------------------------------------------------------------
// Name: khepera_lpz.cpp
// Description: Template for creating a new Webots controller.
// Author: Olivier Michel <Olivier.Michel@cyberbotics.com>
// Date: September 9th, 2004
//-----------------------------------------------------------------------------

#include <selforg/agent.h>
#include <selforg/noisegenerator.h>
#include <selforg/one2onewiring.h>
#include <selforg/sinecontroller.h>
#include "kheperarobot.h"

AbstractRobot* robot = 0;
AbstractController* controller = 0;
AbstractWiring* wiring = 0;
Agent* agent =0;

//-----------------------------------------------------------------------------
// This is the reset function called once upon initialization of the robot.
//-----------------------------------------------------------------------------
static void reset(void) {
  if(robot) delete robot;
  if(controller) delete controller;
  if(wiring) delete wiring;
  robot = new KheperaRobot();
  controller =  new SineController();
  wiring = new One2OneWiring(new ColorUniformNoise(0.1));
  agent  = new Agent(PlotOption(GuiLogger));
  agent->init(controller, robot, wiring);    
}



//-----------------------------------------------------------------------------
// This is the main control loop function, it is called repeatedly by Webots
//-----------------------------------------------------------------------------
static int run(int ms) {
  if(agent){
    agent->step(0.1); // noise
  }
  return 64; // this is the time step value, expressed in milliseconds.
}

//-----------------------------------------------------------------------------
// This is the main program which sets up the reset and run function.
//-----------------------------------------------------------------------------
int main() {
  robot_live(reset); // initialization
  robot_run(run);    // starts the controller
  return 0;
}
