/*
  This program is used to transform Deneb IGRIP or TGRIP device, 
  workcell and recording file to VRML2

        Usage:   workcell  -option ...
	-d device translation  : workcell -d input_igrip output_vrml
	-w workcell translation : workcell -w input_igrip output_vrml
	-p workcell recording translation: 
	   workcell -s input_igrip output_vrml recording_file timecycle

	To compile, cc -o workcell workcell.c -lm

	Qiming 	Wang
	Visualization and Virtual Reality Group
	Information Technology Lab
	NIST
	
*/


#include <stdio.h>
#include <strings.h>
#include <math.h>

#define NUMBER_COLOR 32
#define MAXDEV 30
#define MAXPART 150
#define MAXPART_DEV 50
#define MAXKIN 10
#define MAXJOINT 10
#define MAXLEN 500
#define Q_LENGTH 300
#define MAXTIME 2000
#define MAXNAME 80
#define BUFSIZE 200
  
typedef struct {
  char *name;
  char *out_name;
  char *path_name;
  int global_id;
  int device_id;
  char attach[20];
  float matrix[4][4];
  int kin_num;
  int *kin;
  int joint_num;
  int *joint;
} Part;


/* only one attach in recording*/
typedef struct {
  char *devicename;
  int  global_id;
  int  part_number;
  float matrix[4][4];
  int dof;
  int parts[MAXPART_DEV];
  int attach;
  int attach_done;
  float *joint_init;
  int *joint_type;
  float *joint_lower;
  float *joint_upper;
  int anim_attach;
  int anim_attach_done;
  int attach_time[MAXTIME];
  float attach_mat[4][4];
} Device;

Device devices[MAXDEV];
Part   parts[MAXPART];

typedef struct {
	char *name;
	int old_c;
	float new_c[3];
} table;

int mode;
int part_global;
int n_device;
char s[MAXLEN], buffer[BUFSIZE];
float time[MAXTIME], timecycle;
int time_step;
char **device_name;

FILE *tmp_file;

void read_workcell_file();
void scan_record();
void output_vrml();
void output_anim();
void device_anim();
void animate();
void unmatrix();
void insert();
int q_length();
void limit();
void route();
void part2vrml2();
void t_color();
void get_parts_directory();
void transfer();

typedef struct {
  int dof;
  char *device_name;
  float value[MAXTIME];
} Animation;

Animation **animation;

typedef struct {
  char *device_name;
  float translation[MAXTIME][3];
  float rotation[MAXTIME][4];
} Transform;

Transform *transform;

table table_data[] = { {"white", 4, {1., 1., 1.}},
			{"grey", 5, {0.5, 0.5, 0.5}},
			{"black", 6, {0.2,0.2,0.2}},
			{"red", 7, {1., 0., 0.}},
			{"green", 8, {0.,1.,0.}},
			{"blue", 9, {0., 0., 1.}},
			{"yellow", 10, {1., 1., 0.}},
			{"cyan", 11, {0., 1., 1.}},
			{"purple", 12, {0.53, 0.12, 0.47}},
			{"orange", 13, {1., 0.5, 0.}},
			{"magenta", 14, {1., 0., 1.}},
			{"cherry", 15, {0.74, 0.56, 0.56}},
			{"light blue", 16, {0., 0., 0.75}},
			{"brown", 17, {0.65, 0.16, 0.16}},
			{"lime", 18, {0.20, 0.8, 0.2}},
			{"tan", 19, {0.85, 0.58, 0.44}},
			{"all black", 20, {0.,0.,0}},
			{"grey-75", 21, {0.75,0.75,0.75}},
			{"slate grey", 22, {0.18, 0.31, 0.31}},
			{"teal", 23, {0.2, 0.2, 0.8}},
			{"dodger blue", 24, {0.26, 0.43, 0.26}},
			{"cobalt", 25, {0.14, 0.14, 0.56}},
			{"dark plum", 26, {0.92, 0.68, 0.92}},
			{"brass", 27, {0.71, 0.65, 0.25}},
			{"color25", 28, {0.85, 0.58,0.44}},
			{"color26", 29, {0.20,0.20, 0.8}},
			{"color27", 30, {1.,1.,1.}},
			{"color28", 31, {0.20,0.20,0.8}},
			{"color29", 32, {0.74, 0.56, 0.56}},
			{"color30", 33, {0.92, 0.68, 0.92}},
			{"color31", 34, {1.,1.,1.}},
			{"color32", 35, {0.,0.,0.}},
			};

char path[MAXNAME];
int n_dir;
char *dirs[MAXNAME];
char *vrml_files[MAXNAME];
int n_vrml_files=0;



/*
 * process a command line argument
 */

void scan_args(char *argv[])
{
  char c;

  c = argv[1][1];
  switch (c) {
  case 'd' :
    printf("Translating device %s to VRML2 %s\n\n",argv[2],argv[3]);
    mode = 0;
    break;
  case 'w' :
    printf("Translating workcell %s to VRML2 %s\n\n", argv[2],argv[3]);
    mode = 1;
    break;
  case 'p' :
    printf("Translating workcell %s with simulation recording\n\n",argv[2]);
    mode = 3;
    break;
  default:
    printf("Unknown option. \n\t-d for device\t-w for workcell\t-r for resource\n\t-p for playing recording\n");
    exit();
  }
}

main( int argc, char **argv)
{
	FILE *file, *out_file, *record_file;

	printf("\n\n*******************************\n");
	printf("Deneb IGRIP to VRML2\n");
	printf("By Qiming Wang, VVRG, ITL, NIST\n");
	printf("********************************\n\n");

	scan_args(argv);

	get_parts_directory();

	file = fopen(argv[2], "r");
	out_file = fopen(argv[3], "w");  

	if (mode == 3) {
	  record_file = fopen(argv[4],"r");
	  timecycle = atof(argv[5]);
	}

	tmp_file = fopen("tmp.wrl", "w");
	
	read_workcell_file(file);

	/* read recording file */
	if (mode==3)
	  scan_record(record_file);

	/*  output vrml workcell file*/
	output_vrml(out_file, n_device);

	/* output recording */
	if (mode == 3)
	  output_anim(out_file);

	/* end of the translation */
	printf("\nTranslation is finished\n");
}

