/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <FL/Fl_Double_Window.H>

#define DEBUG_OUT
#ifdef DEBUG_OUT
#include <fstream>
#endif

#include <iostream>

#include "agent/Vision/colors.h"
#include "agent/headers/Utility.h"
#include "agent/headers/matrix.h"

#include "../DataInterface.h"
#include "../State.h"
#include "GlobalView.h"

#ifdef USE_GSL
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>

#include "ChokeChainCP.h"

// Calculate the SVD of matrix P
bool SVD(Matrix P, Matrix & U, Matrix & S, Matrix & V) {
  // Set the current matrix to U

  int columns = P.ncols();
  int rows = P.nrows();

  gsl_vector * s = gsl_vector_alloc(columns);
  gsl_vector * work = gsl_vector_alloc(columns);

  gsl_matrix * U_m = gsl_matrix_calloc(rows, columns);
  gsl_matrix * V_m = gsl_matrix_calloc(columns, columns);

  for (int i=0;i<rows;i++) {
    for (int j=0;j<columns;j++) {
      U_m->data[i * U_m->tda + j] = P.e(i,j);
    }
  }
  if (0!=gsl_linalg_SV_decomp(U_m,V_m,s,work)) {
    return false;
  }
  U.resize(rows,columns);

  for (int i=0;i<rows;i++) {
    for (int j=0;j<columns;j++) {
      U.e(i,j) = U_m->data[i * U_m->tda + j];
    }
  }

  V=Matrix(columns,columns);
  for (int i=0;i<columns;i++) {
    for (int j=0;j<columns;j++) {
      V.e(i,j) = V_m->data[i * V_m->tda + j];
    }
  }

  // Grab the values of the vector s and put them into the 
  // diagonals of the square matrix S
  int max_idx=0;
  if (columns < rows) {
    max_idx=columns;
  } else {
    max_idx=rows;
  }
  S=Matrix(max_idx,max_idx);

  for (int i=0;i<max_idx;i++) {
    S.e(i,i) = gsl_vector_get(s,i);
  }

  gsl_vector_free(s);
  gsl_vector_free(work);

  return true;
}

//====================================================================
// Use the GSL to display a 2D covariance matrix
Matrix ellipse(Matrix x,Matrix P,double mu, int N) {

  // This generates a 2D ellipse from a 2D covariance matrix
  // Most of this assumes that P is a 2x2 matrix and x is 2x1

  Matrix U;
  Matrix S;
  Matrix V;
  Matrix pts;
  Matrix tmp;

  // Calculate the svd
  P.inverse();
  SVD(P,U,S,V);
  tmp=Matrix(U.nrows(),N+1);
  // Generate the points of the ellipse
  double angle = 0.0;
  for (int i=0;i<=N;i++) {
    angle=double(i)*(2*M_PI/N);
    tmp.e(0,i) = cos(angle)*mu/sqrt(S.e(0,0));
    tmp.e(1,i) = sin(angle)*mu/sqrt(S.e(1,1));
  }
  // Rotate the points around the U matrix
  pts=U;
  pts = pts * tmp;
  // Add the x and y displacements
  for (int i=0;i<pts.ncols();i++) {
    pts.e(0,i) = pts.e(0,i)+x.e(0,0);
    pts.e(1,i) = pts.e(1,i)+x.e(1,0);
  }
  return pts;
}
#endif

//====================================================================

extern sem_t GuiDataSemaphore;
extern DataInterface *DataControl;

#ifdef DEBUG_OUT
ofstream debug_output("chokechain.out");
#endif

void set_color(int color_idx)
{
  fl_color(ColorVals[color_idx].red,
           ColorVals[color_idx].green,
           ColorVals[color_idx].blue);
}

//====================================================================

GlobalView::GlobalView(ChokeChainCP * _cp, int width,int height) :
  Fl_Double_Window(0,0,width,height,"Global View")
{
  resizable(this);
  setSizeWorldView(w(),h());
  cp=_cp;
}

GlobalView::~GlobalView()
{
}

//=========================================================================
void GlobalView::resize(int x,int y,int w, int h)
{
  Fl_Double_Window::resize(x,y,w,h);
  setSizeWorldView(w,h);
}

//=========================================================================
void GlobalView::drawWVField()
{
  // draw background
  fl_color(FL_BLACK);
  fl_rectf(0,0,w(),h());
  
  // draw field surface
  vector2i *pts;
  pts = wvState.getFieldPoints();
  fl_color(0x00,0x80,0x00);  
  fl_begin_complex_polygon();
  for(int i=0; i<17; i++){
    fl_vertex(V2COMP(pts[i]));
  }
  fl_end_complex_polygon();

  // draw stripes
  fl_color(FL_WHITE);
  fl_line(wvState.xWorldToScreen(0.0),wvState.yWorldToScreen( 1350.0),
          wvState.xWorldToScreen(0.0),wvState.yWorldToScreen(-1350.0));
}

