#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "geometry.h"

#include "Gview.H"


static void translation(Matrix *T, float x, float y, float z);
static void rotation(Matrix *T, float x, float y, float z);

Fl_Gview::Fl_Gview(int x, int y, int w, int h, const char *label)
		: Fl_Gl_Window(x,y,w,h,label) {
  init();
  end();
}

void Fl_Gview::lookvec(int axis, const Point *vec) {
  Point v = *vec;
  if(axis < 0 || axis > 3) return;
  if(axis == 2) vscale(&v, -1,&v);	/* cam looks toward -Z */
  /* XXX fill in orthogonalization, invert, load into Tw2c */
  /* Don't need this right now */
  Fl::warning("Fl_Gview::lookvec() not implemented yet");
  redraw();
}

const Matrix *Fl_Gview::Tc2w() const {
  return &Tc2w_;
}

const Matrix *Fl_Gview::Tw2c() const {
  return &Tw2c_;
}

void Fl_Gview::Tc2w( const Matrix *newTc2w ) {
  Tc2w_ = *newTc2w;
  eucinv(&Tw2c_, &Tc2w_);
  notify();
  redraw();
}

void Fl_Gview::Tw2c( const Matrix *newTw2c ) {
  Tw2c_ = *newTw2c;
  eucinv(&Tc2w_, &Tw2c_);
  notify();
  redraw();
}

void Fl_Gview::reset( int id ) {
  if(id == GV_ID_CAMERA) {
    translation( &Tc2w_, 0, 0, focallen_ );
    Tc2w( &Tc2w_ );
  } else {
    const Matrix *o2w = To2w( id );
    if(o2w && o2w != &Tidentity) {
	*(Matrix *)o2w = Tidentity;	/* Reset! */
    }
  }
  notify();
  redraw();
}

void Fl_Gview::focallen(float flen) {
  if(flen == 0) {
    Fl::warning("Can't set Fl_Gview::focallen() to zero");
  } else {
    if(persp_)
	halfyfov_ *= flen / focallen_;
    focallen_ = flen;
  }
  notify();
  redraw();
}

float Fl_Gview::angyfov() const {
  return 2*atan(halfyfov_ / focallen_)*180/M_PI;
}

void Fl_Gview::angyfov(float deg) {
  if(deg == 0) Fl::warning("Can't set Fl_Gview::angyfov() to zero");
  else if(deg <= -180 || deg >= 180)
	Fl::warning("Can't set Fl_Gview::angyfov() to >= 180");
  else halfyfov_ = focallen_ * tan((deg/2)*M_PI/180);
  notify();
  redraw();
}

void Fl_Gview::halfyfov( float hyfov ) {
  if(hyfov == 0) Fl::warning("Can't set Fl_Gview::halfyfov() to zero");
  else halfyfov_ = hyfov;
  redraw();
}

void Fl_Gview::perspective( int bepersp ) {
  if((bepersp!=0) == persp_)
    return;
  persp_ = (bepersp != 0);
  redraw();
  notify();
}

void Fl_Gview::farclip( float cfar ) {
  if(cfar == far_) return;
  if(cfar != 0 || !persp_) {
    far_ = cfar;
    notify();
    redraw();
  }
}

void Fl_Gview::nearclip( float cnear ) {
  if(cnear == near_) return;
  if(cnear != 0 || !persp_) {
    near_ = cnear;
    notify();
    redraw();
  }
}

void Fl_Gview::center( const Point *cenw ) {
  if(cenw) pcenw_ = *cenw;
  else pcenw_.x[0] = pcenw_.x[1] = pcenw_.x[2] = 0;
  notify();
}

int Fl_Gview::next_id() const {
  int id;
  for(id = 1; withid(id) >= 0; id++)
    ;
  return id;
}

