/* animate_motion.c */

/*
 * Animate motion for Swumsuit
 *     since 2005.1.16-
 *     (c) Motomu Nakashima
 *
*/

#include "togl.h"
#include <math.h>
#include <stdlib.h>
#include <tcl.h>
#include <tk.h>
#include <GL/glu.h>

#ifndef M_PI
#  define M_PI 3.14159265
#endif

static GLuint limit;
static GLuint count = 1;
static GLubyte polycolor[4] = {255, 255, 255, 255};

FILE *mdata;
FILE *gdata;
FILE *cdata;
float xa[21][2000][20],ya[21][2000][20],za[21][2000][20]
     ,xb[21][2000][20],yb[21][2000][20],zb[21][2000][20]
     ,e1x[21][2000][20],e1y[21][2000][20],e1z[21][2000][20]
     ,e2x[21][2000][20],e2y[21][2000][20],e2z[21][2000][20]
     ,ra1[21][20],ra2[21][20],rb1[21][20],rb2[21][20];

GLfloat x1d[21][2000][17][20],y1d[21][2000][17][20]
                             ,z1d[21][2000][17][20]
       ,x2d[21][2000][17][20],y2d[21][2000][17][20]
                             ,z2d[21][2000][17][20]
       ,v3x[21][2000][17][20],v3y[21][2000][17][20]
                             ,v3z[21][2000][17][20];

GLfloat  xdist[21][2000][20][20], ydist[21][2000][20][20]
                                , zdist[21][2000][20][20]
       ,xdist2[21][2000][20][20],ydist2[21][2000][20][20]
                                ,zdist2[21][2000][20][20]
       ,fxdist[21][2000][20][20],fydist[21][2000][20][20]
                                ,fzdist[21][2000][20][20];

GLfloat xcog[2000][20],ycog[2000][20],zcog[2000][20]
       ,xcob[2000][20],ycob[2000][20],zcob[2000][20]
       ,e1xall[2000][20],e1yall[2000][20],e1zall[2000][20]
       ,e2xall[2000][20],e2yall[2000][20],e2zall[2000][20]
       ,e3xall[2000][20],e3yall[2000][20],e3zall[2000][20];

GLfloat Rotx,Roty,Rotz,scale;
GLfloat scale = 1.0;
GLfloat absmag = 1.0;
GLfloat relmag = 1.0;
GLfloat fscale=50.0;

static GLint time = 0;
GLint divt = 16;

GLint numl2,numb[20],divl[20],multi,numsub,col_code;
GLfloat stlen, muki;
GLint loadnum = 1;
GLint cog_exist;
GLint cog_display;
GLint cog_display2;