/* read IGRIP workcell file */
void read_workcell_file(FILE *file)
{
	int version, prog, max_digit, max_analg, dof,dynamic, signat, n_home,
	    n_uframe, n_utool, n_config, dummy;
	char device_name[MAXNAME], kin_type[MAXNAME], 
	  partname[MAXNAME], new_name[MAXNAME], attach[MAXNAME], s1[10],
	  s2[10], program_name[MAXNAME];
	int i,j,k, i_dummy, nn,ii, left_right, end_flag, kk;
	int part_device, kin_num, joint_num, kin[MAXKIN], 
	  joint[MAXJOINT];
	float d_mat[4][4], p_mat[4][4], f_dummy;

	if (mode == 1 || mode == 3) {
	  for (i=0; i<4; i++)
	    fgets(s, MAXLEN, file);

	  /* comments */
	  fscanf(file, "%d\n", &nn);
	  if (nn>0) 
	    for (i=0; i<nn; i++)
	      fgets(s, MAXLEN, file);

	  /* viewpoint */
	  for (i=0; i<4; i++)
	    fgets(s, MAXLEN, file);

	  /* 10 lines */
	  for (i=0; i<10; i++)
	    fgets(s, MAXLEN, file);

	  /* nn lines include workcell name */
	  fscanf(file, "%d\n", &nn);
	  for (i=0; i<nn; i++)
	    fgets(s, MAXLEN, file);

	  fscanf(file, "%d\n", &n_device);
	  printf("\nnumber of device = %d\n\n", n_device);
	}

	if (mode == 0) n_device = 1;

	/* device */	
	part_global = 0;
	for (kk=0; kk<n_device; kk++) {
	  part_device = 0;

	  if (mode != 0) fgets(s, MAXLEN, file);
	  
	  /*  read version and number of coordinate system */
	  fscanf(file, "%d\n", &version);
	  if (version != 11) {
	    printf("This is not version 11, exit\n");
	    exit();
	  }

	  fscanf(file, "%s\n", device_name);
	  for (i=0; i<strlen(device_name); i++)
	    if (device_name[i] == '#' || device_name[i]=='/') 
	      device_name[i] = '_';

	  /*printf("%s\n", device_name);*/
	  devices[kk].devicename = 
	    (char *)malloc((strlen(device_name)+1)*sizeof(char));
	  strcpy(devices[kk].devicename, device_name);
	  devices[kk].global_id = kk;
	  fscanf(file, "%d\n", &prog);
	  fscanf(file, "%d\n", &max_digit);
	  fscanf(file, "%d\n", &max_analg);
	  fscanf(file, "%d\n", &dof);
	  fscanf(file, "%d\n", &dynamic);
	  fscanf(file, "%d\n", &signat);
	  printf("device_name=%s, dof=%d, ",device_name, dof);
	  devices[kk].dof = dof;
	  devices[kk].attach = -1;
	  devices[kk].attach_done = 0;   
	  devices[kk].part_number = 0;
	  devices[kk].anim_attach = -1;
	  devices[kk].anim_attach_done = 0;

	  /*    3 lines ?  */
	  fgets(s, MAXLEN, file);
	  fscanf(file, "	%d\n", &nn);

	  for (i=0; i<nn; i++)
	    fgets(s, MAXLEN, file);

	  fgets(s, MAXLEN, file);

	  /* program */
	  if (prog==1) 
	    fscanf(file, "%s\n", program_name);

	  /*	 1 line */
	  fgets(s, MAXLEN, file);
	
	  if (dof==0) printf("\n");
	  if (dof != 0 ) {
	    fscanf(file, "	%d\n", &n_home);
	    fscanf(file, "%d\n", &n_uframe);
	    fscanf(file, "%d\n", &n_utool);
	    fscanf(file, "%d\n", &n_config);


	    /*   10 lines */
	    /*   simulation attribute tyupe */
	    fgets(s, MAXLEN, file);
	    /* 2 lines */
	    for (i=0; i<2; i++)
	      fgets(s, MAXLEN, file);
	    /* tcp limit information */
	    fgets(s, MAXLEN, file);
	    /* speed limit */
	    fgets(s, MAXLEN, file);
	    /* accelerate rate limit */
	    fgets(s, MAXLEN, file);
	    /* time rate limit */
	    fgets(s, MAXLEN, file);
	    /* 3 lines ? */
	    for (i=0; i<3; i++)
	      fgets(s, MAXLEN, file);

	    fscanf(file, "%s\n", kin_type);
	    printf("%s\n", kin_type);

	    /* 3	 lines ?*/
	    for (i=0; i<3; i++)
	      fgets(s, MAXLEN, file);
	    /* config informta	tion */
	    if ( n_config != 0) {
	      for (i=0; i<n_config; i++) {
		fscanf(file, "%d", &i_dummy);
	      }
	      fscanf(file, "\n");
	      for (i=0; i<n_config; i++)
		fscanf(file, "%s\n", s);
	    }
	    
	    /* 1 line ? */
	    fgets(s, MAXLEN, file);
	    
	    /* motion planner */
	    for (i=0; i<6; i++)
	      fgets(s, MAXLEN, file);
	    
	    /* joint type */
	    devices[kk].joint_type = (int *)malloc(dof*sizeof(int));
	    for (i=0; i<dof; i++)
	      fscanf(file, "%d", &devices[kk].joint_type[i]);
	    fscanf(file, "\n");   

	    for (i=0; i<dof; i++)
	      fscanf(file, "%d", &dummy);
	    fscanf(file, "\n");
	    
	    /* inital joint value */
	    devices[kk].joint_init=(float *)malloc(dof*sizeof(float));
	    for (i=0; i<dof; i++) 
	      fscanf(file, "%f", &devices[kk].joint_init[i]);
	    fscanf(file, "\n");


	    /* 1	 line ? */
	    fgets(s, MAXLEN, file);

	    /* joint * n_home */
	    for (i=0; i<n_home; i++) {
	      for (j=0; j<dof; j++)
		fscanf(file, "%f", &f_dummy);
	      fscanf(file, "\n");
	    }

	    /* kinematics map */
	    for (i=0; i<dof; i++)
	      fgets(s, MAXLEN, file);

	    for (i=0; i<dof; i++) {
	      fscanf(file, "%d", &i_dummy);
	    }
	    fscanf(file, "\n");

	    /* 3 lines ? */
	    for (i=0; i<3; i++)
	      fgets(s, MAXLEN, file);
	
	    /* 1 line ? */
	    fgets(s, MAXLEN, file);

	    /* home, uframe, utool coordinate */
	    nn = 4 + 1 + 4 * n_uframe +1 + 4 * n_utool;
	    for (i=0; i<nn; i++)
	      fgets(s, MAXLEN, file);
	    
	    /* length */
	    fgets(s, MAXLEN, file);
	    /* offset */
	    fgets(s, MAXLEN, file);
	    /* aux */
	    fgets(s, MAXLEN, file);
	    
	    /* joint travel limit */
	    devices[kk].joint_lower = (float *)malloc(dof*sizeof(float));
	    for (i=0; i<dof; i++) {
	      fscanf(file, "%s\n", s);
	      if (s[0] == '=') {
		/* = ...dof() */
		printf(" dof:%d, joint lower limit %s\n", i, s);
		devices[kk].joint_lower[i] = 0.;
	      }
	      else 
		devices[kk].joint_lower[i]=atof(s);
	    }
	    devices[kk].joint_upper = (float *)malloc(dof*sizeof(float));
	    for (i=0; i<dof; i++) {
	      fscanf(file, "%s\n", s);
	      if (s[0] == '=') {
		/* = ...dof() */
		printf(" dof:%d, joint upper limit %s\n", i, s);
		devices[kk].joint_upper[i]=0.;
	      }
	      else
		devices[kk].joint_upper[i] = atof(s);
	    
	    }


	    /* speed limit */
	    fgets(s, MAXLEN, file);
	
	    /* accelerate limit */
	    fgets(s, MAXLEN, file);

	    /* time limit */
	    fgets(s, MAXLEN, file);

	    /* log time */
	    fgets(s, MAXLEN, file);
	
	    /* settle time */
	    fgets(s, MAXLEN, file);

	    /* nn */
	    fscanf(file, "%d\n", &nn);
	    for (i=0; i<nn; i++)
	      fgets(s, MAXLEN, file);

	    /* 20 lines ?*/
	    for (i=0; i<20; i++) 
	      fgets(s, MAXLEN, file);

	    /* nn */
	    fscanf(file, "%d\n", &nn);
	    for (i=0; i<nn; i++) 
	      fgets(s, MAXLEN, file);
	    
	  }
    

	  if (prog == 1) {
	    fscanf(file, "%d\n", &nn);
	    for (i=0; i<nn; i++) 
	      fgets(s, MAXLEN, file);
	  }

    
	  /*	 2 lines ? */
	  for (i=0; i<2; i++)
	    fgets(s, MAXLEN, file);
	  fscanf(file, "%d\n", &nn);
	  for (i=0; i<nn; i++)
	    fgets(s, MAXLEN, file);
	  
	  /* 6 lines ? */
	  for (i=0; i<6; i++)
	    fgets(s, MAXLEN, file);

	  /* ??? */
	  fscanf(file, "%d\n", &nn);
	  if (nn == 1) ii=6;
	  else ii=5;
	  for (i=0; i<ii; i++) 
	    fgets(s, MAXLEN, file);

	  /* read part information */
	  left_right=1; end_flag=0;
	  while (end_flag == 0) {
	    /* read tranform matrix */
	    for (i=0; i<4; i++) 
	      fscanf(file, "%f %f %f %f\n", &d_mat[i][0],&d_mat[i][1],
		     &d_mat[i][2],&d_mat[i][3]);
	    for (i=0; i<4; i++)
	      for (j=0; j<4; j++)
		parts[part_global].matrix[i][j] = d_mat[i][j];

	    fscanf(file, "%s\n", partname);

	    /*   to form vrml part file name */
	    transfer(partname, new_name, device_name);

	    parts[part_global].path_name = 
	      (char *)malloc((strlen(new_name)+1)*sizeof(char));
	    strcpy(parts[part_global].path_name, new_name);

	    parts[part_global].global_id = part_global;

	    parts[part_global].device_id = kk;

	    /* form part out_name=devicename+partname */
	    parts[part_global].out_name = 
	      (char *)malloc((strlen(devices[kk].devicename)+
			     strlen(parts[part_global].name)+1)*sizeof(char));
	    strcpy(parts[part_global].out_name, devices[kk].devicename);
	    strcat(parts[part_global].out_name, "_");
	    strcat(parts[part_global].out_name, parts[part_global].name);

	    devices[kk].parts[devices[kk].part_number] = part_global;
	    devices[kk].part_number++;

	    /* kinematics information */
	    fscanf(file, "%d", &kin_num);
	    parts[part_global].kin_num = kin_num;
	    parts[part_global].kin = (int *)malloc(kin_num * sizeof(int));
	    for (i=0; i<kin_num; i++) {
	      fscanf(file, "%d", &kin[i]);
	      parts[part_global].kin[i] = kin[i];
	    }
	    fscanf(file, "\n");

	    fscanf(file, "%d", &joint_num) ;
	    parts[part_global].joint_num = joint_num;
	    parts[part_global].joint = (int *)malloc(joint_num *sizeof(int));
	    for (i=0; i<joint_num; i++) {
	      fscanf(file, "%d", &joint[i]);
	      parts[part_global].joint[i] = joint[i];
	    }
	    fscanf(file, "\n");
 
	    fscanf(file, "%d\n", &nn);
	    if (nn != 0) {
	      /* joint value is a function of dof() */
	      /*
	      if (joint_num ==0) {
		parts[part_global].joint_num=nn;
		parts[part_global].joint = (int *)malloc(nn*sizeof(int));
	      }
	      for (i=0; i<nn; i++) {
		char s[3];int l;
		fgets(s, MAXLEN, file);
		for (j=0; j<strlen(s); j++)
		  if (s[j]=='(') break;
		l=0;
		for (k=j+1; k<strlen(s); k++)
		  if (s[j]==')') break;
		  else {
		    s1[l]=s[k];l++;
		  }
		parts[part_global].joint[i] = atoi(s1);
	      } */
	      for (i=0; i<nn; i++) {
		fgets(s, MAXLEN, file);
		printf("    Joint function not be transfered: %s\n", s);
	      }
		  
	    }

	    fscanf(file, "%d\n", &nn);
	    for (i=0; i<nn; i++)
	      fgets(s, MAXLEN, file);

	  /******  attach   */
	  fscanf(file , "%s\n", attach);
	  strcpy(parts[part_global].attach, attach);
	  part_global++;
	  
	
	    for (i=0; i<strlen(attach); i++) {
	      switch(attach[i]) {
	      case '1':
		left_right++;
		break;
		      
	      case '0':
		left_right--;
		break;
		      
	      defaule: 
		break;
	      }

	      if (left_right == 0) {
		end_flag = 1;
		break;
	      }
	    }

	    if (end_flag == 1) break;
	    
	    for (i=0; i<5; i++)
	      fgets(s, MAXLEN, file);

	  }

	  for (i=0; i<2; i++)
	    fgets(s, MAXLEN, file);
	  for (i=0; i<dof; i++)
	    fgets(s, MAXLEN, file);
	  fgets(s, MAXLEN, file);
	  fscanf(file, "%d\n", &nn);
	  for (i=0; i<nn; i++)
	    fgets(s, MAXLEN, file);
	  fgets(s, MAXLEN, file);
	}
	/*  end  kk  */

	if (mode != 0)
	  /* after device data, read device attachment information */
	  for (i=0; i<n_device; i++) 
	    fscanf(file, "%d\n", &devices[i].attach);

	/* change same part name */
	for (i=0; i<n_device; i++) {
	  for (j=0; j<devices[i].part_number; j++) {
	    char s[1];
	    int kk;
	    kk=0;
	    for (k=j+1; k<devices[i].part_number; k++) {
	      if (strcmp(parts[devices[i].parts[j]].name, 
			 parts[devices[i].parts[k]].name)==0) {
		kk++;
		strcat(parts[devices[i].parts[k]].name, "_");
		strcat(parts[devices[i].parts[k]].out_name, "_");
		sprintf(s, "%d", kk);
		strcat(parts[devices[i].parts[k]].name,s); 
		strcat(parts[devices[i].parts[k]].out_name, s);
	      }
	    }
	  }
	}
}
 

