/***********************************************************
 *
 *  tinsmooth.c - Smooth surface normals for all triangles
 *                in a specified TIN file
 *
 *  Mark J. Stock, Sept 14, 1998, version 0.0
 *
 *  1998-09-24  changed abs to fabs when finding matching nodes
 *              commented out innermost loop (match z-value)
 *
 ***********************************************************/


#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <malloc.h>

#define TRUE 1
#define FALSE 0

/*
 * Here are some structs and pointers to hold the triangle data
 */

typedef struct three_d_vector {
   double x;
   double y;
   double z;
} VEC;

typedef struct node_record *node_ptr;
typedef struct tri_record *tri_pointer;

typedef struct node_record {
   VEC loc;	/* node's location in space */
   int num_conn;/* number of triangles using this node */
   tri_pointer conn_tri[20]; /* pointers to triangles using it */
   int conn_tri_node[20]; /* index (0,1,2) of this node in the tri */
   node_ptr next_node;
} NODE;
node_ptr node_head = NULL;

typedef struct tri_record {
   node_ptr node[3];	/* pointer to the three nodes, CCW */
   VEC norm[3];	/* normal directions at the nodes */
   tri_pointer next_tri;
                /* pointer to the next element in the list */
} TRI;

int num_tri = 0;
double match_thresh = 0.001;	/* threshhold to match node locations */


void main(int argc,char **argv) {

   int i = 0;
   int num_wrote = 0;
   double xp,yp,zp;
   int smooth_convex = TRUE;			/* smooth all convex edges, easy to render */
   int smooth_concave = FALSE;			/* smooth all concave edges, harder, default=off */
   int use_thresh = TRUE;			/* always use the threshhold...for now */
   double v_thresh = 0.3;			/* convex threshhold angle, radians */
   double c_thresh = 0.1;			/* concave threshhold angle, radians */
   char infile[80];
   char progname[80];
   tri_pointer tri_head = NULL;
   tri_pointer tri_curr = NULL;

   tri_pointer read_tin(char [80]);
   int smooth_tri(tri_pointer,double,double);
   int write_tin(tri_pointer);
   int write_obj(tri_pointer);

   /* Parse command-line args */
   (void) strcpy(progname,argv[0]);
   if (argc < 2) (void) Usage(progname,0);
   if (strncmp(argv[1], "-help", 2) == 0)
      (void) Usage(progname,0);
   (void) strcpy(infile,argv[1]);
   for (i=2; i<argc; i++) {
      if (strncmp(argv[i], "-v", 2) == 0) {
         smooth_convex = TRUE;
         if (i < argc-1) {
            if (strncmp(argv[i+1], "-", 1) != 0) {
               v_thresh = atof(argv[++i]); }}
      }
      if (strncmp(argv[i], "-c", 2) == 0) {
         smooth_concave = TRUE;
         if (i < argc-1) {
            if (strncmp(argv[i+1], "-", 1) != 0) {
               c_thresh = atof(argv[++i]); }}
      }
      else
         (void) Usage(progname,0);
   }


   /* read in the input TIN file */
   tri_head = read_tin(infile);


   /* perform the smoothing routine */
   (void) smooth_tri(tri_head,v_thresh,c_thresh);


   /* write smoothed triangles to output file (stdout) */
   /* num_wrote = write_tin(tri_head); */
   num_wrote = write_obj(tri_head);


   /* write statistics */
   /* fprintf(stderr,"# %s: TIN ASCII file, %d triangles\n",infile,num_tri); */

}


/*
 * Read in a TIN file
 */
tri_pointer read_tin(char filename[80]) {

   int i;
   char onechar;
   char twochar[2];
   char sbuf[128];
   char xs[20],ys[20],zs[20];
   VEC location;
   node_ptr the_node;
   tri_pointer head = NULL;
   tri_pointer new_tri = NULL;
   FILE *fp;

   node_ptr add_to_nodes_list(tri_pointer,int,VEC);

   /* open the .tin file for reading */
   fp = fopen(filename,"r");
   if (fp==NULL) {
      fprintf(stderr,"Could not open input file %s\n",filename);
      fflush(stderr);
      exit(0);
   }
   fprintf(stderr,"Opening file %s\n",filename);
   fflush(stderr);

   /* read the input file and update statistics */
   while (fread(&onechar,sizeof(char),1,fp) == 1) {

      if (onechar == '#') {
         /* read a comment line */
         fscanf(fp,"%[^\n]",sbuf);	/* read comment beyond '#' */
         fscanf(fp,"%[\n]",twochar);	/* read newline */
         /* fprintf(stdout,"#%s\n",sbuf);	/* write comment */

      } else if (onechar == 't') {
         /* read a triangle line */
         new_tri = (TRI *)malloc(sizeof(TRI));
         /* fprintf(stdout,"t"); */
         for (i=0; i<3; i++) {
            fscanf(fp,"%s %s %s",xs,ys,zs);
            location.x = atof(xs);
            location.y = atof(ys);
            location.z = atof(zs);
            the_node = add_to_nodes_list(new_tri,i,location);
            new_tri->node[i] = the_node;
            /* fprintf(stdout," %g %g %g",location.x,location.y,location.z); */
         }
         num_tri++;

         /* add it on as the new head of the list */
         if (head) {
            new_tri->next_tri = head;
            head = new_tri;
         } else {
            head = new_tri;
            head->next_tri = NULL;
         }

         fscanf(fp,"%[\n]",twochar);	/* read newline */
         /* fprintf(stdout,"\n"); */

      } else {
         /* if its not identifiable, skip it, do not scale, do not write */
         fscanf(fp,"%[^\n]",sbuf);	/* read line beyond first char */
         fscanf(fp,"%[\n]",twochar);	/* read newline */

      }
   }

   fclose(fp);
   return head;
}


