/********************************************************************************
*										*
*	Plot Force field data for Bussard fusor.                           	*
*   Add electric field and electron density later.                              *
*                                                                               *
*        Author = Mike Rosing                                                   *
*         date  = Sept. 12, 2007                                                *
*										*
*********************************************************************************/

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

#define UNUMPOINTS 512
#define HNUMPOINTS 512
png_byte magplt[UNUMPOINTS][HNUMPOINTS][3];
png_bytep mptr[UNUMPOINTS];

/*  This subroutine writes a png data structure to disk.
	Enter with file name, and pointer to data as well as
	row pointer data.  Returns 1 if successful, 0 if errors.
*/

int write_png( char *filename, png_bytep row_ptr[], int rowdepth)
{
	FILE *pngfile;
	png_structp	png_ptr;
	png_infop		info_ptr;
	int	check, chk;
	
/*  follow png write file commands from example.c  */

	pngfile = fopen( filename, "wb");
	if( pngfile == NULL)
	{
		printf(" Can't create png file %s.\n", filename);
		exit(0);
	}

/*  create png_struct.  I do as little as possible!  */

	png_ptr = png_create_write_struct( PNG_LIBPNG_VER_STRING,
				NULL, NULL, NULL);
	if( png_ptr == NULL)
	{
		printf("Can't create png structure. \n");
		fclose( pngfile);
	}

/*  allocate and initialize info structure  */

	info_ptr = png_create_info_struct( png_ptr);
	if( info_ptr == NULL)
	{
		fclose( pngfile);
		png_destroy_write_struct( &png_ptr, (png_infopp) NULL);
		printf(" Can't create info pointer.\n");
		exit(0);
	}

/*  haven't a clue what this does .... */

	if( setjmp(png_ptr->jmpbuf))
	{
		fclose( pngfile);
		printf("Failure in setjmp (file reading?)\n");
		png_destroy_write_struct( &png_ptr, (png_infopp) NULL);
		exit(0);
	}

/*  set up output control  */

	png_init_io( png_ptr, pngfile);
	
/*  set up image form.  8 bit RGB for now  */

	png_set_IHDR( png_ptr, info_ptr, rowdepth, rowdepth, 8, PNG_COLOR_TYPE_RGB,
				PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE,
				PNG_FILTER_TYPE_BASE);

/*  write file header info */

	png_write_info( png_ptr, info_ptr);
	
/*  write image info  */

	png_write_image( png_ptr, row_ptr);
	printf("wrote image data\n");
	
/*  finish file data  */

	png_write_end( png_ptr, info_ptr);

/*  eliminate all data allocated on heap  */

	png_destroy_write_struct( &png_ptr, (png_infopp) NULL);
	fclose(pngfile);
}

/*  Add color with maximum saturation.
	Enter with pointer to RGB triple, and red, green, blue
	new values to sum into it.
*/

void setcolor( png_byte rgb[3], int red, int green, int blue)
{
	if( rgb[0] + red > 255) rgb[0] = 255;
	else rgb[0] += (png_byte) red;
	if( rgb[1] + green > 255) rgb[1] = 255;
	else rgb[1] += (png_byte)green;
	if( rgb[2] + blue > 255) rgb[2] = 255;
	else rgb[2] += (png_byte) blue;		
}

/*  compute a 3x3 determinant.  Enter with "a" pointing to a 9 element array
    of doubles in order 11, 12, 13, 21, 22, 23, 31, 32, 33.
                        0   1   2   3   4   5   6   7   8
*/

double det( double *a)
{
    double t1, t2, t3, t4, t5, t6, t7;

    t1 = a[0] * a[4] * a[8];
    t2 = a[1] * a[5] * a[6];
    t3 = a[2] * a[3] * a[7];
    t4 = a[2] * a[4] * a[6];
    t5 = a[0] * a[5] * a[7];
    t6 = a[1] * a[3] * a[8];
    t7 = t1 + t2 + t3 - t4 - t5 - t6;
    return t7;
}

/*  Transform a 3D position to a 2D screen position.
    Input endpoint "w", "dptr" matrix, point to be transformed "p"
    and parameters qmax, rmax, smax and determinant of dptr "divisor"
    as params[0], params[1], params[2], params[3], params[4] = tan(uangle),
    params[5] = tan(hangle).
    outputs integer h, v referenced to center of screen for horizontal and
    vertical offset.
    returns 1 if point can be plotted, 0 if outside view volume.
*/

