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

task main ()
{
	int nblobs;
  int stop_now = 0;
  int bc[10];
  int bl[10];
  int bt[10];
  int br[10];
  int bb[10];
	int init, n, i;
	int center_x, center_y;
  string msg, topdown, leftright;

  init = NXTCam_Init(camPort, CAMADDR);

/*
 *  This program assumes one color is 
 *  uploaded on your NXTCam and tries 
 *  to keep object of that color at the
 *  center of the NXTCam.
 */

  while (stop_now == 0) {
		TextOut(40, LCD_LINE1, "", true);
    n ++;
    NXTCam_GetBlobs(camPort, nblobs, bc, bl, bt, br, bb);
		if ( nblobs > 0 ) {
		  center_x = (bl[0] + br[0])/2;
		  center_y = (bt[0] + bb[0])/2;
      msg = "CX ";
      msg += NumToStr(i);
      msg += ": ";
      msg += NumToStr(center_x);
      TextOut(40, LCD_LINE1, msg, false);
      msg = "CY ";
      msg += NumToStr(i);
      msg += ": ";
      msg += NumToStr(center_y);
      TextOut(40, LCD_LINE2, msg, false);
      msg = "               ";
      TextOut(0, LCD_LINE7, msg, false);
			/* cam center is at 44 x 72 */
			topdown = "stay put";
			leftright = "stay put";
			if ( center_y > 54 || center_y < 34 ) {
			  if ( center_y >54 ) {
				  topdown = "move cam down";
				} else if ( center_y < 34 ) {
				  topdown = "move cam";
				}
			}
			if ( center_x > 82 || center_x < 62 ) {
			  if ( center_x >82 ) {
				  leftright = "move cam right";
				} else if ( center_x < 62 ) {
				  leftright = "move cam left";
				}
			}
      TextOut(0, LCD_LINE5, topdown, false);
      TextOut(0, LCD_LINE6, leftright, false);
      Wait(400);
	  } else {
      TextOut(0, LCD_LINE5, "              ", false);
      TextOut(0, LCD_LINE6, "              ", false);
      msg = "no object found";
      TextOut(0, LCD_LINE7, msg, false);
      Wait(400);
		}
	}

}