/**************************** ming.c *****************************/
/*                                                               */
/*  A program to minimize MD energy of polygonal knots.          */
/*  Open Inventer version.                                       */
/*                                                               */
/*  This part contains the energy minimizing process and all the */
/*  related mathematical calculation.  It is the core of the     */
/*  project.                                                     */
/*                                                               */
/********************** Ying-Qing Wu, 12/95 **********************/

#include <stdio.h>
#include <math.h>
#include <ctype.h>
#include <string.h>
#include <X11/Intrinsic.h>
#include <Inventor/Xt/SoXt.h>
#include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
#include "class.h"
#define  print(a,b,c,d) {fprintf(fp,a,b,c,d); printf(a,b,c,d);}

const    float  minlambda = 0.01;
float    badrunlimit = 0.05;   
float    turnpoint   = 0;    
FILE     *fp;
int      runnum, vnum, ctr;
int      pinchForce = 100;
int      runWithPinch=0, pinchStatus = 0, pinchEdge = -1;

knots    knot, vtr, grad;
double   derivative[size], lenvtr[size], mindistance[size];   
double   mindist, maxgrad, maxderiv, enr, enr_vert, enr_edge;
extern   Widget               myWindow;

/* functions */
double  min(int, double *, double &);  /* min dist from pt to edge i       */
int     knotData();                    /* 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(char *);               /* input of knot                    */
void    printfile(char *);             /* write result into file           */
void    add_vertex(int);               /* add vertices to the knot         */
double  stdcirc(int);                  /* compute energies of regular n-gon*/
void    run(int);                      /* run the minimizer                */

char    outfile[80];
void    render();
void    error(Widget, XtPointer, XtPointer);

extern  int  videoFlag;
extern  int  Vstep;
extern  char Vfile[80];



/******************************************************************
*  Save intermediate knot data to video file "fpt"
******************************************************************/
void saveVideo(FILE *fpt)
{  
   fprintf(fpt, "{%d}\n", vnum);
   for (int j=0; j<vnum; j++)  
      fprintf(fpt, "%14.8f, %14.8f, %14.8f\n", 
                    knot[j][0], knot[j][1], knot[j][2]);
}


/******************************************************************
*  Run the energy minimizing process n times (n=runnum).
******************************************************************/
void run(int runnum)
{
   int    i, badrun;
   double move, temp, old_enr = 0, badlim, lambda=1;

   if (videoFlag && !(fp= fopen(Vfile, "a")))
   { 
       error(myWindow, "Error:  Can not open file for write.", NULL);
       return;
   }

   badlim = badrunlimit*runnum;
   for (badrun = 0, ctr = 0; ctr<runnum; ctr++)
   { 
       knotData();
       render();
       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]);        
       }    
       if (videoFlag && ((ctr+1)%Vstep == 0))
              saveVideo(fp);
   }
   if (videoFlag)
        fclose(fp);
   knotData();
   printf("%-8d %14.8f \n", ctr, enr); 
   render();
   ctr = 0;
}


/******************************************************************
*  Return the minimum distance from point p to the i-th edge.
******************************************************************/
double min(int i, vector p, double& ratio) 
{
    vector e = p - knot[i];
    ratio = (e * vtr[i])/(vtr[i] * vtr[i]);
    ratio = (ratio<0) ? 0 : (ratio>1) ? 1 : ratio;
    return (e - (ratio * vtr[i])).norm();
}


/******************************************************************
*  Used in knotData().
******************************************************************/
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;

    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++) // find n such that m[n] is minimal
	   {  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;  
           }
        if (n<2)  rate_ij = s[n], rate_ji = n;
        else      rate_ij = n-2,  rate_ji = s[n];
      }
}