void scan_record(FILE *input)
{
  int i, j, k,l, id, *num_dof, num_device, tmp, cli, kk, kkk, dummy;
  char devicename[MAXNAME], partname[MAXNAME];
  float tt[MAXTIME], jj[MAXTIME], mat[4][4], tmp_anim, tmp_time,tranrot[7];
  int device_id, attach_part_id, attach_device_id;

  for (i=0; i<7; i++)
    fgets(s, MAXLEN, input);

  printf("\nstart to read recording file\n");
  fscanf(input, "%d\n", &num_device);
  num_dof = (int *)malloc(num_device * sizeof(int));
  device_name = (char **)malloc(num_device*sizeof(char *));
  
  /*printf("device number from recording file, %d\n", num_device);*/
  for (i=0; i<num_device; i++) {
    device_name[i] = (char *)malloc(MAXNAME*sizeof(char));
    fscanf(input, "%s\n", device_name[i]);
    for (j=0; j<strlen(device_name[i]); j++)
      if (device_name[i][j]=='#' || device_name[i][j]=='/' ) 
	device_name[i][j] = '_';

    fscanf(input, "%d\n", &num_dof[i]);
    /*printf("%s, %d\n", device_name[i], num_dof[i]);*/
  }

  animation = (Animation **)malloc(num_device*sizeof(
				   Animation *));
  for (i=0; i<num_device; i++) {
    animation[i] = (Animation *)malloc(num_dof[i]*sizeof(
				   Animation));

    for (j=0; j<num_dof[i]; j++) {
      animation[i][j].dof = j;
      animation[i][j].device_name = (char *)malloc(
				     strlen(device_name[i])*sizeof(char));
      strcpy(animation[i][j].device_name, device_name[i]);
    }
  }

  transform = (Transform *)malloc(num_device*sizeof(Transform));
  for (i=0; i<num_device; i++) {
    transform[i].device_name = (char *)malloc(strlen(device_name[i])*
						     sizeof(char));
    strcpy(transform[i].device_name, device_name[i]);
  }


  time_step = 0;
  
  while (1==1) {
    dummy=1;
    cli = 0;
    while (cli == 0) {
      if( fgets(s, MAXLEN, input) == NULL) {
	printf("time_step=%d, time=%f\n", time_step, time[time_step-1]);
	return;
      }
      if (strncmp(s, "CLI", 3) == 0) cli = 0;
      else cli = 1;
    }
    sscanf(s, "%f\n", &tmp_time);
    if (time_step > 0 && time[time_step-1]>0 && tmp_time==0.0) {
      break;
    }
    if (time_step > 0 && time[time_step-1]==tmp_time) dummy=0;    
    if (dummy) time[time_step]=tmp_time;

    for (i=0; i<num_device; i++) {
      device_id = find_device_id(device_name[i]);
      fscanf(input, "%d\n", &tmp);

      for (j=0; j<tmp; j++) {
 	for (k=0; k<num_dof[i]; k++) {
	  fscanf(input, "%f", &tmp_anim);
	  if (dummy) {
	      animation[i][k].value[time_step]=tmp_anim;
	  }
	}
	fscanf(input, "\n");
      }

      /* scan tranform matrix */
      fscanf(input, "%d\n", &tmp);
      for (j=0; j<tmp; j++) {
	for (k=0; k<3; k++) {
	  fscanf(input, "%f %f %f\n",&mat[k][0],&mat[k][1],&mat[k][2]);
	  mat[k][3]=0;
	}
	
	fscanf(input, "%f %f %f\n", &mat[3][0],&mat[3][1],&mat[3][2]);
	mat[3][3]=1;
      }
      unmatrix(mat, tranrot);


      transform[device_id].translation[time_step][0]=tranrot[0];
      transform[device_id].translation[time_step][1]=tranrot[1];
      transform[device_id].translation[time_step][2]=tranrot[2];
      transform[device_id].rotation[time_step][0]=tranrot[3];
      transform[device_id].rotation[time_step][1]=tranrot[4];
      transform[device_id].rotation[time_step][2]=tranrot[5];
      transform[device_id].rotation[time_step][3]=tranrot[6];

      fscanf(input, "%s\n", buffer);
      if (dummy) {
      if (strlen(buffer) > 1) {

	/* get touch information */
	l=0;
	for (j=1; j<strlen(buffer); j++) {
	  if (buffer[j]=='#' || buffer[j]=='.' || buffer[j]=='/') 
	    buffer[j]='_';
	  if (buffer[j]!=':') {devicename[l]=buffer[j]; l++;}
	  else
	    break;
	}
	devicename[l] = '\0';
	l=0;
	for (k=j+1; k<strlen(buffer); k++) {
	  if (buffer[k]=='#' || buffer[k]=='.' || buffer[k]=='/')
	    buffer[k]='_';
	  partname[l]=buffer[k]; l++;
	}
	partname[l]='\0';
	attach_device_id = find_device_id(devicename);
	attach_part_id = find_part_id(attach_device_id, partname);
	if (devices[device_id].attach == -1) 
	  devices[device_id].anim_attach = attach_part_id;
	else if (devices[device_id].attach != attach_part_id)
	  devices[device_id].anim_attach = attach_part_id;

	for (j=0; j<4; j++)
	  for (k=0; k<4; k++)
	    devices[device_id].attach_mat[j][k]=mat[j][k];

        devices[device_id].attach_time[time_step]=1;
      }
      else
	devices[device_id].attach_time[time_step]=0;
      }

      fgets(buffer, MAXLEN, input);

    }
    
    for (i=0; i<11; i++) {
      fgets(buffer, MAXLEN, input);
    }
    if (dummy) time_step++;
  }
  printf("time_step=%d, time=%f\n", time_step, time[time_step-1]);
}

void limit(Part part, int j, int num, float lower[3], float upper[3],
	   float initial[3], int sign)
  {
    int id, i;

    id = part.device_id;

    for (i=0; i<num; i++) {
      initial[i] = devices[id].joint_init[part.joint[j+i]-1];
      lower[i] = devices[id].joint_lower[part.joint[j+i]-1];
      upper[i] = devices[id].joint_upper[part.joint[j+i]-1];
      if (sign < 0) {
	initial[i] = -initial[i];
	lower[i] = -lower[i];
	upper[i] = -upper[i];
      }
    }
  }

void route(FILE *file, char *interp, char *name, char *zero, 
	   char *lower_value,char *upper_value, char *to, int choice)
  {
    char joint_name[MAXNAME];

    strcpy(joint_name, name);
    if (choice == 191) strcat(joint_name,"_x");
    if (choice == 192) strcat(joint_name, "_y");
    if (choice == 193) strcat(joint_name, "_z");

    fprintf(file, "DEF TIME_%s TimeSensor {cycleInterval 3}\n", joint_name);
    fprintf(file, "DEF JOINT_%s %sInterpolator {\n",joint_name,interp);
    fprintf(file, "	key [0., 0.25, 0.5, 0.75, 1.]\n");
    fprintf(file, "	keyValue [%s,%s,%s,%s,%s]\n", zero,lower_value, zero,upper_value, zero);
    fprintf(file, "	}\n");
    fprintf(file, "ROUTE TOUCH_%s.touchTime TO TIME_%s.startTime\n",name,joint_name);
    fprintf(file, "ROUTE TIME_%s.fraction_changed TO JOINT_%s.set_fraction\n",
	    joint_name, joint_name);
    if (choice == 8) {
      fprintf(file, "ROUTE JOINT_%s.value_changed TO Scale_xyz.scalefactor\n",
	      joint_name);
      fprintf(file, "ROUTE Joint_Scale_xyz.scalevector TO %s.%s\n",joint_name,to);
    } else
    if (choice == 9) {
      fprintf(file, "ROUTE JOINT_%s.value_changed TO Scale_x.scalefactor\n",
	      joint_name);
      fprintf(file, "ROUTE Joint_Scale_x.scalevector TO %s.%s\n",joint_name,to);
    }  else
    if (choice == 10) {
      fprintf(file, "ROUTE JOINT_%s.value_changed TO Scale_y.scalefactor\n",
	      joint_name);
      fprintf(file, "ROUTE Joint_Scale_y.scalevector TO %s.%s\n",joint_name,to);
    } else
      if (choice == 11) {
	fprintf(file,"ROUTE JOINT_%s.value_changed TO Scale_z.scalefactor\n",
		joint_name);
	fprintf(file,"ROUTE Joint_Scale_z.scalevector TO %s.%s\n",joint_name,to);
      } else
    fprintf(file, "ROUTE JOINT_%s.value_changed TO %s.%s\n", joint_name, name, to);

}
  

