/// @file eyebot_actors.cc

#include "eyebot_hardware_types.h"
#include "output/eyebot_actors.hh"

namespace EyeMind {

bool Lcd::Update()
{
	if (image)
		LCDPutColorGraphic(image);

	if (string[0]) {
		LCDSetString(x, y, string);
	}

	return true;
}


Servo::Servo(DeviceSemantics device, int start_position)
{
	///angle = SERVO_START_POSITION; JB: Where is SERVO_START_POSITION defined?????
	angle = start_position; 
	handle = SERVOInit(device);
	SERVOSet(handle, angle);
	safe_min = 0; safe_max = 255;
}


DCMotor::DCMotor(DeviceSemantics md, DeviceSemantics qd) :
	 min(-100), max(100)
{
	quad_type *qt;
	motor_type *mt;

	signal = 0;
	pos = 0;
	vel = 0;
	acc = 0;

	motor = MOTORInit(md);
	quad = QUADInit(qd);
	MOTORDrive(motor, 0);

	dcmotor_bt *dc = (dcmotor_bt*) HDTFindEntry(B_DCMOTOR,0);
	if(dc==NULL) OSPanic("Cannot find\n dcmotor data\n");
	resolution = dc->ticks_per_revolution;

	Timebase(dc->timebase);
}


DCMotor::~DCMotor()
{
	DEBUG_PRINT("~DCMotor\n");
	MOTORDrive(motor, 0);
	MOTORRelease(motor);
	QUADRelease(quad);
}


void DCMotor::Signal(int s)
{
	signal = clip(s, min, max);
}


bool DCMotor::Update()
{
	int last_pos = pos;
	int last_vel = vel;

	pos = QUADRead(quad);
	vel = pos - last_pos;
	acc = vel - last_vel;
	
	//DEBUG_PRINTF("motor w=%d\n",vel);

	return 0 == MOTORDrive(motor, signal);
}


}; // namespace EyeMind