#ifdef USE_WARP
/*
 * Time-dependent warp of a particle field,
 * for fake differentially-rotating galaxies.
 * Stuart Levy, slevy@ncsa.uiuc.edu
 * National Center for Supercomputing Applications,
 * University of Illinois 2001.
 * This file is part of partiview, released under the
 * Illinois Open Source License; see the file LICENSE.partiview for details.
 */

#ifdef WIN32
# include "winjunk.h"
#endif

#if unix
# include <unistd.h>
#endif

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

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

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

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

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

#define COORD_DISK   0
#define COORD_WORLD  (-1)
#define COORD_OBJECT (-2)

struct warpstuff {
    float tfrom, tto;
    float zerotime;
    int has_trange, has_zerotime;
    float period0;
    float rcore, routercore;
    int core_coordsys;
    float rcoredisk, routercoredisk;
    double fixedp[3];
    float rfixed;
    int has_fixed, fixed_coordsys;
    float fixedrdisk;
    float rigidrot;
    double d2o[16], o2d[16];
    int has_d2o, has_o2d;
    int valid;
    int locked;

    float tfrac, dtfrac;
    double realtime;
    float fixomega;

    struct speckcache slc[MAXDSPCTX];
};

extern float fastsin2pi( float v );
static void inittables(void);
struct specklist *warp_get_parti( struct dyndata *dd, struct stuff *st, double realtime );
int warp_get_trange( struct dyndata *dd, struct stuff *st, double *tminp, double *tmaxp );
int warp_parse_args( struct dyndata *dd, struct stuff *st, int argc, char **argv );
struct warpstuff *warp_setup( struct warpstuff *ws, int argc, char **argv );

static int warp_coordsys( const char *str ) {
    if(strchr(str, 'w')) return COORD_WORLD;
    if(strchr(str, 'o')) return COORD_OBJECT;
    if(strchr(str, 'd')) return COORD_DISK;
    return COORD_DISK;
}

int parsematrix( double tfm[16], char *str, const char *whose )
{
    double m[16];
    Matrix T;
    Point xyz;
    float aer[3];
    float scl = 1.0;
    int k = sscanf(str, 
      "%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",
      &m[0],&m[1],&m[2],&m[3],
      &m[4],&m[5],&m[6],&m[7],
      &m[8],&m[9],&m[10],&m[11],
      &m[12],&m[13],&m[14],&m[15]);

    switch(k) {

    case 1:	/* simple scaling */
	memset(tfm, 0, 16*sizeof(double));
	tfm[0*4+0] = tfm[1*4+1] = tfm[2*4+2] = m[0];
	tfm[3*4+3] = 1;
	return 1;

    case 7:	/* virdir style Tx Ty Tz Rx Ry Rz Scl */
	scl = m[6];	/* and fall into ... */

    case 6:	/* virdir style Tx Ty Tz Rx Ry Rz */
	xyz.x[0] = m[0];
	xyz.x[1] = m[1];
	xyz.x[2] = m[2];
	aer[1] = m[3];
	aer[0] = m[4];
	aer[2] = m[5];
	xyzaer2tfm( &T, &xyz, aer );
	for(k = 0; k < 3*4; k++)
	    tfm[k] = T.m[k] * scl;
	for(k = 3*4; k < 4*4; k++)
	    tfm[k] = T.m[k];
	return 1;

    case 9:	/* 3x3 linear */
	memset(tfm, 0, 16*sizeof(double));
	for(k = 0; k < 3; k++) {
	    tfm[k*4+0] = m[k*3+0];
	    tfm[k*4+1] = m[k*3+1];
	    tfm[k*4+2] = m[k*3+2];
	}
	tfm[3*4+3] = 1;
	return 1;

    case 16:
	memcpy(tfm, m, 16*sizeof(double));
	break; /* great */

    default:
	if(whose)
	    msg("%s %s: expected 1 or 6 or 7 or 9 or 16 numbers", whose, str);
	return 0;
    }
    return 1;
}

