/*
** Find the Cone
**
** Finds an orange cone in a picture, well ok it does not need to be a cone
** but in needs to be orange.
**
** Copyright 2004 Jim Wright, Jim Culbertson
*/

#include "stdafx.h"
#include "cv.h"
#include "cv.hpp"
#include "highgui.h"
#include <conio.h>
#include <math.h>
#include <stdio.h>
#include <windows.h>
#include <assert.h>
#include <commctrl.h>
#include <stdlib.h>
#include <process.h>
#include <stddef.h>
#include <conio.h>
#include <string.h>
#include <ctype.h>
#include <time.h>

//#define DEBUGROBOT 1

const int MINWIDTH         =  20;
const int MINLENGTH        =  20;
const int PICMOVELEFT      = 110;
const int PICMOVESMALLLEFT = 100;
const int PICCENTER        =  80;
const int PICMOVESMALLRIGHT = 70;

/*
**  The Saturation value, just change the min value
*/
const int MINS             = 150;
const int MAXS             = 255;

/*
** The V values are from the Cam Shift Demo twiddleing.
*/
const int MINV             = 128;
const int MAXV             = 255;


#define uchar unsigned char

#define START_BYTE  0x02
#define END_BYTE    0x03
#define CONV_BYTE   0x04
#define CONV_AMOUNT 0x10
#define STRINGPAD           2
#define MAXPORTSIZE        10
#define GOOD                0
#define FAIL               -1

#define ASCII_XON	 0x11
#define ASCII_XOFF 0x13

char gPort[MAXPORTSIZE+STRINGPAD];
int  gBaud;
int  gStopBit;
int  gParity;
int  gByteSize;

/*
** Read in the settings from the INI file
*/
void ReadIniFile(void)
{
	        GetPrivateProfileString("ComPortReader","ComPort", "COM1:",gPort,MAXPORTSIZE,".\\ComPortReader.ini");
  gBaud    = GetPrivateProfileInt("ComPortReader","BaudRate",19200,".\\ComPortReader.ini");
  gStopBit = GetPrivateProfileInt("ComPortReader","StopBit", 0,".\\ComPortReader.ini");
  gParity  = GetPrivateProfileInt("ComPortReader","Parity",  0,".\\ComPortReader.ini");
  gByteSize= GetPrivateProfileInt("ComPortReader","ByteSize",8,".\\ComPortReader.ini");
}

HANDLE OpenPort(void)
{
  HANDLE hCommPort;
    
  hCommPort = CreateFile( gPort,  
							GENERIC_READ | GENERIC_WRITE, 
							0, 
							0, 
							OPEN_EXISTING,
							FILE_ATTRIBUTE_NORMAL,
							0);

  return hCommPort;
}

int InitPort(HANDLE hCommPort)
{
  DCB dcb = {0};

  dcb.DCBlength = sizeof(dcb);
  
  /*
  ** Get the current com port settings
  */

  if (!GetCommState(hCommPort, &dcb))
  {
    return(FAIL);
  }

  /*
  ** set the com port to our settings
  */
  dcb.BaudRate	= gBaud;
  dcb.ByteSize	= gByteSize;
  dcb.Parity		= gParity;
  dcb.StopBits	= gStopBit;

  dcb.fDtrControl       = DTR_CONTROL_DISABLE;
  dcb.fRtsControl       = RTS_CONTROL_DISABLE;
  dcb.fOutxCtsFlow      = FALSE;
  dcb.fOutxDsrFlow      = FALSE;
  dcb.fDsrSensitivity   = FALSE;
  dcb.fOutX             = FALSE;
  dcb.fInX              = FALSE;
  dcb.fTXContinueOnXoff = FALSE;
  dcb.XonChar           = ASCII_XON;
  dcb.XoffChar          = ASCII_XOFF;
  dcb.XonLim            = 0;
  dcb.XoffLim           = 0;
  dcb.fParity			      = TRUE;

  /*
  ** set the com port to our settings
  */
  if (!SetCommState(hCommPort, &dcb))
  {
    return(FAIL);
  }

  return(GOOD);
}