//=========================================================================
void GlobalView::drawWVRobot(int robot_id)
{

  static const int circle_size=11;
  static const int circle_size_2=5;
  static const int indicator_length=20;

  if(DataControl && DataControl->allRobotViewData) {
    // draw robot
    int x,y;
    x = wvState.xWorldToScreen(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x);
    y = wvState.yWorldToScreen(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
#if 0
    fl_color(FL_RED);
    fl_pie(x-circle_size_2,y-circle_size_2,
           circle_size,circle_size,
           0.0,360.0);
#else
    fl_color(0xFF,0xFF,0xFF);
    fl_rectf(x-circle_size_2,y-circle_size_2,
	     circle_size,circle_size);
#endif
    
    // draw direction indicator
    int di_end_x,di_end_y; // direction indicator
    double robot_angle = DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle();
    di_end_x = (int)rint(x + indicator_length * cos(robot_angle));
    di_end_y = (int)rint(y - indicator_length * sin(robot_angle));
    fl_line(x, y, di_end_x, di_end_y);

    // draw direction deviations
    float iangle,iangle_dev;
    double robot_angle_dev = DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.sMaj;
    iangle     = (180*robot_angle    /M_PI+.5);
    iangle_dev = (180*robot_angle_dev/M_PI+.5);
    iangle_dev = min(iangle_dev,360.0f);
    fl_arc(x-indicator_length,y-indicator_length,indicator_length*2,indicator_length*2,
           iangle-iangle_dev/2,iangle+iangle_dev/2);

    // draw deviations
    drawDev(x,y,
            DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sMaj,
            DataControl->allRobotViewData->robots[robot_id].robotPosn.position.sMin,
            DataControl->allRobotViewData->robots[robot_id].robotPosn.position.thetaMaj);
  }
}

//=========================================================================
void GlobalView::drawWVWMBall(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelOldBall())) { return; }

  static const int circle_size=11;
  static const int circle_size_2=5;

  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *ball;
    ball=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::BallIdx];

    vector2d ball_loc;
    ball_loc = ball->mean;
    //    printf("wm bl odo %g %g\n",ball_loc.x,ball_loc.y);
#ifdef DEBUG_OUT
    debug_output << "wm bl local " 
		 << ball_loc.x << " "
		 << ball_loc.y << " "
		 << DataControl->getPacketTimestamp()
		 << endl;
#endif
    ball_loc = ball_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    ball_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                         DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);

    int x,y;
    x=wvState.xWorldToScreen(ball_loc.x);
    y=wvState.yWorldToScreen(ball_loc.y);
    
    // draw ball
    // printf("wm bl gbl %g %g\n",ball_loc.x,ball_loc.y);
    
    fl_color(0x5B,0x2C,0x00);
    fl_pie(x-circle_size_2,
	   y-circle_size_2,
	   circle_size,circle_size,
	   0.0,360.0);

    drawDev(x,y,
	    ball->sMaj,
	    ball->sMaj,
	    ball->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());

  }
}

//=========================================================================
void GlobalView::drawWVWMRedRobot(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelRobots())) { return; }

  static const int cross_size=11;

  if(DataControl && DataControl->allRobotViewData) {
    if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[WorldModelPacketDecoder::RedRobotObsIdx]) {
      Gaussian2 *robot=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[WorldModelPacketDecoder::RedRobotObsIdx];
      
      int x,y;
      x=wvState.xWorldToScreen(robot->mean.x);
      y=wvState.yWorldToScreen(robot->mean.y);

      // printf("wm red robot %g %g\n",robot->mean.x,robot->mean.y);
      
      fl_color(0xff,0x00,0x00);
      fl_line_style(FL_SOLID,3);
      fl_line(x+cross_size/2,y+cross_size/2,x-cross_size/2,y-cross_size/2);
      fl_line(x-cross_size/2,y+cross_size/2,x+cross_size/2,y-cross_size/2);
      fl_line_style(0);

      drawDev(x,y,
	      robot->sMaj,
	      robot->sMaj,
	      robot->thetaMaj);
    }
  }  
}

//=========================================================================
void GlobalView::drawWVWMBlueRobot(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelRobots())) { return; }

  static const int cross_size=11;

  if(DataControl && DataControl->allRobotViewData) {
    if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[WorldModelPacketDecoder::BlueRobotObsIdx]) {
      Gaussian2 *robot=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[WorldModelPacketDecoder::BlueRobotObsIdx];
      
      int x,y;
      x=wvState.xWorldToScreen(robot->mean.x);
      y=wvState.yWorldToScreen(robot->mean.y);
      
      // printf("wm blue robot %g %g\n",robot->mean.x,robot->mean.y);

      fl_color(0x00,0x00,0xff);
      fl_line_style(FL_SOLID,3);
      fl_line(x+cross_size/2,y+cross_size/2,x-cross_size/2,y-cross_size/2);
      fl_line(x-cross_size/2,y+cross_size/2,x+cross_size/2,y-cross_size/2);
      fl_line_style(0);

      drawDev(x,y,
	      robot->sMaj,
	      robot->sMaj,
	      robot->thetaMaj);
    }
  }  
}