struct warpstuff *warp_setup( struct warpstuff *oldws, int argc, char **argv )
{
  struct warpstuff *ws;
  struct warpstuff tws;
  int i, c;
  char key;
  float v;

  inittables();

  if(oldws)
    tws = *oldws;
  else
    memset(&tws, 0, sizeof(tws));

  for(i = 1; i < argc-1; i += 2) {
    char *optarg = argv[i+1];
    if(!strcmp(argv[i], "-p")) {
	sscanf(optarg, "%f", &tws.period0);
    } else if(!strcmp(argv[i], "-f")) {
	tws.has_trange = sscanf(optarg,"%f%*c%f",&tws.tfrom,&tws.tto);
    } else if(!strcmp(argv[i], "-z")) {
	tws.has_zerotime = sscanf(optarg, "%f", &tws.zerotime);
    } else if(!strcmp(argv[i], "-t")) {
	sscanf(optarg, "%f%*c%f", &tws.tfrac, &tws.dtfrac);
    } else if(!strncmp(argv[i], "-fix", 4)) {
	tws.has_fixed = 0;
	tws.fixed_coordsys = warp_coordsys( optarg );
	switch(sscanf(optarg, "%lf%*c%lf%*c%lf",
			&tws.fixedp[0], &tws.fixedp[1], &tws.fixedp[2])) {
	case 1: tws.has_fixed = 1; break;
	case 3: tws.has_fixed = 3; break;
	default:
	    msg("warp -fix {x,y,z|radius}[w] not %s", optarg);
	}
	
    } else if(!strcmp(argv[i], "-r")) {
	tws.core_coordsys = warp_coordsys( optarg );
	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;
	}
    } else if(!strcmp(argv[i], "-T")) {
	tws.has_o2d = parsematrix( tws.o2d, optarg, "warp -T" );

    } else if(!strcmp(argv[i], "-F")) {
	tws.has_d2o = parsematrix( tws.d2o, optarg, "warp -F" );

    } else if(!strcmp(argv[i], "-rigidrot") || !strcmp(argv[i], "-R")) {
	sscanf(optarg, "%g", &tws.rigidrot);	/* given as degrees */
	tws.rigidrot /= 360;	/* internally, kept as fraction of 1 turn */
    } else {
	argc = -1;
	break;
    }
  }
  if(argc < 3) {
    msg("Usage: %s  [-f fin,fout][-p period0][-z zerotime][-R rot][-T o2d][-F d2o] [-r rcore[,transition][w]]] [-fix x,y,z[w]|radius[w]]", argv[0]);
    msg("-r : core rad. w/optional width of transition region (fraction of rcore)");
    msg("   : or (\"-\" separator) -r innercore-outercore");
    msg("-R(or -rigidrot) : 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("  \"o2d\" = object-to-disk tfm, where disk center=0,0,0, in XZ plane.");
    msg("-fix x,y,z[w]  or  -fix r[w] (fix point or radius); \"w\" = \"world coords\".");
    msg(" sense: p' = p * o2d * warp(X,Z plane) * d2o");
    return NULL;
  }

  ws = oldws ? oldws : NewN( struct warpstuff, 1 );
  *ws = tws;
  ws->valid = 0;			/* Force recomputing parameters */
  return ws;
}

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->rcoredisk) ? 1
		: (r>=ws->routercoredisk) ? ws->routercoredisk/r
		: slerp( 1+(r-ws->routercoredisk)/(ws->routercoredisk-ws->rcoredisk),
			1, ws->routercoredisk/r );
    float theta, s, c;

    theta = (omega - ws->fixomega) * ws->tfrac + ws->rigidrot;
    s = fastsin2pi( theta );
    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 dvtfmpoint( Point *dst, CONST Point *src, CONST double T[16] )
{
  int i;
  for(i = 0; i < 3; i++)
    dst->x[i] = src->x[0]*T[i] + src->x[1]*T[i+4] + src->x[2]*T[i+8] + T[i+12];
}