int Fl_Gview::add_drawer( void (*func)( Fl_Gview *, void *obj, void *arg ),
	void *obj, void *arg, char *name, int id )
{
  if(id < 0) id = next_id();
  int dno = withid( id );
  if(dno < 0) {
    if(ndrawers_ >= maxdrawers_) {
	maxdrawers_ = ndrawers_*2 + 15;
	int room = maxdrawers_ * sizeof(struct drawer);
	drawers_ = (struct drawer *)
		    (drawers_==NULL ? malloc(room) : realloc(drawers_, room));
    }
    dno = ndrawers_++;
  }
  struct drawer *dp = &drawers_[dno];
  dp->func = func;
  dp->obj = obj;
  dp->arg = arg;
  dp->To2w = Tidentity;
  dp->name = name;
  dp->id = id;
  notify();
  redraw();
  return id;
}

int Fl_Gview::withid( int id ) const {
  for(int dno = 0; dno < ndrawers_; dno++)
    if(drawers_[dno].id == id)
	return dno;
  return -1;
}

const Matrix *Fl_Gview::To2w( int id ) const {
  if(id == GV_ID_CAMERA)
    return Tc2w();
  int dno = withid( id );
  return (dno < 0) ? &Tidentity : &drawers_[dno].To2w;
}

int Fl_Gview::To2w( int id, const Matrix *newT ) {
  if(id == GV_ID_CAMERA) {
    Tc2w( newT );
    return -1;
  }
  int drawerno = withid( id );
  if(drawerno < 0) return 0;
  drawers_[drawerno].To2w = newT ? *newT : Tidentity;
  notify();
  redraw();
  return 1;
}

#define VIEW_CLEAR  0x1
#define	VIEW_RED    0x2
#define	VIEW_CYAN   0x4

void Fl_Gview::draw_scene( int how, const Matrix *postproj ) {
  /* draw scene */

  glClearDepth( 1.0 );
  if(how & VIEW_CLEAR) {
    glColorMask( 1, 1, 1, 1 );
    glClearColor( bgcolor_.x[0], bgcolor_.x[1], bgcolor_.x[2], 0 );
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
  } else {
    glClear( GL_DEPTH_BUFFER_BIT );
  }

  if(how & VIEW_RED)
    glColorMask( 1, 0, 0, 1 );
  else if(how & VIEW_CYAN)
    glColorMask( 0, 1, 1, 1 );

  glEnable( GL_DEPTH_TEST );
  glDisable( GL_LIGHTING );
  glDisable( GL_TEXTURE_2D );
  glDisable( GL_COLOR_MATERIAL );

  if(how || postproj) {
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    if(inpick()) {
	GLint vp[4] = {0, 0, w(), h()};
	gluPickMatrix( pickx_, picky_, pickwidth_, pickheight_, vp );
    }
    if(persp_) {
	float nthf = near_ * halfyfov_ / focallen_;
	glFrustum( -nthf * aspect_, nthf * aspect_,
		   -nthf, nthf,
		   near_, far_ );
    } else {
	glOrtho( -aspect_*halfyfov_, aspect_*halfyfov_,
		 -halfyfov_, halfyfov_,
		  near_, far_ );
    }
    if(postproj)
	glMultMatrixf( postproj->m );

    glMatrixMode( GL_MODELVIEW );
    glLoadMatrixf( Tw2c()->m );
  }

  for(int i = 0; i < ndrawers_; i++) {
    if(drawers_[i].func != NULL) {
	glPushMatrix();
	glMultMatrixf( drawers_[i].To2w.m );
	if(inpick_) {
	    glLoadName( drawers_[i].id );
	    glPushName(0);
	}
	(*drawers_[i].func)(this, drawers_[i].obj, drawers_[i].arg);
	if(inpick_)
	     glPopName();
	glPopMatrix();
    }
  }
}

static void stereoeye( Matrix *dst, float stereosep, float focallen ) {
  *dst = Tidentity;
  dst->m[4*2+0] = stereosep;
  dst->m[4*3+0] = stereosep * focallen;
}