//=========================================================================
void GlobalView::drawWVWMTeammateRedRobot(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelTeammateRobots())) { return; }

  static const int cross_size=11;

  if(DataControl && DataControl->allRobotViewData) {

    for (int i=WorldModelPacketDecoder::Teammate0RedRobotObsIdx;
	 i<=WorldModelPacketDecoder::Teammate2RedRobotObsIdx;
	 i++) {
      if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[i]) {
	Gaussian2 *robot=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[i];
      
	int x,y;
	x=wvState.xWorldToScreen(robot->mean.x);
	y=wvState.yWorldToScreen(robot->mean.y);
	
	// printf("wm teammate %d red robot %g %g\n",i-WorldModelPacketDecoder::Teammate0RedRobotIdx,robot->mean.x,robot->mean.y);

	fl_color(0xAA,0x00,0x00);
	fl_line_style(FL_SOLID,3);
	fl_line(x+cross_size/2,y+cross_size/2,x-cross_size/2,y-cross_size/2);
	fl_line(x-cross_size/2,y+cross_size/2,x+cross_size/2,y-cross_size/2);
	fl_line_style(0);
	
	drawDev(x,y,
		robot->sMaj,
		robot->sMaj,
		robot->thetaMaj);

	char name[10];
	snprintf(name,10,"Id=%d",i-WorldModelPacketDecoder::Teammate0RedRobotObsIdx);
	int font=fl_font();int size=fl_size();fl_font(font,12);
	fl_draw(name,strlen(name),x-cross_size/2,y-cross_size/2);
	fl_font(font,size);
      }
    }
  }  
}

//=========================================================================
void GlobalView::drawWVWMTeammateBlueRobot(int robot_id)
{

  if (!(cp->worldModelActive() && cp->worldModelTeammateRobots())) { return; }

  static const int cross_size=11;

  if(DataControl && DataControl->allRobotViewData) {

    for (int i=WorldModelPacketDecoder::Teammate0BlueRobotObsIdx;
	 i<=WorldModelPacketDecoder::Teammate2BlueRobotObsIdx;
	 i++) {
      if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[i]) {
	Gaussian2 *robot=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[i];
      
	int x,y;
	x=wvState.xWorldToScreen(robot->mean.x);
	y=wvState.yWorldToScreen(robot->mean.y);
	
	// printf("wm teammate %d red robot %g %g\n",i-WorldModelPacketDecoder::Teammate0BlueRobotIdx,robot->mean.x,robot->mean.y);

	fl_color(0x00,0x00,0xAA);
	fl_line_style(FL_SOLID,3);
	fl_line(x+cross_size/2,y+cross_size/2,x-cross_size/2,y-cross_size/2);
	fl_line(x-cross_size/2,y+cross_size/2,x+cross_size/2,y-cross_size/2);
	fl_line_style(0);
	
	drawDev(x,y,
		robot->sMaj,
		robot->sMaj,
		robot->thetaMaj);

	char name[10];
	snprintf(name,10,"Id=%d",i-WorldModelPacketDecoder::Teammate0BlueRobotObsIdx);
	int font=fl_font();int size=fl_size();fl_font(font,12);
	fl_draw(name,strlen(name),x-cross_size/2,y-cross_size/2);
	fl_font(font,size);
      }
    }
  }  
}

//=========================================================================
void GlobalView::drawWVWMTeammateBall(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelTeammateBall())) { return; }

  static const int circle_size=11;
  static const int circle_size_2=5;

  if(DataControl && DataControl->allRobotViewData) {

    for (int i=WorldModelPacketDecoder::Teammate0BallIdx;
	 i<=WorldModelPacketDecoder::Teammate2BallIdx;
	 i++) {

      if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[i]) {
	Gaussian2 *ball=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[i];
	
	int x,y;
	x=wvState.xWorldToScreen(ball->mean.x);
	y=wvState.yWorldToScreen(ball->mean.y);
	
	// draw ball
	//printf("wm teammate %d bl %g %g\n",i,ball->mean.x,ball->mean.y);
	
	fl_color(0xFF,0xEA,0x00); // This matches the tracker color
	//	fl_color(0x3B,0x2C,0x00);
	fl_pie(x-circle_size_2,
	       y-circle_size_2,
	       circle_size,circle_size,
	       0.0,360.0);
	drawDev(x,y,
		ball->sMaj,
		ball->sMaj,
		ball->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());

	char name[10];
	snprintf(name,10,"Id=%d",i-WorldModelPacketDecoder::Teammate0BallIdx);
	int font=fl_font();int size=fl_size();fl_font(font,12);
	fl_draw(name,strlen(name),x-circle_size_2,y-circle_size_2);
	fl_font(font,size);
      }
    }
  }
}