void windup_o2d( struct warpstuff *ws, Point *out, CONST Point *in ) {
    Point tin, tout;

    dvtfmpoint( &tin, in, ws->o2d );
    windup( ws, &tout, &tin );
    dvtfmpoint( out, &tout, ws->d2o );
}

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++) {
    if(ws->has_o2d)
	windup_o2d( ws, &sp->p, &osp->p );
    else
	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;
  }
}

static double dvlength( double v[3] ) {
  return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
}

static void ddmmul( double dest[16], Matrix *a, double b[16] ) {
    int i,j;
    for(i = 0; i < 4; i++) {
	for(j = 0; j < 4; j++) {
	    dest[i*4+j]	= a->m[i*4+0]*b[0*4+j]
			+ a->m[i*4+1]*b[1*4+j]
			+ a->m[i*4+2]*b[2*4+j]
			+ a->m[i*4+3]*b[3*4+j];
	}
    }
}

static void dvmmul( double dest[3], double src[3], double T[16] ) {
    double w = src[0]*T[0*4+3] + src[1]*T[1*4+3] + src[2]*T[2*4+3] + T[3*4+3];
    w = 1/w;
    dest[0] = (src[0]*T[0*4+0] + src[1]*T[1*4+0] + src[2]*T[2*4+0] + T[3*4+0])*w;
    dest[1] = (src[0]*T[0*4+1] + src[1]*T[1*4+1] + src[2]*T[2*4+1] + T[3*4+1])*w;
    dest[2] = (src[0]*T[0*4+2] + src[1]*T[1*4+2] + src[2]*T[2*4+2] + T[3*4+2])*w;
}

static int getT2d( double T2d[16], struct stuff *st, struct warpstuff *ws, int coordsys )
{
    Matrix o2w, w2o;
    switch(coordsys) {
    case COORD_DISK:
	return 0;
    case COORD_OBJECT:
	if(!ws->has_o2d)
	    return 0;
	memcpy( T2d, ws->o2d, 16*sizeof(double) );
	return 1;
    case COORD_WORLD:
	parti_geto2w( st, parti_idof(st), &o2w );
	eucinv( &w2o, &o2w );
	if(ws->has_o2d) {
	    ddmmul( T2d, &w2o, ws->o2d );
	} else {
	    int i;
	    for(i = 0; i < 4*4; i++)
		T2d[i] = w2o.m[i];
	}
	return 1;
    }
    msg("warp getT2d %d", coordsys);
    return 0;
}

static double rtodisk( struct stuff *st, struct warpstuff *ws, double radius, int coordsys ) {
  double w2d[16];
  switch(coordsys) {
  case COORD_DISK:
	return radius;
  case COORD_OBJECT:
	return (ws->has_o2d ?  radius * dvlength( &ws->o2d[0] ) : radius);
  case COORD_WORLD:
	getT2d( w2d, st, ws, COORD_WORLD );
	return radius * dvlength( &w2d[0] );
  }
  msg("warp: rtodisk");
  return 0;
}

/* Delay computing transformable quantities until we really need them */
static void setup_coords( struct stuff *st, struct warpstuff *ws ) {
    double fixed2d[16];
    double fixedpdisk[3];

    if(!ws->valid) {
	ws->rcoredisk = rtodisk( st, ws, ws->rcore, ws->core_coordsys );
	ws->routercoredisk = rtodisk( st, ws, ws->routercore, ws->core_coordsys );
	switch(ws->has_fixed) {
	case 1:
	    ws->fixedrdisk = rtodisk( st, ws, ws->fixedp[0], ws->fixed_coordsys );
	    break;
	case 3:
	    if(getT2d( fixed2d, st, ws, ws->fixed_coordsys )) {
		dvmmul( fixedpdisk, ws->fixedp, fixed2d );
		ws->fixedrdisk = hypot( fixedpdisk[0], fixedpdisk[2] );
	    } else {
		ws->fixedrdisk = hypot( ws->fixedp[0], ws->fixedp[2] );
	    }
	    break;
	}
	ws->valid = 1;
    }
    if(ws->has_fixed) {
	ws->fixomega =
	    (ws->fixedrdisk <= ws->rcoredisk) ? 1
	    : (ws->fixedrdisk >= ws->routercoredisk) ?
			ws->routercoredisk / ws->fixedrdisk
	    : slerp( 1 + (ws->fixedrdisk - ws->routercoredisk)
			/ (ws->routercoredisk - ws->rcoredisk),
		 1, ws->routercoredisk / ws->fixedrdisk );
    } else {
	ws->fixomega = 0;
    }
}


