/************************************************************************/
/*                                                                      */
/* Program Name: colormap-read.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             */
/*                                                                      */
/************************************************************************/

/*
 * History
 * ------------------------------------------------
 * Author     Date      Comments
 * Deepak     08/16/08  Initial Authoring.
 */
#include "nxtcamlib.nxc"

const byte sensorPort  =  IN_1;
#define ADDR     0x02

//Print sensor information from its register
void ShowSensorInfo(byte prt, byte Addres )
{
   if( i2cread(prt,Addres,0x00,1)>1)
   {
		 // sensor name
     TextOut(0, LCD_LINE8, i2cReadString(prt, Addres, 0x10, 8), false);
		 // sensor firmware version.
     TextOut(50, LCD_LINE8, i2cReadString(prt, Addres, 0x00, 8), false);
   }
}

task main()
{
	int init, n, count, i, j;
  byte message[20];
  byte nByteReady = 0;
  byte buf[20], red[16], green[16], blue[16];
	string msg, x;

  init = NXTCam_Init(sensorPort, ADDR);

	ClearScreen();
	ShowSensorInfo(sensorPort, ADDR);
  NXTCam_SendCommand(sensorPort, ADDR, 'G'); // Get the colormap

  count = 8;

  ArrayBuild(message, ADDR, 0x80); // red begins at 0x80
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  red[0] = buf[0];
	  red[1] = buf[1];
	  red[2] = buf[2];
	  red[3] = buf[3];
	  red[4] = buf[4];
	  red[5] = buf[5];
	  red[6] = buf[6];
	  red[7] = buf[7];
	}

  ArrayBuild(message, ADDR, 0x88);
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  red[8] = buf[0];
	  red[9] = buf[1];
	  red[10] = buf[2];
	  red[11] = buf[3];
	  red[12] = buf[4];
	  red[13] = buf[5];
	  red[14] = buf[6];
	  red[15] = buf[7];
	}

  ArrayBuild(message, ADDR, 0x90); // green begins at 0x90
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  green[0] = buf[0];
	  green[1] = buf[1];
	  green[2] = buf[2];
	  green[3] = buf[3];
	  green[4] = buf[4];
	  green[5] = buf[5];
	  green[6] = buf[6];
	  green[7] = buf[7];
	}

  ArrayBuild(message, ADDR, 0x98);
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  green[8] = buf[0];
	  green[9] = buf[1];
	  green[10] = buf[2];
	  green[11] = buf[3];
	  green[12] = buf[4];
	  green[13] = buf[5];
	  green[14] = buf[6];
	  green[15] = buf[7];
	}

  ArrayBuild(message, ADDR, 0xA0); // blue begins at 0xA0
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  blue[0] = buf[0];
	  blue[1] = buf[1];
	  blue[2] = buf[2];
	  blue[3] = buf[3];
	  blue[4] = buf[4];
	  blue[5] = buf[5];
	  blue[6] = buf[6];
	  blue[7] = buf[7];
	}

  ArrayBuild(message, ADDR, 0xA8);
  while (I2CStatus(sensorPort, nByteReady) ==  STAT_COMM_PENDING);
  if(I2CBytes(sensorPort, message, count, buf)) {
	  blue[8] = buf[0];
	  blue[9] = buf[1];
	  blue[10] = buf[2];
	  blue[11] = buf[3];
	  blue[12] = buf[4];
	  blue[13] = buf[5];
	  blue[14] = buf[6];
	  blue[15] = buf[7];
	}

	msg = "Red map        ";
	TextOut(0, LCD_LINE1, msg, false);
	msg = "|  |  |  |  |  ";
	TextOut(0, LCD_LINE2, msg, false);
	TextOut(0, LCD_LINE6, msg, false);
	msg = "Press ORNGE btn";
	TextOut(0, LCD_LINE7, msg, false);
	msg = "               ";

	for ( j=0; j <= 4; j++) {
		x = NumToStr(red[j]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE3, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(red[j+5]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE4, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(red[j+10]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE5, msg, false);
	}

	i = 0;
	while (i == 0 ) {
		if ( ButtonPressed(BTNCENTER, true) ) {
		  i = 1;
		}
  }
	Wait(1000);

	msg = "Green map        ";
	TextOut(0, LCD_LINE1, msg, false);
	msg = "|  |  |  |  |  ";
	TextOut(0, LCD_LINE2, msg, false);
	TextOut(0, LCD_LINE6, msg, false);
	msg = "Press ORNGE btn";
	TextOut(0, LCD_LINE7, msg, false);

	msg = "               ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(green[j]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE3, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(green[j+5]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE4, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(green[j+10]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE5, msg, false);
	}

	i = 0;
	while (i == 0 ) {
		if ( ButtonPressed(BTNCENTER, true) ) {
		  i = 1;
		}
  }
	Wait(1000);

	msg = "blue map        ";
	TextOut(0, LCD_LINE1, msg, false);
	msg = "|  |  |  |  |  ";
	TextOut(0, LCD_LINE2, msg, false);
	TextOut(0, LCD_LINE6, msg, false);
	msg = "Press ORNGE btn";
	TextOut(0, LCD_LINE7, msg, false);

	msg = "               ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(blue[j]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE3, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(blue[j+5]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE4, msg, false);
	}
	msg = "                ";
	for ( j=0; j <= 4; j++) {
		x = NumToStr(blue[j+10]);
		msg = StrReplace(msg, j*3, x);
		TextOut(0, LCD_LINE5, msg, false);
	}

	i = 0;
	while (i == 0 ) {
		if ( ButtonPressed(BTNCENTER, true) ) {
		  i = 1;
		}
  }
	Wait(1000);


}