
#include <stdio.h>
#include <gl/gl.h>
#include <math.h>
/* #include <UTILITY.h>
*/
#include "dynavars.h"
#include "hiddenvars.h"

extern long window_id;

Matrix	rotmat ;

struct	linkrec {
	double	x, y, z ;
	double	r00, r01, r02 ;
	double	r10, r11, r12 ;
	double	r20, r21, r22 ;
	double	cx, cy, cz ;
	}	jrnpos ;

extern	double	vx, vy, vz ;

#define	NUMFACES  8	/* number of faces on tube */

FILE	*jrnfp ;

graphinit( name )
char	*name ;
{
    if( name )
	jrnfp = fopen( name, "w" );
    else
	jrnfp = fopen( "journal", "w" );

    jrngraphinit();
}

graphend()
{
    jrngraphend();
    fclose( jrnfp );
}


dorot( link )
int	link ;
{
    rotmat[0][0] = RI[link][0][0] ;
    rotmat[0][1] = RI[link][1][0] ;
    rotmat[0][2] = RI[link][2][0] ;

    rotmat[1][0] = RI[link][0][1] ;
    rotmat[1][1] = RI[link][1][1] ;
    rotmat[1][2] = RI[link][2][1] ;

    rotmat[2][0] = RI[link][0][2] ;
    rotmat[2][1] = RI[link][1][2] ;
    rotmat[2][2] = RI[link][2][2] ;
}


drawsnake()
{
int	i, link;
double	pos[3] ;

    for( i = 0 ; i < 3; i++ )
	pos[i] = 0.0 ;
    for( link = 1 ; link <= numlinks; link++ )
    {
	/* this is a cheat. we know center of mass is at ( 0, 0, 0.5 ), so
	   rotate this by extracting the appropriate elements of RI */
	pos[0] += jrnpos.cx = RI[link][0][2] * 0.5 + pH[link][0] ;
	pos[1] += jrnpos.cy = RI[link][1][2] * 0.5 + pH[link][1] ;
	pos[2] += jrnpos.cz = RI[link][2][2] * 0.5 + pH[link][2] ;

	jrnpos.x = pH[link][0] ;
	jrnpos.y = pH[link][1] ;
	jrnpos.z = pH[link][2] ;

	jrnpos.r00 = RI[link][0][0] ;
	jrnpos.r10 = RI[link][1][0] ;
	jrnpos.r20 = RI[link][2][0] ;

	jrnpos.r01 = RI[link][0][1] ;
	jrnpos.r11 = RI[link][1][1] ;
	jrnpos.r21 = RI[link][2][1] ;

	jrnpos.r02 = RI[link][0][2] ;
	jrnpos.r12 = RI[link][1][2] ;
	jrnpos.r22 = RI[link][2][2] ;

	fwrite( (char *)&jrnpos, sizeof( struct linkrec ), 1, jrnfp );
    }
    if( draw_bkgrnd1( 1, pos ) )
	drawsnake1();
    if( draw_bkgrnd1( 2, pos ) )
	drawsnake1();
    if( draw_bkgrnd1( 3, pos ) )
	drawsnake1();

    swapbuffers();
}


drawsnake1()
{
int	i, j, link;
double	dist[5] ;
int	order[5] ;
double	dfx, dfy, dfz ;
double	ccx, ccy, ccz ;

    for( link = 1 ; link <= numlinks; link++ )
    {
	order[link] = link ;

	ccx = RI[link][0][2] * 0.5 + pH[link][0] ;
	ccy = RI[link][1][2] * 0.5 + pH[link][1] ;
	ccz = RI[link][2][2] * 0.5 + pH[link][2] ;
	dfx = ( ccx - vx ) * ( ccx - vx ) ;
	dfy = ( ccy - vy ) * ( ccy - vy ) ;
	dfz = ( ccz - vz ) * ( ccz - vz ) ;
	dist[link] = sqrt( dfx + dfy + dfz );
    }

    for( link = 1 ; link < numlinks ; link++ )
    {
	if( dist[link] < dist[link+1] )
	{
	    dfx = dist[link] ; dist[link] = dist[link+1] ; dist[link+1] = dfx ;
	    i = order[link] ; order[link] = order[link+1] ; order[link+1] = i ;
	}
    }
    
    for( link = 1 ; link < numlinks ; link++ )
    {
	if( dist[link] < dist[link+1] )
	{
	    dfx = dist[link] ; dist[link] = dist[link+1] ; dist[link+1] = dfx ;
	    i = order[link] ; order[link] = order[link+1] ; order[link+1] = i ;
	}
    }
    for( link = 1 ; link <= numlinks; link++ )
    {
	j = order[link];
	/* centre of mass is at (0, 0, 0.5), so make a tube from (0,0,0)
		to (0,0,1) and rotate it */
	pushmatrix();
	translate( pH[j][0], pH[j][1], pH[j][2] );
	dorot( j );
	multmatrix(rotmat);
	callobj( j+10 );
	callobj( j );
	popmatrix();
    }
}