#define MAXINTERP 1024

float sintbl[MAXINTERP+1];

static void inittables(void) {
  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]);
}

void deucinv( double dst[16], CONST double src[16] )
{
    int i, j;
    double s = src[0]*src[0] + src[1]*src[1] + src[2]*src[2];
    Point trans;
    double t[16];
    if(src == dst) {
	memcpy( t, src, sizeof(t) );
	src = t;
    }
    for(i = 0; i < 3; i++) {
	for(j = 0; j < 3; j++)
	    dst[i*4+j] = src[j*4+i] / s;
	dst[i*4+3] = 0;
    }
    for(i = 0; i < 3; i++)
	dst[3*4+i] = -(src[3*4+0]*dst[0*4+i] + src[3*4+1]*dst[1*4+i] + src[3*4+2]*dst[2*4+i]);
    dst[3*4+3] = 1;
}


#ifndef STANDALONE

int warp_parse_args( struct dyndata *dd, struct stuff *st, int argc, char **argv )
{
  struct warpstuff *ws;
  if(argc < 1 || 0!=strcmp(argv[0], "warp"))
    return 0;
  if(dd == NULL || (ws = (struct warpstuff *)dd->data) == NULL) {
    msg("No warp enabled");
    return 1;
  }
  if(argc>1 && (!strcmp(argv[1], "lock") || !strcmp(argv[1], "off"))) {
    ws->locked = 1;
  } else if(argc>1 && (!strcmp(argv[1], "unlock") || !strcmp(argv[1], "on"))) {
    ws->valid = 0;	/* force recomputing */
    ws->locked = 0;
  }
  msg("warp %stfrac %g at time %g  fixp %g %g %g rfixd %g fixsys %d rcored %g",
	ws->locked ? "locked " : "",
	ws->tfrac, ws->realtime,
	ws->fixedp[0],ws->fixedp[1],ws->fixedp[2], ws->fixedrdisk, ws->fixed_coordsys, ws->rcoredisk);
  return 1;
}

int warp_get_trange( struct dyndata *dd, struct stuff *st, double *tminp, double *tmaxp )
{
  *tminp = -10000;
  *tmaxp = 10000;
  return 1;
}

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


int warp_read( struct stuff **stp, int argc, char *argv[], char *fname, void *etc ) {
    struct stuff *st = *stp;
    struct warpstuff *ws;

    if(argc < 1 || strcmp(argv[0], "warp") != 0)
	return 0;
    st->clk->continuous = 1;

    if(st->dyn.ctlcmd == warp_parse_args) {
	ws = (struct warpstuff *)st->dyn.data;
    } else {
	ws = NULL;
    }
    ws = warp_setup( ws, argc, argv );
    if(ws == NULL)
	return 0;

    memset(&st->dyn, 0, sizeof(st->dyn));
    st->dyn.data = ws;
    st->dyn.getspecks = warp_get_parti;
    st->dyn.trange = warp_get_trange;
    st->dyn.ctlcmd = warp_parse_args;
    st->dyn.draw = NULL;
    st->dyn.free = NULL;
    st->dyn.enabled = 1;
    return 1;
}

void warp_init() {
    parti_add_reader( warp_read, "warp", NULL );
}

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

#if unix
  if(once-- == 0 && getenv("DBG")) {
    fprintf(stderr, "warp_get_parti:   dbx -p %d\n", getpid());
    sleep(atoi(getenv("DBG")));
  }
