/**************************** min.c ******************************/
/*  A program for computing mininal energy of polygonal knots    */
/*  Version 2.1                                                  */
/********************** Ying-Qing Wu, 5/95 ***********************/

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

#define  size         1010    /* max number of vertices                     */
#define  minlambda    0.01

float    badrunlimit= 0.05;   /* change status if badrun exceed this*runnum */
int      add_vtx  =  0;       /* number of vertices to be added             */
int      printstyle = 0;      /* "1" to print also gradient and edge length */
float    turnpoint = -1;    
char     method = 'm';

typedef  double vec[3];
typedef  vec knt[size];

int      runnum, vnum;
knt      knot, oldknot, vtr, grad, oldgrad;
double   derivative[size], lenvtr[size], mindistance[size];   
double   dist[size][size], rate[size][size]; 
vec      minvtr[size][size];
double   mindist, maxgrad, maxderiv, enr, enr_vert, enr_edge;
vec      u, u1, u2;
FILE     *fp;
char     infile[20], outfile[20];

/* functions */
double  min(int, double *, double *);  /* min dist from pt to edge i       */
void    edgedist();                    /* min vector from edge to edge     */
void    gradient();                    /* grad of energy as ftn of vertices*/
void    edgevtr();                     /* vector from knot[i] to knot[i+1] */
void    findderiv();                   /* max derivative of gradient       */
double  energy();                      /* energy of the knot               */
double  vert_energy();                 /* vertex energy of the knot        */
double  edge_energy();                 /* edge energy of the knot          */
int     getfile();                     /* input of knot                    */
void    printfile();                   /* write result into file           */
void    add_vertex();                  /* add vertices to the knot         */
double  stdcirc(int);                  /* compute energies of regular n-gon*/
void    help();                        /* print help menu                  */
void    run();                         /* run the minimizer                */
void    find_rate();                   /* compute position of min vector   */

/* vector calculation tools */
double   *add(double *, double *, double *);
double   *subt(double *, double *, double *);
double   *cross(double *, double *, double *);
double   *mult(double, double *, double *);
double   dot(double *, double *);
double   norm(double *);
void     assign(double *, double *);

int main(int argc, char *argv[])
{ 
  char cmd[80], dumb[10], *style[]={"short","long"};
  int stdvnum;

  if (argc>1) 
      strcpy(infile, argv[1]), getfile();
  if (argc>2) strcpy(outfile, argv[2]);
  while (1) {
    printf ("Enter a command (\"h\" for help): ");
    gets(cmd);          
    switch (cmd[0]) {  
      case 'h':  help(); break;
      case 'q':  exit(0);
      case 'a':  case 'd':  
           if (cmd[0] == 'd') add_vtx = vnum;
           else if(sscanf(cmd, "%s %d", dumb, &add_vtx)<2) add_vtx = 0;
           add_vertex();
           break;
      case 's': 
           sscanf(cmd, "%s %d", dumb, &stdvnum);
           printf("  Min-dist energy:           %14.8f\n", stdcirc(stdvnum));
           printf("  Edge energy:               %14.8f\n", enr_edge);
           printf("  Vertex energy:             %14.8f\n", enr_vert);
           break;
      case 'f':
           sscanf(cmd, "%s %s %s", dumb, infile, outfile);
           getfile();
           break;
      case 'r':
           if (sscanf(cmd, "%s %d", dumb, &runnum)==1) runnum = 0;
           run();
           break;
      case 'p':
           printstyle = (printstyle == 0) ? 1 : 0;
           printf("  Set output style to %s print\n",style[printstyle]);
           break;
      case 'm':
           method = (method == 'm') ? 'e' : 'm';
           printf("  Set flow method to %c\n", method);
           break;
      case 't':
           sscanf(cmd, "%s %f", dumb, &turnpoint);
           printf("  Set turning point to %1.2f\n",turnpoint);
           break;
      default:
           printf("Wrong command.  Please try again.\n");
           break;
      }
  }
}

void help()
{  printf(
    "\nChoose one of the following:\n"
    "  file knot1 knot2:    set knot1 as input file and knot2 as output file\n"
    "  run n:               run the minimizer n times\n"
    "  add n:               add n vertices to the knot\n"
    "  double:              double the vertices, = add vert_number \n"
    "  std n:               compute the energy of a standard n-gon\n"
    "  method:              toggle flow method between MD grad and E grad\n" 
    "  pstyle:              toggle between short (default) and long output\n"
    "  tpoint x:            set turning point to x (between 0 and 1)\n"
    "  help:                print this help menu\n"
    "  quit:                quit min\n"
   );
}