static void display( struct Togl *togl )
{
   GLint i,j,n,a;
   GLfloat xw1,xw2,xw3,xw4,xw5,xw6,yw1,yw2,yw3,yw4,yw5,yw6;
   GLfloat water_width;
   GLfloat vtx,vty,vtz;

   /*glOrtho(-0.25 , 0.25 , -0.25 , 0.25 , 1.0 , -1.0);*/

   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   glPushMatrix();
   glRotatef( -Rotx, 1.0, 0.0, 0.0 );
   glRotatef( -Roty, 0.0, 1.0, 0.0 );
   glRotatef( -Rotz, 0.0, 0.0, 1.0 );

   glEnable(GL_DEPTH_TEST);
   cog_display2 = 0;
   if (cog_display==1) { cog_display2 = 1; }
   if (cog_display2==1) { glEnable(GL_BLEND); }


   /* Draw human body */
   for (n=0;n<=numsub;n++) {

       glDisable(GL_LIGHTING);
       glDisable(GL_LIGHT0);

       /* Draw center of gravity */
       glColor3f(0.2, 0.2, 1.0);
       glTranslated(xcog[time][n]*scale,
                    zcog[time][n]*scale,
                    ycog[time][n]*scale);
       if (cog_display2==1) {
           GLUquadricObj *sphere;
           sphere = gluNewQuadric();
           gluSphere(sphere, 0.019, 10.0, 10.0);
       }
       glTranslated(-xcog[time][n]*scale,
                    -zcog[time][n]*scale,
                    -ycog[time][n]*scale);

       /* Draw principal axis of intertia */
       if (cog_display2==1) {
           glLineWidth(2.0);

           /* 1 */
           glColor3f(1.0, 0.0, 0.0);
           glBegin(GL_LINES);
           glVertex3f(xcog[time][n]*scale
                     ,zcog[time][n]*scale
                     ,ycog[time][n]*scale);
           glVertex3f((xcog[time][n]+e1xall[time][n]*0.5)*scale
                     ,(zcog[time][n]+e1zall[time][n]*0.5)*scale
                     ,(ycog[time][n]+e1yall[time][n]*0.5)*scale);
           glNormal3f(0.0, 0.0, 1.0);
           glEnd();

           /* 2 */
           glColor3f(0.0, 0.0, 1.0);
           glBegin(GL_LINES);
           glVertex3f(xcog[time][n]*scale
                     ,zcog[time][n]*scale
                     ,ycog[time][n]*scale);
           glVertex3f((xcog[time][n]+e2xall[time][n]*0.5)*scale
                     ,(zcog[time][n]+e2zall[time][n]*0.5)*scale
                     ,(ycog[time][n]+e2yall[time][n]*0.5)*scale);
           glNormal3f(0.0, 0.0, 1.0);
           glEnd();

           /* 3 */
           glColor3f(0.0, 1.0, 0.0);
           glBegin(GL_LINES);
           glVertex3f(xcog[time][n]*scale
                     ,zcog[time][n]*scale
                     ,ycog[time][n]*scale);
           glVertex3f((xcog[time][n]+e3xall[time][n]*0.5)*scale
                     ,(zcog[time][n]+e3zall[time][n]*0.5)*scale
                     ,(ycog[time][n]+e3yall[time][n]*0.5)*scale);
           glNormal3f(0.0, 0.0, 1.0);
           glEnd();

       }


       /* Draw center of buoyancy */
       /* Draw center of buoyancy */
       glColor3f(1.0, 0.2, 0.2);
       glTranslated(xcob[time][n]*scale,
                    zcob[time][n]*scale,
                    ycob[time][n]*scale);
       if (cog_display2==1) {
           GLUquadricObj *sphere;
           sphere = gluNewQuadric();
           gluSphere(sphere, 0.019, 10.0, 10.0);
       }
       glTranslated(-xcob[time][n]*scale,
                    -zcob[time][n]*scale,
                    -ycob[time][n]*scale);

       glEnable(GL_LIGHTING);
       glEnable(GL_LIGHT0);

   /* Color coding for multi project mode */
   if (col_code==2) {
       /* each subject */
       if (n==0) {
           if (cog_display2==1) { 
               glColor4f(1.0, 0.9, 0.0, 0.3); /* yellow */ 
           } else {
               glColor3f(1.0, 0.9, 0.0); /* yellow */
           }
       } else {
           a = (n-1)%5;
           if (a==0) {
               if (cog_display2==1) { 
                   glColor4f(0.4, 1.0, 0.4, 0.3); /* green */
               } else {
                   glColor3f(0.4, 1.0, 0.4); /* green */
               }
           } else if (a==1) {
               if (cog_display2==1) { 
                   glColor4f(0.0, 0.9, 1.0, 0.3); /* cyan */
               } else {
                   glColor3f(0.0, 0.9, 1.0); /* cyan */
               }
           } else if (a==2) {
               if (cog_display2==1) { 
                   glColor4f(0.4, 0.4, 1.0, 0.3); /* blue */
               } else {
                   glColor3f(0.4, 0.4, 1.0); /* blue */
               }
           } else if (a==3) {
               if (cog_display2==1) { 
                   glColor4f(1.0, 0.5, 1.0, 0.3); /* purple */
               } else {
                   glColor3f(1.0, 0.5, 1.0); /* purple */
               }
           } else {
               if (cog_display2==1) { 
                   glColor4f(1.0, 0.7, 0.6, 0.3); /* pink */
               } else {
                   glColor3f(1.0, 0.7, 0.6); /* pink */
               }
           }
       }
   } else if (col_code==1) {
       /* main or sub project(s) */
       if (n==0) {
           if (cog_display2==1) { 
               glColor4f(1.0, 0.9, 0.0, 0.3); /* yellow */
           } else {
               glColor3f(1.0, 0.9, 0.0); /* yellow */
           }
       } else {
           if (cog_display2==1) { 
               glColor4f(0.5, 1.0, 0.5, 0.3); /* green */
           } else {
               glColor3f(0.5, 1.0, 0.5); /* green */
           }
       }
   } else {
       /* none (all are same color) */
       if (cog_display2==1) { 
           glColor4f(1.0, 0.9, 0.0, 0.3); /* yellow */
       } else {
           glColor3f(1.0, 0.9, 0.0); /* yellow */
       }
   }

   for (i=0;i<=numb[n]-1;i++) {
      /* Draw side surface */
      glBegin(GL_QUAD_STRIP);
      for (j=0;j<=divt;j++) {
         glNormal3f( -v3x[i][time][j][n], 
                     -v3z[i][time][j][n], 
                     -v3y[i][time][j][n] );
         glVertex3f( x1d[i][time][j][n]*scale,
                     z1d[i][time][j][n]*scale,
                     y1d[i][time][j][n]*scale );
         glVertex3f( x2d[i][time][j][n]*scale,
                     z2d[i][time][j][n]*scale,
                     y2d[i][time][j][n]*scale );
      }
      glEnd();

      /* Draw top and bottom surface */
      vtx = e1y[i][time][n]*e2z[i][time][n]
          - e1z[i][time][n]*e2y[i][time][n];
      vty = e1z[i][time][n]*e2x[i][time][n]
          - e1x[i][time][n]*e2z[i][time][n];
      vtz = e1x[i][time][n]*e2y[i][time][n]
          - e1y[i][time][n]*e2x[i][time][n];

      for (j=0;j<=divt-1;j++) {
         glBegin(GL_TRIANGLES);
         glNormal3f( +vtx, +vtz, +vty );
         glVertex3f( xa[i][time][n]*scale, za[i][time][n]*scale,
                                        ya[i][time][n]*scale );
         glVertex3f( x1d[i][time][j  ][n]*scale,
                     z1d[i][time][j  ][n]*scale,
                     y1d[i][time][j  ][n]*scale );
         glVertex3f( x1d[i][time][j+1][n]*scale,
                     z1d[i][time][j+1][n]*scale,
                     y1d[i][time][j+1][n]*scale );
         glEnd();

         glBegin(GL_TRIANGLES);
         glNormal3f( -vtx, -vtz, -vty );
         glVertex3f( xb[i][time][n]*scale,
                     zb[i][time][n]*scale,
                     yb[i][time][n]*scale );
         glVertex3f( x2d[i][time][j  ][n]*scale,
                     z2d[i][time][j  ][n]*scale,
                     y2d[i][time][j  ][n]*scale );
         glVertex3f( x2d[i][time][j+1][n]*scale,
                     z2d[i][time][j+1][n]*scale,
                     y2d[i][time][j+1][n]*scale );
         glEnd();
      }

   }
   }

   if (cog_display2==1) { glDisable(GL_BLEND); }

   /* Draw fluid force */
   glDisable(GL_LIGHTING);
   glDisable(GL_LIGHT0);
   glColor3f(1.0, 0.0, 0.0);  /* red */
   glLineWidth(3.0*absmag);
   for (n=0;n<=numsub;n++) {
   for (i=0;i<=numb[n]-1;i++) {
   for (j=0;j<=divl[n]-1;j++) {
      glBegin(GL_LINES);
      glVertex3f(xdist[i][time][j][n]*scale
                ,zdist[i][time][j][n]*scale
               , ydist[i][time][j][n]*scale);
      glVertex3f(xdist2[i][time][j][n]*scale
                ,zdist2[i][time][j][n]*scale
               , ydist2[i][time][j][n]*scale);
      glNormal3f(0.0, 0.0, 1.0);
      glEnd();
   }}}
   glEnable(GL_LIGHTING);
   glEnable(GL_LIGHT0);

   /*glEnable(GL_COLOR_MATERIAL);*/

   /* Draw water surface */
   glDisable(GL_LIGHTING);
   glDisable(GL_LIGHT0);
   water_width = 0.5;
   glEnable(GL_BLEND);
   for (i=-10;i<=10;i++) {
      xw1 = (i  )*stlen*0.5*scale+stlen*time/numl2*scale;
      xw2 = (i+1)*stlen*0.5*scale+stlen*time/numl2*scale;
      xw3 = xw2;
      xw4 = xw1;
      xw5 = xw2;
      xw6 = xw1;
      yw1 =  0.0;
      yw2 =  0.0;
      yw3 =  water_width*scale;
      yw4 =  water_width*scale;
      yw5 = -water_width*scale;
      yw6 = -water_width*scale;

      if (i%2==0) {
         /*glColor4f(0.7, 0.9, 1.0, 0.75);   pale blue */
         glColor4f(0.9, 0.9, 1.0, 0.75);  /* pale blue */
         glBegin(GL_QUADS);
         glNormal3f(0.0,-1.0,0.0);
         glVertex3f(xw1,0.0,-yw1);
         glVertex3f(xw2,0.0,-yw2);
         glVertex3f(xw3,0.0,-yw3);
         glVertex3f(xw4,0.0,-yw4);
         glEnd();

         /*glColor4f(0.5, 0.8, 1.0, 0.75);   blue */
         glColor4f(0.5, 0.8, 1.0, 0.75);  /* blue */
         glBegin(GL_QUADS);
         glNormal3f(0.0,-1.0,0.0);
         glVertex3f(xw2,0.0,-yw2);
         glVertex3f(xw1,0.0,-yw1);
         glVertex3f(xw6,0.0,-yw6);
         glVertex3f(xw5,0.0,-yw5);
         glEnd();
      }
      else {
         glColor4f(0.5, 0.8, 1.0, 0.75); /* blue */
         glBegin(GL_QUADS);
         glNormal3f(0.0,-1.0,0.0);
         glVertex3f(xw1,0.0,-yw1);
         glVertex3f(xw2,0.0,-yw2);
         glVertex3f(xw3,0.0,-yw3);
         glVertex3f(xw4,0.0,-yw4);
         glEnd();

         glColor4f(0.9, 0.9, 1.0, 0.75); /* pale blue */
         glBegin(GL_QUADS);
         glNormal3f(0.0,-1.0,0.0);
         glVertex3f(xw2,0.0,-yw2);
         glVertex3f(xw1,0.0,-yw1);
         glVertex3f(xw6,0.0,-yw6);
         glVertex3f(xw5,0.0,-yw5);
         glEnd();
      }
   }


   glDisable(GL_BLEND);
   glEnable(GL_LIGHTING);
   glEnable(GL_LIGHT0);

   glPopMatrix();
   glFlush();
   Togl_SwapBuffers(togl);
}



