/**@file eyebot_sensors.cc
 * @author Joshua Petitt <petitj01@tartarus.uwa.edu.au>
 * @author Stefan Schmitt <sschmitt@ee.uwa.edu.au>
 * @date 2003-06
 *
 * Implements eyebot_sensors.hh.
 */

#ifdef EYEBOT

#include "eyebot_sensors.hh"
#include "cam.h"
//#include "rs232.h"
#include "librobi/librobi.h"
#include <cstring>

namespace EyeMind {

#if 0
/**
 * Keypad Port
 */
Keypad::Keypad() : Port()
{
	key = 0;
}

bool Keypad::Update()
{
	int k = key;
	key = KEYRead();

	return (k != key);
}
#endif


/**
 * Psd Port
 */
// define static member var
Psd *Psd::psds[] = { NULL };

Psd::Psd(DeviceSemantics device) : Port()
{
	unsigned i = 0;
	PSDHandle bitmask = 0;

	x = 0;
	dx = 0;
	rawmode = false;

	handle = PSDInit(device);

	// add this PSD to list
	do {
		if (psds[i] == NULL) {
			psds[i] = this;
			break;
		};
	} while (++i < PSD_LIST_SIZE);
	OSPanicIf(i > PSD_LIST_SIZE, "Too many Psd objects");

	// loop through other PSDs, restarting them;
	for (i = 0; i < PSD_LIST_SIZE; ++i)
		if (psds[i] != NULL)
			bitmask |= psds[i]->handle;

	PSDStart(bitmask, TRUE);
}

Psd::~Psd()
{
	unsigned i;
	PSDHandle bitmask = 0;

	// delete this PSD from list
	// loop through other PSDs, restarting them;
	for (i = 0; i < PSD_LIST_SIZE; ++i) {
		if (psds[i] == this)
			psds[i] = NULL;
		if (psds[i] != NULL)
			bitmask |= psds[i]->handle;
	};

	PSDStop();
	OSWait(20);

	PSDStart(bitmask, TRUE);
};


bool Psd::Update()
{
	if (rawmode)
		dx = PSDGetRaw(handle) - x;
	else
		dx = PSDGet(handle) - x;

	x += dx;
	return 0 != dx;
}



/**
 * Compass Port
 */
Compass::Compass()
{
	angle = 0;

	COMPASSInit(COMPASS);
	COMPASSStart(1);

	Active(true);
}

Compass::~Compass()
{
	COMPASSRelease();
}

int Compass::Angle()
{
	return angle;
}

bool Compass::Update()
{
	angle = 180 - COMPASSGet();
	angle = clip(angle,-180,180);
	return true;
}



/**
 * Camera Port
 */
Camera::Camera() : Port()
{
	cam_id = CAMInit(NORMAL);
	Active(true);
}

Camera::~Camera()
{
	CAMRelease();
}

colimage* Camera::Image()
{
	return &image;
}

bool Camera::Update()
{
	CAMGetColFrame(&image, 0);
	return true;
}


#if 0
/**
 * Microphone Port
 */
Microphone::Microphone() : Port()
{
        // intialize the sound buffer
        memset(sndbuffer,0,DEFAULT_MICROPHONE_BUFFER_LENGTH);
        length = 0;
        freq = DEFAULT_MICROPHONE_FREQUENCY;
        sndthreshold = DEFAULT_MICROPHONE_SOUND_THRESHOLD;
}

Microphone::~Microphone()
{
        // delete the sound buffer
        memset(sndbuffer,0,DEFAULT_MICROPHONE_BUFFER_LENGTH);
        length = 0;
}

bool Microphone::Update()
{
        int s = AUCaptureMic();

        if(s>sndthreshold)
                return true;
        else
                return false;
}

void Microphone::Record(int len)
{
        if(!AUCheckRecord() && len<DEFAULT_MICROPHONE_BUFFER_LENGTH)
        {
                AURecordSample(sndbuffer,len,freq);
                length = len;
        }
}

void Microphone::GetSample(BYTE* b, unsigned* len)
{
        b = sndbuffer;
        *len = length;
}
#endif


/**
 * IRtv Port
 */

IRtv::IRtv() : Port()
{
        key = 0;
        OSPanicIf(IRTVInitHDT(IRTV),"IR error\n");
}

IRtv::~IRtv()
{
        IRTVTerm();
}

bool IRtv::Update()
{
        key = IRTVPressed();
        return key;
}


#if 0
/**
 *  SerialRS232 Port
 */

SerialRS232::SerialRS232()
{
        memset(ibuffer,0,DEFAULT_RS232_BUFFER_LENGTH);
        memset(obuffer,0,DEFAULT_RS232_BUFFER_LENGTH);

        ibuffer_length=0;
        obuffer_length=0;
        max_buffer_length = DEFAULT_RS232_BUFFER_LENGTH;
        msg_flag = false;

        OSPanicIf(OSInitRS232(SER115200,RTSCTS,SERIAL1),
                  "RS232 init error\n");
}

SerialRS232::~SerialRS232()
{

}

bool SerialRS232::Update()
{
        return true;
}

char* SerialRS232::Message()
{
        return NULL;
}

int Message(BYTE* mes)
{
}

int Message(const char* mes)
{
}

#endif




/**
 * RadioComm Port
 */
RadioComm::RadioComm()
{
	radio_id = OSMachineID();
	sender_id = '\0';
	ibuffer_length = 0;
	obuffer_length = 0;
	max_buffer_length = DEFAULT_RADIO_BUFFER_LENGTH;

	error = 0;                     ///< error flag
	error_count     = 0;           ///< error count
	max_error_count = 10;          ///< maximum error count

	memset(ibuffer,0,DEFAULT_RADIO_BUFFER_LENGTH);
	memset(obuffer,0,DEFAULT_RADIO_BUFFER_LENGTH);

	LCDPrintf("Radio %d",radio_id);

	if( RADIOInit() )
	{
		LCDPrintf(" err\n");
	}
	else
		LCDPrintf(" ok\n");
}


RadioComm::~RadioComm()
{
	LCDPrintf("~Radio");
	if(RADIOTerm()==0)
	{
		LCDPrintf(" ok\n");
	}
	else
	{
		LCDPrintf(" err\n");
	}
}


bool RadioComm::Update()
{
	/* Get any incoming messages */
	if(RADIOCheck())
	{
		RADIORecv(&sender_id, &ibuffer_length, ibuffer);
		msg_flag = true;

		/* Assure a null terminated string */
		ibuffer[ibuffer_length] = (char) 0;

		LCDPrintf("%d - %s\n",sender_id,ibuffer);
	}
	else
		msg_flag = false;


	/* Send any outgoing messages */
	if (obuffer_length>0)
	{
		error = RADIOSend(BROADCAST,obuffer_length, obuffer);

		//DEBUG_PRINTF("%d\n",obuffer_length);

		if(!error)
		{
			obuffer_length = 0;
			error_count = 0;
		}
		else
		{

			DEBUG_PRINTF("radio error %d\n",error);
			error_count++;
			if (error_count > max_error_count)
			{
				obuffer_length = 0;
				error_count = 0;
			}
		}
	}

	return true;
}



char* RadioComm::Message()
{
  if (msg_flag == OK)
    msg_flag = NOK;

  return (char*) ibuffer;
}
  

int RadioComm::Message(BYTE* mes)
{
  int i = 0;

  while ( (mes[i] != (char) 0) && (i<max_buffer_length) )
  {
    obuffer[i] = (BYTE) mes[i];
    i++;
  }

  obuffer_length = i;
  return OK;
}


int RadioComm::Message(const char* mes)
{
  int i = 0;

  while ((mes[i]!=(char)0) && (i<max_buffer_length) )
  {
    obuffer[i] = (BYTE) mes[i];
    i++;
  }
  obuffer[i]=0; // null terminate the string
  obuffer_length = i;
  return OK;
}



}; // namespace EyeMind

#endif //EYEBOT