/************************************************************************/
/*                                                                      */
/* Program Name: nxtcamlib.nxc                                          */
/* ===========================                                          */
/*                                                                      */
/* Copyright (c) 2008 by mindsensors.com                                */
/* Email: info (<at>) mindsensors (<dot>) com                           */
/*                                                                      */
/* 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; version 3 of the License.              */
/* Read the license at: http://www.gnu.org/licenses/gpl.txt             */
/*                                                                      */
/************************************************************************/

/*
  nxtcamlib.nxc
  NXC Library routines to simplify use of NXTcam 
  Written by Deepak Patil
  Jan 2008
  */

// Constants associated with NXTCam
#ifndef CAMADDR
#define CAMADDR     0x02
#endif

#define MAX_BLOBS 8
#define NXTCAM_REG_CMD      0x41
#define NXTCAM_REG_COUNT    0x42
#define NXTCAM_REG_DATA     0x43

// #define MS_DEBUG

// read data from sensor register(s)
byte i2cread(byte prt, byte adr, byte reg, byte cnt)
{
  byte result = -1;             // default: error value
  byte outbuf[];               // here we will get data
  byte cmdbuf[];               // register number holder
	int loop, nByteReady;

  ArrayBuild(cmdbuf, adr, reg);
  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(prt, nByteReady);
  }
  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    result = outbuf[0];       // read value

    if(cnt==2)
      result = result + outbuf[1]*256;
// if 2 registers (16 bit), then add the MSB part
  }

  return result;              // returns -1 if I2CBytes failed
}

/*
This function reads a string from the i2c device buffer which is connected to NXT.
prt: the port number on NXT (1, 2, 3, or 4) to which this device is connected.
adr: i2c address of the device.
reg: The device buffer is a large area. This is the register to read a string from this area.
cnt: Number of bytes to read from the device.
*/
string i2cReadString(byte prt, byte adr, byte reg, byte cnt)
{
  byte outbuf[];
  byte cmdbuf[];
  string temp = "";
  int n;

  ArrayBuild(cmdbuf, adr, reg);
  byte nByteReady = 0;

  n = STAT_COMM_PENDING;
  while ( n == STAT_COMM_PENDING ) {
    n = I2CStatus(prt, nByteReady);
  }

  if(I2CBytes(prt, cmdbuf, cnt, outbuf))
  {
    temp = ByteArrayToStr(outbuf);
  }
  return temp;
}

/*
Flush the NXT's buffer for a given port, and discard any data that may be there.
camPort: The port number on NXT to which device is connected.
*/
void NXTCam_Flush(byte camPort)
{
  int n;
  byte nByteReady = 0;
  byte buf[10];
  
  while (I2CStatus(camPort, nByteReady) ==  STAT_COMM_PENDING);
  n = I2CRead(camPort, 1, buf);
  while (n > 0) {
    while (I2CStatus(camPort, nByteReady) ==  STAT_COMM_PENDING);
    n = I2CRead(camPort, 1, buf);
  }
}

/*
Send a command to the i2c device.  The commands are interpreted by the device and acted upon.
camPort: The port number on NXT to which device is connected.
camAddr: The i2c address of the device.
*/
void NXTCam_SendCommand(byte camPort, byte camAddr, byte cmd) {
  byte cmdBuf[];
  ArrayBuild(cmdBuf, camAddr, NXTCAM_REG_CMD, cmd);
  I2CWrite(camPort, 0, cmdBuf);
  int status = I2CCheckStatus(camPort);
  while (status > NO_ERR)
    status = I2CCheckStatus(camPort);
  //Stop(status < NO_ERR);
}

// void NXTCam_Init() - Initialises camera ready to find blobs.
int NXTCam_Init(byte camPort, byte camAddr)
{
	string s, msg;
	int i;

  SetSensorLowspeed(camPort);

  s = "";
	i = 0;
	Wait(100);
	// wait until you can read the vendor name mndsnsrs
	// compare only first letter.
	// try a few times
  while (s[0] != 'm' && i < 200 ) {
  	s = i2cReadString(camPort, camAddr, 0x08, 8);
#ifdef MS_DEBUG
    msg = "connecting:";
    msg += s;
    msg += "  ";
    TextOut(0, LCD_LINE8, msg, true);
    Wait(10);
		msg = "i: ";
    msg += NumToStr(i);
    TextOut(0, LCD_LINE7, msg, false);
#else
    Wait(40);
#endif
		i++;
  }
  NXTCam_SendCommand(camPort, camAddr, 'A'); // Sort by size
  NXTCam_SendCommand(camPort, camAddr, 'E'); // Start finding
	return i;
}