static void zap( struct Togl *togl )
{
   /*struct WHIRLYGIZMO *Wg;	
   Wg = Togl_GetClientData(togl);
   free(Wg);*/
}


static void idle( struct Togl *togl )
{
   Togl_PostRedisplay(togl);
}


/* new window size or exposure */
static void reshape( struct Togl *togl)
{
}


static void init( struct Togl *togl )
{
   int i,j,k,dumint1,dumint2;
   GLfloat v1x,v1y,v1z,v2x,v2y,v2z,v3xd,v3yd,v3zd,vabs;
   GLfloat dummy1,dummy2;

   /*glOrtho(-1.0 , 1.0 , -1.0 , 1.0 , 1.0 , -1.0);}*/

   /*static GLfloat pos[4] = {5.0, 5.0, 10.0, 0.0 };*/

   /*glClearColor (0.5, 0.8, 1.0, 0.0);*/
   glClearColor (0.4, 0.6, 1.0, 0.0);
   glShadeModel (GL_SMOOTH); 

   /*glLightfv( GL_LIGHT0, GL_POSITION, pos );*/
   glEnable(GL_LIGHTING);
   glEnable(GL_LIGHT0);
   glEnable(GL_COLOR_MATERIAL);
   /*glBlendFunc(GL_SRC_ALPHA, GL_ONE);*/
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);



   Rotx = 0.0;
   Roty = 0.0;

   mdata = fopen("Output_data/motion.dat","r");
   fscanf(mdata,"%d %d %d",&numl2,&dumint1,&dumint2);
   fscanf(mdata,"%d",&numb );
   fclose(mdata);

}




