#ifdef USE_WARP

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

#include "shmem.h"
#include "specks.h"
#include "partiviewc.h"

#include "textures.h"		/* for get_dsp_context() */
#include <getopt.h>

static char local_id[] = "$Id$";

struct speckcache {
    float tfrac;
    struct specklist *sl;
};

struct warpstuff {
    float tfrom, tto;
    float zerotime;
    int has_trange, has_zerotime;
    float period0;
    float rcore, routercore;
    float rigidrot;
    float d2o[16], o2d[16];
    int has_d2o, has_o2d;

    float tfrac;

    struct speckcache slc[MAXDSPCTX];
};

extern float fastsin2pi( float v );
static void inittables(void);
struct specklist *warp_get_parti( struct stuff *st, double realtime );


void
warp_setup( struct stuff *st, int argc, char *argv[] )
{
  struct warpstuff *ws;
  struct warpstuff tws;
  int c;
  char key;
  float v;

  inittables();
  memset(&tws, 0, sizeof(tws));

  while((c = getopt(argc, argv, "p:f:z:r:T:F:R:")) != EOF) {
    switch(c) {
    case 'p': sscanf(optarg, "%f", &tws.period0);
		break;
    case 'f': tws.has_trange = sscanf(optarg,"%f%*c%f",&tws.tfrom,&tws.tto);
		break;
    case 'z': tws.has_zerotime = sscanf(optarg, "%f", &tws.zerotime);
		break;
    case 'r':
	switch(sscanf(optarg, "%f%c%f", &tws.rcore, &key, &v)) {
	case 3:
	   if(key == '-') tws.routercore = v;
	   else tws.routercore = tws.rcore*(1+v);
	   break;
	case 1:
	   tws.routercore = tws.rcore * 1.1;
	   break;
	default:
	   argc = 0;
	}
	break;

    case 'T':
	if(16!=sscanf(optarg, 
	  "%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf",
	  &tws.o2d[0],&tws.o2d[1],&tws.o2d[2],&tws.o2d[3],
	  &tws.o2d[4],&tws.o2d[5],&tws.o2d[6],&tws.o2d[7],
	  &tws.o2d[8],&tws.o2d[9],&tws.o2d[10],&tws.o2d[11],
	  &tws.o2d[12],&tws.o2d[13],&tws.o2d[14],&tws.o2d[15])) {
	    msg("%s: -T: expected 16 numbers\n", argv[0]);
	    return;
	}
	tws.has_o2d = 1;
	break;

    case 'F':
	if(16!=sscanf(optarg, 
	  "%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf%*c%lf",
	  &tws.d2o[0],&tws.d2o[1],&tws.d2o[2],&tws.d2o[3],
	  &tws.d2o[4],&tws.d2o[5],&tws.d2o[6],&tws.d2o[7],
	  &tws.d2o[8],&tws.d2o[9],&tws.d2o[10],&tws.d2o[11],
	  &tws.d2o[12],&tws.d2o[13],&tws.d2o[14],&tws.d2o[15])) {
	    msg("%s: -F: expected 16 numbers\n", argv[0]);
	    return;
	}
	tws.has_d2o = 1;
	break;

    case 'R':
	sscanf(optarg, "%g", &tws.rigidrot);	/* given as degrees */
	tws.rigidrot /= 360;	/* internally, kept as fraction of 1 turn */
	break;

    default:
	argc = 0;
	break;
    }
  }
  if(argc == 0) {
    msg("Usage: %s  [-f fin,fout][-P period0][-z zerotime][-R rot][-T o2d][-F d2o] [-r rcore[,transition]]");
    msg("-r : core rad. w/optional width of transition region (fraction of rcore)");
    msg("   : or (\"-\" separator) -r innercore-outercore");
    msg("-R : add rigid rotation by \"rot\" degrees to entire body.");
    msg("-T, -F  sixteen numbers.  If only -T or -F is given, other is computed.");
    msg(" sense: p' = p * o2d * warp(X,Z plane) * d2o");
    return;
  }

  st->dyndata = 1;
  st->dyndatafunc = warp_get_parti;
  st->dyndatadata = ws = NewN( struct warpstuff, 1 );
  *ws = tws;
}

static void scrapsl( struct specklist **slp ) {
  struct specklist *sl, *slnext;
  if(slp == NULL) return;
  sl = *slp;
  *slp = NULL;
  for( ; sl; sl = slnext) {
    slnext = sl->next;
    if(sl->specks) free(sl->specks);
    free(sl);
  }
}


float slerp( float frac, float from, float to )
{
  if(frac<=0) return from;
  if(frac>=1) return to;
  return from + frac*frac*(3-2*frac)*(to-from);
}

