/// @file eyebot_actors.hh
/// @author Jarrod Bassan <bassan@iinet.com.au>
/// @author Joshua Petitt <petitj01@tartarus.uwa.edu.au>
/// @author Stefan Schmitt <sschmitt@ee.uwa.edu.au>
/// @version 1.0
/// @date 2003-06
/// 
/// Declares wrapper classes for EyeBot actors.
///

#ifdef EYEBOT
#ifndef EYEBOT_ACTORS_HH
#define EYEBOT_ACTORS_HH

#include "base/templates.hh"
#include "base/base.hh"
#include "eyebot_hardware_sem.h"
#include <cmath>
#include <cstring>
#include "eyebot_behavior_types.h"

namespace EyeMind {

/// Lcd screen wrapper; Singleton. To be improved...

class Lcd : public Port
{
private:
	int x; ///< X cursor position.
	int y; ///< Y cursor position

	///Length of LCD string buffer.
	static const unsigned LCD_STRING_BUFFER_SIZE = 16;

	/// String to be printed.
	char string[LCD_STRING_BUFFER_SIZE];
	colimage *image; ///< Image to be displayed.

	/// Default construtor; initalize data members.
	Lcd(){ x=0; y=0; image = NULL; string[0] = '\0'; }

	NO_COPYING(Lcd);
		
public:
	SINGLETON(Lcd);

	/// Write the buffer to the LCD.
	virtual bool Update();
	
	///@name Attribute manipulation functions
	//@{
	void Image(colimage *i){ image = i; }
	void String(const char s[]){ strncpy(string, s, LCD_STRING_BUFFER_SIZE); }
	//@}
};



/// Servo actor class. This class cannot be instantiated directly (protected
/// constructor). You will have to derive a class calling the constructor with
/// the desired semantics.
/// @bug Partly implemented in header file
/// @bug Uses a wild mix of floats and ints, too confusing.

class Servo : public Port
{
private:
	/// Initial position for Servo.
	static const unsigned SERVO_START_POSITION = 127;

	ServoHandle handle;  ///< Servo handle.
	unsigned angle; ///< Angle of Servo.
	float range;
	unsigned safe_min; ///< safe minimum raw servo value (default is 0)
	unsigned safe_max; ///< save maximum raw servo value (default is 255)

protected:
	Servo(DeviceSemantics device, int start_position);

public:
	/// Virtual destructor.
	virtual ~Servo(){ SERVORelease(handle); }

	/// Update servo position.
	virtual	bool Update(){ return 0 == SERVOSet(handle, angle); }

	/// Calibrate the range (in degrees) of the servo.
	void CalibrateRange(float min_degrees, float max_degrees) {
		if (max_degrees > min_degrees) {
			range = max_degrees - min_degrees;
		}
	}

	///@name Attribute manipulation functions
	//@{
	/// Get servo setpoint (0-255)
	unsigned Angle(){ return angle; }

	/// Set servo setpoint (0-255)
	void Angle(unsigned a){ angle = a; }
	/// Get servo setpoint (Degrees)

	float AngleDeg(){	return -(angle-128.0)*range/256.0; }
	/// Set servo setpoint (Degrees)
	void AngleDeg(float a)
	{
		angle = (int)((-a*256.0 / (float)range)+128.0);
		if (angle > safe_max) {angle = safe_max;}
		if (angle < safe_min) {angle = safe_min;}
	}
	


	/// Get servo setpoint (Radians)
	float AngleRad(){	return AngleDeg() * M_PI / 180.0; }
	/// Set servo setpoint (Radians)
	void AngleRad(float a){	AngleDeg(a * 180.0 / M_PI); }

