/*
 * c't-Bot
 * 
 * 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.
 * 
 */

/*! @file 	behaviour_drive_distance.c
 * @brief 	Bot faehrt ein Stueck
 * 
 * @author 	Benjamin Benz (bbe@heise.de)
 * @date 	03.11.06
*/

#include "bot-logic/bot-logik.h"
#include "log.h"

#ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE


/* Parameter fuer das bot_drive_distance_behaviour() */
int16 drive_distance_target;	/*!< Zu fahrende Distanz bzw. angepeilter Stand der Radencoder sensEncL bzw. sensEncR */
int8 drive_distance_curve;		/*!< Kruemmung der zu fahrenden Strecke. */
int16 drive_distance_speed;		/*!< Angepeilte Geschwindigkeit. */


/*!
 * laesst den Bot in eine Richtung fahren. 
 * Es handelt sich hierbei nicht im eigentlichen Sinn um ein Verhalten, sondern ist nur eine Abstraktion der Motorkontrollen.
 * @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts)
 * @param speed Gibt an, wie schnell der Bot fahren soll. */
void bot_drive(int8 curve, int16 speed){
	// Wenn etwas ausgewichen wurde, bricht das Verhalten hier ab, sonst wuerde es evtl. die Handlungsanweisungen von bot_avoid_harm() stoeren.
	//if(bot_avoid_harm()) return;
	if(curve < 0) {
		speedWishLeft = speed * (1.0 + 2.0*((float)curve/127));
		speedWishRight = speed;
	} else if (curve > 0) {
		speedWishRight = speed * (1.0 - 2.0*((float)curve/127));
		speedWishLeft = speed;
	} else {
		speedWishLeft = speed;
		speedWishRight = speed;	
	}
}

/*!
 * Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren.
 * @param *data der Verhaltensdatensatz
 * @see bot_drive_distance() 
 */
void bot_drive_distance_behaviour(Behaviour_t* data){
	int16 *encoder;
	int16 to_drive;

	if (drive_distance_curve > 0){
		// Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet
		encoder = (int16*)&sensEncL;
	} else {
		encoder = (int16*)&sensEncR;
	}
	
	to_drive = drive_distance_target - *encoder;
	if(drive_distance_speed < 0) to_drive = -to_drive;
	
	if(to_drive <= 0){
		return_from_behaviour(data);
	} else {
		if((drive_distance_speed > BOT_SPEED_SLOW || drive_distance_speed < -BOT_SPEED_SLOW) && to_drive < (0.1 * ENCODER_MARKS)) bot_drive(drive_distance_curve, drive_distance_speed / 2);
		else bot_drive(drive_distance_curve, drive_distance_speed);
	}	 
}



/*! 
 * Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren. Dabei legt die Geschwindigkeit fest, ob der Bot vorwaerts oder rueckwaerts fahren soll.
 * @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts)
 * @param speed Gibt an, wie schnell der Bot fahren soll. Negative Werte lassen den Bot rueckwaerts fahren.
 * @param cm Gibt an, wie weit der Bot fahren soll. In cm :-) Die Strecke muss positiv sein, die Fahrtrichtung wird ueber speed geregelt.
 */
void bot_drive_distance(Behaviour_t* caller,int8 curve, int16 speed, int16 cm){
//	LOG_DEBUG(("curve= %d, speed= %d, cm =%d",curve, speed, cm));

	int32 tmp = cm;
	tmp*= 10 * ENCODER_MARKS;
	tmp /= WHEEL_PERIMETER;
	int16 marks_to_drive = (int16) tmp;
	
	int16 *encoder;
	drive_distance_curve = curve;
	drive_distance_speed = speed;

	if (curve > 0){
		// Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet
		encoder = (int16*)&sensEncL;
	} else {
		encoder = (int16*)&sensEncR;
	}
	if(speed < 0){
		// Es soll rueckwaerts gefahren werden. Der Zielwert ist also kleiner als der aktuelle Encoder-Stand.
		drive_distance_target = *encoder - marks_to_drive;
	} else {
		drive_distance_target = *encoder + marks_to_drive;	
	}
	switch_to_behaviour(caller, bot_drive_distance_behaviour,NOOVERRIDE);
}
#endif