/*
 * Add a node to the list of nodes
 */
node_ptr add_to_nodes_list(tri_pointer the_tri,int index,VEC location) {

   node_ptr curr_node = node_head;
   int found_match = FALSE;

   /* first, search the list for a node close to this */
   while (curr_node) {
      fprintf(stderr,"does location (%g %g) match node at (%g %g)? ",location.x,location.y,curr_node->loc.x,curr_node->loc.y);
      if (fabs(curr_node->loc.x - location.x) < match_thresh) {
         if (fabs(curr_node->loc.y - location.y) < match_thresh) {
            /* if (fabs(curr_node->loc.z - location.z) < match_thresh) { */
               found_match = TRUE;
               fprintf(stderr,"yes!\n");
               break;
            /* } */
         } else {
            fprintf(stderr,"no.\n");
         }
      } else {
         fprintf(stderr,"no.\n");
      }
      curr_node = curr_node->next_node;
   }

   /* did we find a match in the list of existing nodes? */
   if (found_match) {
      /* add some data to the specific node entry */
      curr_node->conn_tri[curr_node->num_conn] = the_tri;
      curr_node->conn_tri_node[curr_node->num_conn] = index;
      curr_node->num_conn++;
      fprintf(stderr,"add triangle to existing node (%g %g), num_conn now = %d\n",location.x,location.y,curr_node->num_conn);
   } else {
      /* if not, create one and add it to the list */
      curr_node = (NODE *)malloc(sizeof(NODE));
      curr_node->loc.x = location.x;
      curr_node->loc.y = location.y;
      curr_node->loc.z = location.z;
      curr_node->conn_tri[0] = the_tri;
      curr_node->conn_tri_node[0] = index;
      curr_node->num_conn = 1;
      curr_node->next_node = node_head;
      /* add it to the head of the list */
      node_head = curr_node;
      fprintf(stderr,"adding new node at %g %g %g, num_conn= 1\n",location.x,location.y,location.z);
   }

   return curr_node;
}


/*
 * Smooth the normals of a list of triangles
 */