int transform3d ( double *plot, double *w, double *dptr, double *params, 
		  int *h, int *v)
{
    double b[3], cptr[9], rtst, stst;
    double q, r, s;
    int j;

	    b[0] = plot[0] - w[0];
	    b[1] = plot[1] - w[1];
	    b[2] = plot[2] - w[2];

	    for(j=0; j<9; j++) cptr[j] = dptr[j];
	    cptr[0] = b[0];
	    cptr[3] = b[1];
	    cptr[6] = b[2];
	    q = det(cptr)/params[3];
	    for(j = 0; j<9; j++) cptr[j] = dptr[j];
	    cptr[1] = b[0];
	    cptr[4] = b[1];
	    cptr[7] = b[2];
	    r = det(cptr)/params[3];
	    if (r < 0.0) rtst = -r;
	    else rtst = r;
	    for(j = 0; j<9; j++) cptr[j] = dptr[j];
	    cptr[2] = b[0];
	    cptr[5] = b[1];
	    cptr[8] = b[2];
	    s = det(cptr)/params[3];
	    if (s < 0.0) stst = -s;
	    else stst = s;
	    
/*	    printf("q = %lf r = %lf s = %lf\n",
		    q, r, s);

	    printf("params[4] = %lf params[5] = %lf\n", params[4], params[5]);
	    printf("params[1] = %lf params[2] = %lf\n", params[1], params[2]);
*/
//  do testing here and print results!!@!

	    if( q > params[0] || q < 0.0)
	    {
//		printf("point %d not in volume q = %lf qmax = %lf\n",
//		       i, q[i], qmax);
		return 0;
	    }
	    if( rtst > params[1] + (params[0] - q)*params[4])
	    {
//		printf("point %d not in volume r = %lf rmax = %lf\n",
//		       i, r[i], rmax);
		return 0;
	    }
	    if( stst > params[2] + (params[0] - q)*params[5])
	    {
//		printf("point %d not in volume s = %lf smax = %lf\n",
//		       i, s[i], smax);
		return 0;
	    }
	    *h = s/params[2] * HNUMPOINTS/2;
	    *v = r/params[1] * UNUMPOINTS/2;
	    return 1;
}

/*  draw a line in magplt buffer.  Enter with end points and color.
*/

int drawline( int h1, int v1, int h2, int v2, int color[3])
{
    int vcount, hcount, vcheck, hcheck;
    int checklength, mark;
    int hdex, vdex, start, end;
    double vfrac, hfrac, slope, bcept;

/*  translate input from center reference to screen reference  */

    h1 = HNUMPOINTS/2 + h1;
    h2 = HNUMPOINTS/2 + h2;
    v1 = UNUMPOINTS/2 - v1;
    v2 = UNUMPOINTS/2 - v2;

/*  see if vector is long enough to draw  */

    vcount = v2 - v1;
    hcount = h2 - h1;
//    printf("h1 = %d v1 = %d h2 = %d v2 = %d\n", h1, v1, h2, v2);
    if ((v1 < 0) && (v2 < 0)) return 0;
    if ((h1 < 0) && (h2 < 0)) return 0;
    if ((h1 > HNUMPOINTS) && (h2 > HNUMPOINTS)) return 0;
    if ((v1 > UNUMPOINTS) && (v2 > UNUMPOINTS)) return 0;
    checklength = vcount*vcount + hcount*hcount;
    if (checklength < 1) return 0;
    
/*  draw a line between head and tail onscreen  */

    if (vcount < 0) vcheck = -vcount;
    else vcheck = vcount;
    if (hcount < 0) hcheck = -hcount;
    else hcheck = hcount;
    if (vcheck > UNUMPOINTS) return 0;
    if (hcheck > HNUMPOINTS) return 0;
    if (hcheck >= vcheck)
    {
	if (h1 < h2) 
	{
	    start = h1;
	    end = h2;
	}
	else
	{
	    start = h2;
	    end = h1;
	}
	if(start < 0) start = 0;
	if(end >= HNUMPOINTS) end = HNUMPOINTS -1;
	vfrac = v2 - v1;
	hfrac = h2 - h1;
	slope = vfrac/hfrac;
	bcept = v1 - slope*h1;
	for (hdex=start; hdex<=end; hdex++)
	{
	    vdex = slope*(double)hdex + bcept;
	    if ((vdex > UNUMPOINTS) || (vdex < 0)) break;
/*	    if(hdex < start+2)
	    {
		magplt[vdex][hdex][0] = 255;
		magplt[vdex][hdex][1] = 0;
		magplt[vdex][hdex][2] = 0;
	    }
	    else
*/	    {
		magplt[vdex][hdex][0] = color[0];
		magplt[vdex][hdex][1] = color[1];
		magplt[vdex][hdex][2] = color[2];
	    }
	}
    }
    else
    {
	if (v1 < v2) 
	{
	    start = v1;
	    end = v2;
	}
	else
	{
	    start = v2;
	    end = v1;
	}
	vfrac = v2 - v1;
	hfrac = h2 - h1;
	slope = hfrac/vfrac;
	bcept = h1 - slope*v1;
	if (end >= UNUMPOINTS) end = UNUMPOINTS - 1;
	if (start < 0) start = 0;
	for(vdex=start; vdex<=end; vdex++)
	{
	    hdex = slope*(double)vdex + bcept;
	    if( (hdex > HNUMPOINTS) || (hdex < 0)) break;
/*	    if(vdex < start+4)
	    {
		magplt[vdex][hdex][0] = 255;
		magplt[vdex][hdex][1] = 0;
		magplt[vdex][hdex][2] = 0;
	    }
	    else
*/	    {
		magplt[vdex][hdex][0] = color[0];
		magplt[vdex][hdex][1] = color[1];
		magplt[vdex][hdex][2] = color[2];
	    }
	}
    }
    return 1;
}

