/*

This simple modification of OpenCV's 'camshiftdemo.c' adds support for OSC,
sending the 2D location of the center of the (ROI) Region Of Interest to a local
or remote host. 

It may be useful in the event you want to track the movement of regions (a cat,
a bicycle courier or the top of someone's head) and send the coordinates to an
application on another computer (like PureData, Blender), as control data.

It should compile just fine on a Linux system with OpenCV and LibLo installed.
If you're on a Debian or Ubuntu system, just:

	sudo apt-get install libcv-dev liblo-dev.

Then to compile it:

	gcc -lcv -lcvaux -lhighgui -llo -I/usr/include/opencv \
	-I/usr/include/lo camshiftOSC.c -o camshiftOSC	

Execute it as follows:
 
	./camshiftOSC <camera index> <IP> <port>

For example, to send to a client (like PureData or Processing) on the same host
you run this application, capturing from /dev/video1 on your machine, do:

	./camshiftOSC 1 127.0.0.1 4950 

NOTE: It will only start sending OSC data once you have selected a region for
tracking with the mouse. Play with the 'vmax' slider to improve tracking. 

There was no license in the header of the original file so I've just used the
GPLv3. It's compatible with the license that Intel uses anyway.

Julian Oliver <julian@julianoliver.com>

http://julianoliver.com

This file 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; either version 3, or (at your option) any later version.

This file is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE.  See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with GNU
Radio; see the file COPYING.  If not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Boston, MA 02110-1301, USA.

*/

#ifdef _CH_
#pragma package <opencv>
#endif

/* my additions */
#include "lo_types.h"
#include "lo.h"
#include "lo_endian.h"
#include "lo_types.h"
#include "lo_osc_types.h"
#include "lo_errors.h"
#include "lo_lowlevel.h"
#include <string.h>
#include <stdlib.h>

#ifndef _EiC
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#endif

IplImage *image = 0, *hsv = 0, *hue = 0, *mask = 0, *backproject = 0, *histimg = 0;
CvHistogram *hist = 0;

CvPoint2D32f *cursor;

int backproject_mode = 0;
int select_object = 0;
int track_object = 0;
int show_hist = 0;
CvPoint origin;
CvRect selection;
CvRect track_window;
CvBox2D track_box;
CvConnectedComp track_comp;
int hdims = 16;
float hranges_arr[] = {0,180};
float* hranges = hranges_arr;
int vmin = 10, vmax = 100, smin = 30;

CvFont font;
int bX;
int bY;
int bAng;

char sX[3];
char sY[3];

void on_mouse( int event, int x, int y, int flags, void* param )
{
    if( !image )
        return;

    if( image->origin )
        y = image->height - y;

    if( select_object )
    {
        selection.x = MIN(x,origin.x);
        selection.y = MIN(y,origin.y);
        selection.width = selection.x + CV_IABS(x - origin.x);
        selection.height = selection.y + CV_IABS(y - origin.y);
        
        selection.x = MAX( selection.x, 0 );
        selection.y = MAX( selection.y, 0 );
        selection.width = MIN( selection.width, image->width );
        selection.height = MIN( selection.height, image->height );
        selection.width -= selection.x;
        selection.height -= selection.y;
    }

    switch( event )
    {
    case CV_EVENT_LBUTTONDOWN:
        origin = cvPoint(x,y);
        selection = cvRect(x,y,0,0);
        select_object = 1;
        break;
    case CV_EVENT_LBUTTONUP:
        select_object = 0;
        if( selection.width > 0 && selection.height > 0 )
            track_object = -1;
        break;
    }
}


CvScalar hsv2rgb( float hue )
{
    int rgb[3], p, sector;
    static const int sector_data[][3]=
        {{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}};
    hue *= 0.033333333333333333333333333333333f;
    sector = cvFloor(hue);
    p = cvRound(255*(hue - sector));
    p ^= sector & 1 ? 255 : 0;

    rgb[sector_data[sector][0]] = 255;
    rgb[sector_data[sector][1]] = 0;
    rgb[sector_data[sector][2]] = p;

    return cvScalar(rgb[2], rgb[1], rgb[0],0);
}

