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

// Global
int cb; // Current blob index to display
int nblobs;
int stop_flag;

// task button_handler() - increments blob index when right button is pressed, decrements
// when left button is pressed. Keeps values between 0 and 7.


task button_handler()
{
  while (true) {
  
     if ( ButtonPressed(BTNCENTER, true) ) {
         while (ButtonPressed(BTNCENTER, true)) ;            //debounce the switch
      stop_flag ++;
    }

    if ( ButtonPressed(BTNLEFT, true) ) {
         while (ButtonPressed(BTNLEFT, true)) ;            //debounce the switch
      if (cb <= 0) {
        cb = 0;
      } else {
        cb--;
      }
    }
    if (ButtonPressed(BTNRIGHT, true) ) {
        while(ButtonPressed(BTNRIGHT, true));              //debounce the switch
      if (cb > 7) {
        cb = 7;
      } else {
        cb++;
      }
    }

    if(cb>nblobs)  cb=nblobs;
  }// end of while
    Wait(500);   // don't hog the CPU.
}

//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 bc[10];
  int bl[10];
  int bt[10];
  int br[10];
  int bb[10];
  string msg;
  string str;
  string buf2;
  int i;
  int n;
  int stop_now = 0;
  int high_x, high_y;
	int init;
  
  
  high_x = 0;
  high_y = 0;

  // Initialise the camera
  init = NXTCam_Init(camPort, CAMADDR);

  // Start with blob 0
  cb = 0;

  // Setup button handler
  StartTask(button_handler);

  n = 0;
  while (stop_now == 0) {
    n ++;

    // Get the current blob data from the camera

    // Print the data on the screen
    ClearScreen();
		ShowSensorInfo(camPort, CAMADDR);

    msg = "n:";
    msg += NumToStr(n);
    msg += "  ";
    TextOut(65, LCD_LINE1, msg, false);
    msg += "      ";
    
    NXTCam_GetBlobs(camPort, nblobs, bc, bl, bt, br, bb);

    // draw the bounding box of
    // NXTCam field of view.
    // top-left X, top left Y, bottom right X, bottom right Y

    DrawRectangle(0, 0, 176/3, 144/3);
    for ( i = 0; i < nblobs; i++) {

      msg = "B:";
      msg += NumToStr(i);
      msg += "|";
      msg += NumToStr(nblobs);
      TextOut(60, LCD_LINE2, msg, false);
      msg = "t:";
      msg += NumToStr(bt[i]);
      msg += "      ";
      TextOut(60, LCD_LINE3, msg, false);
      msg = "l:";
      msg += NumToStr(bl[i]);
      msg += "      ";
      TextOut(60, LCD_LINE4, msg, false);
      msg = "b:";
      msg += NumToStr(bb[i]);
      msg += "      ";
      TextOut(60, LCD_LINE5, msg, false);
      msg = "r:";
      msg += NumToStr(br[i]);
      msg += "      ";
      TextOut(60, LCD_LINE6, msg, false);
      msg = "clr:";
      msg += NumToStr(bc[i]);
      msg += "      ";
      TextOut(60, LCD_LINE7, msg, false);

      DrawRectangle(bl[i]/3,bt[i]/3, br[i]/3,bb[i]/3);
      Wait(400);
    }

//    PlayTone(440, 30);

    if ( stop_flag > 1 ) {  // if stop is indicated by pressing button
                            // twice or more,
       Wait(1000);          // wait for a second for all the communicaiton
                            // to finish
       stop_now = 1;        // and break from the loop to exit.
    }

  }

}