void windup( struct warpstuff *ws, Point *out, CONST Point *in )
{
  /*
   * v(r) =  (vmax/rcore)*r  r in 0..rcore
   *         vmax	      r > rcore
   * period(r) = 2pi/(vmax/rcore)  r<rcore  [Call this T0.]
   *		 2pi/(vmax/r) = (r/rcore)*T0
   * theta(t/T0,r) = (t/T0)*T0*(r < rcore ? 1 : rcore/r)
   * So rotate in the XZ plane about (0,0,0) by theta(t/T0,r)
   */
   float x = in->x[0];
   float z = in->x[2];
   float r = hypotf( x, z );
   float omega =  (r<=ws->rcore) ? 1
		: (r>=ws->routercore) ? ws->routercore/r
		: slerp( 1+(r-ws->routercore)/(ws->routercore-ws->rcore),
			1, ws->routercore/r );
   float theta = omega * ws->tfrac + ws->rigidrot;
   float s = fastsin2pi( theta );
   float c = fastsin2pi( theta + 0.25 );

   out->x[0] = x*c - z*s;
   out->x[1] = in->x[1];
   out->x[2] = x*s + z*c;
}

void warpspecks( struct warpstuff *ws,
		 struct stuff *st,
		 struct specklist *osl,
		 struct specklist *sl )
{
  struct speck *osp, *sp;
  int i, n;

  if(osl->specks == NULL) return;
  n = osl->nspecks;
  /* memcpy( sl->specks, osl->specks, osl->bytesperspeck * n ); */
  osp = osl->specks;
  sp = sl->specks;
  for(i = 0; i < n; i++) {
    windup( ws, &sp->p, &osp->p );
    osp = NextSpeck(osp, osl, 1);
    sp = NextSpeck(sp, sl, 1);
  }
}
    
static float warpfrac( struct warpstuff *ws, double time )
{
  if(ws->has_trange == 2 && ws->tfrom != ws->tto) {
    return slerp( (time - ws->tfrom) / (ws->tto - ws->tfrom), -1, 1 );
  } else if(ws->period0 != 0) {
    return 2 * (time - ws->tfrom) / ws->period0 - 1;
  } else {
    return 0;
  }
}

struct specklist *
warp_get_parti( struct stuff *st, double realtime )
{
  struct warpstuff *ws = (struct warpstuff *)st->dyndatadata;
  int ctx = get_dsp_context();
  struct speckcache *sc = &ws->slc[ctx];
  struct specklist *sl, *osl, **slp;

  ws->tfrac = warpfrac(ws, realtime * 30);
  if(ws->has_zerotime)
    ws->tfrac -= warpfrac(ws, ws->zerotime);
  if(ws->has_trange && ws->period0 != 0)
    ws->tfrac *= (ws->tto - ws->tfrom) / ws->period0;

  if(sc->sl != NULL && sc->tfrac == ws->tfrac)
    return sc->sl;

  if(ws->has_trange == 2 && ws->tfrom != ws->tto) {
    ws->tfrac = slerp( (realtime - ws->tfrom) / (ws->tto - ws->tfrom), -1, 1 );
  } else if(ws->period0 != 0) {
    ws->tfrac = 2 * (realtime - ws->tfrom) / ws->period0 - 1;
  } else {
    ws->tfrac = 0;
  }

  osl = specks_timespecks( st, 0, 0 );
  slp = &sc->sl;
  for(sl = *slp; osl != NULL; osl = osl->next, slp = &sl->next, sl = *slp) {
    if(sl == NULL || sl->nspecks != osl->nspecks) {
	scrapsl(slp);
	sl = (struct specklist *)malloc( sizeof(struct specklist) );
	*sl = *osl;
	sl->specks = NULL;
	if(osl->specks) {
	    int len = osl->bytesperspeck * osl->nspecks;
	    sl->specks = (struct speck *)malloc( len );
	    memcpy( sl->specks, osl->specks, len );
	}
	*slp = sl;
    }
    warpspecks( ws, st, osl, sl );
  }

  sc->tfrac = ws->tfrac;
  return sc->sl;
}

#define MAXINTERP 1024

float sintbl[MAXINTERP+1];

static void inittables() {
  int i;

  for(i = 0; i <= MAXINTERP; i++)
    sintbl[i] = sinf(2*M_PI*i / MAXINTERP);
}

/* fastsin2pi( x ) returns approximately sin( 2*pi*x ),
 * i.e. sin(x) if x is given as a fraction of a full 360-degree turn.
 */
float fastsin2pi( float v ) {
  int i;
  while(v < 0) v += 1;
  while(v > 1) v -= 1;
  v *= MAXINTERP;
  i = (int)v;
  return sintbl[i] + (v - i)*(sintbl[i+1]-sintbl[i]);
}

#endif /*USE_WARP*/