int smooth_tri(tri_pointer tri_head,double cv_t,double cc_t) {

   int i;
   int num = 0;
   double length;
   double start[20], end[20], width[20], between[20];
   VEC normal;
   node_ptr curr_node = node_head;
   tri_pointer curr_tri = tri_head;
   tri_pointer tri_list[16]; /* pointers to triangles using it */

   VEC find_normal(VEC,VEC,VEC);	/* finds normal of triangle given 3 corners */
   double theta(VEC,VEC);		/* finds z-angle of vector from vec1 to vec2 */

   fprintf(stderr,"Smoothing triangles...\n");
   fflush(stderr);

   /* first, replace each triangles' three normals with the triangle's normal */
   while (curr_tri) {
      normal = find_normal(curr_tri->node[0]->loc,curr_tri->node[1]->loc,curr_tri->node[2]->loc);
      for (i=0; i<3; i++) {
         curr_tri->norm[i].x = normal.x;
         curr_tri->norm[i].y = normal.y;
         curr_tri->norm[i].z = normal.z;
      }
      curr_tri = curr_tri->next_tri;
   }

   /* then, order the triangles in each node's triangle list */
   /*   and find the angles around the node that each take up */
   while (curr_node) {

      /* fprintf(stderr,"\nThis node is at %g %g\n",curr_node->loc.x,curr_node->loc.y); */

      /* We should probably find the arc angles for each triangle after
         projecting them onto an average plane...*/
      /* for each triangle connected to that node... */
      for (i=0; i<curr_node->num_conn; i++) {

         /* see which of that triangle's corners shares the node */
         if (curr_node->conn_tri_node[i] == 0) {
            start[i] = theta(curr_node->conn_tri[i]->node[0]->loc,curr_node->conn_tri[i]->node[1]->loc);
            end[i] = theta(curr_node->conn_tri[i]->node[0]->loc,curr_node->conn_tri[i]->node[2]->loc);
            width[i] = end[i] - start[i];
            /* fprintf(stderr,"tri %d has this node in spot %d\n",i,curr_node->conn_tri_node[i]); */
            /* fprintf(stderr,"tri %d's two other nodes are at %g %g and %g %g\n",i,curr_node->conn_tri[i]->node[1]->loc.x,curr_node->conn_tri[i]->node[1]->loc.y,curr_node->conn_tri[i]->node[2]->loc.x,curr_node->conn_tri[i]->node[2]->loc.y); */
         } else if (curr_node->conn_tri_node[i] == 1) {
            start[i] = theta(curr_node->conn_tri[i]->node[1]->loc,curr_node->conn_tri[i]->node[2]->loc);
            end[i] = theta(curr_node->conn_tri[i]->node[1]->loc,curr_node->conn_tri[i]->node[0]->loc);
            width[i] = end[i] - start[i];
            /* fprintf(stderr,"tri %d has this node in spot %d\n",i,curr_node->conn_tri_node[i]); */
            /* fprintf(stderr,"tri %d's two other nodes are at %g %g and %g %g\n",i,curr_node->conn_tri[i]->node[2]->loc.x,curr_node->conn_tri[i]->node[2]->loc.y,curr_node->conn_tri[i]->node[0]->loc.x,curr_node->conn_tri[i]->node[0]->loc.y); */
         } else {
            start[i] = theta(curr_node->conn_tri[i]->node[2]->loc,curr_node->conn_tri[i]->node[0]->loc);
            end[i] = theta(curr_node->conn_tri[i]->node[2]->loc,curr_node->conn_tri[i]->node[1]->loc);
            width[i] = end[i] - start[i];
            /* fprintf(stderr,"tri %d has this node in spot %d\n",i,curr_node->conn_tri_node[i]); */
            /* fprintf(stderr,"tri %d's two other nodes are at %g %g and %g %g\n",i,curr_node->conn_tri[i]->node[0]->loc.x,curr_node->conn_tri[i]->node[0]->loc.y,curr_node->conn_tri[i]->node[1]->loc.x,curr_node->conn_tri[i]->node[1]->loc.y); */
         }
         if (width[i] < 0) width[i] += 2*M_PI;
         /* fprintf(stderr,"tri %d starts at %g and ends at %g for a width of %g\n",i,start[i],end[i],width[i]); */
      }
      /* if (curr_node->num_conn > 3) exit(0); */
      curr_node = curr_node->next_node;
   }

   /* lastly, loop through the nodes, modifying the normals as necessary */
   curr_node = node_head;
   while (curr_node) {

      /* depending on the number of triangles using it, run a different algorithm */
      if (curr_node->num_conn == 1) {
         /* do no modification */
         fprintf(stderr,"No smoothing for node at %g %g\n",curr_node->loc.x,curr_node->loc.y);
      } else {
         /* for starters, just use all nodes to find the shared normal vector */
         fprintf(stderr,"Smoothing node at %g %g\n",curr_node->loc.x,curr_node->loc.y);
         normal.x = 0;
         normal.y = 0;
         normal.z = 0;
         for (i=0; i<curr_node->num_conn; i++) {
            normal.x += width[i]*curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].x;
            normal.y += width[i]*curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].y;
            normal.z += width[i]*curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].z;
         }
         length = sqrt(normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
         normal.x /= length;
         normal.y /= length;
         normal.z /= length;
         for (i=0; i<curr_node->num_conn; i++) {
            curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].x = normal.x;
            curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].y = normal.y;
            curr_node->conn_tri[i]->norm[curr_node->conn_tri_node[i]].z = normal.z;
         }
      }

      curr_node = curr_node->next_node;
   }

   return num;
}


/*
 * Return the normal from three points
 */
VEC find_normal(VEC pt1,VEC pt2,VEC pt3) {

   double length;
   fprintf(stderr,"finding normal, first point is %g %g %g\n",pt1.x,pt1.y,pt1.z);
   fprintf(stderr,"finding normal, first point is %g %g %g\n",pt2.x,pt2.y,pt2.z);
   fprintf(stderr,"finding normal, first point is %g %g %g\n",pt3.x,pt3.y,pt3.z);

   /* pt2 = subtract(pt2,pt1); */
   pt2.x -= pt1.x;
   pt2.y -= pt1.y;
   pt2.z -= pt1.z;
   /* pt3 = subtract(pt3,pt1); */
   pt3.x -= pt1.x;
   pt3.y -= pt1.y;
   pt3.z -= pt1.z;
   /* pt1 = cross(pt2,pt3); */
   pt1.x = pt2.y*pt3.z - pt2.z*pt3.y;
   pt1.y = pt2.z*pt3.x - pt2.x*pt3.z;
   pt1.z = pt2.x*pt3.y - pt2.y*pt3.x;
   fprintf(stderr,"                cross product is %g %g %g\n",pt1.x,pt1.y,pt1.z);
   /* pt1 = normalize(pt1); */
   length = sqrt((pt1.x*pt1.x) + (pt1.y*pt1.y) + (pt1.z*pt1.z));
   fprintf(stderr,"                length is %lf\n",length);
   pt1.x /= length;
   pt1.y /= length;
   pt1.z /= length;

   return pt1;
}