void run()
{  int i,ctr,badrun;
   double move, temp, old_enr, badlim, lambda=1;

   badlim = badrunlimit*runnum;
   printf(" \n   Number of edges:  %d\n", vnum);
   for (badrun = 0, ctr = 0; ctr<runnum; ctr++)
     { old_enr = enr;
       energy(); 
       if (enr>=old_enr && ctr>1)   badrun++;
       if (badrun>3*badlim && lambda > minlambda)
           {  lambda = 0.9*lambda; 
              badrun = 2*badlim;
           }
       temp = (1.0*ctr)/runnum;
       printf("%-8d %14.8f \n", ctr, enr); 
       findderiv();
       for (i=0; i<vnum; i++)      
        { if ((turnpoint < 0 && badrun < badlim) || 
               (turnpoint > temp))
              move = lambda/derivative[i];
          else move = lambda/maxderiv;   
          assign(knot[i],add(oldknot[i], mult(move,oldgrad[i],u),u1));
        }    
     }
   printf("%-8d %14.8f\n", ctr, energy());
   printfile();
}

void add_vertex()
{  int i, j;
   if (vnum == 0) {
       printf("Please specify a knot file.\n");
       return;
     }
   while (add_vtx > 0) {
     j = (add_vtx > vnum) ? 0 : vnum - add_vtx;
     for (i = vnum-j; i > 0; i--) {
       if (i<vnum) assign (knot[j+2*i], knot[j+i]);
       mult(0.5, add(knot[(j+i)%vnum], knot[j+i-1], u), knot[j+2*i-1]);
     }
     add_vtx -= vnum; vnum = 2*vnum-j;
   }
}

double stdcirc(int stdvnum)       /* energy of regular n-gon */
{  int i;
   knt tempknot;
   double pi = 3.1415926535;
   int tempvnum = vnum;

   for (i=0; i<vnum; i++)    assign(tempknot[i], knot[i]);
   for (i=0; i<(vnum =stdvnum); i++)
     { knot[i][0] = cos(2*pi*i/stdvnum);
       knot[i][1] = sin(2*pi*i/stdvnum);
       knot[i][2] = 0; 
     } 
   energy(); 
   edge_energy();
   vert_energy();
   for (i=0; i<(vnum = tempvnum); i++)     
         assign(knot[i], tempknot[i]);
   edgedist();
   return enr;
}

int getfile (void)
{  int i,j,k,n=0;
   float x;
   char str[80];

   if ((fp = fopen (infile, "r")) == NULL) {
      printf ("Cannot open %s for reading\n", infile);
      return 0; 
    }
   while (n<2)
    {  fgets(str, 80, fp);
       for (i=0; isdigit(str[i])==0 && str[i] != '-' && i < strlen(str); i++)
                 str[i] = ' ';
       if (isdigit(str[i]) != 0|| str[i]=='-') n++;
       if (n == 1)    sscanf(str, "%d",&vnum);
    }
   for (j=0; j<vnum; j++)
   { for (k = 0; k < 3; k++) 
      { for (i=0; isdigit(str[i]) == 0 && str[i] != '-'; i++)
           str[i] = ' ';
        sscanf(str, "%f", &x);
        knot[j][k] = x;
        for ( ; isdigit(str[i]) != 0 || str[i] == '-' || str[i] == '.'; i++)
           str[i] = ' ';
      }
     fgets(str, 80, fp);
   }
   fclose(fp);
   return 1;
}

double min(int i, vec p, double *ratio) 
{ vec e;
  assign(e, subt(p,knot[i],u));
  *ratio = dot(e, vtr[i])/dot(vtr[i],vtr[i]);
  if (*ratio < 0) 
     {*ratio =0;  return norm(e);}
  else if (*ratio < 1) 
     return norm(subt(e, mult(*ratio, vtr[i],u), u1));
  else  
     {*ratio = 1;  return norm(subt(e,vtr[i],u));}
}