main()
{
	FILE  *readmag;
	int numread, tlimit;
	int test, kmin, kmax, jmin, jmax;
	int i, j, k;
	char filename[64], input[8];
	double uangle, hangle;
	double *magblock, *vectors;
	double vectorhead[3], vectortail[3], bx, by, bz;
	double ex, ey, ez, f, cx, cy, cz, ax, ay, az;
	double wx, wy, wz, ux, uy, uz, hx, hy, hz;
	double ulen, uscale, eyelength, hscale;
	int xb, yb, zb, ix, iy, iz;
	double px, py, pz, eyefrac;
	double amin[3], divisor, dptr[9], cptr[9];
	double cprmx, cprmy, cprmz, uprmx, uprmy, uprmz, hprmx, hprmy, hprmz;
	double ptest[8][3], q[8], r[8], s[8], qmax, rmax, smax;
	double vtst;
	int numpoints, h[8], v[8], red[3], green[3], blue[3], white[3];
	double params[6], w[3], vectormax, scalevector;

	tlimit = 128;
	magblock = (double *)calloc(tlimit*tlimit*tlimit, 3*sizeof(double));
	if( magblock == NULL)
	{
	    printf("failed to allocate memory space \n");
	    exit(0);
	}
	vectors = (double *)calloc(tlimit*tlimit*tlimit, 3*sizeof(double));
	if( vectors == NULL)
	{
	    printf("failed to allocate vectors memory space \n");
	    exit(0);
	}

/*  set up some simple color references  */

	red[0] = 255;
	red[1] = 0;
	red[2] = 0;
	green[0] = 0;
	green[1] = 255;
	green[2] = 0;
	blue[0] = 0;
	blue[1] = 0;
	blue[2] = 255;
	white[0] = 255;
	white[1] = 255;
	white[2] = 255;

	printf("name of force file data? ");
	scanf( "%s", filename);
	readmag = fopen(filename, "rb");
	if( !readmag)
	{
	    printf("can't read in force data file %s.\n", filename);
	    exit(0);
	}

/*  gather view angle data  */

	printf("enter eye position point:\n");
	printf("x: ");
	scanf("%lf", &ex);
	printf("y: ");
	scanf("%lf", &ey);
	printf("z: ");
	scanf("%lf", &ez);
	printf("enter end of viewing volume point: \n");
	printf("x: ");
	scanf("%lf", &wx);
	printf("y: ");
	scanf("%lf", &wy);
	printf("z: ");
	scanf("%lf", &wz);
	w[0] = wx;
	w[1] = wy;
	w[2] = wz;
	printf("theta up: ");
	scanf("%lf", &uangle);
	printf("theta horizontal: ");
	scanf("%lf", &hangle);
	printf("fractional distance to screen: ");
	scanf("%lf", &f);
	printf("Vector length scale factor: ");
	scanf("%lf", &scalevector);

/*  compute geometric factors  */

	ax = ex - wx;
	ay = ey - wy;
	az = ez - wz;
	eyelength = sqrt( ax*ax + ay*ay + az*az);
	printf("ax = %lf  ay = %lf  az = %lf\n", ax, ay, az);
	cx = (1.0 - f)*ax;
	cy = (1.0 - f)*ay;
	cz = (1.0 - f)*az;
	printf("cx = %lf  cy = %lf  cz = %lf\n", cx, cy, cz);

/*  determine "up" based on smallest direction cosine  */

	if (ax < 0) amin[0] = -ax;
	else amin[0] = ax;
	if (ay < 0) amin[1] = -ay;
	else amin[1] = ay;
	if (az < 0) amin[2] = -az;
	else amin[2] = az;
	if ((amin[2] <= amin[0]) && (amin[2] <= amin[1]))
	{
	    ulen = ax*ax + ay*ay;
	    uscale = f*tan(uangle)/sqrt(ulen);
	    ux = -ax*az*uscale;
	    uy = -ay*az*uscale;
	    uz = ulen*uscale;
	    hscale = f*tan(hangle)/sqrt(ulen)*eyelength;
	    hx = ay*hscale;
	    hy = -ax*hscale;
	    hz = 0.0;
	}
	else if ((amin[0] <= amin[1]) && (amin[0] <= amin[2]))
	{
	    ulen = ay*ay + az*az;
	    uscale = f*tan(uangle)/sqrt(ulen);
	    ux = ulen*uscale;
	    uy = -ax*ay*uscale;
	    uz = - ax*az*uscale;
	    hscale = f*tan(hangle)/sqrt(ulen)*eyelength;
	    hx = 0.0;
	    hy = az*hscale;
	    hz = -ay*hscale;
	}
	else if ((amin[1] <= amin[0]) && (amin[1] <= amin[2]))
	{
	    ulen = ax*ax + az*az;
	    uscale = f*tan(uangle)/sqrt(ulen);
	    ux = -ax*ay*uscale;
	    uy = ulen*uscale;
	    uz = -ay*az*uscale;
	    hscale = f*tan(hangle)/sqrt(ulen)*eyelength;
	    hx = -az*hscale;
	    hy = 0.0;
	    hz = ax*hscale;
	}
	else
	{
	    printf("sorting test failed!!\n");
	    printf("amin[0] = %lf amin[1] = %lf amin[2] = %lf\n", 
		   amin[0], amin[1], amin[2]);
	    exit(0);
	}

/*  compute view volume unit vectors.  w is origin, c is screen.  */

	divisor = sqrt(cx*cx + cy*cy + cz*cz);
	qmax = divisor;
	params[0] = qmax;
	cprmx = cx/divisor;
	cprmy = cy/divisor;
	cprmz = cz/divisor;
	printf("cprm x = %lf  y = %lf  z = %lf\n", cprmx, cprmy, cprmz);
	divisor = sqrt(ux*ux + uy*uy + uz*uz);
	rmax = divisor;
	params[1] = rmax;
	uprmx = ux/divisor;
	uprmy = uy/divisor;
	uprmz = uz/divisor;
	printf("uprm x = %lf  y = %lf  z = %lf\n", uprmx, uprmy, uprmz);
	divisor = sqrt(hx*hx + hy*hy + hz*hz);
	smax = divisor;
	params[2] = smax;
	hprmx = hx/divisor;
	hprmy = hy/divisor;
	hprmz = hz/divisor;
	printf("hprm x = %lf  y = %lf  z = %lf\n", hprmx, hprmy, hprmz);
	dptr[0] = cprmx;
	dptr[1] = uprmx;
	dptr[2] = hprmx;
	dptr[3] = cprmy;
	dptr[4] = uprmy;
	dptr[5] = hprmy;
	dptr[6] = cprmz;
	dptr[7] = uprmz;
	dptr[8] = hprmz;
	divisor = det(dptr);
	params[3] = divisor;
	if (divisor < 1e-6)
	{
	    printf("view volume determinant too small!!\n");
	    printf("divisor = %lf\n\n", divisor);
	    exit(0);
	}
	params[4] = tan(uangle);
	params[5] = tan(hangle);

/*  Draw central axis  */

	ptest[0][0] = 0.0;
	ptest[0][1] = tlimit/2.0;
	ptest[0][2] = tlimit/2.0;

	ptest[1][0] = tlimit;
	ptest[1][1] = tlimit/2.0;
	ptest[1][2] = tlimit/2.0;

	ptest[2][0] = tlimit/2.0;
	ptest[2][1] = 0.0;
	ptest[2][2] = tlimit/2.0;

	ptest[3][0] = tlimit/2.0;
	ptest[3][1] = tlimit;
	ptest[3][2] = tlimit/2.0;

	ptest[4][0] = tlimit/2.0;
	ptest[4][1] = tlimit/2.0;
	ptest[4][2] = 0.0;

	ptest[5][0] = tlimit/2.0;
	ptest[5][1] = tlimit/2.0;
	ptest[5][2] = tlimit;

	numpoints = 0;
	for(i=0; i<6; i++)
	{
	    if (transform3d ( ptest[i], w, dptr, params, &h[i], &v[i]))
	    {
	        printf("point %d is in volume\n", i); 
	        numpoints++;
	    }
	}

	drawline( h[0], v[0], h[1], v[1], red);
	drawline( h[2], v[2], h[3], v[3], green);
	drawline( h[4], v[4], h[5], v[5], blue);

/* note first test - bx only  */

	numread = fread( magblock, 3*sizeof(double), tlimit*tlimit*tlimit , readmag);
	if( numread < tlimit*tlimit*tlimit) 
	    printf("Warning, did not read all of tau file.\n");

/*  compute vector length for large blocks and map to screen */

	vectormax = 0.0;
	for(i=0; i<tlimit/8; i++)
	{
//	    printf("i= %d\n",i);
	    xb = i*8;
	    for(j=0; j<tlimit/8; j++)
	    {
		yb = j*8;
		for(k=0; k<tlimit/8; k++)
		{
		    zb = k*8;
		    bx = 0.0;
		    for(ix = 0; ix<8; ix++)
		    {
			for(iy = 0; iy<8; iy++)
			{
//			    printf("iy=%d\n", iy);
			    for(iz = 0; iz<8; iz++)
			    {
				bx += magblock[3*(xb+ix + tlimit*((yb + iy) + tlimit*(zb + iz)))];
				by += magblock[3*(xb+ix + tlimit*((yb + iy) + tlimit*(zb + iz))) + 1];
				bz += magblock[3*(xb+ix + tlimit*((yb + iy) + tlimit*(zb + iz))) + 2];
			    }
			}
//			printf("ix=%d\n", ix);
		    }
		    bx /= 8*8*8;
		    by /= 8*8*8;
		    bz /= 8*8*8;
		    vectors[3*(i + tlimit*(j + tlimit*k))] = bx;
		    vectors[3*(i + tlimit*(j + tlimit*k)) + 1] = by;
		    vectors[3*(i + tlimit*(j + tlimit*k)) + 2] = bz;
		    vtst = sqrt(bx*bx + by*by + bz*bz);
		    if (vtst > vectormax) vectormax = vtst;
		}
	    }
	}
	vectormax /= scalevector;
	for(i=0; i<tlimit/8; i++)
	{
	    xb = i*8;
	    for(j=0; j<tlimit/8; j++)
	    {
		yb = j*8;
		for(k=0; k<tlimit/8; k++)
		{
		    zb = k*8;
		    vectorhead[0] = xb + 4.0 + 
			vectors[3*(i + tlimit*(j + tlimit*k))]/vectormax*2.0; 
		    vectortail[0] = xb + 4.0 - 
			vectors[3*(i + tlimit*(j + tlimit*k))]/vectormax*2.0; 
		    vectorhead[1] = yb + 4.0 +
			vectors[3*(i + tlimit*(j + tlimit*k)) + 1]/vectormax*2.0; 
		    vectortail[1] = yb + 4.0 -
			vectors[3*(i + tlimit*(j + tlimit*k)) + 1]/vectormax*2.0; 
		    vectorhead[2] = zb + 4.0 +
			vectors[3*(i + tlimit*(j + tlimit*k)) + 2]/vectormax*2.0; 
		    vectortail[2] = zb + 4.0 -
			vectors[3*(i + tlimit*(j + tlimit*k)) + 2]/vectormax*2.0; 
		    if (transform3d( vectorhead, w, dptr, params, &h[0], &v[0]) &&
			transform3d( vectortail, w, dptr, params, &h[1], &v[1]))
		    {
//			printf(" p_0 = %d %d  p_1 = %d %d\n", h[0], v[0], h[1], v[1]);
			drawline( h[0], v[0], h[1], v[1], white);
		    }
//		    printf("vechead_x = %lf vechead_y = %lf vechead_z = %lf\n",
//			   vectorhead[0], vectorhead[1], vectorhead[2]);

		}
	    }
	}

	for( j=0; j<UNUMPOINTS; j++) mptr[j] = (png_bytep) magplt + j*3*HNUMPOINTS;

	write_png("forceplot.png", (png_bytep*)mptr, UNUMPOINTS);
	fclose(readmag);
}