/*
	arm.c
	Prints out one arm configuration to reach given target.
	Assumes number of links >= 3.
*/

#include 	<stdio.h>
#include	<math.h>
#define	ERROR	1
#define	X	0
#define	Y	1
#define	NLINKS	20
typedef	enum { FALSE, TRUE }	bool;

#define DIM     2               /* Dimension of points */
typedef int     tPointi[DIM];   /* type integer point */
typedef double  tPointd[DIM];   /* type double point */

#define	FUZZ	1.0e-10

void	ReadTarget( tPointi target );
int	ReadLinks( void );
void	PrintLinks( void );
void	PrintPointi( tPointi p );
void	PrintPointd( tPointd p );
bool	Solve3( int l1, int l2, int l3, tPointi target );
int	TwoCircles( tPointi c1, int r1, tPointi c2, int r2, tPointd p );
int	TwoCircles0a( int r1, tPointi c2, int r2, tPointd p );
int	TwoCircles0b( int r1, tPointi c2, int r2, tPointd p );
void	TwoCircles00( int r1, double a2, int r2, tPointd p );
void	SubVec( tPointi a, tPointi b, tPointi c );
bool	EqPointi( tPointi a, tPointi b );
int	Length2( tPointi v );
bool	Solve2( int l1, int l2, tPointi target, tPointd J );
bool	Nested( int l1, int l2, int l3, tPointi target );
bool	Solven( int nlinks );
double	RadDeg( double x );

int	linklen[NLINKS];	/* link lengths */
int	nlinks;			/* number of links */
tPointi	target;			/* target point */

main(argc, argv)
int	argc;
char	*argv[];
{

	ReadTarget( target );
	nlinks = ReadLinks( );

	if ( !Solven( nlinks ) ) 
		printf("Solven: no solutions!\n");
}


bool    Solven( int nlinks )
{
	int	i;
        int     m;              /* index of median link */
        int     l1, l2, l3;     /* length of links between kinks */
        int     totlength;      /* total length of all links */
        int     halflength;     /* floor of half of total */

	/* Compute total & half length. */
        totlength = 0;
        for ( i = 0; i < nlinks; i++ ) 
                totlength += linklen[i];
        halflength = totlength / 2;

	/* Find median link. */
        l1 = 0;
        for ( m = 0; m < nlinks; m++ ) {
                if ( (l1 + linklen[m]) > halflength)
                        break;
                l1 += linklen[m];
        }

        l2 = linklen[m];
        l3 = totlength - l1 - l2;
        if ( Solve3( l1, l2, l3, target ) )
                return  TRUE;
        else    return  FALSE;
}

bool    Solve3( int l1, int l2, int l3, tPointi target )
{
	tPointd Jk;	/* coords of kinked joint returned by Solve2 */
	tPointi J1;	/* Joint1 on x-axis */
        tPointi Ttarget;/* translated target */

	printf("==>Solve3: links = %d,%d,%d\n", l1, l2, l3);
	if ( Solve2( l1 + l2, l3, target, Jk ) ) {
		printf("Solve3: link1=%d, link2=%d, joint=",
			l1 + l2, l3);
		PrintPointd( Jk );
		return	TRUE;
	}
	else if ( Solve2( l1, l2 + l3, target, Jk ) ) {
		printf("Solve3: link1=%d, link2=%d, joint=",
			l1, l2 + l3);
		PrintPointd( Jk );
		return	TRUE;
	}
	else {	/* pin J0 to 0. */
		/* Shift so J1 is origin. */
		J1[X] = l1;	J1[Y] = 0;
		SubVec( target, J1, Ttarget );
		if ( Solve2( l2, l3, Ttarget, Jk ) ) {
			/* Shift solution back to origin. */
			Jk[X] += l1;
			printf("Solve3: link1=%d, link2=%d, link3=%d,\
				joints=", l1, l2, l3);
			PrintPointi( J1 );
			PrintPointd( Jk );
			return	TRUE;
		}
		else	return	FALSE;
	}
}
bool    Solve2( int l1, int l2, tPointi target, tPointd J )
{
        tPointi c1 = {0,0};     /* center of circle 1 */
        int     nsoln;          /* # of solns: 0,1,2,3(infinite) */

        nsoln = TwoCircles( c1, l1, target, l2, J );
	printf("<==Solve2: l1=%d, l2=%d; nsoln=%d\n", l1, l2, nsoln); 
        return	nsoln != 0;
}


/*
        TwoCircles finds the intersection points between two circles.
        This is the general routine, no assumptions.
	One intersection point is placed in p.
*/
int     TwoCircles( tPointi c1, int r1, tPointi c2, int r2, tPointd p)
{
        tPointi c;

	SubVec( c2, c1, c );
	return TwoCircles0a( r1, c, r2, p );
}