void Fl_Gview::draw() {

  if(!valid() || damage() || inpick() || (stereo_ != GV_MONO)) {
    /* Assume reshaped */
    valid(1);
    glViewport( 0, 0, w(), h() );

    aspect_ = h() > 0 ? (float)w() / (float)h() : 1.0;

    Matrix postproj;

    switch(stereo_) {
    case GV_MONO:
    default:
	draw_scene( VIEW_CLEAR, NULL );
	break;

    case GV_REDCYAN:
	stereoeye( &postproj, stereosep_, focallen_ );
	draw_scene( VIEW_CLEAR|VIEW_RED, &postproj );
	stereoeye( &postproj, -stereosep_, focallen_ );
	draw_scene( VIEW_CYAN, &postproj );
	glColorMask( 1, 1, 1, 1 );
	break;

    case GV_QUADBUFFERED:
	stereoeye( &postproj, -stereosep_, focallen_ );
	glDrawBuffer( GL_BACK_RIGHT );
	draw_scene( VIEW_CLEAR, &postproj );

	stereoeye( &postproj, stereosep_, focallen_ );
	glDrawBuffer( GL_BACK_LEFT );
	draw_scene( VIEW_CLEAR, &postproj );

	break;
    }
  } else {
    draw_scene( VIEW_CLEAR, NULL );
  }

  /* draw (I hope) any children lying on top of us */
  if(children() > 0) Fl_Gl_Window::draw();
}

int Fl_Gview::snapshot( int x, int y, int w, int h,  void *packedrgb )
{
  make_current();
  glPixelStorei( GL_PACK_ALIGNMENT, 1 );
  glReadBuffer( GL_FRONT );
  glReadPixels(x, y, w, h, GL_RGB, GL_UNSIGNED_BYTE, packedrgb);
  return 1; // Might return whether this window was properly uncovered?
}

static int verbose;	/* for debugging! */

#define	XYBUTTON	FL_BUTTON1
#define	PICKBUTTON	FL_BUTTON2
#define	ZBUTTON		FL_BUTTON3

#define	SLOWKEY		FL_SHIFT
#define	CONSTRAINKEY	FL_CTRL


