/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* Test code for the motion tracking algorithm used in chaseDrive.c */

#include "../h/cdefs.h"
#include <math.h>

#define TEST	1

typedef struct {
	float time;
	float p0;
	float v0;
	float a0;
} SEGMENT;

#define EPSILON 1e-7

computeSyncMotionSegments (seg, Dp, p0, v0, amax, time)
register SEGMENT *seg;
float Dp, p0, v0, amax;
float time;
{
	float vi, a, t0, Dpr, tr, D;
	float t1, t2, tc, a1, a2;

	t0 = FABS(v0)/amax;		/* min. time to bring velocity to 0 */
	tr = time - t0;			/* time we have left after that */
	if (v0 < 0.0)
	 { vi = -v0;
	   a = -amax;
	   Dpr = -Dp - 0.5*vi*t0;	/* remaining displacement */
	 }
	else
	 { vi = v0;
	   a = amax;
	   Dpr = Dp - 0.5*vi*t0;	/* remaining displacement */
	 }

	if (Dpr < 0.0)			/* overshoot case */
	 { D = amax*tr*tr/4.0 + Dpr;
	   if (D <= 0.0)		/* no velocity clipping */
	    { t2 = tr/2.0;
	      t1 = time-t2;
	      tc = 0.0;
	    }
	   else				/* clip velocity to stretch time */
	    { t2 = tr/2.0 - sqrt(amax*D)/amax;
	      t1 = t0+t2;
	      tc = time-t1-t2;
	    }
	   a1 = -a;
	   a2 = a;
	 }
	else if (Dpr >= vi*tr)
	 { float t = t0+tr/2.0;
	   D = amax*t*t - vi*t0 - Dpr;
	   if (D <= 0.0)
	    { t1 = tr/2.0;
	      t2 = time - t1;
	      tc = 0.0;
	    }
	   else
	    { t1 = tr/2.0 - sqrt(amax*D)/amax;
	      t2 = t0+t1;
	      tc = time-t1-t2;
	    }
	   a1 = a;
	   a2 = -a;
	 }
	else
	 { if (FABS(tr) < EPSILON)
	    { t2 = 0.0;
	    }
	   else
	    { t2 = Dpr/(tr*amax);
	    }
	   tc = tr;
	   t1 = t0-t2;
	   a1 = -a;
	   a2 = -a;
	 }

	seg->time = t1;
	seg->a0 = a1;
	seg->v0 = v0;
	seg->p0 = p0;
	seg++;

	if (tc != 0.0)
	 { seg->time = t1+tc;
	   seg->a0 = 0.0;
	   seg->v0 = v0 + t1*a1;
	   seg->p0 = p0 + t1*(v0 + t1*(0.5*a1));
	   seg++;
	 }
	
	seg->time = time;
	seg->a0 = a2;
	seg->v0 = -a2*t2;
	seg->p0 = p0 + Dp - 0.5*seg->v0*t2;
	seg++;

	seg->time = HUGE;
}

computeAsyncMotionSegments (seg, Dp, p0, v0, vmax, amax)
register SEGMENT *seg;
float Dp, p0, v0, vmax, amax;
{
	float vi, a, t0, Dpr, D;
	float t1, t2, tc, a1, a2;

	t0 = FABS(v0)/amax;
	if (v0 < 0.0)
	 { vi = -v0;
	   a = -amax;
	   Dpr = -Dp - 0.5*vi*t0;
	 }
	else
	 { vi = v0;
	   a = amax;
	   Dpr = Dp - 0.5*vi*t0;
	 }

	if (Dpr < 0.0)
	 { D = - Dpr - vmax*vmax/amax;
	   if (D < 0.0)
	    { t2 = sqrt (-Dpr/amax);
	      t1 = t2+t0;
	      tc = 0.0;
	    }
	   else
	    { t2 = vmax/amax;
	      t1 = t2+t0;
	      tc = D/vmax;
	    }
	   a1 = -a;
	   a2 = a;
	 }
	else if (vi <= vmax)
	 { D = Dpr - vmax*vmax/amax + amax*t0*t0;
	   if (D < 0.0)
	    { t2 = sqrt(Dpr/amax + t0*t0);
	      t1 = t2-t0;
	      tc = 0.0;
	    }
	   else
	    { t2 = vmax / amax;
	      t1 = t2-t0;
	      tc = D / vmax;
	    }
	   a1 = a;
	   a2 = -a;
	 }
	else
	 { t2 = vmax / amax;
	   tc = Dpr / vmax;
	   t1 = t0-t2;
	   a1 = -a;
	   a2 = -a;
	 }

	seg->time = t1;
	seg->a0 = a1;
	seg->v0 = v0;
	seg->p0 = p0;
	seg++;

	if (tc != 0.0)
	 { seg->time = t1+tc;
	   seg->a0 = 0.0;
	   seg->v0 = v0 + t1*a1;
	   seg->p0 = p0 + t1*(v0 + t1*(0.5*a1));
	   seg++;
	 }
	
	seg->time = t1+tc+t2;
	seg->a0 = a2;
	seg->v0 = -a2*t2;
	seg->p0 = p0 + Dp - 0.5*seg->v0*t2;
	seg++;

	seg->time = HUGE;
}

#define FSGN(x)	((x) < 0.0 ? -1 : 1)

float computeMinimumTime (dP, v0, vmax, amax)
float dP, v0, vmax, amax;
{
	float t0, D, dP0, time;

	t0 = FABS(v0)/amax;
	dP0 = 0.5*v0*t0;

	if (FSGN(dP-dP0) == FSGN(dP0))
	 { if (FABS(v0) > vmax)
	    { return (t0 + FABS(dP-dP0)/vmax);
	    }
	   else
	    { D = FABS(dP+dP0);
	      time = -t0;
	    }
	 }
	else
	 { D = FABS(dP-dP0);
	   time = t0;
	 }
	if (D <= (vmax*vmax)/amax)
	 { time += 2*sqrt(D/amax);
	 }
	else
	 { time += vmax/amax + D/vmax;
	 }
	return (time);
}