int main( int argc, char** argv )
{
	// setup address and port.
  	lo_address addr = lo_address_new(argv[2], argv[3]);
    CvCapture* capture = 0;

	// setup a font
	cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX, .4, .4, 0.0, 1, CV_AA);

    capture = cvCaptureFromCAM(atoi(argv[1]));

    if( !capture )
    {
        fprintf(stderr,"Could not initialize capturing...\n");
        return -1;
    }

    printf( "Hot keys: \n"
        "\tESC - quit the program\n"
        "\tc - stop the tracking\n"
        "\tb - switch to/from backprojection view\n"
        "\th - show/hide object histogram\n"
        "To initialize tracking, select the object with mouse\n" );

    //cvNamedWindow( "Histogram", 1 );
    cvNamedWindow( "camshiftOSC", 1 );
    cvSetMouseCallback( "camshiftOSC", on_mouse, 0 );
    cvCreateTrackbar( "Vmin", "camshiftOSC", &vmin, 256, 0 );
    cvCreateTrackbar( "Vmax", "camshiftOSC", &vmax, 256, 0 );
    cvCreateTrackbar( "Smin", "camshiftOSC", &smin, 256, 0 );

    for(;;)
    {
        IplImage* frame = 0;
        int i, bin_w, c;

        frame = cvQueryFrame( capture );
        if( !frame )
            break;

        if( !image )
        {
            /* allocate all the buffers */
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
            hue = cvCreateImage( cvGetSize(frame), 8, 1 );
            mask = cvCreateImage( cvGetSize(frame), 8, 1 );
            backproject = cvCreateImage( cvGetSize(frame), 8, 1 );
            hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
            histimg = cvCreateImage( cvSize(320,200), 8, 3 );
            cvZero( histimg );
        }

        cvCopy( frame, image, 0 );
        cvCvtColor( image, hsv, CV_BGR2HSV );

        if( track_object )
        {
            int _vmin = vmin, _vmax = vmax;

            cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),
                        cvScalar(180,256,MAX(_vmin,_vmax),0), mask );
            cvSplit( hsv, hue, 0, 0, 0 );

            if( track_object < 0 )
            {
                float max_val = 0.f;
                cvSetImageROI( hue, selection );
                cvSetImageROI( mask, selection );
                cvCalcHist( &hue, hist, 0, mask );
                cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );
                cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 );
                cvResetImageROI( hue );
                cvResetImageROI( mask );
                track_window = selection;
                track_object = 1;

                cvZero( histimg );
                bin_w = histimg->width / hdims;
                for( i = 0; i < hdims; i++ )
                {
                    int val = cvRound( cvGetReal1D(hist->bins,i)*histimg->height/255 );
                    CvScalar color = hsv2rgb(i*180.f/hdims);
                    cvRectangle( histimg, cvPoint(i*bin_w,histimg->height),
                                 cvPoint((i+1)*bin_w,histimg->height - val),
                                 color, -1, 8, 0 );
                }
            }

            cvCalcBackProject( &hue, backproject, hist );
            cvAnd( backproject, mask, backproject, 0 );
            cvCamShift( backproject, track_window,
                        cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                        &track_comp, &track_box );
            track_window = track_comp.rect;

			// create variables for the center of the box
			cursor = &track_box.center;
			bX = cursor[0].x;
			bY = cursor[0].y;
            bAng = track_box.angle;

            if( backproject_mode )
                cvCvtColor( backproject, image, CV_GRAY2BGR );
            if( !image->origin )
                track_box.angle = -track_box.angle;

			float bAng = track_box.angle;
			// send the location of the center of the box, the angle and a
			// placeholder var
			lo_send(addr, "/my/osc/path", "iiff", bX, bY, bAng, 0.0);
			//printf("Sending: (%i, %i) to host %s on port %s \n", bX, bY, argv[2], argv[3]);
            cvEllipseBox( image, track_box, CV_RGB(255, 100, 0), 4, CV_AA, 0 );
			cvLine(image, cvPoint((cursor[0].x)-20, cursor[0].y), cvPoint((cursor[0].x)+20, cursor[0].y), CV_RGB(255, 255, 255), .5, CV_AA, 0);
			cvLine(image, cvPoint(cursor[0].x, (cursor[0].y-20)), cvPoint(cursor[0].x, (cursor[0].y)+20), CV_RGB(255, 255, 255), .5, CV_AA, 0);
			// places a dot in the center of the tracked area
			cvCircle(image, cvPoint(cursor[0].x, cursor[0].y), 2, CV_RGB(255,0,0), 3, CV_AA, 0);
			
			gcvt(bX, 10, sX);	
			gcvt(bY, 10, sY);	

			char ss[10];
			strcpy(ss, sX);
			strcat(ss, ", ");
			strcat(ss, sY);

			char saddr[40];
			strcpy(saddr, "Sending to host ");
			strcat(saddr, argv[2]);
			strcat(saddr, " on port ");
			strcat(saddr, argv[3]);

			cvRectangle(image, cvPoint(0, (image->height)-20), cvPoint(image->width, image->height), CV_RGB(10, 10, 10), -1, 8, 0); 
			cvPutText(image, ss , cvPoint(bX+10, bY-10), &font, CV_RGB(255,0,0));
			cvPutText(image, saddr , cvPoint(5, (image->height)-7), &font, CV_RGB(255,255,255));
        }
        
        if( select_object && selection.width > 0 && selection.height > 0 )
        {
            cvSetImageROI( image, selection );
            cvXorS( image, cvScalarAll(255), image, 0 );
            cvResetImageROI( image );
        }

        cvShowImage( "camshiftOSC", image );
        cvShowImage( "Histogram", histimg );

        c = cvWaitKey(10);
        if( (char) c == 27 )
            break;
        switch( (char) c )
        {
        case 'b':
            backproject_mode ^= 1;
            break;
        case 'c':
            track_object = 0;
            cvZero( histimg );
            break;
        case 'h':
            show_hist ^= 1;
            if( !show_hist )
                cvDestroyWindow( "Histogram" );
            else
                cvNamedWindow( "Histogram", 1 );
            break;
        default:
            ;
        }
    }

    cvReleaseCapture( &capture );
    cvDestroyWindow("camshiftOSC");

    return 0;
}

#ifdef _EiC
main(1,"camshiftOSC.c");
#endif