//=========================================================================
void GlobalView::drawWVWMTracker(int robot_id)
{
  if (!cp->worldTrackerActive()) { return; }

  static const int circle_size=11;
  static const int circle_size_2=5;

  if(DataControl && DataControl->allRobotViewData) {
    for (int i=0;i<TrackerPacketDecoder::NumGaussians;i++) {
      if (1==DataControl->allRobotViewData->robots[robot_id].trackerObjsValid[i]) {
	Gaussian2 *ball_pos;
	ball_pos=&DataControl->allRobotViewData->robots[robot_id].trackerObjs[i];
	
	vector2d ball_loc;
	ball_loc = ball_pos->mean;
	
	// draw ball
#ifdef DEBUG_OUT
	debug_output << "tk bl local " 
		     << ball_loc.x << " "
		     << ball_loc.y << " "
		     << DataControl->getPacketTimestamp() << " "
		     << " idx " << i
		     << endl;
#endif
	if (5 > i) {
	  // Rotate to local coordinates
	  ball_loc = ball_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
	  ball_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
			       DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
	  
	  if (0==i) {
	    // The 1st Gaussian is our own best estimate
	    fl_color(0xFF,0x7E,0x00);
	  } else {
	    // The 2nd-5th Gaussians are the rest of our estimates
	    fl_color(0xAB,0x53,0x00);
	  }
	} else {
	  
	  if (5==i) {
	    // The 6th Gaussian is the best estimate from our teammates
	    fl_color(0xFF,0xEA,0x00);
	  } else {
	    // All of the other Gaussians are the estimates from our teammates
	    fl_color(0x9D,0x8E,0x00);
	  }
	}
	int x,y;
	x=wvState.xWorldToScreen(ball_loc.x);
	y=wvState.yWorldToScreen(ball_loc.y);
	
	if (((0 == i) && (cp->worldTrackerSelfBall())) ||
	    (((5 > i)&&(0 < i)) && 
	     (cp->worldTrackerSelfBall()) &&
	     (cp->worldTrackerSelfBallHypotheses())) ||
	    ((5 == i) && (cp->worldTrackerTeammateBall())) || 
	    (((10>i)&&(5<i)) && 
	     (cp->worldTrackerTeammateBall()) &&
	     (cp->worldTrackerTeammateBallHypotheses()))) {

	  fl_arc(x-circle_size_2,
		 y-circle_size_2,
		 circle_size,circle_size,
		 0.0,360.0);

	  if (((5 > i) && (cp->worldTrackerSelfBallEllipses())) ||
	      (((5 <= i)&&(10 > i)) && (cp->worldTrackerTeammateBallEllipses()))) {
	    drawDev2(ball_loc.x,
		     ball_loc.y,
		     ball_pos->sMaj,
		     ball_pos->sMaj,
		     ball_pos->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle(),
		     1);
	  }
	}
      }
    }
  }
}

//=========================================================================
void GlobalView::drawWVVisBall(int robot_id)
{
  if (!(cp->visionActive() && cp->visionBall())) { return; }

  static const int circle_size=11;
  static const int circle_size_2=5;

  if(DataControl && DataControl->allRobotViewData) {
    if(DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.confidence > 0.4) {
      vector2d ball_loc;
      ball_loc.x = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.x;
      ball_loc.y = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.y;
      //      printf("vs bl odo %g %g\n",ball_loc.x,ball_loc.y);
#ifdef DEBUG_OUT
      debug_output << "vs bl local " 
		   << ball_loc.x << " "
		   << ball_loc.y << " "
		   << DataControl->getPacketTimestamp()
		   << endl;
#endif
      ball_loc = ball_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
      ball_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                           DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
      
      int x,y;
      x=wvState.xWorldToScreen(ball_loc.x);
      y=wvState.yWorldToScreen(ball_loc.y);

      // draw ball
      // printf("vs bl gbl %g %g\n",ball_loc.x,ball_loc.y);
      fl_color(0xFF,0x7E,0x00);
      fl_pie(x-circle_size_2,
	     y-circle_size_2,
	     circle_size,circle_size,
	     0.0,360.0);
    }
  }
}

//=========================================================================
void GlobalView::drawWVVisMarkers(int robot_id)
{
  if (!(cp->visionActive() && cp->visionMarkers())) { return; }

  for(int marker_idx=0; marker_idx<NUM_MARKERS; marker_idx++) {
    VObject *marker;
    marker = &DataControl->allRobotViewData->robots[robot_id].visionObjs.marker[marker_idx];

    if(marker->confidence > 0.4) {
      if(marker->edge || marker->distance > 4000.0)
        set_color(ColorBrightGreen);
      else
        set_color(ColorWhite);
      vector2d marker_loc = MarkerLocations[marker_idx];

      vector2i marker_pt = wvState.worldToScreen(marker_loc);

      int marker_size_x=(int)rint(wvState.xWorldDiffToScreen(50.0));
      int marker_size_y=(int)rint(wvState.yWorldDiffToScreen(50.0));

      int r_x,r_y;
      r_x=(int)rint(wvState.xWorldDiffToScreen(marker->distance));
      r_y=(int)rint(wvState.yWorldDiffToScreen(marker->distance));

      fl_pie(marker_pt.x-marker_size_x,marker_pt.y-marker_size_y,
             marker_size_x*2,marker_size_y*2,
             0,360);
      fl_arc(marker_pt.x-r_x,marker_pt.y-r_y,
             r_x*2,r_y*2,
             0,360);

      static const int num_segments=16;
      int i;
      double t;
      for(t=0.0, i=0; i<num_segments; i++, t+=2*M_PI/num_segments) {
        vector2d ind_start;
        ind_start.set(marker->distance,0.0);
        ind_start = ind_start.rotate(t);
        ind_start += marker_loc;

        vector2i ind_start_pt;
        ind_start_pt = wvState.worldToScreen(ind_start);

        static const float seg_len=100.0;

        vector2d ind_end;
        ind_end.set(seg_len,0.0);
        ind_end = ind_end.rotate(M_PI+t-atan2(marker->loc.y,marker->loc.x));
        ind_end += ind_start;

        vector2i ind_end_pt;
        ind_end_pt = wvState.worldToScreen(ind_end);

        fl_line(V2COMP(ind_start_pt),V2COMP(ind_end_pt));
      }
    }
  }
}

