/* Demo of modified Lucas-Kanade optical flow algorithm.
   See the printf below */

#ifdef _CH_
#pragma package <opencv>
#endif

#define CV_NO_BACKWARD_COMPATIBILITY

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

IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp;

int win_size = 10;
const int MAX_COUNT = 30;
const int MIN_COUNT = 15;
const double SPEED_CONSTANT = 0.08;
const double TURN_CONSTANT = 0.05;
const double TURN_INTERVAL = 0.025;
const double PIXELS_PER_METER = 2171.4;
CvPoint2D32f* points[3] = {0,0}, *swap_points;
char* status = 0;
int count = 0;
int flags = 0;
int add_remove_pt = 0;
CvPoint pt;
int showImage = 0;
int backAndForth = 0;
int backAndForthDirection = 0;
double backAndForthStartPointY = 0;
int forceImmediateMotorChange = 0;

const double backAndForthSpeedInterval = .02;

double direction = 0;
double speed = 0;


int main( int argc, char** argv )
{
	double backAndForthSpeed = SPEED_CONSTANT;
    CvCapture* capture = 0;

	//Player variables
	playerc_client_t *client;
	playerc_position2d_t *motors;
	playerc_position2d_t *compass;

	//Create a client object and connect to the server; the server must be running on "localhost" at port 6665
	client = playerc_client_create(NULL, "localhost", 6665);
	if (playerc_client_connect(client) != 0)
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	//Create a position2d proxy for motors (device id "position2d:0") and subscribe in read/write mode
	motors = playerc_position2d_create(client, 0);
	if (playerc_position2d_subscribe(motors, PLAYERC_OPEN_MODE) != 0)
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	//Create a position2d proxy for compass (device id "position2d:1") and subscribe in read/write mode
	compass = playerc_position2d_create(client, 1);
	if (playerc_position2d_subscribe(compass, PLAYERC_OPEN_MODE) != 0)
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	//Enable the motors (the compass doesn't need to be enabled)
	playerc_position2d_enable(motors, 1);

    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
    else if( argc == 2 )
        capture = cvCaptureFromAVI( argv[1] );

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

    /* print a welcome message */

    printf( "Hot keys: \n"
            "\tESC - quit the program\n"
			"\ti, j, k, and l are used to move forward, left, back, and right\n"
            "\ts - Toggle Showing image\n"
			"\tc - clear camera counter\n"
			"\tz - go back and forth in .5 meter intervals\n"
			"\tspace - stop motion\n"
			"\t1 - decrease speed\n"
			"\t2 - increase speed\n" );

	double total_x = 0;
	double total_y = 0;

	cvNamedWindow( "DotTracker", 0 );

	//intervalCounter is used to allow image to be updated only sporadically.
	int intervalCounter = 0;

    for(;;)
    {
		printf("***Count: %d\n",count); 
		intervalCounter++;
        IplImage* frame = 0;
        int i, j, k, c;
	
		//Capture the frame to be used in this loop.
        frame = cvQueryFrame( capture );
        if( !frame )
            break;

        if( !image )
        {
            /* allocate all the buffers */
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
            pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
            prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
            points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            points[2] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
            status = (char*)cvAlloc(MAX_COUNT);
            flags = 0;
        }

        cvCopy( frame, image, 0 );
        cvCvtColor( image, grey, CV_BGR2GRAY );

		//Still do this with existing points
        if( count > 0 )
        {
			double frame_total_x = 0;
			double frame_total_y = 0;
			int node_counter = 0;

            cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
                points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
            flags |= CV_LKFLOW_PYR_A_READY;
            for( i = k = 0; i < count; i++ )
            {
                // status array element is false, then point wasn't found.
                if( !status[i] )
                    continue;
				
				//This code only happens on points that still exist.
				node_counter += 1;
				frame_total_x += points[1][i].x - points[0][i].x;
				frame_total_y += points[1][i].y - points[0][i].y;

				//Move points higher up in array to fill void left by points not found.
                points[1][k++] = points[1][i];
                cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
        	}

			count = node_counter;
		
			if ( count > 0 )
			{
				total_x += (frame_total_x / node_counter);	
				total_y += (frame_total_y / node_counter);
			}
			printf ("Forward movement: %f m, Lateral movement: %f m\n", total_y / PIXELS_PER_METER, total_x / PIXELS_PER_METER );
        }

		//Move previous array into current array.
        CV_SWAP( prev_grey, grey, swap_temp );
        CV_SWAP( prev_pyramid, pyramid, swap_temp );
        CV_SWAP( points[0], points[1], swap_points );

/*GET MORE POINTS HERE IF NEEDED*/
        if( count < MIN_COUNT )
        {
            /* Snags more points*/
            IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 );
            IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 );
            double quality = 0.01;
            double min_distance = 10;

			//go ahead and get MAX_COUNT more points, figuring there may be overlap.
            int requestedCount = MAX_COUNT;
            cvGoodFeaturesToTrack( grey, eig, temp, points[2], &requestedCount,
                                   quality, min_distance, 0, 3, 0, 0.04 );
            cvFindCornerSubPix( grey, points[2], requestedCount,
                cvSize(win_size,win_size), cvSize(-1,-1),
                cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
            cvReleaseImage( &eig );
            cvReleaseImage( &temp );

			
			//copy the points over to point[0] if they're not close to existing points.
			i=0;
			while(i<requestedCount && count<MAX_COUNT)			
			{
				int invalid_point = 0;
				for (j=0; j<count; j++)
				{
					double dx = points[2][i].x - points[0][j].x;
	            	double dy = points[2][i].y - points[0][j].y;
	                if( dx*dx + dy*dy < 25 )
	                {
	                    invalid_point = 1;					
	                    continue;
	                }
				}
				//If point is valid
				if ( invalid_point < 1)
				{
					points[0][count++] = points[2][i];
            		cvCircle( image, cvPointFrom32f(points[0][count-1]), 3, CV_RGB(0,255,0), -1, 8,0);
				}
				i++;
			}

			//Clear the add flag.
            add_remove_pt = 0;
        }
/*END -- GET MORE POINTS*/

		if (showImage)
		{
        	cvShowImage( "DotTracker", image );
		}
		//This constant resending of messages is necessary because of a safety feature in the motor drivers.
		if (intervalCounter%10 == 0 || forceImmediateMotorChange)
		{
        	playerc_position2d_set_cmd_vel(motors, speed, 0, direction, 1);
			forceImmediateMotorChange = 0;
		}

		//Handle back and forth logic
		if ( backAndForth )
		{
			double location_m = (total_y - backAndForthStartPointY) / PIXELS_PER_METER;
			printf("Back and Forth distance: %f\n", location_m);
			//If moving forward, and past .5 meters, change direction
			if ( backAndForthDirection == 0 && location_m > .5)
			{
				backAndForthDirection = 1;
				speed = -backAndForthSpeed;
				forceImmediateMotorChange = 1;
			}
			else if ( backAndForthDirection == 1 && location_m < 0)
			{
				backAndForthDirection = 0;
				speed = backAndForthSpeed;
				forceImmediateMotorChange = 1;
			}
		}


/*** COMPASS READING CODE ***

		//playerc_position2d_get_geom(compass);
		playerc_client_read(client);
		printf ("\n***Compass: %f****\n", compass->pa);
*/
        c = cvWaitKey(10);
        if( (char)c == 27 )
            break;
        switch( (char) c )
        {
		    case 'c':
		        //count = 0;
				total_x = 0;
				total_y = 0;
		        break;
			//GO HOME.  Using motor readings, attempts to go back to origin.  Stops when it gets close.
		    case 'q':
				//Get latest readings.	
		        playerc_client_read(client);


		        break;
			case 'i':
				//Move forwards
				speed = backAndForthSpeed;
				forceImmediateMotorChange = 1;
				break;
			case 'k':
				//Move backwards
				speed = -backAndForthSpeed;
				forceImmediateMotorChange = 1;
				break;
			case 'j':
				//Turn left
				direction += TURN_INTERVAL;
				forceImmediateMotorChange = 1;
				break;
			case 'l':
				//Turn right
				direction -= TURN_INTERVAL;
				forceImmediateMotorChange = 1;
				break;		 
			case ' ':
				//Stop motion
				speed = 0;
				direction = 0;
				backAndForth = 0;
				forceImmediateMotorChange = 1;
				break;
			case 's':
				//Toggle image.
				showImage = (showImage + 1)%2;
				break;
			case 'z':
				//Go back and forth in half-meter increments
				backAndForth = (backAndForth + 1)%2;
				backAndForthStartPointY = total_y;
				backAndForthDirection = 0;
				speed = backAndForthSpeed;
				direction = 0;
				forceImmediateMotorChange = 1;
				break;
			case '1':
				//Decrement back and forth speed.
				backAndForthSpeed -= backAndForthSpeedInterval;
				forceImmediateMotorChange = 1;
				break;
			case '2':
				//Increment back and forth speed.
				backAndForthSpeed += backAndForthSpeedInterval;
				forceImmediateMotorChange = 1;
				break;
		    default:
		        ;
        }
    }

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

	//Shutdown Player stuff
	playerc_pos