/*
void NXTCam_GetBlobs() - loads the current blobs into the global data structure over I2C
Get the blob information of all the blobs that NXTCam is tracking.
There could be upto 8 blobs being tracked by NXTCam. 
This function will return color and coordinate information for all the blobs.
If the NXTCam has found 3 blobs, the information for 4th, 5th, 6th and 7th blob is not valid.
Port: the NXT port to which the device is connected.
Nblobs: number of blobs found by the NXTCam
Color[]: a array of colors for the blobs 
Left[], top[], right[] bottom[]: the coordinate values of the blobs.
*/
void NXTCam_GetBlobs(byte port, int &nblobs,
                     int &color[], int &left[],
                     int &top[], int &right[], int &bottom[])
{
  byte nByteReady = 0;
  byte msg[3];
  byte reply[5];
  int i;
  int n;
  int x;
  byte buf[10];
  string str;

  // Initialize the returning arrays with zeros.
  for (i = 0; i < 10; i++) {
    color[i]   = 0;
    left[i]   = -1;
    top[i]     = -1;
    right[i]   = -1;
    bottom[i] = -1;
  }

  NXTCam_Flush(port);

  // Request number of blobs from the count register
  // NXTCAM_REG_COUNT is where number of detected objects are stored.
  ArrayBuild(msg, CAMADDR, NXTCAM_REG_COUNT);

	while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

  n = I2CWrite(port, 1, msg);

  if ( n != NO_ERR ) {
#ifdef MS_DEBUG
    string msg2;
    msg2 = "WriteError1:";
    msg2 += NumToStr(n);
    msg2 += "  ";
    TextOut(0, LCD_LINE8, msg2, false);
#endif
    return;
  }

	while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

  // Get the reply and put into nblobs global
  n = I2CRead(port, 1, buf);
  if ( n != NO_ERR ) {
#ifdef MS_DEBUG
    string msg2;
    msg2 = "ReadError1:";
    msg2 += NumToStr(n);
    msg2 += "  ";
    TextOut(0, LCD_LINE8, msg2, false);
#endif
    return;
  }
	while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

  nblobs = buf[0]& 0x00FF;

  // work around for out of bounds nblobs value.
  if ( nblobs < 0 || nblobs > 8 ) {  
    nblobs = 0;
  }

  // Get nblobs of blob data from the camera
  for (i = 0; i < nblobs; i++) {

    // Request blob data
    x = NXTCAM_REG_DATA+(i*5);
    ArrayBuild(msg, CAMADDR, x);
		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
    n = I2CWrite(port, 5, msg);
    if ( n != NO_ERR ) {
#ifdef MS_DEBUG
      string msg2;
      msg2 = "WriteError2:";
      msg2 += NumToStr(n);
      msg2 += "  ";
      TextOut(0, LCD_LINE8, msg2, false);
#endif
      return;
    }

    // Get blob data reply
		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

    n = I2CRead(port, 5, buf);
    if ( n != NO_ERR ) {
#ifdef MS_DEBUG
      string msg2;
      msg2 = "ReadError2:";
      msg2 += NumToStr(n);
      msg2 += "  ";
      TextOut(0, LCD_LINE8, msg2, false);
#endif
      return;
    }

		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

    // Put data into global variables.
    color[i]   = buf[0] & 0x00FF;
    left[i]   = buf[1] & 0x00FF;
    top[i]     = buf[2] & 0x00FF;
    right[i]   = buf[3] & 0x00FF;
    bottom[i] = buf[4] & 0x00FF;

    // NXTCam_Flush(port);
  }

}

/*
This function draws a rectangle on the NXT LCD screen.
Draw a rectangle parameters:
top-left X, top left Y, bottom right X, bottom right Y
the NXTCam returns coordinates based on top left corner being zero-zero
where as NXT screen drawing is based on bottom left corner being zero-zero.
so correct the y coordinate.
*/
void DrawRectangle(byte X1,byte Y1,byte X2,byte Y2)
{
// vertical pixels: 63
  int ny1, ny2;
  ny1 = 63 - Y1;
  ny2 = 63 - Y2;
  RectOut (X1, ny1, X2-X1, ny2-ny1,false);
}



