/* calc.c */
#include "tdp.h"

void calc()
{
   register USHORT i, ix, iy;
   int npts, ymid, xmid;
   register int *x_i, *y_i; /* ASSUMES THAT FFP AND int ARE BOTH 4 BYTES */
   register FFP *x_f, *y_f; /* ASSUMES THAT FFP AND int ARE BOTH 4 BYTES */
   FFP xd, yd, xmax, xmin, ymin, ymax, yfact;
   register FFP *z_f;
   FFP dist, tmpf, xscale, yscale, zdmax, zdmin, zscale, zsfact, zdoffset;
   
   /*** FIND DATA MAX ***/
   npts = nxpts*nypts;
   zdmax = -FFPLARGE; zdmin = FFPLARGE;
   for (z_f = &zd[0]; z_f < &zd[0]+npts; z_f++)
      {zdmax = max(zdmax,*z_f); zdmin = min(zdmin,*z_f);}
   if (debug) {printf("zdmax = %f, zdmin = %f\n",zdmax,zdmin);}

   /*** SCALE ZD[] AND CENTER ABOUT ZERO ***/
   zsfact = 1.;
   zdoffset = (zdmax+zdmin)/2.;
   zscale = zsfact * (FFP)min(nxpts,nypts);
   tmpf = zdmax - zdmin;
   if (abs(tmpf) >= FFPSMALL) zscale = zscale/tmpf; else zscale = 1.;
   if (debug) printf("zscale = %f\n",zscale);
   for (z_f = &zd[0]; z_f < &zd[0]+npts; z_f++)
      *z_f = (*z_f - zdoffset) * zscale;

   /*** ROTATE, PROJECT ***/
   sinp=sin(phi); cosp=cos(phi); sint=sin(theta); cost=cos(theta);
   xmax = (ymax = -FFPLARGE); xmin = (ymin = FFPLARGE);
   xmid = nxpts/2; ymid = nypts/2;
   dist = 20. * (FFP)max(nxpts,nypts);
   yfact = (FFP)nxpts / (FFP)nypts;

   for (iy = 0, z_f = &zd[0]; iy < nypts; iy++) {
      for (/*init: */ ix = 0, x_f = (FFP *)&x[iy][0], y_f = (FFP *)&y[iy][0];
           /*while:*/ ix < nxpts;
           /*next: */ ix++, x_f++, y_f++, z_f++) {

         /*** MAKE COORDINATE NET ***/
         yd = yfact * (FFP)(iy-ymid); xd = (FFP)(ix-xmid);

         /********************
         * ROTATE ABOUT Z AXIS
         * x' =  x * cos(phi) + y * sin(phi)
         * y' = -x * sin(phi) + y * cos(phi)
         * z' =  z
         *****************************/
         tmpf = -xd*sinp + yd*cosp;
         xd = xd*cosp + yd*sinp;

         /************************
         * ROTATE ABOUT NEW X AXIS
         * x'' =  x'
         * y'' = -y' * cos(theta) + z' * sin(theta)
         * z'' = -y' * sin(theta) + z' * cos(theta)
         *****************************/
         yd = tmpf*cost + (*z_f) * sint;
         *z_f = -tmpf * sint + (*z_f) *cost;

         /***********************
         * PROJECT ONTO X,Y PLANE
         * X =  x'' / (y'' + dist)
         * Y =  z'' / (y'' + dist)
         *****************************/
         tmpf = max(FFPSMALL, dist+yd);
         *y_f = *z_f / tmpf;
         *x_f = xd / tmpf;
         if (debug>1) printf("x = %f, y = %f\n", *x_f, *y_f);

         /*** ACCUMULATE MIN, MAX ***/
         xmin = min(xmin,*x_f); xmax = max(xmax,*x_f);
         ymin = min(ymin,*y_f); ymax = max(ymax,*y_f);
      }
   }
   if (debug) {
      printf("xmin,xmax,ymin,ymax");
      printf("%f %f %f %f\n", xmin, xmax, ymin, ymax);
   }

   /*** MAKE DATA FOR AXES ***/
   if ((axes == 'y') || (axes == 'Y')) {

      xA[0] = (FFP)(-xmid);
      xA[1] = (FFP)(nxpts-1-xmid);
      xA[2] = xA[0];
      xA[3] = xA[0];

      yA[0] = yfact * (FFP)(-ymid);
      yA[1] = yA[0];
      yA[2] = yfact * (FFP)(nypts-1-ymid);
      yA[3] = yA[0];

      zA[0] = zdmin;
      zA[1] = zA[0];
      zA[2] = zA[0];
      zA[3] = zdmax;

      x_f = &xA[0]; y_f = &yA[0]; z_f = &zA[0];
      for (i=0; i<4; i++, x_f++, y_f++, z_f++) {
         *z_f = (*z_f - zdoffset) * zscale;
            /* rotate */
         tmpf = -(*x_f) * sinp + (*y_f) * cosp;
         *x_f = (*x_f) * cosp + (*y_f) * sinp;
            /* rotate */
         *y_f = tmpf * cost + (*z_f) * sint;
         *z_f = -tmpf * sint + (*z_f) * cost;
            /* project */
         tmpf = max(FFPSMALL, dist+(*y_f));
         *y_f = (*z_f) / tmpf;
         *x_f = (*x_f) / tmpf;
            /* accumulate min, max */
         xmin = min(xmin,*x_f); xmax = max(xmax,*x_f);
         ymin = min(ymin,*y_f); ymax = max(ymax,*y_f);
      }
   }

   /*** SCALE DATA FOR PLOT ***/
   xscale = (yscale = 0.);
   if (xmax > xmin) xscale = (FFP)(MAXHORIZ-20) / (xmax - xmin);
   if (ymax > ymin) yscale = (FFP)(MAXVERT-20) / (ymax - ymin);
   if (debug) printf("xscale = %f, yscale = %f\n", xscale, yscale);

   /* NOTE x_i and x_f point to same locations, same for y_i, y_f */
   for (iy=0; iy<nypts; iy++) {
      for (ix=0, x_f=(FFP *)&x[iy][0], y_f=(FFP *)&y[iy][0];
           ix<nxpts;
           ix++, x_f++, y_f++
          ) {
         x_i = (int *)x_f; y_i = (int *)y_f;
         *x_i = 10 + (int) ((*x_f - xmin) * xscale);
         *y_i = 10 + (int) ((*y_f - ymin) * yscale);
         if (debug>1) printf("x,y %d, %d\n",*x_i,*y_i);
      }
   }

   if ((axes == 'y') || (axes == 'Y')) {
      for (i=0; i<4; i++) {
         xA_i[i] = 10 + (int) ((*x_f - xmin) * xscale);
         yA_i[i] = 10 + (int) ((*y_f - ymin) * yscale);
      }
   }
}