//=========================================================================
void GlobalView::drawWVWMTeammate(int robot_id)
{
  if (!(cp->worldModelActive() && cp->worldModelTeammates())) { return; }

  static const int circle_size=11;
  static const int circle_size_2=5;
  
  for (int i=WorldModelPacketDecoder::Teammate0Idx;
       i<=WorldModelPacketDecoder::Teammate2Idx;
       i++) {
    
    if (1==DataControl->allRobotViewData->robots[robot_id].worldModelObjsValid[i]) {
      Gaussian2 *robot=&DataControl->allRobotViewData->robots[robot_id].worldModelObjs[i];
      
      int x,y;
      x=wvState.xWorldToScreen(robot->mean.x);
      y=wvState.yWorldToScreen(robot->mean.y);
      
      // draw opponent
      //printf("wm teammate %d pos %g %g\n",i,robot->mean.x,robot->mean.y);
      fl_color(0xAD,0xAD,0xAD);
#if 0
      fl_pie(x-circle_size_2,
	     y-circle_size_2,
	     circle_size,circle_size,
	     0.0,360.0);
#else
      fl_rectf(x-circle_size_2,
	       y-circle_size_2,
	       circle_size,circle_size);

#endif
      drawDev(x,y,
	      robot->sMaj,
	      robot->sMaj,
	      robot->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
      char name[10];
      snprintf(name,10,"Robot=%d",i-WorldModelPacketDecoder::Teammate0Idx);
      int font=fl_font();int size=fl_size();fl_font(font,12);
      fl_draw(name,strlen(name),x-circle_size_2,y-circle_size_2);
      fl_font(font,size);
    }
  }
}

//=========================================================================
#if 0
void GlobalView::drawWVWMOpponent(int robot_id)
{
  static const int circle_size=11;
  static const int circle_size_2=5;

  /* There are three opponents, so it's the same code, 3 times */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *opponent;
    opponent=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::OpponentRobot0Idx];

    vector2d opponent_loc;
    opponent_loc = opponent->mean;
    opponent_loc = opponent_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    opponent_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x, DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
    GdkPoint opponent_loc_gdk;
    opponent_loc_gdk = wvState.worldToScreen(opponent_loc);

    // draw opponent
    //printf("wm bl %g %g\n",opponent_loc.x,opponent_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorBlue]);
    gdk_draw_arc(win, gc, true, opponent_loc_gdk.x-circle_size_2,
		 opponent_loc_gdk.y-circle_size_2,
                 circle_size, circle_size,
                 0, 64*360);

    // draw deviations
    drawDev(win,gc,opponent_loc_gdk.x,opponent_loc_gdk.y,
            opponent->sMaj,
            opponent->sMaj,
            opponent->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }
   
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *opponent;
    opponent=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::OpponentRobot1Idx];

    vector2d opponent_loc;
    opponent_loc = opponent->mean;
    opponent_loc = opponent_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    opponent_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x, DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
    GdkPoint opponent_loc_gdk;
    opponent_loc_gdk = wvState.worldToScreen(opponent_loc);

    // draw opponent
    //printf("wm bl %g %g\n",opponent_loc.x,opponent_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorBlue]);
    gdk_draw_arc(win, gc, true, opponent_loc_gdk.x-circle_size_2,
		 opponent_loc_gdk.y-circle_size_2,
                 circle_size, circle_size,
                 0, 64*360);

    // draw deviations
    drawDev(win,gc,opponent_loc_gdk.x,opponent_loc_gdk.y,
            opponent->sMaj,
            opponent->sMaj,
            opponent->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }

  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *opponent;
    opponent=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::OpponentRobot2Idx];

    vector2d opponent_loc;
    opponent_loc = opponent->mean;
    opponent_loc = opponent_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    opponent_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x, DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
    GdkPoint opponent_loc_gdk;
    opponent_loc_gdk = wvState.worldToScreen(opponent_loc);

    // draw opponent
    //printf("wm bl %g %g\n",opponent_loc.x,opponent_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorBlue]);
    gdk_draw_arc(win, gc, true, opponent_loc_gdk.x-circle_size_2,
		 opponent_loc_gdk.y-circle_size_2,
                 circle_size, circle_size,
                 0, 64*360);

    // draw deviations
    drawDev(win,gc,opponent_loc_gdk.x,opponent_loc_gdk.y,
            opponent->sMaj,
            opponent->sMaj,
            opponent->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }
  
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *opponent;
    opponent=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::OpponentRobot3Idx];

    vector2d opponent_loc;
    opponent_loc = opponent->mean;
    opponent_loc = opponent_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    opponent_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x, DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);
    GdkPoint opponent_loc_gdk;
    opponent_loc_gdk = wvState.worldToScreen(opponent_loc);

    // draw opponent
    //printf("wm bl %g %g\n",opponent_loc.x,opponent_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorBlue]);
    gdk_draw_arc(win, gc, true, opponent_loc_gdk.x-circle_size_2,
		 opponent_loc_gdk.y-circle_size_2,
                 circle_size, circle_size,
                 0, 64*360);

    // draw deviations
    drawDev(win,gc,opponent_loc_gdk.x,opponent_loc_gdk.y,
            opponent->sMaj,
            opponent->sMaj,
            opponent->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }
}
#endif