int Fl_Gview::handle(int ev) {

  if(eventhook) {
    /* Allow clients to pre-screen our events without subclassing */
    switch((*eventhook)(this, ev)) {
    case 1: return 1;	/* pre-screener handled it */
    case -1: return 0;  /* pre-screener commands us to ignore it too */
    default: break;	/* Else just process event normally below */
    }
  }

  switch(ev) {
  case FL_FOCUS:   hasfocus_ = 1; return 1;  // Yes, we want FL_KEYBOARD events
  case FL_UNFOCUS: hasfocus_ = 0; return 1;

  case FL_PUSH:
	{  // Besides navigating, grab keyboard focus if we're clicked-on
	    if(!hasfocus_) take_focus();
	}
  case FL_DRAG:
  case FL_RELEASE:
    if(Fl::event_state(XYBUTTON|ZBUTTON) && !Fl::event_state(PICKBUTTON)) {
	do_nav(ev, Fl::event_state(FL_SHIFT), Fl::event_state(ZBUTTON),
		   Fl::event_state(CONSTRAINKEY));
	return 1;
    } else if(Fl::event_state(PICKBUTTON)
		&& !Fl::event_state(XYBUTTON|ZBUTTON)
		&& pickcb_ != NULL) {

	dpickx_ = Fl::event_x();
	dpicky_ = h() - Fl::event_y();

	if(ev == FL_PUSH) {
	    do_pick( dpickx_, dpicky_ );
	    pickneeded_ = 0;
	} else {
	    if(!pickneeded_) {
		pickneeded_ = 1;
		Fl::add_idle( Fl_Gview::idlepick, (void *)this );
	    }
	}
	return 1;
    }
    /* Maybe do right-button pick? */
    return 0;

  case FL_ENTER:
    take_focus();
    return 1;

  case FL_KEYBOARD:

    if(Fl::event_text() == NULL || Fl::event_text()[0] == '\0')
	return 0;

    int c = Fl::event_text()[0];

    if(num.addchar( c ))
	return 1;


    switch(c) {
    case 'w': reset( retarget() ); notify(); redraw(); break;
    case 'r': retarget(); nav( GV_ROTATE ); break;
    case 'p':
    case 'P': if(num.has) retarget();
	      else {
		do_pick( Fl::event_x_root() - x_root(),
			 y_root() + h() - Fl::event_y_root() );
	      }
	      break;
    case 'f': retarget(); nav( GV_FLY ); break;
    case 't': retarget(); nav( GV_TRANSLATE ); break;
    case 'o': retarget(); nav( GV_ORBIT ); break;
    case 'O': perspective( num.value( !perspective() )  );
	      if(msg) msg("Perspective %s\n", persp_?"on":"off");
	      notify();
	      break;

    case 'v':
    case 'V':	if(num.has) angyfov( num.fvalue() );
		else halfyfov( halfyfov() * (c=='v' ? 1.25 : 1/1.25) );
		if(msg) msg("angyfov %g\n", angyfov());
		notify();
		break;

    case 'v'&0x1F: verbose = !verbose; break;

    case '@':
	Point cpos, cquat;
	vgettranslation( &cpos, To2w( retarget() ) );
	tfm2quat( &cquat, To2w( retarget() ) );
	if(msg) msg("%s at %.4g %.4g %.4g  quat %.4g %.4g %.4g\n",
		dname( retarget() ),
		cpos.x[0],cpos.x[1],cpos.x[2],
		cquat.x[0],cquat.x[1],cquat.x[2]);
	break;

    case '=':
	const float *fp;
	int me;
	me = retarget();
	fp = &To2w(me)->m[0];
	if(msg) msg("%s To2w():\n", dname(me));
	int i;
	for(i = 0; i < 16; i+=4)
	    if(msg) msg("\t%9.5g %9.5g %9.5g %9.5g\n", fp[i],fp[i+1],fp[i+2],fp[i+3]);
	Matrix w2o;
	eucinv( &w2o, To2w(me) );
	fp = &w2o.m[0];
	if(msg) msg("%s Tw2o():\n", dname(me));
	for(i = 0; i < 16; i+=4)
	    if(msg) msg("\t%9.5g %9.5g %9.5g %9.5g\n", fp[i],fp[i+1],fp[i+2],fp[i+3]);
	float aer[3];
	Point xyz;
	tfm2xyzaer( &xyz, aer, To2w(me) );
	if(msg) msg("%s o2w = XYZ Ry Rx Rz FOV:\n  %g %g %g  %g %g %g  %g\n",
		dname(me),
		xyz.x[0],xyz.x[1],xyz.x[2],
		aer[1],aer[0],aer[2],
		perspective() ? angyfov() : -2*halfyfov() );
	break;

    case '\033': exit(0);  /* ESC */
    // case FL_End: Fl::warning("End key!"); return 1; // test!

    default: c = 0;
    }

    num.clear();

    /* Maybe check Fl::event_key(FL_HOME), etc.? */
    return c==0 ? 0 : 1;

  }
  return 0;
}

void Fl_Gview::idlepick( void *vthis ) {
  Fl_Gview *me = (Fl_Gview *)vthis;

  if(me->pickneeded_) {
    me->do_pick( me->dpickx_, me->dpicky_ );
    me->pickneeded_ = 0;
    Fl::remove_idle( Fl_Gview::idlepick, vthis );
  }
}


void Fl_Gview::notifier( void (*func)(Fl_Gview*,void*), void *arg ) {
  notify_ = func;
  notifyarg_ = arg;
}

void Fl_Gview::notify() {
  if(notify_ != NULL)
    (*notify_)( this, notifyarg_ );
}

void Fl_Gview::nav( enum Gv_Nav newnav ) {
  if(nav_ != newnav) {
    nav_ = newnav;
    notify();
  }
}