int loaddata( struct Togl *togl, int argc, char *argv[] )
{
   int i,j,k,l,n,dumint1,dumint2,dumint3,loop;
   GLfloat v1x,v1y,v1z,v2x,v2y,v2z,v3xd,v3yd,v3zd,vabs;
   GLfloat dummy1,dummy2;
   char filename[35];

   loop   = atoi( argv[2] );
   multi  = atoi( argv[3] );
   numsub = atoi( argv[4] );
   cog_exist = atoi( argv[5] );

   /* If not multi project mode, only main project (numsub=0) */
   if (multi==1) { numsub=0; }
   if (multi==0) { numsub=0; }

   for (n=0;n<=numsub;n++) {

   if (n==0) { 
       /* n is 0 means main */
       mdata = fopen("Output_data/motion.dat","r");
       gdata = fopen("body_geometry.dat","r");
       if (cog_exist==1) { 
       cdata = fopen("Output_data/cogcob.dat","r");
       }
   } else {
       /* n is not 0 means sub */
       sprintf(filename,"../sub%d/Output_data/motion.dat",n);
       mdata = fopen(filename,"r");
       sprintf(filename,"../sub%d/body_geometry.dat",n);
       gdata = fopen(filename,"r");
       if (cog_exist==1) { 
       sprintf(filename,"../sub%d/Output_data/cogcob.dat",n);
       cdata = fopen(filename,"r");
       }
   }


   for (l=1;l<=loop;l++) {
      /* Load motion data files */
      if (n==0) { 
          fscanf(mdata,"%d %d %d",&numl2,&dumint1,&dumint2);
      } else {
          fscanf(mdata,"%d %d %d",&dumint3,&dumint1,&dumint2);
      }
      fscanf(mdata,"%d",&numb[n] );
      fscanf(mdata,"%d",&divl[n] );
      if (n==0) { 
          fscanf(mdata,"%f",&stlen);
          fscanf(mdata,"%f",&muki );
      } else {
          fscanf(mdata,"%f",&dummy1);
          fscanf(mdata,"%f",&dummy2);
      }
      for (j=0;j<=numl2-1;j++) {
         for (i=0;i<=numb[n]-1;i++) {
            fscanf(mdata,"%f %f %f %f %f %f %f %f %f %f %f %f"
                   ,&xa[i][j][n],&ya[i][j][n],&za[i][j][n]
                   ,&xb[i][j][n],&yb[i][j][n],&zb[i][j][n]
                   ,&e1x[i][j][n],&e1y[i][j][n],&e1z[i][j][n]
                   ,&e2x[i][j][n],&e2y[i][j][n],&e2z[i][j][n]);
         }
         for (i=0;i<=numb[n]-1;i++) {
         for (k=0;k<=divl[n]-1;k++) {
            fscanf(mdata,"%f %f %f %f %f %f"
                  ,&xdist[i][j][k][n]
                  ,&ydist[i][j][k][n]
                  ,&zdist[i][j][k][n]
                 ,&fxdist[i][j][k][n]
                 ,&fydist[i][j][k][n]
                 ,&fzdist[i][j][k][n]);
         }}
         /* load cog, cob, principal axes */
         if (cog_exist==1) { 
            fscanf(cdata,"%f %f %f %f %f %f"
                  ,&xcog[j][n]
                  ,&ycog[j][n]
                  ,&zcog[j][n]
                  ,&xcob[j][n]
                  ,&ycob[j][n]
                  ,&zcob[j][n]);
            fscanf(cdata,"%f %f %f"
                  ,&e1xall[j][n]
                  ,&e1yall[j][n]
                  ,&e1zall[j][n]);
            fscanf(cdata,"%f %f %f"
                  ,&e2xall[j][n]
                  ,&e2yall[j][n]
                  ,&e2zall[j][n]);
            fscanf(cdata,"%f %f %f"
                  ,&e3xall[j][n]
                  ,&e3yall[j][n]
                  ,&e3zall[j][n]);
         }
      }
   }
   fclose(mdata);
   if (cog_exist==1) { fclose(cdata); }


   /* Load geometry data files */
   for (i=0;i<=numb[n]-1;i++) {
       fscanf(gdata,"%f %f %f %f %f %f",
         &ra1[i][n],&ra2[i][n],&rb1[i][n],&rb2[i][n],&dummy1,&dummy2);
   }
   fclose(gdata);


   /* Calculate coordinate data */
   for (j=0;j<=numl2-1;j++) {
   for (i=0;i<=numb[n]-1;i++) {
      for (k=0;k<=divt;k++) {

         /* calculate coordinates */
         x1d[i][j][k][n] = xa[i][j][n]
                       + ra1[i][n] * e1x[i][j][n] *cos(2.0*M_PI*k/divt)
                       + ra2[i][n] * e2x[i][j][n] *sin(2.0*M_PI*k/divt);
         y1d[i][j][k][n] = ya[i][j][n]
                       + ra1[i][n] * e1y[i][j][n] *cos(2.0*M_PI*k/divt)
                       + ra2[i][n] * e2y[i][j][n] *sin(2.0*M_PI*k/divt);
         z1d[i][j][k][n] = za[i][j][n]
                       + ra1[i][n] * e1z[i][j][n] *cos(2.0*M_PI*k/divt)
                       + ra2[i][n] * e2z[i][j][n] *sin(2.0*M_PI*k/divt);
         x2d[i][j][k][n] = xb[i][j][n]
                       + rb1[i][n] * e1x[i][j][n] *cos(2.0*M_PI*k/divt)
                       + rb2[i][n] * e2x[i][j][n] *sin(2.0*M_PI*k/divt);
         y2d[i][j][k][n] = yb[i][j][n]
                       + rb1[i][n] * e1y[i][j][n] *cos(2.0*M_PI*k/divt)
                       + rb2[i][n] * e2y[i][j][n] *sin(2.0*M_PI*k/divt);
         z2d[i][j][k][n] = zb[i][j][n]
                       + rb1[i][n] * e1z[i][j][n] *cos(2.0*M_PI*k/divt)
                       + rb2[i][n] * e2z[i][j][n] *sin(2.0*M_PI*k/divt);

         /* calculate normal vector */
         
         /* first, tangential vector */
         v1x = (ra1[i][n]+rb1[i][n])
                        * e1x[i][j][n] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i][n]+rb2[i][n])
                        * e2x[i][j][n] *( cos(2.0*M_PI*k/divt));
         v1y = (ra1[i][n]+rb1[i][n])
                        * e1y[i][j][n] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i][n]+rb2[i][n])
                        * e2y[i][j][n] *( cos(2.0*M_PI*k/divt));
         v1z = (ra1[i][n]+rb1[i][n])
                        * e1z[i][j][n] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i][n]+rb2[i][n])
                        * e2z[i][j][n] *( cos(2.0*M_PI*k/divt));

         /* second, vector between 1 and 2 */
         v2x = x2d[i][j][k][n]- x1d[i][j][k][n];
         v2y = y2d[i][j][k][n]- y1d[i][j][k][n];
         v2z = z2d[i][j][k][n]- z1d[i][j][k][n];

         /* finally, the normal vector is calculated as outer product */
         v3xd = v1y*v2z - v1z*v2y;
         v3yd = v1z*v2x - v1x*v2z;
         v3zd = v1x*v2y - v1y*v2x;

         /* and normalize it */
         vabs = sqrt(v3xd*v3xd+v3yd*v3yd+v3zd*v3zd);
         v3x[i][j][k][n] = v3xd /vabs;
         v3y[i][j][k][n] = v3yd /vabs;
         v3z[i][j][k][n] = v3zd /vabs;
      }
   }}

   /* Calculate coordinates for fluid force */
   for (i=0;i<=numb[n]-1;i++) {
   for (j=0;j<=numl2-1;j++) {
   for (k=0;k<=divl[n]-1;k++) {
      xdist[i][j][k][n]= xa[i][j][n] 
              + (xb[i][j][n]-xa[i][j][n])*(k+0.5)/divl[n];
      ydist[i][j][k][n]= ya[i][j][n] 
              + (yb[i][j][n]-ya[i][j][n])*(k+0.5)/divl[n];
      zdist[i][j][k][n]= za[i][j][n] 
              + (zb[i][j][n]-za[i][j][n])*(k+0.5)/divl[n];

      xdist2[i][j][k][n]= xdist[i][j][k][n] 
              + fxdist[i][j][k][n] * fscale;
      ydist2[i][j][k][n]= ydist[i][j][k][n] 
              + fydist[i][j][k][n] * fscale;
      zdist2[i][j][k][n]= zdist[i][j][k][n] 
              + fzdist[i][j][k][n] * fscale;
   }}}
   }
   return TCL_OK;
}