int main(int argc, char* argv[])
{
  CvCamShiftTracker myTracker;
  IplImage *TheImageFromTheCamera;
  IplImage *TheBackImageForDebugging;
  CRect window;
  bool rc;
  int i;
	BOOL   ReturnCode;
  HANDLE hCommPort;
  uchar  WriteBuffer[100];
  DWORD  CharsWrite;

  ReadIniFile();
  printf("Opening Port:%s\n",gPort);
  printf("        Baud:%d\n",gBaud);
  printf("    Stop Bit:%d\n",gStopBit);
  printf("      Parity:%d\n",gParity);

  hCommPort = OpenPort();
  if (INVALID_HANDLE_VALUE == hCommPort)
	{
    printf("Error in opening the Com Port\n");
    return -1; 
  }

  if(GOOD != InitPort(hCommPort))
  {
    CloseHandle(hCommPort);
    printf("Error while setting up the com port\n");
    return -1;
  }



  /*
  ** Load the image that we want to generate our histogram from
  */
  IplImage *SolidOrange = cvLoadImage("Orange.bmp");   // <-- CHANGE IT TO YOUR NEEDS
  if (!SolidOrange) 
  {
    printf("NO INPUT IMAGE !\n");
    return 0;
  }

  /*
  ** Start up the camera and open a window for output.
  */
  CvCapture *capture = cvCaptureFromCAM(0);
  cvNamedWindow( "Source", 1 );

  /*
  ** Set the bounding box, since we are using an Orange Bitmap then it can be very small
  */
  window.top    = 10;
  window.left   = 10;
  window.bottom = 70;
  window.right  = 70;

  /*
  ** Not sure why 20 bins why not.  For these values run the CamShiftDemo and twiddle with those
  ** controls to get an idea of what to do
  */
  int dims[] = { 20 };
  myTracker.set_hist_dims( 1, dims );
  myTracker.set_hist_bin_range( 0, 0, 255 );
  myTracker.set_threshold( 0 );

  /*
  **  The Saturation value, just change the min value
  */
  myTracker.set_min_ch_val( 1, MINS );   // S MIN
  myTracker.set_max_ch_val( 1, MAXS );  // S MAX

  /*
  ** The V values are from the Cam Shift Demo twiddleing.
  */
  myTracker.set_min_ch_val( 2, MINV );   // V MIN
  myTracker.set_max_ch_val( 2, MAXV );  // V MAX

  /*
  ** Clear out any junk in the histogram to load in our values
  */
  myTracker.reset_histogram();

  for(i=0;i<20;i++)
  {
    /*
    ** Draw a box on the image.  This is important!  You just can't se the Window.
    ** Set the window and get a histogram.
    */
    cvRectangle(SolidOrange, cvPoint(window.left,window.top), cvPoint(window.right,window.bottom), 0xffffff, 1);
    rc = myTracker.set_window(RectToCvRect( window ));
    rc = myTracker.update_histogram( static_cast<CvImage *>(SolidOrange) );

    if(0 == rc)
    {
      printf("Problems in setting up the histogram!\n");
      return(0);
    }
  }

  /*
  ** On the production robot use the while 1, when debugging use the kbhit to get out.
  */
  while(1)
//  while(!kbhit())
  {
    /*
    ** Get a frame from the camera
    */
    cvGrabFrame(capture );
    TheImageFromTheCamera = cvRetrieveFrame( capture );
 
    /*
    ** The moment we all came here for....
    */
    myTracker.track_object(TheImageFromTheCamera);

    /*
    ** Get the bounding box and find the center of it.
    */
    CvRect rect = myTracker.get_window();
    
    int x      = rect.x + rect.width / 2;
    int y      = rect.y + rect.height / 2;

    /*
    ** Don't use the bounding box width and length
    */
    int length = (int)myTracker.get_length();
    int width  = (int)myTracker.get_width();

#ifdef DEBUGROBOT
    printf("X:%3d Y:%3d L:%8.3f W:%3d  ",x,y,length,width);
#endif

    /*
    ** Replace this section calling the robot DLL to tell it which direction the 
    ** cone is.
    */
    if((length>MINLENGTH)||(width>MINWIDTH))
    {
      if(x>PICMOVELEFT)
      {
        printf("Move Left\n");
        WriteBuffer[0] = 'A';
      }
      else
      {
        if(x>PICMOVESMALLLEFT)
        {
          printf("Move Left Slow\n");
          WriteBuffer[0] = 'B';
        }
        else
        {
          if(x>PICCENTER)
          {
            printf("Dead On\n");
            WriteBuffer[0] = 'C';
          }
          else
          {
            if(x>PICMOVESMALLRIGHT)
            {
              WriteBuffer[0] = 'D';
              printf("Move Right Slow\n");
            }
            else
            {
              WriteBuffer[0] = 'E';
              printf("Move Right\n");
            }
          }
        }
      }
    }
    else
    {
      WriteBuffer[0] = 'H';
      printf("Can't see it\n");
    }
 		ReturnCode = WriteFile(hCommPort, WriteBuffer, 1, &CharsWrite, NULL);

    sprintf((char*)WriteBuffer,"%04d\n",width);
 		ReturnCode = WriteFile(hCommPort, WriteBuffer, strlen((char*)WriteBuffer), &CharsWrite, NULL);

    /*
    ** Do the work to draw the X, it's worth it.
    */
    float cs = (float)cos( myTracker.get_orientation() );
    float sn = (float)sin( myTracker.get_orientation() );

    CvPoint p1 = {(int)(x + length * cs / 2), (int)(y + length * sn / 2)};
    CvPoint p2 = {(int)(x - length * cs / 2), (int)(y - length * sn / 2)};
    CvPoint p3 = {(int)(x + width * sn / 2), (int)(y - width * cs / 2)};
    CvPoint p4 = {(int)(x - width * sn / 2), (int)(y + width * cs / 2)};
    cvLine( TheImageFromTheCamera, p1, p2, CV_RGB(255,255,255) );
    cvLine( TheImageFromTheCamera, p4, p3, CV_RGB(255,255,255) );

    TheBackImageForDebugging = myTracker.get_back_project();
#ifdef DEBUGROBOT
    cvShowImage( "Source", TheBackImageForDebugging );
#else
    cvShowImage( "Source", TheImageFromTheCamera );
#endif

    cvSaveImage("Current.bmp",TheImageFromTheCamera);
    cvSaveImage("Back.bmp",TheBackImageForDebugging);

    /*
    ** Don't clogg the CPU!
    */
    Sleep(250);
  }

  cvReleaseCapture(&capture);
  CloseHandle(hCommPort);

  return 0;
}