/*
 * rot through theta about vector {x,y,z}, where tan(theta/2) = length(xyz)
 */
static void rotation(Matrix *T, float x, float y, float z)
{
  Point rp = {x,y,z};
  vscale(&rp, 1./sqrt(vdot(&rp,&rp) + 1.), &rp);
  quat2tfm(T, &rp);
}

static void translation(Matrix *T, float x, float y, float z)
{
  Point p = {x,y,z};
  *T = Tidentity;
  vsettranslation( T, &p );
}

void Fl_Gview::start_nav( int mytarget ) {
    evx_ = Fl::event_x();
    evy_ = Fl::event_y();
    evTc2w_ = Tc2w_, evTw2c_ = Tw2c_;
    evTobj2w_ = *To2w( mytarget );
}

void Fl_Gview::do_nav(int ev, int slow, int zaxis, int constrained) {

  if((slow != evslow_ || zaxis != evzaxis_ || constrained != evconstrain_)
	&& (ev == FL_DRAG)) {
    /* If conditions changed, pretend button was released
     * (so we commit to this nav update) and
     * pushed again.
     */
    do_nav( FL_RELEASE, evslow_, evzaxis_, evconstrain_ );
    ev = FL_PUSH;
  }

  Gv_Nav curnav = (constrained && nav_==GV_ORBIT)
			? (zaxis ? GV_ROTATE : GV_FLY)
			: nav_;

  int mytarget = (curnav == GV_ORBIT || curnav == GV_FLY || !movingtarget())
		? GV_ID_CAMERA
		: target();

  if(ev == FL_PUSH) {
    start_nav( mytarget );
    evslow_ = slow;
    evzaxis_ = zaxis;
    evconstrain_ = constrained;
    /* and, stop animation (idlefunc)! */
    return;
  }

  if((ev == FL_DRAG || ev == FL_RELEASE) && w() > 0) {
    float slowrate = slow ? 0.05 : 1.0;
    int field = w() > h() ? w() : h();
    float dx = -(Fl::event_x() - evx_);
    float dy =  (Fl::event_y() - evy_);
    float angfield = halfyfov_ / focallen_;

    if(ev == FL_RELEASE && (fabs(dx) < nullthresh_ && fabs(dy) < nullthresh_)) {
	dx = dy = 0;
    }

    dx *= slowrate / field;
    dy *= slowrate / field;

    if(constrained && curnav == nav_) {
	// CTRL key means "constrain to X/Y axis",
	// except in Orbit mode where it means "fly" or "twist"!
	if(fabs(dx) < fabs(dy)) dx = 0;
	else dy = 0;
    }

    Matrix Tincr;
    const Matrix *Tf2w, *Tw2f;
    const Point *pcenterw = NULL;

    if(owncoords_) {
	Tf2w = Tw2f = NULL;
    } else {
	Tf2w = &evTc2w_, Tw2f = &evTw2c_;
    }

    switch(curnav) {
    case GV_ROTATE:
	if(zaxis)
	    rotation(&Tincr, 0, 0, (dx+dy));
	else
	    rotation(&Tincr, -2*dy, 2*dx, 0);
	pcenterw = &pcenw_;
	break;
    case GV_ORBIT:
	if(zaxis) {
	    Point pcamw;
	    vgettranslation( &pcamw, Tc2w() );
	    translation(&Tincr, 0, 0, (dx+dy) * vdist(&pcenw_, &pcamw));
	    start_nav( mytarget );
	} else {
	    rotation(&Tincr, -2*dy, 2*dx, 0);
	}
	pcenterw = &pcenw_;
	break;
    case GV_FLY:
	if(zaxis) {
	    translation(&Tincr, 0, 0, (dx+dy) * zspeed * focallen_);
	} else {
	    rotation(&Tincr, dy*angfield, -dx*angfield, 0);
	}
	pcenterw = NULL;
	break;
    case GV_TRANSLATE:
	if(zaxis) {
	    translation(&Tincr, 0, 0, -(dx+dy) * zspeed * focallen_);
	} else {
	    translation(&Tincr, 2 * dx * halfyfov_ * focallen_, 2 * dy * halfyfov_ * focallen_, 0);
	}
	pcenterw = NULL;
	break;
    default:
	fprintf(stderr, "Fl_Gview::do_nav(): Unknown nav mode %d\n", nav_);
	Tincr = Tidentity;
    }

    if(verbose) {
	Point dp, dq;
	vgettranslation( &dp, &Tincr );
	tfm2quat( &dq, &Tincr );
	if(msg) msg("dx %.3f dy %.3f  angfield %.3g  dp %.4g %.4g %.4g  dq %.4g %.4g %.4g\n",
		dx, dy, angfield,
		dp.x[0],dp.x[1],dp.x[2],
		dq.x[0],dq.x[1],dq.x[2]);
    }

    Matrix newTobj2w;
    mconjugate( &newTobj2w, &evTobj2w_, &Tincr,
		Tf2w, Tw2f, pcenterw, NULL );
    float scaling = vlength( (Point *)&evTobj2w_.m[0] );
    if(fabs(scaling - 1) < .01) {
	Point p, q;
	vgettranslation( &p, &newTobj2w );
	tfm2quat( &q, &newTobj2w );
	quat2tfm( &newTobj2w, &q );
	vscale( (Point *)&newTobj2w.m[0], scaling, (Point *)&newTobj2w.m[0] );
	vscale( (Point *)&newTobj2w.m[4], scaling, (Point *)&newTobj2w.m[4] );
	vscale( (Point *)&newTobj2w.m[8], scaling, (Point *)&newTobj2w.m[8] );
	vsettranslation( &newTobj2w, &p );
    }

    To2w( mytarget, &newTobj2w );
    redraw();
  }
}

