/***************************************************************************
 *   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: kheperarobot.h,v $
 *   Revision 1.1.2.1  2005/11/22 13:55:21  martius
 *   first working version for selforg integration to webots
 *
 *   Revision 1.1  2005/11/22 13:53:38  martius
 *   Initial revision
 *
 ***************************************************************************/
#ifndef __KHEPERAROBOT_H
#define __KHEPERAROBOT_H

#include <device/robot.h>
#include <device/differential_wheels.h>
#include <selforg/abstractrobot.h>

/**
 * Khepera robot wrapper for webots Khepera
 * 
 */
class KheperaRobot : public AbstractRobot {
public:

  /**
   * Constructor
   * @param name name of the robot
   */
  KheperaRobot(const char* name="khepera")
    : AbstractRobot(name) {
    sensornumber=2;
    differential_wheels_enable_encoders(64);    
    left=0;
    right=0;
  };

  virtual ~KheperaRobot(){}

  /// returns the name of the robot
  string getName() const { return name;}

  /** returns actual sensorvalues
      @param sensors sensors scaled to [-1,1] 
      @param sensornumber length of the sensor array
      @return number of actually written sensors
  */
  virtual int getSensors(sensor* sensors, int sensornumber){
    assert(sensornumber>=2);
    // TODO, take derivative!
    long newl, newr;
    newl = differential_wheels_get_left_encoder();
    newr = differential_wheels_get_right_encoder();
    
    sensors[0] = (newl - left) / 60.0;
    sensors[1] = (newr - right) / 60.0;
    left = newl;
    right =newr;
    return 2;
  };

  /** sets actual motorcommands
      @param motors motors scaled to [-1,1] 
      @param motornumber length of the motor array
  */
  virtual void setMotors(const motor* motors, int motornumber){
    assert(motornumber>=2);
    /* Set the motor speeds */
    differential_wheels_set_speed((short int)round(motors[0]*maxspeed), 
				  (short int)round(motors[1]*maxspeed)); 
  }

  /** returns number of sensors
  */
  virtual int getSensorNumber(){
    return sensornumber;
  }

  /** returns number of motors
  */
  virtual int getMotorNumber(){
    return 2;
  }

  /** returns position of the object
      @return vector of position (x,y,z)
   */
  virtual Position getPosition() const {
    return Position(0,0,0);
  }

  /** returns linear speed vector of the object
      @return vector  (vx,vy,vz)
   */
  virtual Position getSpeed() const {
    return Position(0,0,0);
  }

  /** returns the orientation of the object
      @return 3x3 rotation matrix
   */
  virtual Matrix getOrientation() const {
    Matrix m(3,3);
    return m^0; // identity matrix
  }


protected:
  static const int maxspeed = 10;
  long left;
  long right;
  
  int sensornumber;
};

#endif
 
