/*	track.c
 *
 *	Experimental motion tracking using mini_ssc lib.
 *
 *	Copyright 2000, Jeroen Vreeken
 *	This program is published under the GNU Public license
 */
#include "motion.h"
#include "conf.h"
#include "track.h"
#include <mini_ssc.h>


struct trackoptions track={
	0,	/*dev open*/
	0,	/*x*/
	0,	/*y*/
	0,	/*int port*/
	0,	/*int baud*/
	0,	/*int servox*/
	0,	/* servoy*/
	0,	/*int maxx;*/
	0,	/*int maxy;*/
	0,	/*step;*/
	0	/* wait;*/
};


int track_center(void)
{
	if (!track.dev) {
		if (ssc_open(track.port, track.baud)) {
			perror("Unable to open ssc device");
			exit(1);
		}
	}
	ssc_move(track.servox, track.maxx/2);
	ssc_move(track.servoy, track.maxy/2);
	usleep(track.wait * 1000);

	return 0;
}

int track_move(char *image_out, int width, int height)
{
	int newx=track.x, newy=track.y;
	struct coord center;
	
	center=locate_diffs_center(image_out, width, height);
	if (center.x < track.maxx/4) newx=track.x-track.step;
	if (center.x > track.maxx/4*3) newx=track.x+track.step;
	if (center.y < track.maxy/4) newy=track.y-track.step;
	if (center.y > track.maxx/4*3) newy=track.y+track.step;
	if (newx < 0) newx=0;
	if (newx > track.maxx) newx=track.maxx;
	if (newy < 0) newy=0;
	if (newy > track.maxy) newy=track.maxy;
	if (newx!=track.x || newy!=track.y) {
		ssc_move(track.servox, newx);
		ssc_move(track.servoy, newy);
		usleep(track.wait * 1000);
		track.x=newx;
		track.y=newy;
		return 1;
	}
	return 0;
}