void Fl_Gview::picksize( float width, float height )
{
  pickwidth_ = width;
  pickheight_ = height;
}

void Fl_Gview::pickbuffer( int nents, GLuint *buf )
{
  picknents_ = nents;
  pickbuf_ = buf;
}

void Fl_Gview::picker( void (*pickcb)(Fl_Gview *, int, int, GLuint *, void *), void *arg )
{
  pickcb_ = pickcb;
  pickarg_ = arg;
}

void (*Fl_Gview::picker(void **argp))(Fl_Gview *, int, int, GLuint *, void *)
{
  if(argp) *argp = pickarg_;
  return pickcb_;
}

int Fl_Gview::pickresults( int *nentp, GLuint **bufp )
{
  if(nentp) *nentp = picknents_;
  if(bufp) *bufp = pickbuf_;
  return pickhits_;
}

int Fl_Gview::do_pick( float xpick, float ypick )
{
  make_current();
  glSelectBuffer( picknents_, pickbuf_ );
  int ok = glRenderMode( GL_SELECT );
  if(ok != 0) {
    fprintf(stderr, "Trouble in do_pick: glRenderMode( GL_SELECT ) = %d\n", ok);
    ok = glRenderMode( GL_SELECT );
    fprintf(stderr, "Retry: %d\n", ok);
  }
  glPushName(0);

  inpick_ = 1;
  pickx_ = xpick;
  picky_ = ypick;
  draw();
  inpick_ = 0;

  glPopName();
  pickhits_ = glRenderMode( GL_RENDER );
  if(pickcb_)
    (*pickcb_)( this, pickhits_, picknents_, pickbuf_, pickarg_ );
  valid(0);
  return pickhits_;
}

char *Fl_Gview::dname( int id ) {
  static char name[32];
  if(id < 0) return "[c0]";
  int dno = withid( id );
  if(dno >= 0) {
    sprintf(name, drawers_[dno].name ? "[g%d %.22s]" : "[g%d]",
	id, drawers_[dno].name);
  } else {
    sprintf(name, "[g%d?]", id);
  }
  return name;
}