/* output the device and part to vrml2 */   
void output_vrml(FILE *output, int n_device) 
{
  int i, j, k, kk, kkk, left_right, finish,queue[Q_LENGTH], queue_length, 
    end_flag, d_id, scale, trans, rotate, rpy;
  float tranrot[7], tmp_mat[4][4], tmp_tranrot[7];
  char attach[MAXNAME], lower_value[MAXNAME], upper_value[MAXNAME], 
    init_value[MAXNAME];
  float lower[3], upper[3], initial[3];

  int pp, ii;

  /* output the head */
  fprintf(output, "#VRML V2.0 utf8\n\n");

  fprintf(output, "WorldInfo {\n");
  fprintf(output, "	info \"	This VRML2 model is translated from Deneb IGRIP/TGRIP \n");
  fprintf(output, "		using Translator developed by \n");
  fprintf(output, "  			Qiming Wang\n");
  fprintf(output, "  		Visualization and Virtual Reality Group\n");
  fprintf(output, "  		ITL, NIST\"}\n\n");

  fprintf(output, "DEF TOUCH TouchSensor {} \n");

  fprintf(output, "DEF Joint_Scale_x Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=value;\n");
  fprintf(output, "		scalevector[1]=1.;\n");
  fprintf(output, "		scalevector[2]=1.;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Joint_Scale_y Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=1;\n");
  fprintf(output, "		scalevector[1]=value;\n");
  fprintf(output, "		scalevector[2]=1;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Joint_Scale_z Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=1;\n");
  fprintf(output, "		scalevector[1]=1;\n");
  fprintf(output, "		scalevector[2]=value;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Joint_Scale_xyz Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=value;\n");
  fprintf(output, "		scalevector[1]=value;\n");
  fprintf(output, "		scalevector[2]=value;\n");
  fprintf(output, "	    }\n		\"\n	}\n");

  fprintf(output, "DEF Scale_x Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=value;\n");
  fprintf(output, "		scalevector[1]=1.;\n");
  fprintf(output, "		scalevector[2]=1.;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Scale_y Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=1;\n");
  fprintf(output, "		scalevector[1]=value;\n");
  fprintf(output, "		scalevector[2]=1;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Scale_z Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=1;\n");
  fprintf(output, "		scalevector[1]=1;\n");
  fprintf(output, "		scalevector[2]=value;\n");
  fprintf(output, "	    }\n		\"\n	}\n");
  fprintf(output, "DEF Scale_xyz Script {\n");
  fprintf(output, "	eventIn SFFloat scalefactor\n");
  fprintf(output, "	eventOut SFVec3f scalevector\n");
  fprintf(output, "	url \"vrmlscript:\n");
  fprintf(output, "	    function scalefactor(value) {\n");
  fprintf(output, "		scalevector[0]=value;\n");
  fprintf(output, "		scalevector[1]=value;\n");
  fprintf(output, "		scalevector[2]=value;\n");
  fprintf(output, "	    }\n		\"\n	}\n");


  fprintf(output, "Transform {\n");
  fprintf(output, "	scale  0.001 0.001 0.001 \n");
  fprintf(output, "	rotation 1 0 0 -1.57\n");
  fprintf(output, "	children [\n");

  /* form a queue which has the sequence of parts */
  for (i=0; i<Q_LENGTH; i++) queue[i]=-1;

  finish = n_device;

  kkk=-1;
  while (finish > 0) {
    for (i=0; i<n_device; i++) {
      if (devices[i].attach_done == 1) continue;
      kk = kkk;
      if (devices[i].attach != (-1)) {
	for (j=0; j<q_length(queue); j++) {
	  if (queue[j]==-999 || queue[j]==999) continue;
	  if (devices[i].attach != queue[j]) continue;
	  kk = j; goto label;
	}
	goto end;
      }
      else
	if (i>0) 
	  kk = q_length(queue)-2;

    label:
      insert(queue, kk, 999);

      kk++; left_right=1; end_flag=0;
      for (k=0; k<devices[i].part_number; k++) {
   	  insert(queue, kk, devices[i].parts[k]); kk++;
	  strcpy(attach, parts[(devices[i].parts[k])].attach);
	  
	    for (ii=0; ii<strlen(attach); ii++) {
		    switch(attach[ii]) {
		    case '1':
		      left_right++;
		      insert(queue, kk, 999); kk++;
		      break;
		      
		    case '0':
		      left_right--;
		      insert(queue, kk, -999); kk++;
		      break;
		      
		    defaule: 
		      break;
		    }
		    if (left_right == 0) {
		      end_flag = 1;
		      break;
		    }
	    }
      }
      devices[i].attach_done = 1;
      finish--;
      kkk=kk;
    end:
      continue;
    }
  }


  for (i=0; i<part_global; i++) {
    if (parts[i].joint_num  != 0) {
      for (j=0; j<q_length(queue); j++) {
	if (queue[j] == parts[i].global_id) {
	  kk=j;
	  break;
	}
      }
      insert(queue, kk, 888);

      if (queue[kk+2]== -999) {
	insert(queue, kk+1, -888);
	continue;
      }
      k=0;
      for (j=kk+2; j<q_length(queue); j++) {
	if (queue[j] == 999) k++;
	else if (queue[j] == -999) k--;
	if (k==-1) {
	  insert(queue, j, -888);
	  break;
	}
      }
    }
  }
  
  /* output queue */
  printf("length=%d\n", q_length(queue));
  for (i=0; i<q_length(queue); i++)
    printf("%d ", queue[i]);
  printf("\n");

  /* output vrml file */
  for (i=0; i<q_length(queue); i++) {
    if (queue[i]==-1) continue;      
    if (queue[i] == 999) {
      fprintf(output, "Group {\n	children [\n");
      continue;
    }
    if (queue[i] == -999) {
        fprintf(output, "	]}]}\n"); 
	continue;
      }
    if (queue[i] == 888) continue;
    if (queue[i] == -888) {
        if (rpy != 0) fprintf(output, "]}]}\n");
	    fprintf(output, "	]}\n");
	    continue;
    }

    pp = queue[i];
    for (ii=0; ii<7; ii++)
      tranrot[ii] = 0.0;
    unmatrix(parts[pp].matrix, tranrot);

    /* if the part is the first part in the device */
    scale = 0; trans=0; rotate = 0;
    d_id = parts[pp].device_id;
    if (pp == devices[d_id].parts[0]) 
      fprintf(output, "	DEF d_%s Transform {\n", devices[d_id].devicename);
    else
      fprintf(output, "	Transform {\n");

    if (tranrot[0]!=0.0 || tranrot[1]!=0.0 || tranrot[2]!=0.0)
    fprintf(output, "		translation %.3f %.3f %.3f\n",tranrot[0],
	    tranrot[1],tranrot[2]);
    if (tranrot[6] != 0.0) {
      fprintf(output, "		rotation %.3f %.3f %.3f %.3f\n", tranrot[3],
	      tranrot[4],tranrot[5],tranrot[6]);
    }
    fprintf(output, "		children [\n");

    /* insert switch related to anim attach */

    d_id = parts[pp].device_id;
    if (devices[d_id].anim_attach != -1 && 
	devices[d_id].anim_attach_done==0) {
      fprintf(output, "DEF %s_SWITCH_1 Switch {\n", devices[d_id].devicename);
      fprintf(output, "	whichChoice 0\n");
      fprintf(output, "	choice [\n");
      fprintf(output, "DEF %s Group {\n\tchildren [\n", 
	      devices[d_id].devicename);
      fprintf(output, "DEF %s_SCRIPT_1 Script {\n",devices[d_id].devicename);
      fprintf(output, "\teventIn SFFloat switch_time\n");
      fprintf(output, "\teventOut SFInt32 sw\n");
      fprintf(output, "\turl \"vrmlscript:\n");
      fprintf(output, "\tfunction switch_time(value) {\n");
      fprintf(output, "\t  if (value < 0.5) sw = 0;\n\t 	else sw=-1;\n");
      fprintf(output, "\t}\n\t	\"\n\t}\n");
      devices[d_id].anim_attach_done = 1;
    }

    if (queue[i+1] == 888) {
      fprintf(output, "DEF %s Transform {\n", parts[queue[i]].out_name);
      j = 0;
      rpy = 0;
      for (k=0; k<parts[pp].kin_num; k++) {
	scale = 0; trans = 0; rotate = 0;
	switch (abs(parts[pp].kin[k])) {
	case 8:
	  limit(parts[pp],j,3,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f", lower[0]);
	  sprintf(upper_value, "%f", upper[0]);
	  sprintf(init_value, "%f %f %f", initial[0], initial[1],initial[2]);
	  fprintf(output, "	scale %.3f %.3f %.3f\n", initial[0], initial[1],
		  initial[2]);
	  route(tmp_file, "Scalar", parts[pp].out_name, init_value,
		lower_value, upper_value,"scale",8);
	  j=j+3;
	  break;
	case 9:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f", lower[0]);
	  sprintf(upper_value, "%f", upper[0]);
	  sprintf(init_value,"%f", initial[0]);
	  if (scale==0) 
	    fprintf(output, "	scale %.3f 1 1\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n  scale %.3f 1 1\n  children [", initial[0]);
	  scale++;
	  route(tmp_file, "Scalar", parts[pp].out_name, init_value,
		lower_value, upper_value,"scale",9);
	  j++;
	  break;
	case 10:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f", lower[0]);
	  sprintf(upper_value, "%f", upper[0]);
	  sprintf(init_value,"%f", initial[0]);
	  if (scale==0) 
	  fprintf(output, "	scale %.3f 1 1\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n  scale 1 %.3f 1\n  children [", initial[0]);
	  scale++;

	  route(tmp_file, "Scalar", parts[pp].out_name, init_value,lower_value, upper_value,
		"scale",10);
	  j++;
	  break;
	case 11:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f", lower[0]);
	  sprintf(upper_value, "%f", upper[0]);
	  sprintf(init_value,"%f", initial[0]);
	  if (scale==0) 
	  fprintf(output, "	scale %.3f 1 1\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n  scale 1 1 %.3f\n  children [", initial[0]);
	  scale++;
	  route(tmp_file, "Scalar", parts[pp].out_name, init_value,
		lower_value, upper_value,"scale",11);
	  j++;
	  break;	 
	case 12:
	  limit(parts[pp],j,3,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f %f %f", lower[0]*0.001,lower[1]*0.001,
		  lower[2]*0.001);
	  sprintf(upper_value, "%f %f %f", upper[0]*0.001,upper[1]*0.001,
		  upper[2]*0.001);
	  sprintf(init_value,"%f %f %f", initial[0],
		  initial[1],initial[2]);
	  fprintf(output, "    translation %.3f %.3f %.3f\n", initial[0],
		  initial[1], initial[2]);
	  route(tmp_file, "Position", parts[pp].out_name, init_value,
		lower_value, upper_value, "translation",12);
	  j=j+3;
	  break;
	case 13:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "%f 0 0", lower[0]*0.001);
	  sprintf(upper_value, "%f 0 0", upper[0]*0.001);
	  sprintf(init_value,"%f 0 0", initial[0]);	  
	  if (trans == 0)
	    fprintf(output, "	translation %.3f 0 0\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n translation %.3f 0 0 \n  children [", initial[0]);
	  trans++;
	  route(tmp_file, "Position", parts[pp].out_name,init_value,
		lower_value, upper_value, "translation",13);
	  j=j+1;
	  break;
	case 14:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "0 %f 0", lower[0]*0.001);
	  sprintf(upper_value, "0 %f 0", upper[0]*0.001);
	  sprintf(init_value," 0 %f 0", initial[0]);	  
	  if (trans == 0)
	    fprintf(output, "	translation 0 %.3f 0\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n translation 0 %.3f 0 \n  children [", initial[0]);
	  trans++;

	  route(tmp_file, "Position", parts[pp].out_name, init_value,
		lower_value,upper_value, "translation",14); 
	  j=j+1;
	  break;
	case 15:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "0 0 %f", lower[0]*0.001);
	  sprintf(upper_value, "0 0 %f", upper[0]*0.001);
	  sprintf(init_value,"0 0 %f", initial[0]);	  
	  if (trans == 0)
	    fprintf(output, "	translation 0 0 %.3f\n", initial[0]);
	  else
	    fprintf(output, "  children [ \n  Transform {\n translation  0 0 %.3f \n  children [", initial[0]);
	  trans++;
	  route(tmp_file, "Position", parts[pp].out_name, init_value,
		lower_value, upper_value, "translation",15);
	  j=j+1;
	  break;	 
	case 16:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "1 0 0 %f", lower[0]);
	  sprintf(upper_value, "1 0 0 %f", upper[0]);
	  sprintf(init_value,"1 0 0 %f", initial[0]*0.0174533);	  
	  if (rotate == 0)
	    fprintf(output, "	rotation 1 0 0 %.3f\n", initial[0]*0.0174533);
	  else
	    fprintf(output, "  children [ \n  Transform {\n rotation 1 0 0 %.3f \n  children [", initial[0]*0.0174533);
	  rotate++;

	  route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		lower_value, upper_value, "rotation",16);
	  j=j+1;
	  break;
	case 17:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "0 1 0 %f", lower[0]);
	  sprintf(upper_value, "0 1 0 %f", upper[0]);
	  sprintf(init_value,"0 1 0 %f", initial[0]*0.0174533);	  
	  if (rotate == 0)
	    fprintf(output, "	rotation 0 1 0 %.3f\n", initial[0]*0.0174533);
	  else
	    fprintf(output, "  children [ \n  Transform {\n rotation 0 1 0 %.3f \n  children [", initial[0]*0.0174533);
	  rotate++;
	  route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		lower_value, upper_value, "rotation",17);
	  j=j+1;
	  break;
	case 18:
	  limit(parts[pp],j,1,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "0 0 1 %f", lower[0]);
	  sprintf(upper_value, "0 0 1 %f", upper[0]);
	  sprintf(init_value,"0 0 1 %f", initial[0]*0.0174533);
	  if (rotate == 0)
	    fprintf(output, "	rotation 0 0 1 %.3f\n", initial[0]*0.0174533);
	  else
	    fprintf(output, "  children [ \n  Transform {\n rotation 0 0 1 %.3f \n  children [", initial[0]*0.0174533);
	  rotate++;
	  route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		lower_value, upper_value, "rotation",18);
	  j=j+1;
	  break;	
	case 19: /* in IGRIP, r(oll)p(itch)y(aw) is z,y,x */
	  rpy = 1;
	  limit(parts[pp],j,3,lower,upper,initial,parts[pp].kin[k]);
	  sprintf(lower_value, "1 0 0 %f", lower[2]);
	  sprintf(upper_value, "1 0 0 %f", upper[2]);
          sprintf(init_value,  "1 0 0 %f", initial[2]*0.0174533);
          if (rotate ==0)
            fprintf(output, "   rotation 1 0 0 %.3f\n",initial[2]*0.0174533);
          else
            fprintf(output, "   children [\n DEF %s_x Transform {\n rotation 1 0 0 %.3f\n children [", parts[pp].out_name, initial[2]*0.0174533);
          rotate++;
          route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		  lower_value, upper_value, "rotation", 191);

	  sprintf(lower_value, "0 1 0 %f", lower[1]);
	  sprintf(upper_value, "0 1 0 %f", upper[1]);
          sprintf(init_value,  "0 1 0 %f", initial[1]*0.0174533);
          if (rotate ==0)
            fprintf(output, "   rotation 0 1 0 %.3f\n",initial[1]*0.0174533);
          else
            fprintf(output, "   children [\n DEF %s_y Transform {\n rotation 0 1 0 %.3f\n", parts[pp].out_name, initial[1]*0.0174533);
          rotate++;
          route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		  lower_value, upper_value, "rotation", 192);

	  sprintf(lower_value, "0 0 1 %f", lower[0]);
	  sprintf(upper_value, "0 0 1 %f", upper[0]);
          sprintf(init_value,  "0 0 1 %f", initial[0]*0.0174533);
          if (rotate ==0)
            fprintf(output, "   rotation 0 0 1 %.3f\n",initial[0]*0.0174533);
          else
            fprintf(output, "   children [\n DEF %s_z Transform {\n rotation 0 0 1 %.3f\n", parts[pp].out_name,initial[0]*0.0174533);
          rotate++;
          route(tmp_file, "Orientation", parts[pp].out_name, init_value,
		  lower_value, upper_value, "rotation", 193);
	  j=j+1;
	  break;		  
	default:
	  if (parts[pp].kin[k] != 5) printf("unknown %d \n",parts[pp].kin[k]);
	  break;
	}
	
      }
      fprintf(output, "children [\n");
      fprintf(output, "DEF TOUCH_%s TouchSensor {}\n",parts[pp].out_name);
    }

    fprintf(output, "		Inline {url \"%s\"}\n",
	    parts[pp].path_name);      

    if (scale != 0)
      for (k=0; k<scale-1; k++) fprintf(output, " ]}");
    if (trans != 0)
      for (k=0; k<trans-1; k++) fprintf(output, " ]}");
    if (rotate != 0 && rpy == 0)
      for (k=0; k<rotate-1; k++) fprintf(output, " ]}");
    fprintf(output, "\n");

    if (mode == 3) {
      /* add animation attach */
	k = -1;
	for (j=0; j<n_device; j++)
	  if (pp == devices[j].anim_attach) { k=j;break;}
	if (k!= -1) {
	  float tranrot[7];
	  fprintf(output, "DEF %s_SWITCH_2 Switch {\n",devices[k].devicename);
	  fprintf(output, "	whichChoice -1\n");
	  fprintf(output, "	choice [\n");
	  fprintf(output, "	Group {\n\tchildren [\n");
	  fprintf(output, "	Transform {\n");
	  unmatrix(devices[j].attach_mat, tranrot);
	  fprintf(output, "		translation %.3f %.3f %.3f \n", tranrot[0],
		  tranrot[1],tranrot[2]);
	  fprintf(output, "	rotation %.3f %.3f %.3f %.3f \n", tranrot[3],
		  tranrot[4],tranrot[5],tranrot[6]);
	  fprintf(output, "	children [\n");
	  fprintf(output, "	DEF %s_SCRIPT_2 Script {\n",devices[k].devicename);
	  fprintf(output, "	eventIn SFFloat switch_time\n");
	  fprintf(output, "	eventOut SFInt32 sw\n");
	  fprintf(output, "	url \"vrmlscript:\n");
	  fprintf(output, "	function switch_time(value) {\n");
	  fprintf(output, "	if (value < 1.) sw = -1;\n");
	  fprintf(output, "	else sw = 0;\n");
	  fprintf(output, "	}\n");
	  fprintf(output, "	\"\n");
	  fprintf(output, "	\t}\n");
	  fprintf(output, "	USE %s\n", devices[k].devicename);
	  fprintf(output, "	]}]}]}\n");
	}
	
	    if ( devices[parts[pp].device_id].anim_attach_done==1
	    && pp == devices[parts[pp].device_id].parts[devices[parts[pp].device_id].part_number-1]) 
	  fprintf(output, "]}]}\n");
    }
    
  }

    fprintf(output, "]}\n");
    rewind(tmp_file);

    tmp_file = fopen("tmp.wrl", "r");
    while (fgets(buffer, MAXLEN, tmp_file) != NULL) {
      fputs(buffer, output);
    }

}