/*
        TwoCircles0a assumes that the first circle is centered 
	on the origin.  It handles all the special cases, returning
        the number of intersection points: 0, 1, 2, 3 (inf).
*/
int     TwoCircles0a( int r1, tPointi c2, int r2, tPointd p )
{
	int	dc2;	/* dist to center 2 squared */
	int	rplus2, rminus2;	/* (r1 +/- r2)^2 */
	double	f;	/* fraction along c2 for nsoln=1 */

	/* Handle special cases. */
	dc2 = Length2( c2 );
	rplus2  = (r1 + r2) * (r1 + r2);
	rminus2 = (r1 - r2) * (r1 - r2);

	/* No solution if c2 out of reach + or -. */
	if ( ( dc2 > rplus2 ) || ( dc2 < rminus2 ) )
		return	0;

	/* One solution if c2 just reached. */
	/* Then solution is r1-of-the-way (f) to c2. */
	if ( dc2 == rplus2 ) {
		f = r1 / (double)(r1 + r2);
		p[X] = f * c2[X];	p[Y] = f * c2[Y];
		return	1;
	}
	if ( dc2 == rminus2 ) {
		if ( rminus2 == 0 ) {	/* Circles coincide. */
			p[X] = r1;	p[Y] = 0;
			return	3;
		}
		f = r1 / (double)(r1 - r2);
		p[X] = f * c2[X];	p[Y] = f * c2[Y];
		return	1;
	}

        /* Two intersections. */
	return	TwoCircles0b( r1, c2, r2, p );

}
/*
        TwoCircles0b also assumes that the first circle is centered       
        on the origin.  It rotates c2 to lie on the x-axis.
*/
int     TwoCircles0b( int r1, tPointi c2, int r2, tPointd p )
{
        double  a2;     /* center of 2nd circle when rotated to x-axis */
        tPointd q;      /* one solution when c2 on x-axis */
        double  cost, sint;     /* sine and cosine of angle of c2 */

	/* Rotate c2 to a2 on x-axis. */
        a2 = sqrt( (double) Length2( c2 ) );
        cost = c2[X] / a2;
        sint = c2[Y] / a2;

        TwoCircles00( r1, a2, r2, q );

        /* Rotate back */
        p[X] =  cost * q[X] + -sint * q[Y];
        p[Y] =  sint * q[X] +  cost * q[Y];
        
        return	2;
}

/*
        TwoCircles00 assume circle one is origin-centered, and
        that the center of the second circle is at (a2,0).
*/
void     TwoCircles00( int r1, double a2, int r2, tPointd p )
{
        /* Two intersections (only one returned in p). */
        p[X] = ( a2 + ( r1*r1 - r2*r2 ) / a2 ) / 2;
        p[Y] = sqrt( r1*r1 - p[X]*p[X] );
}
void	ReadTarget( tPointi target )
{
	int	i;

	for( i = 0; i < DIM; i++)
		scanf("%d", &target[i] );
	printf("Target (target) point = ");
	PrintPointi( target );
	putchar('\n');
}

void	PrintPointi( tPointi p )
{
	int	i;

	putchar('(');
	for ( i = 0; i < DIM; i++ ) {
		printf("%d", p[i]);
		if ( i != DIM-1 ) putchar(',');
	}
	putchar(')');
}

void	PrintPointd( tPointd p )
{
	int	i;

	putchar('(');
	for ( i = 0; i < DIM; i++ ) {
		printf("%g", p[i]);
		if ( i != DIM-1 ) putchar(',');
	}
	putchar(')');
	putchar('\n');
}

int	ReadLinks( void )
{
	int	length;

	nlinks = 0;
	while( scanf("%d", &length) != EOF ) {
		linklen[nlinks] = length;
		nlinks++;
	}
	PrintLinks( );
	return	nlinks;
}
		
void	PrintLinks( )
{
	int	i;

	for ( i=0; i < nlinks; i++)
		printf("%d:%d\t", i, linklen[i]);
	putchar('\n');
}
bool	EqPointi( tPointi a, tPointi b )
{
	int	i;

	for ( i=0; i < DIM; i++ )
		if ( a[i] != b[i] )
			return FALSE;
	return TRUE;
}

int	Length2( tPointi v )
{
	int	i;
	int	ss;

	ss = 0;
	for ( i=0; i < DIM; i++ )
		ss += v[i] * v[i];
	return ss;
}



double	RadDeg( double x )
{
	return 180 * x / M_PI;
}
void	SubVec( tPointi a, tPointi b, tPointi c )
{
	int	i;

	for ( i=0; i < DIM; i++ ) {
		c[i] = a[i] - b[i];
	}
}