#if TEST

#define AMAX	1.0
#define VMAX	2.0

float testdata [][3] = { 10.0, 0.0,	7.0,
			  8.0, 0.0,	6.0,
			  4.0, 0.0,	4.0,
			  1.0, 0.0, 	2.0,
			-10.0, 0.0,	7.0,
			 -8.0, 0.0,	6.0,
			 -4.0, 0.0,     4.0,
			 -1.0, 0.0,     2.0,

			  8.5, 3.0,	5.0,
			  8.0, 2.0,	5.0,
			  7.5, 1.0,	5.0,
			 
			  4.5, 3.0,	3.0,
			  2.0, 2.0,     2.0,
			  0.5, 1.0,	1.0,
			 
			  2.25,3.0,	6.0,
			  1.0, 2.0,	4.0,
			  0.25,1.0,	2.0,

			 -4.5, 3.0,	9.5,
			 -2.0, 2.0,	6.0,
			 -0.5, 1.0,	3.0,

			 -8.5,-3.0,	5.0,
			 -8.0,-2.0,	5.0,
			 -7.5,-1.0,	5.0,
			 
			 -4.5,-3.0,	3.0,
			 -2.0,-2.0,     2.0,
			 -0.5,-1.0,	1.0,
			 
			 -2.25,-3.0,	6.0,
			 -1.0,-2.0,	4.0,
			 -0.25,-1.0,	2.0,

			  4.5,-3.0,	9.5,
			  2.0,-2.0,	6.0,
			  0.5,-1.0,	3.0,
};

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

main()
{
	char *keytree, *tree_match_parse(), *tree_match_err_at();
	float amax = AMAX;
	float vmax = VMAX;
	float Dp, v0, time;
	SEGMENT seg[4];

	keytree = tree_match_parse("					\
(	quit								\
	autotest							\
	set 								\
	 (amax 								\
	   (%f", &amax, ")						\
	  vmax								\
	   (%f", &vmax, "))						\
	show								\
	minimumTime							\
	 (%f", &Dp, ",,,		\"delta P\"			\
	   (%f", &v0, ",,,		\"v0\"))			\
	syncMotion							\
	 (%f", &Dp, ",,,		\"delta P\"			\
	   (%f", &v0, ",,,		\"v0\"				\
	     (%f", &time, ",,,		\"time\")))			\
	asyncMotion							\
	 (%f", &Dp, ",,,		\"delta P\"			\
	   (%f", &v0, ",,,		\"v0\"))			\
)",0);
#ifdef notdef
	[free lock]							\
	 (all-joints							\
	  one-joint							\
	   (%d",&joint,",1,6,		\"joint number (1 to 6)\")	\
	 )								\
	show (encoder-values)
#endif

	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	do
 	 { tree_match (keytree, "TRACKTEST> ", key_buf);

	   if (COMMAND("show"))
	    { printf ("amax = %g\n", amax);
	      printf ("vmax = %g\n", vmax);
	    }
	   else if (COMMAND("autotest"))
	    {
	    }
	   else if (COMMAND ("minimumTime"))
	    { float time;

	      time = computeMinimumTime (Dp, v0, vmax, amax);
	      printf ("%g\n", time);
	    }
	   else if (COMMAND ("syncMotion"))
	    { int i;
	      time = computeSyncMotionSegments (seg, Dp, 0.0, v0, amax, time);
	      printf ("JNT       time         p0        v0        a0\n");
	      for (i=0; seg[i].time != HUGE; i++)
	       { printf ("%2d  %9.4f  %9.4f %9.4f %9.4f\n",
			  i, seg[i].time, seg[i].p0, seg[i].v0, seg[i].a0);
	       }
	    }
	   else if (COMMAND ("asyncMotion"))
	    { int i;
	      time = computeAsyncMotionSegments(seg, Dp, 0.0, v0,vmax,amax);
	      printf ("JNT       time         p0        v0        a0\n");
	      for (i=0; seg[i].time != HUGE; i++)
	       { printf ("%2d  %9.4f  %9.4f %9.4f %9.4f\n",
			  i, seg[i].time, seg[i].p0, seg[i].v0, seg[i].a0);
	       }
	    }
	 }
	while (!(COMMAND("quit")));
}

#endif
#if 0


main()
{
	int i;
	float time;

	for (i=0; i<ARRAY_SIZE(testdata); i++)
	 { time = mintime (testdata[i][0], testdata[i][1], VMAX, AMAX);
	   if (FABS(time-testdata[i][2]) > EPSILON)
	    { 
printf ("error: input (%f %f) at testdata set %d: %f vs. %f\n",
	       testdata[i][0], testdata[i][1], i, time, testdata[i][2]);
	    }
	 }
}

foo()
{
	for (j=0; j<numJ; j++)
	 { if (!cb->torqueControl[j])
	    { motion = &cm->motions[j];
	      if (motion->running)
	       { motion->time += interval;
		 seg = motion->seg;
		 while (seg->time <= motion->time)
		  { seg++;
		  }
		 if (seg->time == HUGE)
		  { motion->running = 0;
		    cb->angSetpnt[j] = motion->target;
		  }
		 else
		  { float t = seg->time - motion->time;
		    cb->angSetpnt[j] = seg->p0 + t*(seg->v0 + t*(0.5*seg->a0));
		    motion->seg = seg;
		  }
	       }
		  
	    }
	 }
}
#endif