/* output vrml recording part */
void output_anim(FILE *output)
{
    int i, j, id, k, kkk;

  /* time sensor */
  fprintf(output, "DEF TIME TimeSensor {cycleInterval %f}\n",timecycle);
  fprintf(output, "DEF NOSCALE ScalarInterpolator {\n");
  fprintf(output, "	key [0.,1.]\n	keyValue [0.,1.]\n}\n");
  

  fprintf(output, "ROUTE TOUCH.touchTime TO TIME.startTime\n");
  fprintf(output, "ROUTE TIME.fraction_changed TO NOSCALE.set_fraction\n");

  /* output device transform animation */
  for (i=0; i<n_device; i++) 
    device_anim(output, i, time_step);
  
  /* form interpolator node */

  for (i=0; i<part_global; i++) {

    id = parts[i].device_id;
    for (j=0; j<n_device; j++) 
      if (strcmp(devices[id].devicename,device_name[j])==0) {id = j; break;}
    
    if (parts[i].kin_num == 0) continue;
    if (parts[i].joint_num == 0) continue;
    j=0;
    for (k=0; k<parts[i].kin_num; k++) {
    if (parts[i].joint_num==0) continue;
    switch (parts[i].kin[k]) {
	case 8:
	  animate(output, parts[i],"Scalar",j,3,id,time,time_step,8,"scale");
	  j=j+3;
	  break;
	case 9:
	  animate(output, parts[i],"Scalar",j,1,id,time,time_step,9,"scale");
	  j=j+1;
	  break;
	case 10:
	  animate(output, parts[i],"Scalar",j,1,id,time,time_step,10,"scale");
	  j=j+1;
	  break;
	case 11:
	  animate(output, parts[i],"Scalar",j,1,id,time,time_step,11,"scale");
	  j=j+1;
	  break;	 
	case 12:
	  animate(output, parts[i],"Position",j,3,id,time,time_step,6,
		  "translation");
	  j=j+3;
	  break;
	case 13:
	  animate(output, parts[i],"Position",j,1,id,time,time_step,3,
		  "translation");
	  j=j+1;
	  break;
	case 14:
	  animate(output, parts[i],"Position",j,1,id,time,time_step,4,
		  "translation");
	  j=j+1;
	  break;
	case 15:
	  animate(output, parts[i],"Position",j,1,id,time,time_step,5,
		  "translation");
	  j=j+1;
	  break;	 
	case 16:
	  animate(output, parts[i],"Orientation",j,1,id,time,time_step,0,
		  "rotation");
	  j=j+1;
	  break;
	case 17:
	  animate(output, parts[i],"Orientation",j,1,id,time,time_step,1,
		  "rotation");
	  j=j+1;
	  break;
	case 18:
	  animate(output, parts[i],"Orientation",j,1,id,time,time_step,2,
		  "rotation");
	  j=j+1;
	  break;
        case 19:
          animate(output, parts[i],"Orientation",j,3,id,time,time_step,71,
		  "rotation");
	  animate(output, parts[i],"Orientation",j,3,id,time,time_step,72,
		  "rotation");
	  animate(output, parts[i],"Orientation",j,3,id,time,time_step,73,
		  "rotation");
	  j=j+3;
	  break;
    default:
      if (parts[i].kin[k] != 5) printf("unknown %d\n",parts[i].kin[k]);
    break;
    }
    }
  }

  for (id=0; id<n_device; id++) {
    if (devices[id].anim_attach != -1) {
      /* form ScalarInterpolation for anim attah */
      fprintf(output, "DEF %s_SWITCHSCALE ScalarInterpolator {\n",
	      devices[id].devicename);
      fprintf(output, "key [\n");
      kkk=0;
      for (k=0; k<time_step; k++) {
	fprintf(output, "%.3f,", time[k]/time[time_step-1]);
	kkk++;
	if (kkk==10) {fprintf(output, "\n"); kkk=0;}
      }
      fprintf(output, "]\n");
      fprintf(output, "keyValue [\n");
      kkk = 0;
      for (k=0; k<time_step; k++) {
	fprintf(output, "%d, ", devices[id].attach_time[k]);
	kkk++;
	if (kkk==7) {fprintf(output, "\n"); kkk=0;}
      }
      fprintf(output, "]\n}\n");
    fprintf(output, 
	    "ROUTE TIME.fraction_changed TO %s_SWITCHSCALE.set_fraction\n", 
	    devices[id].devicename);
    fprintf(output, 
	   "ROUTE %s_SWITCHSCALE.value_changed TO %s_SCRIPT_1.switch_time\n",
	    devices[id].devicename, devices[id].devicename);
    fprintf(output, "ROUTE %s_SCRIPT_1.sw TO %s_SWITCH_1.whichChoice\n",
	    devices[id].devicename, devices[id].devicename);
    fprintf(output, 
	   "ROUTE %s_SWITCHSCALE.value_changed TO %s_SCRIPT_2.switch_time\n",
	    devices[id].devicename, devices[id].devicename);
    fprintf(output, "ROUTE %s_SCRIPT_2.sw TO %s_SWITCH_2.whichChoice\n",
	    devices[id].devicename, devices[id].devicename);
    }
  }
}