/*
 * Returns z-angle of line from pt1 to pt2
 * angle is in radians, and 0=2pi=positive x axis
 */
double theta(VEC pt1,VEC pt2) {

   double angle;

   pt2.x -= pt1.x;
   pt2.y -= pt1.y;

   if (pt2.x > 0) {
      angle = atan2(pt2.y,pt2.x);
      /* fprintf(stderr,"atan2(%g,%g) = %g\n",pt2.y,pt2.x,angle); */
      if (angle < 0) angle += 2*M_PI;		/* M_PI is pi */
   } else {
      angle = M_PI - atan2(pt2.y,-1.0*pt2.x);
      /* fprintf(stderr,"atan2(%g,%g) = %g\n",pt2.y,-1.0*pt2.x,atan2(pt2.y,-1.0*pt2.x)); */
   }


   return angle;
}


/*
 * Write out a TIN file
 */
int write_tin(tri_pointer head) {

   int i;
   int num = 0;
   tri_pointer curr_tri = head;

   fprintf(stderr,"Writing triangles to stdout\n");
   fflush(stderr);

   /* run thru the triangle list and write them out */
   while (curr_tri) {

      /* write the node normals */
      fprintf(stdout,"n");
      for (i=0; i<3; i++) {
         fprintf(stdout," %g %g %g",curr_tri->norm[i].x,curr_tri->norm[i].y,curr_tri->norm[i].z);
      }
      fprintf(stdout,"\n");

      /* write the node locations */
      fprintf(stdout,"t");
      for (i=0; i<3; i++) {
         fprintf(stdout," %g %g %g",curr_tri->node[i]->loc.x,curr_tri->node[i]->loc.y,curr_tri->node[i]->loc.z);
      }
      fprintf(stdout,"\n");

      num++;
      curr_tri = curr_tri->next_tri;
   }

   return num;
}


/*
 * Write out a Wavefront .obj file
 */
int write_obj(tri_pointer head) {

   int i;
   int num = 0;
   int cnt = 0;
   tri_pointer curr_tri = head;

   fprintf(stderr,"Writing triangles in Wavefront .obj format to stdout\n");
   fflush(stderr);
   fprintf(stdout,"# Triangle mesh written from trismooth\n\n");
   fprintf(stdout,"o tri_mesh\n\n");

   /* run thru the triangle list and write them out */
   while (curr_tri) {

      cnt = num*3 + 1;

      /* write the node normals and locations */
      for (i=0; i<3; i++) {
         fprintf(stdout,"v %g %g %g\n",curr_tri->node[i]->loc.x,curr_tri->node[i]->loc.y,curr_tri->node[i]->loc.z);
         fprintf(stdout,"vn %g %g %g\n",curr_tri->norm[i].x,curr_tri->norm[i].y,curr_tri->norm[i].z);
      }

      /* write the face */
      fprintf(stdout,"f %d//%d %d//%d %d//%d\n\n",cnt,cnt,cnt+1,cnt+1,cnt+2,cnt+2);
      /* face syntax in .obj file: f 45//126 45//126 45//126  Its v//vn */

      num++;
      curr_tri = curr_tri->next_tri;
   }

   return num;
}


/*
 * This function writes basic usage information to stderr,
 * and then quits. Too bad.
 */
int Usage(char progname[80],int status) {

   /* Usage for tinsmooth */
   static char **cpp, *help_message[] =
   {
       "where [-options] are one or more of the following:",
       "   -v [val]    smooth all convex edges, optional smoothing angle threshhold (radians)",
       "   -c [val]    smooth all concave edges, optional smoothing angle threshhold (radians)",
       "   -help       (in place of infile) returns this help information",
       " ",
       "Options may be abbreviated to an unambiguous length (duh).",
       "Output is to stdout",
       NULL
   };

   fprintf(stderr, "usage:\n  %s infile [-options]\n\n", progname);
   for (cpp = help_message; *cpp; cpp++)
      fprintf(stderr, "%s\n", *cpp);
      fflush(stderr);
   exit(status);
   return(0);
}
