/*************************** mincpp.C ****************************/
/*                                                               */
/*  A program to minimize MD energy of polygonal knots.          */
/*  C++ version.  Main difference is the vector operators.       */
/*                                                               */
/********************** Ying-Qing Wu, 9/95 ***********************/

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#define pprint(a,b) {fprintf(fp, a,b); printf(a,b);}
#define print(a,b,c,d) {fprintf(fp,a,b,c,d); printf(a,b,c,d);}

const    int    size = 1010;   // max number of vertices                    
const    float  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';
FILE     *fp;
int      runnum, vnum;

struct vector {
   double coord[3];
   double norm (void);
   double& operator [](int i) { return coord[i];}
   void fprint() 
     { fprintf(fp, "%14.8f, %14.8f, %14.8f\n", coord[0],coord[1],coord[2]);}
 };

struct knots {
   vector* p;
   knots(void) { p = new vector[size]; }
   ~knots(void) { delete p;}
   vector& operator [](int i) { return p[i];}
   knots& operator =(knots& a) {
      for (int i=0; i<vnum; i++) p[i] = a.p[i]; 
      return (*this); }
 };

knots    knot, vtr, grad;
double   derivative[size], lenvtr[size], mindistance[size];   
double   mindist, maxgrad, maxderiv, enr, enr_vert, enr_edge;
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    findderiv();                   /* max derivative of gradient       */
void    find_rate(int, int, double&, double&); 
                                       /* compute position of min vector   */
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                */

/* vector operators */
vector   operator +(vector a, vector b);
vector   operator -(vector a, vector b);
vector   operator -(vector a);
vector   operator *(double x, vector b);  // scalar product
double   operator *(vector a, vector b);  // dot product
vector   operator &(vector a, vector b);  // cross product
vector   vect(double a, double b, double c);

int main(int argc, char *argv[])
{ 
  char cmd[80], dumb[10], *style[2]={"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':  return 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 'm':
           method = (method == 'm') ? 'e' : 'm';
           printf("  Set flow method to %c\n", method);
           break;
      case 'p':
           printstyle ^= 1;
           printf("  Set output style to %s print\n",style[printstyle]);
           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 = 0, badlim, lambda=1;
   knots tempKnot;

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

double min(int i, vector p, double& ratio) 
{ vector e = p - knot[i];
  ratio = (e * vtr[i])/(vtr[i] * vtr[i]);
  if (ratio < 0) ratio = 0;
  if (ratio > 1) ratio = 1;
  return (e - (ratio * vtr[i])).norm();
}

void find_rate(int i, int j, double& rate_ij, double& rate_ji)
{ int k,n;
  double s[4], m[4], a, b, vv;
  vector v;

  if (method == 'e') 
        {     rate_ij = rate_ji = 0.5;
              return;
        }
  else if (i<j+1 && (i>0 || j<vnum-1))
   {  v = vtr[i] & vtr[j];
      if ((vv = v * v)!= 0) {
         a = ((knot[j] - knot[i]) & vtr[j]) * v / vv;
         b = ((knot[j] - knot[i]) & vtr[i]) * v / vv;
       }
      else a = b = -1;

      if (0 <= a && a<=1 && 0<=b && b<=1)
          rate_ij = a,  
          rate_ji = b;
      else  { 
         for (n = k = 0; k < 4; k++)
	   {  m[k] = (k<2) ? min(i, knot[(j+k)%vnum], s[k])
                           : min(j, knot[i+k-2], s[k]);
              if (m[n] > m[k])  n = k;  // find n such that m[n] is minimal
           }
         if (n<2)  rate_ij = s[n], rate_ji = n;
         else      rate_ij = n-2,  rate_ji = s[n];
      }
   }
}

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

  mindist = maxgrad = enr = 0;

  for(i=0; i<vnum; i++)
   {   vtr[i] = knot[(i+1)%vnum] - knot[i];
       lenvtr[i] = vtr[i].norm();
       grad[i] = vect(0,0,0);
       mindistance[i] = 0;
   }

  for (i=0; i<vnum-1; i++)  for (j=i+1; j<vnum ; j++)
   {  double rate_ij, rate_ji, d;
      find_rate(i, j, rate_ij, rate_ji);
      if (i+1<j && j<i+vnum-1 || method == 'e')
      { vector minvtr = (knot[i] + rate_ij * vtr[i])
                      - (knot[j] + rate_ji * vtr[j]);
        d = minvtr.norm();
        for (int k = 0; k < 4; k++)
          { int n = (k<2)? i+k : (j+k-2)%vnum;
            if (mindistance[n] ==0 || mindistance[n] > d)
                  mindistance[n] = d;
          }

        if (d != 0)     // compute the gradient:
          {  vector v = (lenvtr[j]/(d*d*lenvtr[i])) * vtr[i];
             double t = 2*lenvtr[i]*lenvtr[j]/(d*d*d*d);
             double r = (1-rate_ij)*t;
             vector minvtr = (knot[i] + rate_ij * vtr[i])
                           - (knot[j] + rate_ji * vtr[j]);
             grad[i] = grad[i] + v + (r * minvtr);
             grad[i+1] = grad[i+1] - v + ((t-r) * minvtr);
  
             int k = (j+1) % vnum;
             v = (lenvtr[i]/(d*d*lenvtr[j])) * vtr[j];
             r = (1-rate_ji)*t;
             grad[j] = grad[j] + v - (r * minvtr);
             grad[k] = grad[k] - v - ((t-r) * minvtr);
          }
       }
     else d = 0;
     if (mindist == 0 || mindist > d && d != 0) 
             mindist = d;

     if (d != 0)    // compute the energy:
        enr += lenvtr[i]*lenvtr[j]/(d*d);
   }
  for (i=0; i<vnum; i++)
    if ( maxgrad < (g_norm = grad[i].norm() ) ) 
                 maxgrad = g_norm;
}

void findderiv(void)
{  double temp, delta, epsilon = 0.00001;
   knots tGrad, tempGrad, tempKnot;

   tempKnot = knot;
   delta = epsilon*mindist;
   for (int i=0; i<vnum; i++)               // move knot by delta*gradient
       knot[i] = knot[i] + (delta * grad[i]);
   tGrad = grad;
   edgedist();
   tempGrad = grad;
   grad = tGrad;
   for (maxderiv = 0, i=0; i<vnum; i++)     // find derivative of gradient
     { if ((temp = grad[i] * grad[i]) != 0)
              derivative[i] = ( 1 - (tempGrad[i] * grad[i])/temp)/delta; 
       if (maxderiv < derivative[i]) 
              maxderiv = derivative[i];    
       if (derivative[i] < 4 *tempGrad[i].norm() / mindistance[i])  
           derivative[i] = 20* tempGrad[i].norm() /mindistance[i];
     }
   if (maxderiv < 4*maxgrad/mindist)        // lower limit of maxderiv 
            maxderiv = 10*maxgrad/mindist;   
   knot = tempKnot;
}

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

double edge_energy(void)
{  vector mid[size];

   for (int i=0; i<vnum; i++)
       mid[i] = 0.5 * (knot[i] + knot[(i+1)%vnum]);
   for (enr_edge =0, i=0; i<vnum-1; i++)
      for (int j = i+1; j<vnum; j++)
         enr_edge += lenvtr[i]*lenvtr[j]/((mid[i]-mid[j])*(mid[i]-mid[j]));
   return enr_edge;
}

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) knot[j+2*i] = knot[j+i];
         knot[j+2*i-1] = 0.5 * (knot[(j+i)%vnum] + knot[j+i-1]);
     }
     add_vtx -= vnum; 
     vnum = 2*vnum-j;
   }
}

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

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