/* to form device animation VRML */
void device_anim(FILE *output, int device_id, int time_step)
{
  char ss1[80], ss2[80];
  int i,k,kkk;
  
  
  sprintf(ss1, "%s_trans", devices[device_id].devicename);
  fprintf(output, "DEF %s PositionInterpolator {\n",ss1);
  fprintf(output, "key [\n");
  
  kkk = 0;
  for (k=0; k<time_step; k++) {
    fprintf(output, "%.3f,", time[k]/time[time_step-1]);
    kkk++;
      if (kkk==10) {fprintf(output, "\n"); kkk=0;}
  }
  fprintf(output, "]\n");
  fprintf(output, "keyValue [\n");
  kkk = 0;
  for (k=0; k<time_step; k++) {
    kkk++;
    fprintf(output, "%.3f %.3f %.3f, ", transform[device_id].translation[k][0], 
	    transform[device_id].translation[k][1], 
	    transform[device_id].translation[k][2]);
    if (kkk==7) {fprintf(output, "\n"); kkk=0;}
  }
  fprintf(output, "]}\n");

  sprintf(ss2,"%s_rot", devices[device_id].devicename);
  fprintf(output, "DEF %s OrientationInterpolator {\n",ss2);
  fprintf(output, "key [\n");
  
  kkk = 0;
  for (k=0; k<time_step; k++) {
    fprintf(output, "%.3f,", time[k]/time[time_step-1]);
    kkk++;
      if (kkk==10) {fprintf(output, "\n"); kkk=0;}
  }
  fprintf(output, "]\n");
  fprintf(output, "keyValue [\n");
  kkk = 0;
  for (k=0; k<time_step; k++) {
    kkk++;
    fprintf(output, "%.3f %.3f %.3f %.3f, ", transform[device_id].rotation[k][0],
	    transform[device_id].rotation[k][1],
	    transform[device_id].rotation[k][2],
	    transform[device_id].rotation[k][3]);
    if (kkk==7) {fprintf(output, "\n"); kkk=0;}
  }
  fprintf(output, "]}\n\n");

  fprintf(output, "ROUTE NOSCALE.value_changed TO %s.set_fraction\n",ss1);
  fprintf(output, "ROUTE %s.value_changed TO d_%s.translation\n",
	  ss1, devices[device_id].devicename);
  fprintf(output, "ROUTE NOSCALE.value_changed TO %s.set_fraction\n",ss2);
  fprintf(output, "ROUTE %s.value_changed TO d_%s.rotation\n",
	  ss2, devices[device_id].devicename);

}

/* choice: Rx=0; Ry=1; Rz=2; Tx=3; Ty=4; Tz=5 */
void animate(FILE *output, Part part, char *interp, int j, 
	     int num, int id, float *time, int time_step, int choice,char *s) 
{
  char ss[MAXNAME];
  int i,k,kkk,dof;
  float mat[4][4];
  float tranrot[7];

  
  sprintf(ss, "%s_%d", part.out_name, j);
  fprintf(output, "DEF %s %sInterpolator {\n",ss, interp);
  fprintf(output, "key [\n");

  dof = part.joint[j]-1;
  kkk = 0;
  for (k=0; k<time_step; k++) {
    fprintf(output, "%.3f,", time[k]/time[time_step-1]);
    kkk++;
    if (kkk==10) {fprintf(output, "\n"); kkk=0;}
  }
  fprintf(output, "]\n");
  fprintf(output, "keyValue [\n");
  kkk = 0;
  for (k=0; k<time_step; k++) {
    kkk++;
    switch(choice) {
    case 0:
      fprintf(output, "1 0 0 %.3f, ", animation[id][dof].value[k]);
      break;
    case 1:
      fprintf(output, "0 1 0 %.3f, ", animation[id][dof].value[k]);
      break;
    case 2:
      fprintf(output, "0 0 1 %.3f, ", animation[id][dof].value[k]);
      break;
    case 3:
      fprintf(output, "%.3f 0 0, ", animation[id][dof].value[k]);
      break;
    case 4:
      fprintf(output, "0 %.3f 0, ", animation[id][dof].value[k]);
      break;
    case 5:
      fprintf(output, "0 0 %.3f, ", animation[id][dof].value[k]);
      break;
    case 6:
      fprintf(output, "%.3f %.3f %.3f, ", animation[id][dof].value[k]
	      ,animation[id][dof+1].value[k],animation[id][dof+2].value[k]);
      break;
    case 71:
      fprintf(output, "0 0 1 %.3f, ", animation[id][dof].value[k]);
      break;

    case 72:
      fprintf(output, "0 1 0 %.3f, ", animation[id][dof+1].value[k]);
      break;
      
    case 73:
      fprintf(output, "1 0 0 %.3f, ", animation[id][dof+2].value[k]);
      break;
    case 8: /*  problems  for scale recording */
      fprintf(output, "%.3f,", animation[id][dof].value[k]);
      break;
    case 9:
      fprintf(output, "%.3f,", animation[id][dof].value[k]);
      break;
    case 10:
      fprintf(output, "%.3f,", animation[id][dof].value[k]);
      break;
    case 11:
      fprintf(output, "%.3f,", animation[id][dof].value[k]);
      break;

    default:
      if (choice != 5) printf("unknown, %d\n", choice);
      break;
    }
    if (kkk==7) {fprintf(output, "\n"); kkk=0;}
  }

  fprintf(output, "]}\n\n");
  fprintf(output, "ROUTE NOSCALE.value_changed TO %s.set_fraction\n",ss);
  if (choice == 8) {
    fprintf(output, "ROUTE %s.value_changed TO Scale_xyz.scalefactor\n",ss);
    fprintf(output, "ROUTE Scale_xyz.scalevector TO %s.%s\n",part.out_name,s);
  } else 
    if (choice == 9) {
      fprintf(output, "ROUTE %s.value_changed TO Scale_x.scalefactor\n",ss);
      fprintf(output, "ROUTE Scale_x.scalevector TO %s.%s\n",part.out_name,s);
    } else
      if (choice == 10) {
	fprintf(output, "ROUTE %s.value_changed TO Scale_y.scalefactor\n",ss);
	fprintf(output, 
		"ROUTE Scale_y.scalevector TO %s.%s\n",part.out_name,s);
      } else   
	if (choice == 11) {
	  fprintf(output, 
		  "ROUTE %s.value_changed TO Scale_z.scalefactor\n",ss);
	  fprintf(output, 
		  "ROUTE Scale_z.scalevector TO %s.%s\n",part.out_name,s);
	} else
	  if (choice == 72) 
	    fprintf(output,
		    "ROUTE %s.value_changed TO %s_y.%s\n",ss,part.out_name,s);
          else
	    if (choice == 73)
	      fprintf(output,
		      "ROUTE %s.value_changed TO %s_z.%s\n",ss, part.out_name,s);
            else
	  fprintf(output, 
		  "ROUTE %s.value_changed TO %s.%s\n",ss,part.out_name,s);
  
}