void find_rate()
{ int i,j,k,n;
  double s[4], m[4], a, b, v[3];
  
  edgevtr();
  mindist=0;
  for (i=0; i<vnum; i++)      mindistance[i] = 0;
  if (method == 'e') 
     for(i=0; i<vnum-1; i++)  for (j = i+1; j<vnum; j++)
             rate[i][j] = rate[j][i] = 0.5;
  else {
    for (i=0; i<vnum-2; i++)
      for (j=2+i; j<vnum ; j++)
        if (i>0 || j<vnum-1)
         { assign(v, cross(vtr[i], vtr[j], u));
           a = dot(cross(subt(knot[j], knot[i],u1), vtr[j], u2), v)/ dot(v,v);
           b = dot(cross(subt(knot[j], knot[i],u1), vtr[i], u2), v)/ dot(v,v);
           if (0 <= a && a<=1 && 0<=b && b<=1)
               rate[i][j] = a,  rate[j][i] = b;
           else 
             { m[0] = min(i, knot[j], s);
               m[1] = min(i, knot[(j==vnum-1) ? 0 : j+1], s+1);
               m[2] = min(j, knot[i], s+2);
               m[3] = min(j, knot[i+1], s+3);
               for (a = m[0], n = 0, k = 1; k < 4; k++)
                    if (a > m[k])  {a = m[k]; n =k;} 
               if (n==0) {rate[i][j] = s[0]; rate[j][i]=0;}
               if (n==1) {rate[i][j] = s[1]; rate[j][i]=1;}
               if (n==2) {rate[i][j] = 0; rate[j][i]=s[2];}
               if (n==3) {rate[i][j] = 1; rate[j][i]=s[3];}
              }
         }
    }
}

void edgedist(void)  /* calculate rate, dist, and mindist */
{ int i,j,k,n;

  find_rate();
  for (i=0; i<vnum-1; i++)  for (j=i+1; j<vnum ; j++)
   { if (i+1<j && j<i+vnum-1 || method == 'e')
      { add(knot[i], mult(rate[i][j], vtr[i],u),u1);
        add(knot[j], mult(rate[j][i], vtr[j],u),u2);
        subt(u1, u2, minvtr[i][j]);
        subt(u2, u1, minvtr[j][i]);
        dist[j][i] = dist[i][j] = norm(minvtr[i][j]);
        for (k = 0; k < 4; k++)
          { if      (k == 0) n = i;
            else if (k == 1) n = i+1;
            else if (k == 2) n = j;
            else if (k == 3) n = (j+1 == vnum) ? 0 : j+1;
            if (mindistance[n] ==0 || mindistance[n] > dist[i][j])
                  mindistance[n] = dist[i][j];
          }
       }
     else dist[i][j] = dist[j][i] = 0;
     if (mindist == 0 || mindist > dist[i][j] && dist[i][j] != 0) 
             mindist = dist[i][j];
   }
}

void gradient(void)    /* calculate grad[]  */
{ double r,t;
  register double d;
  vec v;
  int i,j,k;

  for (i=0; i< vnum; i++) for (j=0; j<3; j++)  
       oldgrad[i][j] = grad[i][j], 
       grad[i][j] = 0;
  for (maxgrad = 0, i=0; i<vnum; i++)
   { for (j = 0; j<vnum; j++) if ((d= dist[i][j]) != 0)
       {  d = dist[i][j];
          k = (i==vnum-1) ? 0 : i+1;
          mult(lenvtr[j]/(d*d*lenvtr[i]), vtr[i], v);
          t = 2*lenvtr[i]*lenvtr[j]/(d*d*d*d);
          r = (1-rate[i][j])*t;
          add (grad[i], v, grad[i]);
          subt (grad[k], v, grad[k]);
          add (grad[i], mult(r,minvtr[i][j],u1),  grad[i]);
          add(grad[k],  mult(t-r,minvtr[i][j],u1),  grad[k]);
       }
    if ( maxgrad < norm(grad[i]) ) maxgrad = norm(grad[i]);
   }
}

void findderiv(void)
{  int i;
   double temp, delta, epsilon = 0.001;

   gradient();  
   delta = epsilon*mindist;
   for (i=0; i<vnum; i++)              /* move knot by delta*gradient */
     { assign(oldknot[i], knot[i]);
       add (knot[i], mult(delta, grad[i],u1), knot[i]);
       assign (oldgrad[i], grad[i]);   /* copy grad to oldgrad */ 
     }
   edgedist();
   gradient();
   for (maxderiv = 0, i=0; i<vnum; i++)     /* find derivative of gradient*/
     { if ((temp = dot(oldgrad[i],oldgrad[i])) != 0)
              derivative[i] =( 1- dot(grad[i],oldgrad[i])/temp)/delta; 
       if (maxderiv < derivative[i]) 
              maxderiv = derivative[i];    
       if (derivative[i] < 4*norm(grad[i])/mindistance[i])  
              derivative[i] = 20*norm(grad[i])/mindistance[i];
     }
   if (maxderiv < 4*maxgrad/mindist)   /* lower limit of maxderiv */
          maxderiv = 10*maxgrad/mindist;   
}