/******************************************************************
* Calculate rate, dist, and mindist 
******************************************************************/
int knotData(void)  
{
   int         i, j;
   double      g_norm;
   extern int  drawFlag, index;

   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-2; i++)  for (j=i+2; j<vnum && (i>0 || j<vnum-1); j++)
   {
      double rate_ij, rate_ji, d;
      find_rate(i, j, rate_ij, rate_ji);

      vector minvtr = (knot[i] + rate_ij * vtr[i])
                      - (knot[j] + rate_ji * vtr[j]);
      d = minvtr.norm();
      if (mindist == 0 || mindist > d)
              mindist = d;
      enr += lenvtr[i]*lenvtr[j]/(d*d);  // compute the energy


      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)
      {
         if (drawFlag==0)
              error(myWindow, "Error: Bad knot file.", NULL); 
         return 0;
      }
      else     // 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;
         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);
      }
   }

   if (runWithPinch)               // used to pinch-delete edge
   {
      int p = pinchEdge % vnum; 
      if (index>0 && index%2)       // edge selected
         p = index/2;
      else if (pinchEdge<0)  // pinch the edge with max angle with neighbors
      {  double x=-3, dot[3];
         for (i=0; i<vnum; i++)
         {  dot[0] = (1/(vtr[i].norm()*vtr[(i+1)%vnum].norm()))
                    *(vtr[i] * vtr[(i+1)%vnum]);
            dot[1] = (1/(vtr[(i+1)%vnum].norm()*vtr[(i+2)%vnum].norm()))
                    *(vtr[(i+1)%vnum] * vtr[(i+2)%vnum]);
            dot[2] = (1/(vtr[(i)%vnum].norm()*vtr[(i+2)%vnum].norm()))
                    *(vtr[(i)%vnum] * vtr[(i+2)%vnum]);
            if (x < dot[0]+dot[1]+dot[2])
                p=(i+1)%vnum, x=dot[0]+dot[1]+dot[2];
         }
      }            
      grad[p] = grad[p] + pinchForce * vtr[p];
      grad[(p+1)%vnum] = grad[(p+1)%vnum] - pinchForce * vtr[p];
   }

   for (i=0; i<vnum; i++)
      if ( maxgrad < (g_norm = grad[i].norm() ) ) 
                 maxgrad = g_norm;
   return 1;
}



/******************************************************************
*  Calculate the derivative of gradient
******************************************************************/
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;
   knotData();
   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];
//           derivative[i] = 4* tempGrad[i].norm() /mindistance[i];
     }

   if (maxderiv < 4*maxgrad/mindist)        // lower limit of maxderiv 
            maxderiv = 10*maxgrad/mindist;   
//            maxderiv = 4*maxgrad/mindist;   

   knot = tempKnot;
}



/******************************************************************
*  Calculate the edge-energy of the knot
******************************************************************/
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;
}



/******************************************************************
*  Calculate the edge-energy of the knot
******************************************************************/
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;
}


/******************************************************************
* Return the energy of a regular n-gon (n=stdvnum).
******************************************************************/
void add_vertex(int add_vtx)
{
   int  i, t;

   if (vnum + add_vtx > size)
   {   printf("Can not add vertices:  Vertex number would exceed limit.\n");
       return;
   }

   while (add_vtx > 0)
   {  if (add_vtx >= vnum)
      {  for (i=vnum; i>0; i--)
         {  if (i<vnum) knot[2*i] = knot[i];
            knot[2*i-1] = 0.5 * (knot[i%vnum] + knot[i-1]);
         }
         add_vtx -= vnum; 
         vnum = 2*vnum;
      }

      else  //add a vertex to the longest edge
      {  float x, max = 0; 
         for (t=i=0; i<vnum; i++)
             if (max < (x=(knot[(i+1)%vnum]-knot[i]).norm()))
                 max = x, t = i;
         for (i=vnum; i>t+1; i--)
             knot[i] = knot[i-1];
         knot[t+1] = 0.5 * (knot[t] + knot[(t+1)%vnum]);
         add_vtx--,    vnum++;
      }
   }

   printf("Number of vertices:  %d \n", vnum);
   render();
}


/******************************************************************
* Return the energy of a regular n-gon (n=stdvnum).
******************************************************************/
double stdcirc(int stdvnum)       
{  int       i;
   knots     tempknot;
   tempknot  = knot;
   double    temp;
   int       tempvnum = vnum;

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


/******************************************************************
*  Get knot data from knot file "infile".
******************************************************************/
int getfile (char *infile)
{  int       i, j = -2, n;       
   char      str[80];
   knots     myKnot;
   int       myVnum;
   extern    SoXtExaminerViewer   *myViewer;

   if ((fp = fopen (infile, "r")) == NULL) {
       printf ("Cannot open %s for reading\n", infile);
       return 0; 
     }
   while (1)
    {  if (!fgets(str, 80, fp)) return 0;
       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", &myVnum);
           if (myVnum<3)
             { j--; continue;}
           else if (myVnum>510)
               return 0;
        }
       else  
        if (sscanf(str,"%lf %lf %lf",
            &myKnot[j][0],&myKnot[j][1],&myKnot[j][2])<3) continue;
       if (j == myVnum -1) break;
    }
   fclose(fp);

   vnum = myVnum;
   knot = myKnot;
   if (!knotData()) return 0;
   printf(" \n   Number of edges:  %d\n", myVnum);
   run(0);
   render();
   myViewer->viewAll();
   myViewer->saveHomePosition(); //this does not work.  Why?
   return 1;
}