void get_parts_directory()
{
	FILE *pathfile;
	char log[MAXNAME], *config, *home_dir;
	int i, j, k_dir, f_dir;

	home_dir = (char *)getenv("HOME");
	config = (char *)malloc((strlen(home_dir)+12)*sizeof(char));
	strcpy(config, home_dir);
	strcat(config, "/.telpthfig");

	pathfile = fopen(config, "r");
	k_dir=-1;  

	while (fgets(buffer, BUFSIZE, pathfile) != NULL) 
	{
	  j=0; f_dir=0;
	  for (i=0; i<strlen(buffer); i++) 
	    if (buffer[i]==' ' || buffer[i]=='\t' || buffer[i]=='\n') 
	      continue;
	    else 
	      if (f_dir) {
		dirs[k_dir][j]=buffer[i]; 
		dirs[k_dir][j+1]='\0';
		j++;
	      }
	      else 
		if (buffer[i]== '=') {
		  log[j]='\0'; 
		  j = 0;
		  if (!strncmp(log, "PART$LIB", 8)) 
		    {
		      f_dir = 1;
		      k_dir++;
		      dirs[k_dir]=(char *)malloc(50);
		      j=0;
		    }	
		}
		else {
		  log[j]=buffer[i]; 
		  j++;
		}
	}
	
	printf("This workcell's parts paths are:\n");
	n_dir = k_dir+1;
	for (i=0; i<n_dir; i++) printf("	%s\n", dirs[i]);
	printf("\n");

}		
	
int search_vrml_file(char *vrmlfile)
{
	int i;

	for (i=0; i<n_vrml_files; i++) {
		if ( !strcmp(vrml_files[i], vrmlfile)) return 1;
	}
	return 0;
}

void search_dir(char *filename)
{
	int i;
	FILE *tmp;
	
	for (i=0; i<n_dir; i++) {
		strcpy(buffer, dirs[i]);
		
		if (dirs[i][strlen(dirs[i])-1] != '/') 
		  strcat(buffer, "/");
		strcat(buffer, filename);
		if ((tmp=fopen(buffer, "r"))==NULL) continue;
		else {
			strcpy(path, buffer); 
			printf("  Part path=	%s\n", path);
			fclose(tmp);
			return;
	
		}
		printf("can not open file %s\n", buffer);
		exit();
	}
}


int if_device_exist(char *dir) 
{

  int i;
  char buffer[MAXNAME];
  FILE *file;

  system("ls > tmp.dir");
  file = fopen("tmp.dir", "r");
  while (fscanf(file,"%s\n", buffer) > 0) {
    if (strncmp(buffer,  dir, strlen(dir)) == 0) {
      fclose(file);
      system("rm tmp.dir");
      return 1;
    }
  }
  system("rm tmp.dir");
  return 0;
}
      
			
void transfer(char *filename, char *newname, char *device_name)
{
	int i, loc,j;
	char command[BUFSIZE], *vrmlpart;

	/*  if the geometry file name is not default, do following :
	    search the dirs, to see which directory has this file */

	search_dir(filename);
	
	vrmlpart=(char *)malloc((strlen(device_name)+strlen(filename)+4)
				*sizeof(char));
	strcpy(vrmlpart, "");

        strcpy(buffer, "d_");
        strcat(buffer, device_name);

        if (if_device_exist(buffer)==0) {
          sprintf(command, "mkdir d_%s\n", device_name);
          system(command);
        }

	/*	strcat(vrmlpart, "/");*/
	strcat(vrmlpart, "d_");
	strcat(vrmlpart, device_name);
	strcat(vrmlpart, "/");
	loc = 0;
	for (i=0; i<strlen(filename); i++) {
	  if (filename[i]=='.' || filename[i]=='#') filename[i]='_';
		if (filename[i]=='/') loc = i;
	}
	/*if (loc == 0) strcat(vrmlpart, "/");*/
	j=strlen(filename)-loc;

        if (loc == 0) 
	   for (i=0; i<j; i++) 
	     buffer[i]=filename[i];
	else
	  for (i=0; i<j; i++)
	    buffer[i]=filename[loc+i+1];

	buffer[i] = '\0';
	strcat(vrmlpart, buffer);
	/* to form part name */
	parts[part_global].name = (char *)malloc(strlen(buffer)*sizeof(char));
	strcpy(parts[part_global].name, buffer);
	
	strcat(vrmlpart, ".wrl");
	part2vrml2(path, vrmlpart);
	strcpy(newname, vrmlpart);
}


void trans_text(FILE *inp_file, FILE *out_file)
{
	char string[MAXNAME];
	float x, y, z, width;
	float rx, ry, rz, rr;
	float size, slant, aspect, unkown;
	int color, i1, i2, i;
	float rgb[3];

	fgets(string, MAXNAME, inp_file);
	for (i=0; i<MAXNAME; i++)
		if (string[i]=='\n') string[i]='\0';
	fscanf(inp_file, "%f %f %f\n", &x, &y, &z);
	fscanf(inp_file, "%f %f %f %f\n", &rx, &ry, &rz, &rr);
	fscanf(inp_file, "%f %f %f %f\n", &size, &slant, &aspect, &unkown);
	fscanf(inp_file, "%d %d %d\n", &color, &i1, &i2);
	/* text transfer is not implemented yet */
	/*t_color(color, rgb);
	width = strlen(string)*size*aspect*0.7;*/
	/* put it later, not finished yet */
	/*fprintf(out_file, " 	Text {\n");
	fprintf(out_file, "	  string \"%s\"\n", string);
	fprintf(out_file, "	  fontStyle FontStyle { size %f\n, size }\n");
	fprintf(out_file, "	     }\n");*/
}
	

void t_color(int den_color, float vrml_color[3])
{
	int i;

	if (den_color == -2) {
			vrml_color[0] = 0.;
			vrml_color[1] = 1.;
			vrml_color[2] = 1.;
			return;
	}
	for (i=0; i<NUMBER_COLOR; i++) {
		if (table_data[i].old_c == den_color) {
			vrml_color[0] = table_data[i].new_c[0];
			vrml_color[1] = table_data[i].new_c[1];
			vrml_color[2] = table_data[i].new_c[2];
			return;
		}	
	}
	/* could not find the color */
	printf("color is not correct\n"); 
	exit();
}
	