	void SafeLimits(int min, int max) { safe_min = (min < 0 ? 0 : min); safe_max = (max > 255 ? 255: max); }
	int SafeMin() { return safe_min; }
	int SafeMax() { return safe_max; }
	float SafeMinDeg() { return -(safe_min-128.0)*range/256.0; }
	float SafeMaxDeg() { return -(safe_max-128.0)*range/256.0; }
	float SafeMinRad() { return SafeMinDeg() * M_PI / 180.0; }
	float SafeMaxRad() { return SafeMaxDeg() * M_PI / 180.0; }
	//@}
};


/// Head (servo) actor wrapper.

class HeadServo : public Servo
{
private:
	HeadServo() : Servo( SERVO12, 128 ){ CalibrateRange(-50.0,50.0); }	

public:
	SINGLETON(HeadServo);

};


/// DC motor wrapper. This class cannot be instantiated directly (protected
/// constructor). You will have to derive a class calling the constructor with
/// the desired semantics.

class DCMotor : public Port
{
private:
	MotorHandle motor; ///< Motor handle.
	QuadHandle quad;   ///< Encoder handle.

	int signal; ///< Value given to MOTORDrive() function.
	const int min; ///< Min value for signal.
	const int max; ///< Max value for signal.
	int pos; ///< Current motor position (encoder ticks).
	int vel; ///< Current motor velocity (encoder ticks/callback).
	int acc; ///< Current motor acceleration (encoder ticks/callback^2).

	unsigned resolution; ///< resolution of the encoders (ticks/revolution)

protected:
	/// Constructor; initalize data members, motor and encoder.
	/// @param md Device semantics for motor.
	/// @param qd Device semantivs for encoder.

	DCMotor(DeviceSemantics md, DeviceSemantics qd);
	
public:
	virtual ~DCMotor();

	virtual bool Update();

	///Attribute manipulation functions.
	//@{
	int Signal(){ return signal;}
	void Signal(int s);
	int Min(){ return min;}
	int Max(){ return max;}
	
	// position of the wheel in radians
	float Position()
	{ 
		return pos * 2 * M_PI / (float)resolution;
	}

	// velocity of the wheel in radians/sec
	float Velocity()
	{ 
		return vel * 2 * M_PI / ((float)resolution * Timebase());
	}

	// acceleration of the wheel in radians/sec^2
	float Acceleration()
	{ 
		return acc * 2 * M_PI / ((float)resolution * Timebase() * Timebase());
	}
	
	unsigned Resolution(){ return resolution; }
	//@}
};


/// Left driving motor wrapper; Singleton.
class DCMotorLeft : public DCMotor
{
private:
	DCMotorLeft() : DCMotor(MOTOR_LEFT, QUAD_LEFT){};

public:
	SINGLETON(DCMotorLeft);
};


/// Right driving motor wrapper; Singleton.
class DCMotorRight : public DCMotor
{
private:
	DCMotorRight() : DCMotor(MOTOR_RIGHT, QUAD_RIGHT){};

public:
	SINGLETON(DCMotorRight);
};


/// Speaker wrapper; Singleton.
/// @bug Implemented in header file.

class Speaker : public Port
{
private:
	BYTE *sample;
	int tone;
	int duration;

	///Default constructor; initialize data members.
	Speaker(){sample = NULL; tone = 0; duration = 0; }

	NO_COPYING(Speaker);

public:
	SINGLETON(Speaker);

	///Virtual destructor; stops playing.
	virtual ~Speaker(){ AUTone(0, 0); }

	///Attribute manipulation functions.
	//@{
	BYTE* Sample(){ return sample;}
	void Sample(BYTE *s){sample = s;}
	int Tone(){ return tone;}
	void Tone(int t){tone = t;}
	int Duration(){ return duration;}
	void Duration(int d){duration = d;}
	//@}

	bool Playing(){ return !AUCheckTone(); }
	void PlaySample(){ if (sample) AUPlaySample(sample); }
	void PlayTone(){ AUTone(tone, duration); }
	void PlayTone(int t, int d){ tone = t; duration = d; PlayTone(); }
};


}; // namespace EyeMind

#endif // EYEBOT_ACTORS_HH
#endif // EYEBOT