//=========================================================================
#if 0
void GlobalView::drawWVWMGoals(int robot_id)
{
  static const int circle_size=11;
  static const int circle_size_2=5;

  /* Draw the left part of the cyan goal */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *cyan_goal_left;
    cyan_goal_left=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::CyanGoalLeftIdx];

    vector2d cyan_goal_left_loc;
    cyan_goal_left_loc = cyan_goal_left->mean;
    cyan_goal_left_loc = cyan_goal_left_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    cyan_goal_left_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                         DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);

    GdkPoint cyan_goal_left_loc_gdk;
    cyan_goal_left_loc_gdk = wvState.worldToScreen(cyan_goal_left_loc);

    // draw cyan_goal_left
    //printf("wm bl %g %g\n",cyan_goal_left_loc.x,cyan_goal_left_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorCyan]);
    gdk_draw_arc(win, gc,
                 true,cyan_goal_left_loc_gdk.x-circle_size_2,cyan_goal_left_loc_gdk.y-circle_size_2,
                 circle_size,circle_size,
                 0,64*360);

    // draw deviations
    drawDev(win,gc,cyan_goal_left_loc_gdk.x,cyan_goal_left_loc_gdk.y,
            cyan_goal_left->sMaj,
            cyan_goal_left->sMaj,
            cyan_goal_left->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }

  /* Draw the right part of the cyan goal */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *cyan_goal_right;
    cyan_goal_right=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::CyanGoalRightIdx];

    vector2d cyan_goal_right_loc;
    cyan_goal_right_loc = cyan_goal_right->mean;
    cyan_goal_right_loc = cyan_goal_right_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    cyan_goal_right_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                         DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);

    GdkPoint cyan_goal_right_loc_gdk;
    cyan_goal_right_loc_gdk = wvState.worldToScreen(cyan_goal_right_loc);

    // draw cyan_goal_right
    //printf("wm bl %g %g\n",cyan_goal_right_loc.x,cyan_goal_right_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorLightGrey]);
    gdk_draw_arc(win, gc,
                 true,cyan_goal_right_loc_gdk.x-circle_size_2,cyan_goal_right_loc_gdk.y-circle_size_2,
                 circle_size,circle_size,
                 0,64*360);

    // draw deviations
    drawDev(win,gc,cyan_goal_right_loc_gdk.x,cyan_goal_right_loc_gdk.y,
            cyan_goal_right->sMaj,
            cyan_goal_right->sMaj,
            cyan_goal_right->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }

  /* Draw the left part of the yellow goal */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *yellow_goal_left;
    yellow_goal_left=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::YellowGoalLeftIdx];

    vector2d yellow_goal_left_loc;
    yellow_goal_left_loc = yellow_goal_left->mean;
    yellow_goal_left_loc = yellow_goal_left_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    yellow_goal_left_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                         DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);

    GdkPoint yellow_goal_left_loc_gdk;
    yellow_goal_left_loc_gdk = wvState.worldToScreen(yellow_goal_left_loc);

    // draw yellow_goal_left
    //printf("wm bl %g %g\n",yellow_goal_left_loc.x,yellow_goal_left_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorYellow]);
    gdk_draw_arc(win, gc,
                 true,yellow_goal_left_loc_gdk.x-circle_size_2,yellow_goal_left_loc_gdk.y-circle_size_2,
                 circle_size,circle_size,
                 0,64*360);

    // draw deviations
    drawDev(win,gc,yellow_goal_left_loc_gdk.x,yellow_goal_left_loc_gdk.y,
            yellow_goal_left->sMaj,
            yellow_goal_left->sMaj,
            yellow_goal_left->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }

  /* Draw the right part of the yellow goal */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *yellow_goal_right;
    yellow_goal_right=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::YellowGoalRightIdx];

    vector2d yellow_goal_right_loc;
    yellow_goal_right_loc = yellow_goal_right->mean;
    yellow_goal_right_loc = yellow_goal_right_loc.rotate(DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
    yellow_goal_right_loc += vector2d(DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.x,
                         DataControl->allRobotViewData->robots[robot_id].robotPosn.position.mean.y);

    GdkPoint yellow_goal_right_loc_gdk;
    yellow_goal_right_loc_gdk = wvState.worldToScreen(yellow_goal_right_loc);

    // draw yellow_goal_right
    //printf("wm bl %g %g\n",yellow_goal_right_loc.x,yellow_goal_right_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorWhite]);
    gdk_draw_arc(win, gc,
                 true,yellow_goal_right_loc_gdk.x-circle_size_2,yellow_goal_right_loc_gdk.y-circle_size_2,
                 circle_size,circle_size,
                 0,64*360);

    // draw deviations
    drawDev(win,gc,yellow_goal_right_loc_gdk.x,yellow_goal_right_loc_gdk.y,
            yellow_goal_right->sMaj,
            yellow_goal_right->sMaj,
            yellow_goal_right->thetaMaj+DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle());
  }
}
#endif