void part2vrml2(char *denebpart, char *vrmlpart)
{
	FILE *file, *out_file;
	int version, n_coorsys, n_curve, n_surface, n_obj;
	int color_index, n_point, n_line, n_poly, n_seg, n_text, n_texture, 
	  i,j,k,zero, number, number1, number2, nn_line, exist;
	int *n_index, *nn_index;
	int **lines, **segs;
	int **polys, **nn_polys;
	float coor[4][3], color[3];
	float **points;
	char star[2];

	exist = 0;	
	vrml_files[n_vrml_files] = 
	  (char *)malloc(sizeof(char) * strlen(vrmlpart));
	if (n_vrml_files>0) exist = search_vrml_file(vrmlpart);
	if (exist == 1) return;
	else {
	  strcpy(vrml_files[n_vrml_files], vrmlpart);
	  n_vrml_files++;
	}

	file = fopen(denebpart, "r");
	out_file = fopen(vrmlpart, "w");

	fprintf(out_file, "#VRML V2.0 utf8\n\n");
	fprintf(out_file, "Transform {\n");
	/*	fprintf(out_file, "   scale 0.001 0.001 0.001\n");
	fprintf(out_file, "   rotation 1 0 0 -1.57\n");*/
	fprintf(out_file, "   children [\n");

	/*  read  version and number of coordinate system */
	fscanf(file, "%d\n", &version);

	fscanf(file, "%d\n", &n_coorsys);
	for( i=0; i<n_coorsys; i++) {
	  for (j=0; j<4; j++) 
	    fscanf(file, "%f %f %f\n", &coor[j][0], &coor[j][1],
		   &coor[j][2]);
	}

	if (version > 8) {
	/*  curves  */
	fscanf(file, "%d\n", &n_curve);
	if (n_curve != 0) {
	  printf("number of curve = %d, information lost\n", n_curve);
	  for (i=0; i<n_curve; i++) {
	    int ff=0;
	    while (ff==0) {
	      fgets(s, MAXLEN, file);
	      if (strncmp(s, "-2", 2)==0) ff=1;
	    }
	    /*  fprintf(out_file, "]}\n");
	  printf("number of curve > 0\n"); return;*/
	  }
	}

	/*  surface */
	fscanf(file, "%d\n", &n_surface);
	if (n_surface != 0) {
	  printf("number of surface = %d, information lost\n", n_surface);
	  for (i=0; i<n_surface; i++) {
	    int ff=0;
	    while (ff==0) {
	      fgets(s, MAXLEN, file);
	      if (strncmp(s, "-2", 2)==0) ff=1;
	    }
	    /*fprintf(out_file, "]}\n");
	      printf("number of surface > 0 \n"); return;*/
	  }
	}
	}
	fscanf(file, "%d\n", &n_obj);
	for (i=0; i<n_obj; i++) {
	  fscanf(file, "%d\n", &color_index);
	  if (version == 12) fscanf(file, "%s\n", star);
	  fscanf(file, "%d\n", &n_point);
	  /*  read points data */
		points = (float **)malloc(n_point * sizeof(float *));
		for (j=0; j<n_point; j++) {
		  points[j] = (float *)malloc(3 * sizeof(float));
		  fscanf(file, "%f %f %f\n", &points[j][0],
			 &points[j][1], &points[j][2]);
		}

		/*  read line segments */
		fscanf(file, "%d\n", &n_line);
		lines = (int **)malloc(n_line * sizeof(int *));
		for (j=0; j<n_line; j++) {
		  lines[j] = (int *)malloc(sizeof(int)*2);
		  fscanf(file, "%d %d\n", &lines[j][0], &lines[j][1]);
		}

		if (version == 12) {
		  fscanf(file, "%d\n", &n_seg);
		  segs = (int **)malloc(n_seg * sizeof(int *));
		  for (j=0; j<n_seg; j++) {
		    segs[j] = (int *)malloc(sizeof(int)*2);
		    fscanf(file, "%d %d\n", &segs[j][0], &segs[j][1]);
		  }
		}

		/* read polygon information */
		fscanf(file, "%d\n", &n_poly);
		polys = (int **)malloc(n_poly*sizeof(int *));
		n_index = (int *)malloc(n_poly*sizeof(int));
		for (j=0; j<n_poly; j++) {
		  fscanf(file, "%d", &n_index[j]);
		  polys[j] = (int *)malloc(sizeof(int)*n_index[j]);
		  
		  for (k=0; k<n_index[j]; k++) {
		    fscanf(file, "%d", &polys[j][k]);
		  }
		  fscanf(file, "\n");
		}


		/*    for version 12 to deal with poly */
		if (version == 12) {
		  nn_polys = (int **)malloc(n_poly*sizeof(int *));
		  nn_index = (int *)malloc(n_poly*sizeof(int));
		  for (j=0; j<n_poly; j++) {
		    nn_index[j] = 0;
		    nn_polys[j]=(int *)malloc(sizeof(int)*n_index[j]);
		    for (k=0; k<n_index[j]; k++) {
		      nn_line = abs(polys[j][k]);
		      number1 = segs[nn_line-1][0];
		      number2 = segs[nn_line-1][1];
		      if (polys[j][k]<0) 
			nn_polys[j][nn_index[j]]=number2;
		      else	
			nn_polys[j][nn_index[j]]=number1;
		      nn_index[j]=nn_index[j]+1;
		    }
		    
		  }
		}

		fscanf(file, "%d\n", &n_text);
      		for (k=0; k<n_text; k++)
		  trans_text(file, out_file);
		if (n_text > 0) printf("text information lost\n");

		if (version >= 11) {
		fscanf(file, "%d\n", &n_texture);
		for (j=0; j<n_texture; j++)
			fgets(s, MAXLEN, file);
		if (n_texture > 0)
			printf("texture information lost\n");
		}

		/*  now to output object */
	        fprintf(out_file, "   Transform  {\n");
 		fprintf(out_file, "    children [\n");
        	fprintf(out_file, "      Shape {\n");
		/*  get color from table */
		t_color(color_index, color);
        	fprintf(out_file, "         appearance Appearance {\n");
		fprintf(out_file, "            material Material { \n");
        	fprintf(out_file, "	          diffuseColor  %f %f %f\n", 
			color[0],color[1],color[2]);
        	fprintf(out_file, "            }\n");
		fprintf(out_file, "         }\n");	

		if (n_poly > 0 ) {
 		  fprintf(out_file, "         geometry IndexedFaceSet {\n");
		  fprintf(out_file, "            coord Coordinate {\n");
        	  fprintf(out_file, "		    point [\n");
        	  for (j=0; j<n_point; j++) {
		    fprintf(out_file, "                  %f %f %f,\n", 
			    points[j][0], points[j][1], points[j][2]);
		  }
		  
        	  fprintf(out_file, "               ]\n");
        	  fprintf(out_file, "            }\n");
		  
		  
		  fprintf(out_file, "            coordIndex [\n");
		  if (version == 12) 
		    for (j=0; j<n_poly; j++) {
		      fprintf(out_file, "               ");
		      for (k=0; k<nn_index[j]; k++)
			fprintf(out_file, "%d,", nn_polys[j][k]);
		      free(nn_polys[j]);
		      fprintf(out_file, "-1,\n");
		    }
		  else
		    for (j=0; j<n_poly; j++) {
		      fprintf(out_file, "               ");
		      for (k=0; k<n_index[j]; k++)
			fprintf(out_file, "%d,", polys[j][k]);
		      free(polys[j]);
		      fprintf(out_file, "-1,\n");
		    }
		  fprintf(out_file, "            ]\n");
		  fprintf(out_file, "         }\n");
		}

		if (n_line > 0) {
		  fprintf(out_file, "         geometry IndexedLineSet {\n");
		  fprintf(out_file, " 	         coord Coordinate {\n");
        	  fprintf(out_file, "		    point [\n");
        	  for (j=0; j<n_point; j++) {
		    fprintf(out_file, "                  %f %f %f,\n", 
			    points[j][0], points[j][1], points[j][2]);
		    free(points[j]);
		  }
		  
        	  fprintf(out_file, "               ]\n");
        	  fprintf(out_file, "            }\n");
		  
		  fprintf(out_file, "            coordIndex [\n");
		  for (j=0; j<n_line; j++) { 
		    fprintf(out_file, "               %d,%d,-1,\n", 
			    lines[j][0], lines[j][1]);
		    free(lines[j]);
		  }
		  fprintf(out_file, "            ]\n");
		  fprintf(out_file, "         }\n");
		}
		fprintf(out_file, "      }\n");
   		fprintf(out_file, "    ]\n");
		fprintf(out_file, "   },\n");
	}
	
	fprintf(out_file, "   ]\n");
	fprintf(out_file, "}\n");
	fflush(out_file);
}

/* find device index in devices */
int find_device_id(char *name) 
{
  int i;
  for (i=0; i<n_device; i++) 
    if (strcmp(devices[i].devicename, name)==0) return i;
}

int find_part_id(int d_id, char *pname)
{
  int i;
  Part part;

  for (i=0; i<devices[d_id].part_number; i++) {
    part = parts[devices[d_id].parts[i]];
    if (strcmp(part.name, pname)==0) 
      return devices[d_id].parts[i];
  }
}


void unmatrix(float mat[4][4], float tran[7])
{
  int i,j;
  float c;
  float absc1, abs_c1;

  tran[0] = mat[3][0];
  tran[1] = mat[3][1];
  tran[2] = mat[3][2];

  c = (mat[0][0]+mat[1][1]+mat[2][2] - 1) / 2.;
  absc1=((c-1)<=0) ? -(c-1) : (c-1);
  if ( absc1 < 0.00001) {
    tran[3]=0.;tran[4]=0.;tran[5]=0.;tran[6]=0.0; 
    return; }

  abs_c1 = ((c+1)<=0) ? -(c+1) : (c+1);
  if (abs_c1 < 0.00001)  {
    tran[5] = sqrt((mat[2][2]-c)/2.);
    tran[4] = sqrt((mat[1][1]-c)/2.);
    tran[3] = sqrt((mat[0][0]-c)/2.);
    tran[6] = 3.1416;
    if (tran[3]==0) {
      if (mat[1][2]<0) tran[4]=-tran[4];return;}
    else if (tran[4]==0) {
      if (mat[0][2]<0) tran[3] = -tran[3];return;}
    else if (tran[5] == 0) {
      if (mat[0][1]<0) tran[3] = -tran[3];return;}

    if (mat[0][1]>0 && mat[0][2]<0 && mat[1][2]<0) tran[5]=-tran[5];
    if (mat[0][1]<0 && mat[0][2]>0 && mat[1][2]<0) tran[4]=-tran[4];
    if (mat[0][1]<0 && mat[0][2]<0 && mat[1][2]>0) tran[3]=-tran[3];
    
    return;
  }

  tran[6] = acos(c);
  tran[3] = (mat[1][2]-mat[2][1])/(2*sin(tran[6]));
  tran[4] = (mat[2][0]-mat[0][2])/(2*sin(tran[6]));
  tran[5] = (mat[0][1]-mat[1][0])/(2*sin(tran[6]));

  return;
}

void insert(int q[Q_LENGTH], int k, int n)
{
  int i;

    for (i=Q_LENGTH-1; i>k; i--) {
      if (q[i] == -1) continue;
      else 
	q[i+1] = q[i];
    } 
    q[k+1]=n;
}
    
int q_length(int q[Q_LENGTH]) 
{
  int i;
  for (i=0; i<Q_LENGTH; i++)
    if (q[i]==-1) return i+1;
  return -1;
}