int position( struct Togl *togl, int argc, char *argv[] )
{
   char Result[100];
   Tcl_Interp *interp = Togl_Interp(togl);

   sprintf(Result,"%g %g",Roty,Rotx);
   Tcl_SetResult( interp,
                     Result,
                     TCL_VOLATILE );
   return TCL_OK;
}



int positionz( struct Togl *togl, int argc, char *argv[] )
{
   char Result[100];
   Tcl_Interp *interp = Togl_Interp(togl);

   sprintf(Result,"%g",Rotz);
   Tcl_SetResult( interp,
                     Result,
                     TCL_VOLATILE );
   return TCL_OK;
}


int positions( struct Togl *togl, int argc, char *argv[] )
{
   char Result[100];
   Tcl_Interp *interp = Togl_Interp(togl);

   sprintf(Result,"%g",absmag);
   Tcl_SetResult( interp,
                     Result,
                     TCL_VOLATILE );
   return TCL_OK;
}



int rotate( struct Togl *togl, int argc, char *argv[] )
{
   Tcl_Interp *interp = Togl_Interp(togl);
   Roty = atof( argv[2] );
   Rotx = atof(argv[3] );
   Togl_PostRedisplay(togl);

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}


int rotatez( struct Togl *togl, int argc, char *argv[] )
{
   Tcl_Interp *interp = Togl_Interp(togl);
   Rotz = atof( argv[2] );
   Togl_PostRedisplay(togl);

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}


