/***********************************************************************
   (C) Copyright, 1990 by Dean Rubine, Carnegie Mellon University
    Permission to use this code for noncommercial purposes is hereby granted.
    Permission to copy and distribute this code is hereby granted provided
    this copyright notice is retained.  All other rights reserved.
 **********************************************************************/

#include <Types.h>
#include <memory.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#include "util.h"	/* for allocate() */
#include "bitvector.h"
#include "matrix.h"	/* contains Vector and associated functions */
#include "fv.h"	

#ifdef MAC
/* in order to be abe to use the library in code resources (no globals...) */
#define dist_sq_threshold DIST_SQ_THRESHOLD
#define se_th_rolloff SE_TH_ROLLOFF

#else
const FLOAT dist_sq_threshold = DIST_SQ_THRESHOLD;
const FLOAT se_th_rolloff = SE_TH_ROLLOFF;
double invcos2();
static FuncTab acos2;
#endif

#define	EPS	(1.0e-4)


FV
FvAlloc ()
{
	register FV fv;
	
#ifndef MAC
	Z(T) acos2 = GenFuncTab("acos2", 100, -1.0, 1.0, invcos2); 
#endif
	fv = allocate(1, struct fv);
	fv->y = NewVector(NFEATURES);
	FvInit(fv);
	return fv;
}

void    
FvFree (FV fv)
{
    FreeVector (fv->y);
#ifdef MAC
	DisposPtr ((Ptr) (void*) fv);
#else
	free ( (void*) fv)
#endif
}

void
FvInit (fv)
register FV fv;
{
	register int i;

	fv->npoints = 0;
	fv->initial_sin = fv->initial_cos = 0.0;
	fv->maxv = 0;
	fv->path_r = 0;
	fv->path_th = 0;
	fv->abs_th = 0;
	fv->sharpness = 0;
	fv->maxv = 0;
	for(i = 0; i < NFEATURES; i++)
		fv->y[i] = 0.0;
}

void
FvAddPoint (fv, x, y, t)
register FV fv; int x, y; long t;
{
	FLOAT dx1, dy1, magsq1;
	FLOAT dot, cos2, th, absth, d;
#ifdef USE_TIME
	FLOAT v;
#endif
	long lasttime;

	if (++fv->npoints == 1) {
		fv->starttime = fv->endtime = t;
		fv->startx = fv->endx = fv->minx = fv->maxx = x;
		fv->starty = fv->endy = fv->miny = fv->maxy = y;
		fv->endx = x; fv->endy = y;
		return;
	}

	dx1 = x - fv->endx; dy1 = y - fv->endy;
	magsq1 = dx1 * dx1 + dy1 * dy1;

	if(magsq1 <= dist_sq_threshold) {
		fv->npoints--;
		return;		/* ignore this point */
	}

	if(x < fv->minx) fv->minx = x;
	if(x > fv->maxx) fv->maxx = x;
	if(y < fv->miny) fv->miny = y;
	if(y > fv->maxy) fv->maxy = y;

	lasttime = fv->endtime;
	fv->endtime = t;

	d = sqrt(magsq1);
	fv->path_r += d;

	/* calculate initial theta */
	/*if(fv->npoints == 2 || fv->npoints == 3) {*/
	/*if(fv->npoints == 3) {	/*debug*/
	/*if(fv->path_r >= kludge['r'] && fv->path_r <= kludge['R']) {*/
	/*if(fv->npoints <= 3 || fv->path_r <= kludge['R']) {*/
	if(fv->npoints == 3 || (fv->npoints > 3 && fv->path_r <= 0.0)) {

		FLOAT magsq, dx, dy, recip;
		dx = x - fv->startx; dy = y - fv->starty;
		magsq = dx * dx + dy * dy;
		if(magsq > dist_sq_threshold)  {
			/* find angle w.r.t. positive x axis e.g. (1,0) */
			recip = 1 / sqrt(magsq);
			fv->initial_cos = dx * recip;
			fv->initial_sin = dy * recip;
		}
	}


	if(fv->npoints >= 3) {
		/* Z(T) {
			dot = dx1 * fv->dx2 + dy1 * fv->dy2;
			cos2 = dot*dot / (magsq1 * fv->magsq2);
			th = EvalFuncTab(acos2, dot < 0 ? -cos2 : cos2);
			fv->abs_th += th;
			fv->sharpness += th*th;
			if(dx1 * fv->dy2 < fv->dx2 * dy1) / * sin th < 0 ? * /
				th = -th;
			fv->path_th += th;
		}
		else { */
			th = absth = atan2(dx1 * fv->dy2 - fv->dx2 * dy1,
					     dx1 * fv->dx2 + dy1 * fv->dy2);
			if(absth < 0) absth = -absth;
			fv->path_th += th;
			fv->abs_th += absth;
			fv->sharpness += th*th;
		/* } */
#ifdef PF_MAXV
		if(fv->endtime > lasttime &&
		   (v = d / (fv->endtime - lasttime)) > fv->maxv)
			fv->maxv = v;
#endif
	}

	fv->endx = x; fv->endy = y;
	fv->dx2 = dx1; fv->dy2 = dy1;
	fv->magsq2 = magsq1;
}

