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

const byte camPort  =  IN_1;

#define CAMADDR     0x02
#include "nxtcamlib.nxc"

string format_hex ( int loop ) {
  int j;
  string s;
  int b = 0xF0;
  int a = 0x0F;

  j = (loop&b)>>4;

  s = "";
  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  }

  j = loop&a;

  switch ( j ) {
  case 1:
      s += "1";
      break;
  case 2:
      s += "2";
      break;
  case 3:
      s += "3";
      break;
  case 4:
      s += "4";
      break;
  case 5:
      s += "5";
      break;
  case 6:
      s += "6";
      break;
  case 7:
      s += "7";
      break;
  case 8:
      s += "8";
      break;
  case 9:
      s += "9";
      break;
  case 10:
      s += "A";
      break;
  case 11:
      s += "B";
      break;
  case 12:
      s += "C";
      break;
  case 13:
      s += "D";
      break;
  case 14:
      s += "E";
      break;
  case 15:
      s += "F";
      break;
  case 16:
      s += "0";
      break;
  case 0:
      s += "0";
      break;
  }
  return (s);
}

string format_bin ( int loop ) {

  string s;
  int j;
  int b = 0x80;
  s = "";
  for ( j = 0; j < 8; j++) {
    if ( loop&b ) {
      s += NumToStr(1);
    } else {
      s += NumToStr(0);
    }
    b = b>>1;
  }
  return (s);
}

// Global
int nblobs;
int stop_flag;
int loc;

//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 ()
{

  string msg;
  string str;
  string buf2;
  int i;
	int init;
	byte result;
	int count;
	loc = 0;
  
  // Initialise the camera
  init = NXTCam_Init(camPort, CAMADDR);

  //ShowSensorInfo(camPort, CAMADDR);
	// result = my_i2cread(camPort, CAMADDR, 0x10, 8);

	msg = "";
	for ( i=0; i < 5; i++ ) {
		result = NXTCam_Read_CCD_Register(camPort, CAMADDR, 0x01+i);
		msg += format_hex(result);
		msg += " ";
		TextOut(0, LCD_LINE1, msg, false);
		Wait(1000);
	}

	// set the 0x02 register to value 0xEE
	NXTCam_Write_CCD_Register(camPort, CAMADDR, 0x02, 0xEE);

	msg = "";
	for ( i=0; i < 5; i++ ) {
		result = NXTCam_Read_CCD_Register(camPort, CAMADDR, 0x01+i);
		msg += format_hex(result);
		msg += " ";
		TextOut(0, LCD_LINE3, msg, false);
		Wait(1000);
	}

	TextOut(0, LCD_LINE4, i2cReadString(camPort, CAMADDR, 0x10, 8), false);
  TextOut(0, LCD_LINE5, i2cReadString(camPort, CAMADDR, 0x42, 8), false);

	until(ButtonPressed(BTNCENTER, true));

}