//=========================================================================
void GlobalView::drawWVWMDebug(int robot_id) 
{
  static const int circle_size=11;
  static const int circle_size_2=5;

  if (!cp->wmDebugActive()) { return; }

  if(DataControl && DataControl->allRobotViewData) {

    for (int i=0;i<WMDebugPacketDecoder::NUMOBJS;i++) {

      if (1==DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].valid) {
      
	int x,y;
	x = (int)wvState.xWorldToScreen(DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].x);
	y = (int)wvState.yWorldToScreen(DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].y);

	uchar shape;
	shape=DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].shape;

	uchar r,g,b;
	r=DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].red;
	g=DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].green;
	b=DataControl->allRobotViewData->robots[robot_id].wmDebugObjs[i].blue;

	fl_color(r,g,b);

	switch (shape) {
	case WMD_SQUARE:
	  fl_rectf(x-circle_size_2,
		   y-circle_size_2,
		   circle_size,circle_size);
	  break;
	case WMD_HOLLOW_SQUARE:
	  fl_rect(x-circle_size_2,
		  y-circle_size_2,
		  circle_size,circle_size);
	  break;
	case WMD_CIRCLE:
	  fl_pie(x-circle_size_2,
		 y-circle_size_2,
		 circle_size,circle_size,
		 0.0,360.0);
	  break;
	case WMD_HOLLOW_CIRCLE:
	  fl_arc(x-circle_size_2,
		 y-circle_size_2,
		 circle_size,circle_size,
		 0.0,360.0);
	  break;
	case WMD_DIAMOND:
	  fl_polygon(x,y-circle_size_2,
		     x-circle_size_2,y,
		     x,y+circle_size_2,
		     x+circle_size_2,y);
	  break;
	case WMD_HOLLOW_DIAMOND:
	  fl_line(x,y-circle_size_2,
		  x-circle_size_2,y);
	  fl_line(x-circle_size_2,y,
		  x,y+circle_size_2);
	  fl_line(x,y+circle_size_2,
		  x+circle_size_2,y);
	  fl_line(x+circle_size_2,y,
		  x,y-circle_size_2);
	  break;
	case WMD_UP_TRIANGLE:
	  fl_polygon(x,y-circle_size_2,
		     x-circle_size_2,y+circle_size_2,
		     x+circle_size_2,y+circle_size_2);
	  break;
	case WMD_UP_HOLLOW_TRIANGLE:
	  fl_line(x,y-circle_size_2,
		  x-circle_size_2,y+circle_size_2);
	  fl_line(x-circle_size_2,y+circle_size_2,
		  x+circle_size_2,y+circle_size_2);
	  fl_line(x+circle_size_2,y+circle_size_2,
		  x,y-circle_size_2);
	  break;
	case WMD_DOWN_TRIANGLE:
	  fl_polygon(x,y+circle_size_2,
		     x-circle_size_2,y-circle_size_2,
		     x+circle_size_2,y-circle_size_2);
	  break;
	case WMD_DOWN_HOLLOW_TRIANGLE:
	  fl_line(x,y+circle_size_2,
		  x-circle_size_2,y-circle_size_2);
	  fl_line(x-circle_size_2,y-circle_size_2,
		  x+circle_size_2,y-circle_size_2);
	  fl_line(x+circle_size_2,y-circle_size_2,
		  x,y+circle_size_2);
	  break;
	default:
	  fl_pie(x-circle_size_2,
		 y-circle_size_2,
		 circle_size,circle_size,
		 0.0,360.0);
	  break;
	}
      }
    }
  }
}

//=========================================================================
void GlobalView::drawWVAllWMDebug()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMDebug(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawWVAllRobot()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVRobot(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawWVAllWMBall()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMBall(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawWVAllVisBall()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVVisBall(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawWVAllVisMarkers()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVVisMarkers(robot_idx);
  }
}

//=========================================================================
#if 0
void GlobalView::drawWVAllWMOpponent()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMOpponent(robot_idx,win,gc);
  }
}
#endif

