
/* file: main.c
 *
 * Three-link snake
 *
 * Program by:
 *
 * Prof. William W. Armstrong,
 * Department of Computing Science,
 * The University of Alberta,
 * Edmonton, Alberta,
 * Canada,
 * T6G 2H1
 * 
 * Copyright (c) 1989 William W. Armstrong */

/* DynaTree -Solution to the Equations of Motion of Linear and
 * Tree-like Linkages -- Version 1.0  February 8, 1985.
 *
 * Mathematical solution of the equations of motion and simulation
 * program by:
 *
 * Prof. William W. Armstrong,
 * Department of Computing Science,
 * The University of Alberta,
 * Edmonton, Alberta,
 * Canada,
 * T6G 2H1
 * 
 * Copyright (c) 1985 William W. Armstrong
 *
 * Permission is hereby granted for personal, non-commercial,
 * reproduction and use of this program provided that this notice
 * is included in any material copied from it.
 *
 * No warranty of any kind is provided with this software.       
 * This software is not supported.  Neither the authors, nor the
 * University of Alberta, its officers, agents, servants or employees
 * shall be liable or responsible in any way for any damage to
 * property or direct personal or consequential injury of any nature
 * whatsoever that may be suffered or sustained by any licensee, user
 * or any other party as a consequence of the use or disposition of
 * this software.   
 */


#include <stdio.h>

#include <math.h>
/* #include <device.h>  commented out PAF 5/24/91 */

#include "dynavars.h"
#include "stats.h"

char	name[] = "setup" ;
char	*nameptr ;
char	*jrnlname ;
int	threeview ;

main( argc, argv)
int	argc ;
char	*argv[] ;
{
	double display_interval, next_display;
	int link;
	short	dev ;

	if( argc == 1 )
		nameptr = name ;
	else if( argc >= 2 )
		nameptr = argv[1] ;
	if( argc == 3 )
		jrnlname = argv[2] ;
	else
		jrnlname = 0 ;
	threeview = 0 ;

	pf = 0;
	/* pf is a flag which, if==1, enables printing and checking */
	elapsed_time = 0.0;
	slowfreq = 1;
	/* number of fastband executions per
	 * slowband execution; may be overridden by initialization
	 */
	display_interval = 0.1;
	next_display = 0.0;
	initialize();
	init_stats();	/* statistics added Oct 10 90 */
	graphinit( jrnlname );
	/* counter for slowband frequency */
	count = 0;
	while(elapsed_time < time_limit ){
	/* do the slowband once every slowfreq fastbands  */
		if(count == 0) {
			slow_integ();
			compute_forces_torques();
			stats_vec(FE,&FER_stats,InertialFrame);
			stats_vec(GEH,&GEHR_stats,InertialFrame);
			stats_vec(gH,&gH_stats,LinkFrame);
			/* The external force is assumed slowly-varying */
		  	slowband_in();
			stats_mat(ROT,&ROT_stats);
		  	if(elapsed_time >= next_display){
				/* Display the results (only possible after a
				   slow_integ ! -- so the display_interval should
				   be a multiple of deltat * slowfreq.
				   Rob has a correction here!
				*/
				drawsnake();
				next_display += display_interval;
			}
		};
		/* do the fastband */
		fastband_in();
		fastband_out();
		stats_vec(aH,&aH_stats,LinkFrame);
		stats_vec(omegadot,&omegadot_stats,LinkFrame);

		if( pf ) check();

		fast_integ();
		stats_vec(vH,&vHR_stats,InertialFrame);
		stats_vec(omega,&omega_stats,LinkFrame);
		elapsed_time += deltat;
		count +=1;
		if (count == slowfreq) count = 0;
for(link = 2; link <= numlinks; link++){
 	printf("roll and pitch of link %d is <%f, %f>\n",link,
		180.0 * atan2(-ROT[link][2][0],ROT[link][0][0]),
		180.0 * atan2(-ROT[link][1][2],ROT[link][1][1]));
	/* assumes yaw = psi = 0 */
}
	}
	while( 0 )
{
		if( qtest() ) {
			switch( qread( &dev ) ) {
			case LEFTMOUSE :
				if( dev == 1 )
					drawjournal( jrnlname );
					break ;
			case MIDDLEMOUSE :
		   		break ;
			case RIGHTMOUSE :
		 		if( dev == 1 )
				exit();
			}
		}
	}
	graphend();
	print_stats();
}