int getfile (void)
{  int i, j = -2, n;
   char str[80];

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

void printfile(void)
{  
   fprintf((fp= fopen(outfile, "w")), "{%d}\n", vnum);
   for (int i=0; i<vnum; i++)  
               knot[i].fprint();
   double x1 = enr, x2 = stdcirc(vnum), x3 = enr_edge, x4 = enr_vert;
   edge_energy();   
   vert_energy();
   pprint("\n  ENERGIES        KNOT        STD %d-GON        DIFFER\n", vnum);
   print("  Min-dist: %14.8f %14.8f %14.8f\n", x1,x2,x1-x2);
   print("  Edge-Enr: %14.8f %14.8f %14.8f\n", enr_edge,x3,enr_edge-x3);
   print("  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++)  grad[i].fprint();
        fprintf(fp, "\n \nLength of edges:\n");
        for (i=0; i<vnum; i++)
              fprintf(fp, "   %14.8f\n", lenvtr[i]);
      }
   fclose(fp);
}

double vector::norm()
{
    return   sqrt((*this) * (*this));
}

vector operator +(vector a, vector b)
{
   vector temp;
   for (int i = 0; i<3; i++)
       temp.coord[i] = a.coord[i] + b.coord[i];
   return temp;
 }

vector operator -(vector a, vector b)
{
   vector temp;
   for (int i = 0; i<3; i++)
       temp.coord[i] = a.coord[i] - b.coord[i];
   return temp;
 }

vector operator -(vector a)
{
   vector temp;
   for (int i = 0; i<3; i++)
       temp.coord[i] = - a.coord[i];
   return temp;
 }

vector operator *(double x, vector b)
{
   vector temp;
   for (int i = 0; i<3; i++)
       temp.coord[i] = x * b.coord[i];
   return temp;
 }

double operator *(vector a, vector b)
{ return  (a.coord[0]*b.coord[0] + a.coord[1]*b.coord[1]
         + a.coord[2]*b.coord[2]) ;
 }

vector operator &(vector a, vector  b)
{  vector u;
   for (int i = 0; i < 3; i++) 
     {  int j = (i+1) % 3,  k = (i+2) % 3;
        u.coord[i] = a.coord[j]*b.coord[k] - a.coord[k]*b.coord[j];
     }
   return u;
 }

vector vect(double a, double b, double c)
{   vector v; 
    v.coord[0] = a; v.coord[1] = b; v.coord[2] = c; 
    return v;
};

knots operator +(knots a, knots b)
{
   knots temp;
   for (int i = 0; i<vnum; i++)
       temp.p[i] = a.p[i] + b.p[i];
   return temp;
 }