//=========================================================================
void GlobalView::drawWVAllWMRobot()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx]) {
      drawWVWMRedRobot(robot_idx);
      drawWVWMBlueRobot(robot_idx);
    }
  }
}

//=========================================================================
void GlobalView::drawWVAllWMTeammate()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMTeammate(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawWVAllWMTeammateBall() {
  for (int robot_idx=0; robot_idx<RVDMaxRobots;robot_idx++) {
    if (DataControl->allRobotViewData->robotsActive[robot_idx]) {
      drawWVWMTeammateBall(robot_idx);
    }
  }
}

//=========================================================================
void GlobalView::drawWVAllWMTeammateRobot() {
  for (int robot_idx=0; robot_idx<RVDMaxRobots;robot_idx++) {
    if (DataControl->allRobotViewData->robotsActive[robot_idx]) {
      drawWVWMTeammateRedRobot(robot_idx);
      drawWVWMTeammateBlueRobot(robot_idx);
    }
  }
}

//=========================================================================
#if 0
void GlobalView::drawWVAllWMGoals()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMGoals(robot_idx,win,gc);
  }
}
#endif

//=========================================================================
void GlobalView::drawWVAllWMTracker()
{
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMTracker(robot_idx);
  }
}

//=========================================================================
void GlobalView::drawDev(int x, int y,
                         double major, double minor, double theta)
{
#if 0
  fl_color(0,0,0);

  static const double angle_span=.3; // rads
  float ang_diff;
  ang_diff = rad2deg(angle_span);

  float ang1;
  int xsize,ysize;
  xsize = wvState.xWorldDiffToScreen(major);
  ysize = wvState.yWorldDiffToScreen(major);

  ang1 = rad2deg(theta-angle_span/2.0);
  fl_arc(x-xsize/2,y-ysize/2,xsize,ysize,ang1,ang1+ang_diff);

  ang1 = rad2deg(M_PI+theta-angle_span/2.0);
  fl_arc(x-xsize/2,y-ysize/2,xsize,ysize,ang1,ang1+ang_diff);

  xsize = wvState.xWorldDiffToScreen(minor);
  ysize = wvState.yWorldDiffToScreen(minor);

  ang1 = rad2deg(M_PI/2+theta-angle_span/2.0);
  fl_arc(x-xsize/2,y-ysize/2,xsize,ysize,ang1,ang1+ang_diff);

  ang1 = rad2deg(3*M_PI/2+theta-angle_span/2.0);
  fl_arc(x-xsize/2,y-ysize/2,xsize,ysize,ang1,ang1+ang_diff);
#endif
}

#ifdef USE_GSL
void GlobalView::drawDev2(double x, 
			  double y,
			  double major, 
			  double minor, 
			  double theta,
			  int std_dev) {

  Gaussian2 g2;
  g2.setsMajsMin(major,minor,theta);

  Matrix X(2,1);
  Matrix P(2,2);

  X.e(0,0) = x;
  X.e(1,0) = y;

  P.e(0,0) = g2.sx;
  P.e(1,1) = g2.sy;
  P.e(0,1) = g2.psxsy;
  P.e(1,0) = g2.psxsy;

  for (int j=1;j<=std_dev;j++) {
    Matrix pts = ellipse(X,P,j,20);
    for (int i=1;i<pts.ncols();i++) {
      int x1,y1,x2,y2;
      x1 = wvState.xWorldToScreen(pts.e(0,i-1));
      y1 = wvState.yWorldToScreen(pts.e(1,i-1));
      x2 = wvState.xWorldToScreen(pts.e(0,i));
      y2 = wvState.yWorldToScreen(pts.e(1,i));
      fl_line_style(FL_DASH);
      fl_line(x1,y1,x2,y2);
      fl_line_style(0);
    }
  }
}
#endif

//=========================================================================
void GlobalView::setSizeWorldView(int width, int height)
{
  wvState.setSize(width, height);
}

//=========================================================================
void GlobalView::draw()
{
  sem_wait(&GuiDataSemaphore);

  drawWVField        ();

  // Vision
  drawWVAllVisMarkers();

  drawWVAllRobot     ();
  drawWVAllWMBall    ();

  // Vision
  drawWVAllVisBall   ();
  drawWVAllWMRobot   ();

  drawWVAllWMTeammate();
  drawWVAllWMTeammateBall();
  drawWVAllWMTeammateRobot();

  drawWVAllWMTracker ();

  drawWVAllWMDebug();

  //drawWVAllWMOpponent();
  //drawWVAllWMGoals   ();

  sem_post(&GuiDataSemaphore);
}

//=========================================================================
int GlobalView::handle(int event)
{
  switch(event){
    case FL_KEYDOWN:
      {
        int key = Fl::event_key();
        switch(key){
        }
      }
      return 0;
      break;
    case FL_PUSH:
      {
        int xs,ys;

        xs = Fl::event_x();
        ys = Fl::event_y();
      }
      return 0;
      break;
  }

  return 0;
}