Vector
FvCalc(fv)
register FV fv;
{
	FLOAT bblen, selen, factor;

	if(fv->npoints <= 1)
		return fv->y;		/* a feature vector of all zeros */

	fv->y[PF_INIT_COS] = fv->initial_cos;
	fv->y[PF_INIT_SIN] = fv->initial_sin;

	bblen = hypot(fv->maxx - fv->minx, fv->maxy - fv->miny);

	fv->y[PF_BB_LEN] = bblen;

	if(bblen * bblen > dist_sq_threshold) 
		fv->y[PF_BB_TH] = atan2(fv->maxy - fv->miny,
					fv->maxx - fv->minx);

	selen = hypot(fv->endx - fv->startx, fv->endy - fv->starty);
	fv->y[PF_SE_LEN] = selen;

	factor = selen * selen / se_th_rolloff;
	if(factor > 1.0) factor = 1.0;
	factor = selen > EPS ? factor/selen : 0.0;
	fv->y[PF_SE_COS] = (fv->endx - fv->startx) * factor;
	fv->y[PF_SE_SIN] = (fv->endy - fv->starty) * factor;

	fv->y[PF_LEN] =  fv->path_r;
	fv->y[PF_TH] = fv->path_th;
	fv->y[PF_ATH] = fv->abs_th;
	fv->y[PF_SQTH] = fv->sharpness;
	fv->y[PF_NPOINTS] = (FLOAT) (fv->npoints > 1 ? 10 : 0);
#ifdef PF_DUR
	fv->y[PF_DUR] = (fv->endtime - fv->starttime)*.01;

/* thomas : this statement poses some problems to mips, and do
	esn't seem usefull to me, so suppressed it. I may be completely
	wrong... 
	Z(D) { 
		int i;
		for(i = NROWS(fv->y); i > 0; i--) fv->y[i] = fv->y[i-1];
		fv->y[0] = (fv->endtime - fv->starttime)*.01;
	}
	else
		NROWS(fv->y) = NFEATURES - 1;
*/
#endif

#ifdef PF_MAXV
	fv->y[PF_MAXV] = fv->maxv * 10000;
#endif
	return fv->y;
}

/*
  given m = cos^2(t) * sign(cos(t)) return t
  m must be between -1 and 1
  t will be between 0 and pi 
*/

#ifndef MAC
FLOAT
invcos2(m) FLOAT m;
{
	FLOAT x = m < 0 ? -sqrt(-m) : sqrt(m);
	FLOAT y = sqrt( 1.0 - x * x);	/ * y = sin (acos(x)) * /
	return atan2(y, x);
}
#endif