/******************************************************************
*  Save knot data to file "outfile".
******************************************************************/
void printfile(char *outfile)
{  
   if (!(fp= fopen(outfile, "w")))
     { error(myWindow, "Error:  Can not open file for write.", NULL);
       return;
     }

   fprintf(fp, "{%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();

   printf("\n  ENERGIES        KNOT        STD %d-GON        DIFFER\n", vnum);
   fprintf(fp, "\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);

   fclose(fp);
}


/******************************************************************
*  Normalize the knot so that the coordinates do not get too big
*  or too small.
******************************************************************/
void normalize()
{  vector center = vect(0,0,0);
   double x, maxNorm = 0;
   int i;

   for (i=0; i<vnum; i++)
       center = center + (1.0/vnum) * knot[i];  // center=average coord.

   for (i=0; i<vnum; i++) {
       knot[i] = knot[i] - center;
       if ( maxNorm < (x = knot[i].norm()) )
           maxNorm = x;
   }

   for (i=0; i<vnum; i++)
       knot[i] = (50/maxNorm) * knot[i];
}


/******************************************************************/
/*  given a base triangle base[3] and a line segment line[2],     */
/*  determine if the line passes through the triangle.            */
/******************************************************************/
int  isIn(int i, int j)
{
   vector base[3];  // the base triangle
   vector vv[3];    // base triangle vectors
   vector line[2];  // the line segment
   vector temp;
   
   base[0] = knot[(i-1+vnum)%vnum]; 
   base[1] = knot[i];
   base[2] = knot[(i+1)%vnum]; 

   for (int k=0; k<3; k++) 
      vv[k] = base[(k+1)%3] - base[k];
   if ((vv[0] & vv[1]).norm() == 0) return 0;

   double x = vv[0] * (vv[1] & (knot[(j+1)%vnum] - knot[i]));
   double y = vv[0] * (vv[1] & (knot[j] - knot[i]));
   if (x*y>0) return 0;       // not in
   if (x*y==0) return 1;    
   
   if (x>0)  line[0] = knot[j], line[1] = knot[(j+1)%vnum];
   else      line[1] = knot[j], line[0] = knot[(j+1)%vnum];
   // now line[1] is above the plane, and line[0] below

   double test[3];
   for (k=0; k<3; k++) 
    {  temp = (line[1]-line[0]) & vv[k];
       test[k] = (line[1]-base[k]) * ((1/temp.norm())* temp);
       if (test[k] > -0.1) test[k] = 0;
    }
   // line passes the triangle iff all test[k] are non negative.
   if (test[0]< 0 || test[1]< 0 || test[2]< 0) 
        return 0;   
   else return 1;
}

void render2(int i);


/******************************************************************/
/*  These two functions try to delete vertices from a knot        */
/*  without changing the knot type.  "simpleDelete" removes       */
/*  the easy ones.  If it fails, "deleting" tries to pinch the    */
/*  first edge to a point, then remove it, if possible.           */
/******************************************************************/

int simpleDelete(int expect)
{
   int i, t=0, ctr=0, removed=0, removable;

   while (ctr<vnum && removed<expect && vnum>3)
    {  for (removable=1, i=0; i<vnum-4; i++)
             if ( isIn(t,(t+i+2)%vnum) )  removable = 0;
       if (!removable) 
             ctr++;
       else
         {  for(i=0; i<4; i++)
             {   render2(t);
//                 sleep(1);
                 render();
             }
            for (vnum--, removed++, ctr=0, i=t; i<vnum; i++)   
                knot[i] = knot[i+1];
            render();
         }
       t = (t+1)%vnum;
    }
   return removed;
}

void deleting(int expect)
{
   int i, flag=0;
   int temp = turnpoint;
   turnpoint = 1;
   runWithPinch = 1; 
   int removed = simpleDelete(expect);

   if (pinchStatus && (removed<expect)) flag = 1;
   while (flag && (removed<expect) && vnum>3)
    { for (i=0; i<10; i++)
        {  run(20);
           if (flag = simpleDelete(expect-removed))
             { removed +=flag;
               break;
             }
        }
    }
   turnpoint = temp;
   runWithPinch = 0;
   if (removed==expect)
      printf("Done.  Deleted %d vertices.\n", removed);
   else
      printf("Can not delete %d vertices. Deleted %d.\n", expect, removed);
}


/******************************************************************/
/* Class operators                                                */
/******************************************************************/

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;
};

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

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;
}