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

#define P 50
#define I 50
#define D 50

#include "nxtcamlib-default.nxc"
// Global variables.
int cb; // Current blob index to display
int nblobs;  // number of blobs that are in view of NXTCam (value between 0 and 8)
int stop_flag=0;
int bc[10];
int bl[10];
int bt[10];
int br[10];
int bb[10];
int xcent=0;
int ycent=0;
float xdif=0;
float ydif=0;
int xmove=0;
int ymove=0;
string msg;
string str;
string buf2;
int count=0;
int i=0;
int n;
int stop_now = 0;
int init;

//paresh junk
int xmove_previous = 0;
int ymove_previous = 0;

//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 move()
{
Wait(2000);
while (true)
{
NXTCam_GetBlobs(camPort, CAMADDR, nblobs, bc, bl, bt, br, bb);
//                            top-left X, top left Y, bottom right X, bottom right Y
if(nblobs>0){
xcent=((br[i]-bl[i])/2)+bl[i];
ycent=((bt[i]-bb[i])/2)+bb[i];
xdif=xcent-88;
ydif=ycent-72;
xmove=(xdif/88)*100;
ymove=(ydif/72)*20;
if(abs(xdif)>20)
OnFwdRegPID(OUT_A, xmove, OUT_REGMODE_SPEED, 40, 40, 90); // regulate speed
if(abs(ydif)>15)
OnFwdRegPID(OUT_B, ymove, OUT_REGMODE_SPEED, 40, 40, 90); // regulate speed
}
}
}


task print()
{
   while (true) {
    ClearScreen();
    DrawRectangle(0, 0, 176/3, 144/3);
    msg = "xc:";
    msg += NumToStr(xcent);
    msg += "      ";
    TextOut(60, LCD_LINE3, msg, false);
    msg = "yc:";
    msg += NumToStr(ycent);
    msg += "      ";
    TextOut(60, LCD_LINE4, msg, false);
    msg = "xd:";
    msg += NumToStr(xdif);
    msg += "      ";
    TextOut(60, LCD_LINE5, msg, false);
    msg = "yd:";
    msg += NumToStr(ydif);
    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);
    if ( stop_flag > 1 ) {  // if stop is indicated by pressing button
                            // twice or more,
       Wait(100);          // wait for a second for all the communicaiton
                            // to finish
       stop_now = 1;
       Off(OUT_ABC);        // and break from the loop to exit.
       PlayTone(440, 30);
       NXTCam_SendCommand(camPort, CAMADDR, 'D'); // disable tracking
       Wait(500);
       StopAllTasks();
    }
}
}

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
        cb = 1;
    }
    if (ButtonPressed(BTNRIGHT, true) ) {
        while(ButtonPressed(BTNRIGHT, true));              //debounce the switch
        cb = 2;
    }

  }// end of while
    Wait(500);   // don't hog the CPU.
}

task main ()
{
  ShowSensorInfo(camPort, CAMADDR);
  Wait(130);
  // Initialise the camera
  init = NXTCam_Init(camPort, CAMADDR);

  NXTCam_SendCommand(camPort, CAMADDR, 'E'); // enable tracking

  Wait(100);

  cb = 0;
  // Setup button handler
  //StartTask(button_handler);
  n = 0;
  Precedes(move);
}