int fchange( struct Togl *togl, int argc, char *argv[] )
{
   int i,j,k,n;
   Tcl_Interp *interp = Togl_Interp(togl);
   fscale = atof( argv[2] );
   Togl_PostRedisplay(togl);

   /* Recalculate coordinates for fluid force */
   for (n=0;n<=numsub;n++) {
   for (i=0;i<=numb[n]-1;i++) {
   for (j=0;j<=numl2-1;j++) {
   for (k=0;k<=divl[n]-1;k++) {
      xdist[i][j][k][n]= xa[i][j][n] 
              + (xb[i][j][n]-xa[i][j][n])*(k+0.5)/divl[n];
      ydist[i][j][k][n]= ya[i][j][n] 
              + (yb[i][j][n]-ya[i][j][n])*(k+0.5)/divl[n];
      zdist[i][j][k][n]= za[i][j][n] 
              + (zb[i][j][n]-za[i][j][n])*(k+0.5)/divl[n];

      xdist2[i][j][k][n]= xdist[i][j][k][n] 
              + fxdist[i][j][k][n] * fscale;
      ydist2[i][j][k][n]= ydist[i][j][k][n] 
              + fydist[i][j][k][n] * fscale;
      zdist2[i][j][k][n]= zdist[i][j][k][n] 
              + fzdist[i][j][k][n] * fscale;
   }}}}

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}