#endif

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

  setup_coords( st, ws );	/* bind coordsys-related stuff now */

  ws->realtime = realtime;
  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(ws->has_o2d && !ws->has_d2o) {
    deucinv( ws->d2o, ws->o2d );
    ws->has_d2o = -1;
  } else if(!ws->has_o2d && ws->has_d2o) {
    deucinv( ws->o2d, ws->d2o );
    ws->has_o2d = -1;
  }


  if(getenv("WDBG")) printf("%g %g %g # rt frac frac(rt)\n", realtime, ws->tfrac, warpfrac(ws, realtime*30));

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

  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->speckseq != osl->speckseq) {
	scrapsl(slp);
	sl = NewN( struct specklist, 1 );
	*sl = *osl;
	sl->specks = NULL;
	sl->next = NULL;
	if(osl->specks) {
	    int len = osl->bytesperspeck * osl->nspecks;
	    sl->specks = (struct speck *)NewN( char, len );
	    memcpy( sl->specks, osl->specks, len );
	}
	*slp = sl;
    }
    warpspecks( ws, st, osl, sl );
  }

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

#else /*STANDALONE*/

#include <stdarg.h>
#include "stardef.h"

int msg( CONST char *fmt, ... ) {
  char str[10240];

  va_list args;
  va_start(args, fmt);
  vsprintf(str, fmt, args);
  va_end(args);

  return fprintf(stderr, "%s\n", str);
}

int parti_idof( struct stuff *stjunk ) {
  return 1;
}

void parti_geto2w( struct stuff *stjunk, int objid, Matrix *o2w ) {
  int k;
  char *tfm = getenv("tfm");
  Matrix T;
  Point xyz;
  float aer[3], scl;
  if(tfm == NULL) {
    msg("warpsdb: need object-to-world transform, but no \"tfm\" env variable set?");
    exit(1);
  }
  k = sscanf(tfm,
"%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f%*c%f",
    &T.m[0], &T.m[1], &T.m[2], &T.m[3],
    &T.m[4], &T.m[5], &T.m[6], &T.m[7],
    &T.m[8], &T.m[9], &T.m[10], &T.m[11],
    &T.m[12], &T.m[13], &T.m[14], &T.m[15]);

  scl = 1;

  switch(k) {
  case 1:
    scl = T.m[0];
    mscaling( o2w, scl,scl,scl );
    break;

  case 7:
    scl = T.m[6];  /* and fall into... */
  case 6:
    xyz.x[0] = T.m[0]; xyz.x[1] = T.m[1]; xyz.x[2] = T.m[2];
    aer[1] = T.m[3];   aer[0] = T.m[4];   aer[2] = T.m[5];
    xyzaer2tfm( &T, &xyz, aer );
    if(scl == 1) {
	*o2w = T;
    } else {
	Matrix Ts;
	mscaling( &Ts, scl,scl,scl );
	mmmul( o2w, &Ts, &T );
    }
    break;
  case 16:
    *o2w = T;
    break;
  default:
    msg("warpsdb: \"tfm\"=%s; expected 1 or 6 or 7 or 16 numbers", tfm);
    exit(1);
  }
}

