/***************************************************************************
 *   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: sinecontroller.cpp,v $
 *   Revision 1.8  2009/08/10 14:10:09  der
 *   Impuls->Impulse
 *
 *   Revision 1.7  2009/03/31 15:52:59  martius
 *   added amplitude
 *
 *   Revision 1.6  2009/02/02 15:37:30  martius
 *   added saw tooth and impuls oscillators
 *   sinerate is now period which is counted in control cycles
 *   changing the period does not change phase as it was before.
 *
 *   Revision 1.5  2008/05/30 11:58:27  martius
 *   use cmath instead of math.h
 *
 *   Revision 1.4  2008/04/17 14:54:45  martius
 *   randomGen added, which is a random generator with long period and an
 *    internal state. Each Agent has an instance and passed it to the controller
 *    and the wiring. This is good for
 *   a) repeatability on agent basis,
 *   b) parallel execution as done in ode_robots
 *
 *   Revision 1.3  2006/07/14 12:23:59  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.1.2.5  2006/06/25 21:56:07  martius
 *   configureable has name and revision
 *
 *   Revision 1.1.2.4  2006/06/25 16:51:36  martius
 *   configureable has name and revision
 *   a robot is configureable by default
 *
 *   Revision 1.1.2.3  2005/12/11 23:35:08  martius
 *   *** empty log message ***
 *
 *   Revision 1.1.2.2  2005/11/14 17:37:29  martius
 *   moved to selforg
 *
 *   Revision 1.1.2.1  2005/11/14 15:38:17  martius
 *   moved to selforg
 *
 *   Revision 1.4  2005/11/14 12:46:19  martius
 *   added number of controlled motors
 *
 *   Revision 1.3  2005/10/06 17:06:57  martius
 *   switched to stl lists
 *
 *   Revision 1.2  2005/07/07 10:25:47  martius
 *   added Phaseshift
 *
 *   Revision 1.1  2005/07/06 13:55:33  fhesse
 *   initial version, realising sine and cosine outputs
 *                                            * 
 *                                                                         *
 ***************************************************************************/

#include <stdio.h>
#include <cmath>
#include <assert.h>
#include "sinecontroller.h"
#include "controller_misc.h"

using namespace std;

SineController::SineController(unsigned long int controlmask, function func)
  : AbstractController("sinecontroller", "$Id: sinecontroller.cpp,v 1.8 2009/08/10 14:10:09 der Exp $"),
    controlmask(controlmask) {
  phase=0;
  addParameterDef("period", &period,50);
  addParameterDef("phaseshift", &phaseShift, 1);
  addParameterDef("impulswidth", &impulsWidth, 0.5);
  addParameterDef("amplitude", &amplitude, 1);
  switch(func){
  case Sine:
    osci=sine; break;
  case SawTooth:
    osci=sawtooth; break;
  case Impulse:
    osci=impuls; break;
  default:
    assert("Unknown function type");
  }

  number_sensors=0;
  number_motors=0;  
};

/** initialisation of the controller with the given sensor/ motornumber 
    Must be called before use.
*/
void SineController::init(int sensornumber, int motornumber, RandGen* randGen){
  number_sensors=sensornumber;
  number_motors=motornumber;
};
  
void SineController::step(const sensor* sensors, int sensornumber, 
			  motor* motors, int motornumber) {
  stepNoLearning(sensors, sensornumber, motors, motornumber);
};

void SineController::stepNoLearning(const sensor* sensors, int number_sensors, 
				    motor* motors, int number_motors) {
  
  for (int i=0; i<min(number_motors,sizeof(controlmask)*8); i++){
    if(controlmask & (1<<i)){      
      motors[i]=amplitude*osci(phase + i*phaseShift*M_PI/2, impulsWidth);
    }else {
      motors[i]=0;
    }
  }  
  if(period!=0){
    phase += 2*M_PI/period;
    if(phase > 2*M_PI) phase -= 2*M_PI;
  }
};
 
 
double SineController::sine(double x, double _unused){
  return sin(x);
}
 
double SineController::sawtooth(double x, double _unused){
  while(x>M_PI) x-=2*M_PI;
  while(x<-M_PI) x+=2*M_PI;
  // x is centered around -PI and PI.
  if(x>-M_PI/2 && x <= M_PI/2)
    return x/M_PI;
  else{
    if(x<0)
      return -(x+M_PI)/M_PI;
    else
      return -(x-M_PI)/M_PI;
  }
}


double SineController::impuls(double x, double impulsWidth){
  while(x>M_PI) x-=2*M_PI;
  while(x<-M_PI) x+=2*M_PI;
  // x is centered around -PI and PI.
  // +-1 for |x| in ((0.5-impulswidth/2)*M_PI,(0.5+impulswidth/2)*M_PI]
  if(fabs(x) > (0.5-impulsWidth/2)*M_PI && fabs(x) < (0.5+impulsWidth/2)*M_PI)
    return sign(x);
  else
    return 0;
}