int advancetime( struct Togl *togl, int argc, char *argv[] )
{
   time     = atof( argv[2] );
   col_code = atoi( argv[3] );
   cog_display = atoi( argv[4] );
   Togl_PostRedisplay(togl);
   return TCL_OK;
}

int scalechange( struct Togl *togl, int argc, char *argv[] )
{
   absmag = atof( argv[2] );
   glOrtho(-relmag/absmag , relmag/absmag , 
           -relmag/absmag , relmag/absmag , 
                      1.0 , -1.0);
   relmag = absmag;
   Togl_PostRedisplay(togl);
   return TCL_OK;
}

int epsout( struct Togl *togl, int argc, char *argv[] )
{
   Togl_DumpToEpsFile(togl, "swumview_tmp_img.eps", 1, *display );
   return TCL_OK;
}

EXPORT(int,Animate_motion_Init)(Tcl_Interp *interp) 
{
  /*
   * Initialize Tcl, Tk, and the Togl widget module.
   */
#ifdef USE_TCL_STUBS
  if (Tcl_InitStubs(interp, "8.1", 0) == NULL) {return TCL_ERROR;}
#endif
#ifdef USE_TK_STUBS
  if (Tk_InitStubs(interp, "8.1", 0) == NULL) {return TCL_ERROR;}
#endif

   if (Togl_Init(interp) == TCL_ERROR) {
      return TCL_ERROR;
   }

   /*
    * Specify the C callback functions for widget creation, display,
    * and reshape.
    */
   Togl_CreateFunc( init );
   Togl_DestroyFunc( zap );
   Togl_DisplayFunc( display );
   Togl_ReshapeFunc( reshape );
   Togl_TimerFunc( idle );
   Togl_CreateCommand("rotate",rotate);
   Togl_CreateCommand("rotatez",rotatez);
   Togl_CreateCommand("position",position);
   Togl_CreateCommand("positionz",positionz);
   Togl_CreateCommand("positions",positions);
   Togl_CreateCommand("advancetime",advancetime);
   Togl_CreateCommand("fchange",fchange);
   Togl_CreateCommand("scalechange",scalechange);
   Togl_CreateCommand("epsout",epsout);
   Togl_CreateCommand("loaddata",loaddata);
   return TCL_OK;
}