void dwindup( struct warpstuff *ws, float *xyzout, float *dxyzout, CONST float *xyz )
{
  /*
   * 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 = xyz[0];
   float z = xyz[2];
   float r = hypot( x, z );
   float omega = ( (r<=ws->rcoredisk) ? 1
		: (r>=ws->routercoredisk) ? ws->routercoredisk/r
		: slerp( 1 + (r-ws->routercoredisk)/(ws->routercoredisk-ws->rcoredisk),
			1, ws->routercoredisk/r )
		) - ws->fixomega;
 
   float theta = omega * ws->tfrac + ws->rigidrot;
   float dtheta = omega * ws->dtfrac * 2*M_PI;
   float s = fastsin2pi( theta );
   float c = fastsin2pi( theta + 0.25 );

   xyzout[0] = x*c - z*s;
   xyzout[1] = xyz[1];
   xyzout[2] = x*s + z*c;

   dxyzout[0] = -dtheta * (x*s + z*c);
   dxyzout[1] = 0;
   dxyzout[2] = dtheta * (x*c - z*s);
}

void dwindup_o2d( struct warpstuff *ws, float *xyzout, float *dxyzout, CONST float *xyz )
{
    float v[3], w[3], dw[3];
    int i;

    for(i = 0; i < 3; i++)
	v[i] = xyz[0]*ws->o2d[0*4+i] + xyz[1]*ws->o2d[1*4+i]
	     + xyz[2]*ws->o2d[2*4+i] + ws->o2d[3*4+i];
    dwindup( ws, w, dw, v );
    for(i = 0; i < 3; i++) {
	xyzout[i] = w[0]*ws->d2o[0*4+i] + w[1]*ws->d2o[1*4+i]
		  + w[2]*ws->d2o[2*4+i] + ws->d2o[3*4+i];
	dxyzout[i] = dw[0]*ws->d2o[0*4+i] + dw[1]*ws->d2o[1*4+i]
		   + dw[2]*ws->d2o[2*4+i];
    }
}

static void swabsdb( db_star *sp ) {
    int i;
    for(i = 0; i < 9; i++)
	((int *)sp)[i] = htonl(((int *)sp)[i]);	/* x,y,z...,num */
    sp->color = htons(sp->color);
}

void warpsdb( struct warpstuff *ws, FILE *inf, FILE *outf ) {
    static int one = 1;
    int needswab = (*(char *)&one != 0);
    int nstars = 0;
    db_star star;

    if(ws->has_o2d && !ws->has_d2o) {
	deucinv( ws->d2o, ws->o2d );
	ws->has_d2o = -1;
    } else if(!ws->has_o2d && ws->has_d2o) {
	deucinv( ws->o2d, ws->d2o );
	ws->has_o2d = -1;
    }

    while(fread(&star, sizeof(star), 1, inf) > 0) {
	float xyz[3];
	nstars++;
	if(needswab)
	    swabsdb(&star);
	memcpy(xyz, &star.x, 3*sizeof(float));
	if(ws->has_o2d)
	    dwindup_o2d( ws, &star.x, &star.dx, xyz );
	else
	    dwindup( ws, &star.x, &star.dx, xyz );
	if(needswab)
	    swabsdb(&star);
	if(fwrite(&star, sizeof(star), 1, outf) <= 0) {
	    msg("Error writing star #%d", nstars);
	    break;
	}
    }
}

int main(int argc, char *argv[]) {
    struct warpstuff *ws;
    char *infname = NULL;
    FILE *inf = stdin;

#define BIGBUFSIZE 2097152
    char *obuf = (char *)malloc(BIGBUFSIZE);
    char *ibuf = (char *)malloc(BIGBUFSIZE);

    if(argc > 1 && 0==strcmp(&argv[argc-1][strlen(argv[argc-1])-4], ".sdb")) {
	infname = argv[argc-1];
	argc--;
	inf = fopen(infname, "rb");
	if(inf == NULL) {
	    msg("%s: %s: cannot open input", argv[0], infname);
	    exit(1);
	}
    }
    setvbuf( inf,    ibuf, _IOFBF, BIGBUFSIZE );
    setvbuf( stdout, obuf, _IOFBF, BIGBUFSIZE );

    ws = warp_setup( NULL, argc, argv );
    if(ws == NULL)
	exit(1);	/* warp_setup() must have already printed a message */

    setup_coords( NULL, ws );
    if(ws->has_fixed && getenv("WARPDBG")) {
	fprintf(stderr, "warp rcoredisk %g fixrdisk %g fixomega %g\n", ws->rcoredisk, ws->fixedrdisk, ws->fixomega);
    }

    warpsdb( ws, inf, stdout );
    return 0;
}

#endif /*!STANDALONE*/

#endif /*USE_WARP*/