double energy(void)
{  int i,j;
   char c = method;
   
   method = 'm';
   edgedist();
   for (enr=0, i=0; i<vnum-2; i++)
     for (j = i+2; j<vnum; j++)
        if (i>0 || j<vnum-1)
          enr += lenvtr[i]*lenvtr[j]/(dist[i][j]*dist[i][j]);
   method = c;
   return enr;
}

double vert_energy(void)
{ int i,j;
  for (enr_vert =0, i=0; i<vnum-1; i++)
    for (j = i+1; j<vnum; j++)
      enr_vert +=(lenvtr[i]+lenvtr[((i==0)?vnum:i)-1])*(lenvtr[j]+lenvtr[j-1])/
             (4*dot(subt(knot[i], knot[j], u1), subt(knot[i], knot[j], u2)));
  return enr_vert;
}

double edge_energy(void)
{  int i,j;
   vec mid[size];
   for (i=0; i<vnum; i++)
       mult(0.5, add(knot[i], knot[(i==vnum-1)?0:i+1], u), mid[i]);
   for (enr_edge =0, i=0; i<vnum-1; i++)
     for (j = i+1; j<vnum; j++)
        enr_edge += lenvtr[i]*lenvtr[j]/
             dot(subt(mid[i], mid[j], u1), subt(mid[i], mid[j], u2));
   return enr_edge;
}

void edgevtr(void)
{  int i;
   for(i=0; i<vnum; i++)
     { if (i<vnum-1)   subt(knot[i+1], knot[i], vtr[i]);
       else   subt(knot[0], knot[i], vtr[i]);
       lenvtr[i] = norm(vtr[i]);
     }
}

void printfile(void)
{  int i,j;
   double x1,x2,x3,x4;

   fprintf((fp= fopen(outfile, "w")), "{%d}\n", vnum);
   for (i=0; i<vnum; i++)
      fprintf(fp, "{%14.8f, %14.8f, %14.8f}\n",
              knot[i][0], knot[i][1],knot[i][2]);
   x1 = enr, x2 = stdcirc(vnum), x3 = enr_edge, x4 = enr_vert;
   fprintf(fp, "\n  ENERGIES        KNOT"
               "        STD %d-GON        DIFFER\n", vnum);
   fprintf(fp,"  Min-dist: %14.8f %14.8f %14.8f\n", x1,x2,x1-x2);
   fprintf(fp,"  Edge-Enr: %14.8f %14.8f %14.8f\n",
                                   edge_energy(),x3,enr_edge-x3);
   fprintf(fp,"  Vert-Enr: %14.8f %14.8f %14.8f\n", 
                                   vert_energy(),x4,enr_vert-x4);
   printf("\n  ENERGIES        KNOT        STD %d-GON        DIFFER\n", vnum);
   printf("  Min-dist: %14.8f %14.8f %14.8f\n", x1,x2,x1-x2);
   printf("  Edge-Enr: %14.8f %14.8f %14.8f\n", enr_edge,x3,enr_edge-x3);
   printf("  Vert-Enr: %14.8f %14.8f %14.8f\n", enr_vert,x4,enr_vert-x4);
   if (printstyle == 1)
      { fprintf(fp, "\n \nGradient: \n");
        for (i=0; i<vnum; i++)
          { for (j=0; j<3; j++)   fprintf(fp, "%14.8f", grad[i][j]);
            fprintf(fp, "\n");
          }
        fprintf(fp, "\n \n");
        fprintf(fp, "Length of edges:\n");
        for (i=0; i<vnum; i++)
              fprintf(fp, "   %14.8f\n", lenvtr[i]);
      }
   fclose(fp);
}

double  *cross(vec s, vec t, vec u)
{     
   u[0] = s[1]*t[2] - s[2]*t[1];
   u[1] = s[2]*t[0] - s[0]*t[2];
   u[2] = s[0]*t[1] - s[1]*t[0];
   return u;
}

double dot(vec s, vec t)
{  return   s[1]*t[1] + s[2]*t[2] + s[0]*t[0];
}

double *mult(double a, vec s, vec u)
{ int i;
  for  (i=0; i<3; i++)    u[i] = a * s[i];
  return u;
}

double *add(vec s, vec t,vec u)
{  int i;
   for (i=0; i<3; i++)      u[i] = s[i] + t[i];
   return u;
}

double *subt(vec s, vec t, vec u)
{  int i;
   for (i=0; i<3; i++)      u[i] = s[i] - t[i];
   return u;
}

double norm(vec s)
{  return  sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
}

void assign(vec s, vec t)
{  s[0] = t[0]; s[1] = t[1]; s[2] = t[2]; 
}