/*

This function prepares colormap for the objects.
Colormap[]: the prepared colormap is stored in this variable.
Object: the object number (1 to 8) for this colormap being defined.
Red_low, green_low, blue_low: the lowest range of that color you need to match.
Red_high, green_high, blue_high: hte highest range of that color you need to match.
 
pass a colormap array (of size 48)
object numbers range from 1 to 8
pass low and high range color values of RGB

More about colormaps:
The colormap is a 48 byte buffer.
From this buffer each of RGB color is assigned 16 bytes for that color.
i.e. first 16 bytes are for Red, next 16 for green and remaining 16 for blue.

These 16 bytes store matching preference for that color's absolute value ranges as
0-16-32-48-64-80-96-112-128-144-160-176-192-208-224-240-255
i.e. first byte is set for color range 0 to 16, second byte for range 16 to 32 and so on...
each bit in the byte contains mask for each object (i.e. 8 objects) which is set to 1 if you want matching object.

example -
if you want 1st object to match red color in range of 16 to 48, you will set
first bit to 1 in byte 1 and byte 2.
or if you want 4th object to match any shade of red (0 to 255) then you will set
4th bit of all 16 bytes to 1.


*/
void NXTCam_PrepareColormap(byte & colormap[], byte object,
                           byte red_low, byte red_high,
                           byte green_low, byte green_high,
													 byte blue_low, byte blue_high)
{
	/* 0-16-32-48-64-80-96-112-128-144-160-176-192-208-224-240-255
	 */
   
  byte mask, nmask;
	byte a;
	byte i, rl, rh, gl, gh, bl, bh;

	if ( object > 8 ) object = 8;

  mask = 0x01 << (8-object);
	nmask = 0xFF ^ mask;

	rl = red_low/16;
	rh = (red_high/16) + 1;
	gl = (green_low/16) + 16;
	gh = (green_high/16) + 17;
	bl = (blue_low/16) + 32;
	bh = (blue_high/16) + 33;

	if ( rl <  0 || rl > 16) rl = 0;
	if ( rh <  0 || rh > 17) rh = 17;
	if ( gl < 16 || gl > 32) gl = 16;
	if ( gh < 16 || gh > 33) gh = 33;
	if ( bl < 32 || bl > 47) bl = 32;
	if ( bh < 32 || bh > 48) bh = 47;

	for (i=0; i < 48; i++) { colormap[i] |= mask; }

	for (i= 0; i< rl; i++) { colormap[i] &= nmask; }
	for (i=rh; i< 16; i++) { colormap[i] &= nmask; }
	for (i=16; i< gl; i++) { colormap[i] &= nmask; }
	for (i=gh; i< 32; i++) { colormap[i] &= nmask; }
	for (i=32; i< bl; i++) { colormap[i] &= nmask; }
	for (i=bh; i< 48; i++) { colormap[i] &= nmask; }
	
  return ;
}

void NXTCam_WriteColormap (byte colormap[], byte addr, byte port)
{
	string msg, x0,x1,x2,x3,x4,x5,x6,x7, i0, loc;
	byte location, val0, val1, val2, val3, val4, val5, val6, val7;
	int i;
  byte nByteReady = 0;
  byte message[20];

	for (i=0; i< 6; i++) {
	  location = 0x80 + (i*8);
		val0 = colormap[(i*8)+0];
		val1 = colormap[(i*8)+1];
		val2 = colormap[(i*8)+2];
		val3 = colormap[(i*8)+3];
		val4 = colormap[(i*8)+4];
		val5 = colormap[(i*8)+5];
		val6 = colormap[(i*8)+6];
		val7 = colormap[(i*8)+7];

		ArrayBuild(message, addr, location,
										val0, val1, val2, val3, val4, val5, val6, val7);
		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
		I2CWrite(port, 0, message);

		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);

		x0 = NumToStr(val0);
		x1 = NumToStr(val1);
		x2 = NumToStr(val2);
		x3 = NumToStr(val3);
		x4 = NumToStr(val4);
		x5 = NumToStr(val5);
		x6 = NumToStr(val6);
		x7 = NumToStr(val7);

		i0 = NumToStr(i);
		loc = NumToStr(location);
		msg = "loc " + loc + ":   ";
		TextOut(0, LCD_LINE2, msg, false);
		msg = " " +x0+ " " +x1+ " " +x2 + " " + x3 + "   ";
		TextOut(0, LCD_LINE3, msg, false);
		msg = " " + x4+ " " +x5+ " " +x6+ " " +x7 + "   ";
		TextOut(0, LCD_LINE4, msg, false);

		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
		NXTCam_SendCommand(port, addr, 'S'); // Save the colormap
		while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING);
		Wait(2000);
	}

}


void NXTCam_i2cwrite(byte port, byte adr, byte reg, byte data)
{
  byte result = -1;             // default: error value
  byte cmdbuf[];               // register number holder
	int loop, n, nByteReady;

  ArrayBuild(cmdbuf, adr, reg, data);
  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(port, nByteReady);
  }

  // when the I2C bus is ready, send the message you built   LowspeedWrite
  n = I2CWrite(port, 0, cmdbuf);

  loop = STAT_COMM_PENDING;
  while ( loop == STAT_COMM_PENDING ) {
    loop = I2CStatus(port, nByteReady);
  }
}


byte NXTCam_Read_CCD_Register(byte port, byte addr, byte regToRead)
{
	byte result;

	NXTCam_i2cwrite(port, addr, 0x6B, 0x01);  // how many bytes to read
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x6C, regToRead);  // which register to read
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x41, 'H');  // H-command to get cam registers.
	Wait(800);

	// needs to be done again.
	NXTCam_i2cwrite(port, addr, 0x6B, 0x01);  // how many bytes to read
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x6C, regToRead);  // which register to read
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x41, 'H');  // H-command to get cam registers.
	Wait(800);

	result = i2cread(port, addr, 0x6D, 1);

	return result;
}

byte NXTCam_Write_CCD_Register(byte port, byte addr, byte regToWrite, byte value)
{
	byte result;

	NXTCam_i2cwrite(port, addr, 0x6B, 0x01);
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x6C, regToWrite);
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x6D, value);
	Wait(50);
	NXTCam_i2cwrite(port, addr, 0x41, 'C');  // C-command to set cam registers.
	Wait(500);

	return result;
}