/* 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 <semaphore.h>

#include <stdio.h>

#include <gtk/gtk.h>

#include "../../../agent/headers/field.h"
#include "../RobotViewData.h"
#include "../State.h"
#include "UI.h"

extern sem_t GuiDataSemaphore;

void
UI::drawWorldView(GdkWindow *win,GdkGC *gc) {
  GdkGCValues gc_values;
  gdk_gc_get_values(gc,&gc_values);

  sem_wait(&GuiDataSemaphore);

  drawWVField     (win,gc);

  drawWVAllVisMarkers(win,gc);
  drawWVAllRobot     (win,gc);
  drawWVAllWMBall    (win,gc);
  drawWVAllVisBall   (win,gc);
  //drawWVAllWMTeammate(win,gc);
  //drawWVAllWMOpponent(win,gc);
  drawWVAllWMGoals   (win,gc);


  sem_post(&GuiDataSemaphore);

  gdk_gc_set_foreground(gc,&gc_values.foreground);
  gdk_gc_set_background(gc,&gc_values.background);
}

void
UI::drawWVField(GdkWindow *win,GdkGC *gc) {
  // draw background
  gdk_gc_set_foreground(gc,&Colors[ColorBlack]);
  gdk_draw_rectangle(win,gc,true,0,0,wvState.getXSize(),wvState.getYSize());
  
  // draw walls
  //static const double wall_x=halfLength+wallWidth;
  //static const double wall_y=halfWidth +wallWidth;
  //gint x1,x2,y1,y2,w,h;
  //x1 = wvState.xWorldToScreen(-wall_x);
  //x2 = wvState.xWorldToScreen(+wall_x);
  //y1 = wvState.yWorldToScreen(-wall_y);
  //y2 = wvState.yWorldToScreen(+wall_y);
  //w = abs(x2-x1+1);
  //h = abs(y2-y1+1);
  //x1 = min(x1,x2);
  //y1 = min(y1,y2);
  //gdk_gc_set_foreground(gc,&Colors[ColorWhite]);
  //gdk_draw_rectangle(win,gc,true,x1,y1,w,h);
  
  // draw field surface
  gdk_gc_set_foreground(gc,&Colors[ColorGreen]);
  gdk_draw_polygon(win, gc,
                   true,
                   wvState.getFieldPoints(),
                   17);

  // draw stripes
  gdk_gc_set_foreground(gc,&Colors[ColorWhite]);
  gdk_draw_line(win, gc,
                wvState.xWorldToScreen(0.0),wvState.yWorldToScreen( 1350.0),
                wvState.xWorldToScreen(0.0),wvState.yWorldToScreen(-1350.0));
}

void
UI::drawWVRobot(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;
  static const int indicator_length=20;

  if(DataControl && DataControl->allRobotViewData) {
    // draw robot
    gint 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);
    gdk_gc_set_foreground(gc,&Colors[ColorRed]);
    gdk_draw_arc(win, gc,
                 true,x-circle_size/2,y-circle_size/2,
                 circle_size,circle_size,
                 0,64*360);

    // draw direction indicator
    gint di_end_x,di_end_y; // direction indicator
    double robot_angle = DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.mean.angle();
    di_end_x = (gint)rint(x + indicator_length * cos(robot_angle));
    di_end_y = (gint)rint(y - indicator_length * sin(robot_angle));
    gdk_draw_line(win, gc, x, y, di_end_x, di_end_y);

    // draw direction deviations
    gint iangle,iangle_dev;
    double robot_angle_dev = DataControl->allRobotViewData->robots[robot_id].robotPosn.heading.sMaj;
    iangle     = (int)(180*64*robot_angle    /M_PI+.5);
    iangle_dev = (int)(180*64*robot_angle_dev/M_PI+.5);
    iangle_dev = min(iangle_dev,360*64);
    iangle = (iangle-iangle_dev/2+360*64)%(360*64);
    gdk_draw_arc(win,gc,false,x-indicator_length,y-indicator_length,indicator_length*2,indicator_length*2,
                 iangle,iangle_dev);

    // draw deviations
    drawDev(win,gc,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
UI::drawWVWMBall(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;

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

    vector2d ball_loc;
    ball_loc = ball_gnug->mean;
    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);

    GdkPoint ball_loc_gdk;
    ball_loc_gdk = wvState.worldToScreen(ball_loc);

    // draw ball
    //printf("wm bl %g %g\n",ball_loc.x,ball_loc.y);
    gdk_gc_set_foreground(gc,&Colors[ColorBrown]);
    gdk_draw_arc(win, gc,
                 true,ball_loc_gdk.x-circle_size/2,ball_loc_gdk.y-circle_size/2,
                 circle_size,circle_size,
                 0,64*360);

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

void
UI::drawWVVisBall(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;

  if(DataControl && DataControl->allRobotViewData) {
    if(DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.confidence > 0.4) {
      vector2d ball_loc;
      //printf("vs bl %g %g\n",
      //       DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.x,
      //       DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.y);
      ball_loc.x = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.x;
      ball_loc.y = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.y;
      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);
      
      GdkPoint ball_loc_gdk;
      ball_loc_gdk = wvState.worldToScreen(ball_loc);
      
      // draw ball
      //printf("vs bl %g %g\n",ball_loc.x,ball_loc.y);
      gdk_gc_set_foreground(gc,&Colors[ColorOrange]);
      gdk_draw_arc(win, gc,
                   true,ball_loc_gdk.x-circle_size/2,ball_loc_gdk.y-circle_size/2,
                   circle_size,circle_size,
                   0,64*360);
    }
  }
}

void
UI::drawWVVisMarkers(int robot_id,GdkWindow *win,GdkGC *gc) {
  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)
        gdk_gc_set_foreground(gc,&Colors[ColorBrightGreen]);
      else
        gdk_gc_set_foreground(gc,&Colors[ColorWhite    ]);
      vector2d marker_loc = MarkerLocations[marker_idx];

      GdkPoint 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;
      //x=worldToScreenCoorX(marker->loc.x);
      //y=worldToScreenCoorY(marker->loc.y);
      r_x=(int)rint(wvState.xWorldDiffToScreen(marker->distance));
      r_y=(int)rint(wvState.yWorldDiffToScreen(marker->distance));

      gdk_draw_arc(win,gc,true,
                   marker_pt.x-marker_size_x,marker_pt.y-marker_size_y,
                   marker_size_x*2,marker_size_y*2,
                   0,64*360);
      gdk_draw_arc(win,gc,false,
                   marker_pt.x-r_x,marker_pt.y-r_y,
                   r_x*2,r_y*2,
                   0,64*360);

      static const int num_segments=16;
      static GdkSegment segs[num_segments];
      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;

        GdkPoint 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;

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

        segs[i].x1=ind_start_pt.x;
        segs[i].y1=ind_start_pt.y;
        segs[i].x2=  ind_end_pt.x;
        segs[i].y2=  ind_end_pt.y;
      }

      gdk_draw_segments(win,gc,segs,num_segments);
    }
  }
}

void
UI::drawWVWMTeammate(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;
  /* There are three teammates, so it's the same code, 3 times */
  if(DataControl && DataControl->allRobotViewData) {
    Gaussian2 *teammate;
    teammate=&DataControl->allRobotViewData->robots[robot_id].
      worldModelObjs[WorldModelPacketDecoder::TeammateRobot0Idx];

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

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

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

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

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

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

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

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

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

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

void
UI::drawWVWMOpponent(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;
  /* 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());
  }
}

void
UI::drawWVWMGoals(int robot_id,GdkWindow *win,GdkGC *gc) {
  static const int circle_size=11;

  /* 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());
  }
}



void
UI::drawWVAllRobot(GdkWindow *win,GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVRobot(robot_idx,win,gc);
  }
}

void
UI::drawWVAllWMBall(GdkWindow *win,GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMBall(robot_idx,win,gc);
  }
}

void
UI::drawWVAllVisBall(GdkWindow *win,GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVVisBall(robot_idx,win,gc);
  }
}

void
UI::drawWVAllVisMarkers(GdkWindow *win,GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVVisMarkers(robot_idx,win,gc);
  }
}

void
UI::drawWVAllWMOpponent(GdkWindow *win, GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMOpponent(robot_idx,win,gc);
  }
}

void
UI::drawWVAllWMTeammate(GdkWindow *win, GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMTeammate(robot_idx,win,gc);
  }
}

void
UI::drawWVAllWMGoals(GdkWindow *win, GdkGC *gc) {
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawWVWMGoals(robot_idx,win,gc);
  }
}

void
UI::drawDev(GdkWindow *win, GdkGC *gc,
            gint x, gint y,
            double major, double minor, double theta) {
  static const double angle_span=.3; // rads
  gint ang_diff;
  ang_diff = (int)rint(64*rad2deg(angle_span));

  gint size,ang1;
  size = (int)rint(major);

  ang1 = (int)rint(64*rad2deg(theta-angle_span/2.0)) % (360*64);
  gdk_draw_arc(win, gc,
               false,x-size/2,y-size/2,size,size,ang1,ang_diff);

  ang1 = (int)rint(64*rad2deg(M_PI+theta-angle_span/2.0)) % (360*64);
  gdk_draw_arc(win, gc,
               false,x-size/2,y-size/2,size,size,ang1,ang_diff);

  size = (int)rint(minor);

  ang1 = (int)rint(64*rad2deg(M_PI/2+theta-angle_span/2.0)) % (360*64);
  gdk_draw_arc(win, gc,
               false,x-size/2,y-size/2,size,size,ang1,ang_diff);

  ang1 = (int)rint(64*rad2deg(3*M_PI/2+theta-angle_span/2.0)) % (360*64);
  gdk_draw_arc(win, gc,
               false,x-size/2,y-size/2,size,size,ang1,ang_diff);
}

void
UI::setSizeWorldView(gint width, gint height) {
  wvState.setSize(width, height);
}

void
UI::drawEgoView(GdkWindow *win,GdkGC *gc) {
  GdkGCValues gc_values;
  gdk_gc_get_values(gc,&gc_values);

  sem_wait(&GuiDataSemaphore);

  drawEVBackground(win,gc);
  for(int robot_idx=0; robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx])
      drawEVVisBall(robot_idx,win,gc);
  }

  sem_post(&GuiDataSemaphore);

  gdk_gc_set_foreground(gc,&gc_values.foreground);
  gdk_gc_set_background(gc,&gc_values.background);
}

void
UI::drawEVBackground(GdkWindow *win,GdkGC *gc) {
  int width,height,min_size;
  GVector::vector2d<int> center;

  width    = evState.getXSize();
  height   = evState.getYSize();
  min_size = evState.getMinSize();
  center   = evState.getCenter();

  // draw background
  gdk_gc_set_foreground(gc,&Colors[ColorBlack]);
  gdk_draw_rectangle(win,gc,true,0,0,width,height);

  // draw field surface
  gdk_gc_set_foreground(gc,&Colors[ColorGreen]);
  gdk_draw_arc(win,gc,true,center.x-min_size/2,center.y-min_size/2,min_size,min_size,0,64*360);

  // draw distance circles
  gdk_gc_set_foreground(gc,&Colors[ColorDarkGreen]);
  for(double dist=500.0; dist < 3000.0; dist+=1000.0) {
    double screen_dist;
    int screen_dist_img;

    screen_dist = evState.distScale(dist);
    screen_dist_img = (int)(screen_dist*(evState.getMinSize()/2) + .5);
    gdk_draw_arc(win,gc,false,center.x-screen_dist_img,center.y-screen_dist_img,
                 screen_dist_img*2,screen_dist_img*2,0,64*360);    
  }
  gdk_gc_set_foreground(gc,&Colors[ColorBlack]);
  for(double dist=1000.0; dist < 4500.0; dist+=1000.0) {
    double screen_dist;
    int screen_dist_img;

    screen_dist = evState.distScale(dist);
    screen_dist_img = (int)(screen_dist*(evState.getMinSize()/2) + .5);
    gdk_draw_arc(win,gc,false,center.x-screen_dist_img,center.y-screen_dist_img,
                 screen_dist_img*2,screen_dist_img*2,0,64*360);    
  }

  // draw robot representation
  gdk_gc_set_foreground(gc,&Colors[ColorRed]);
  gdk_draw_polygon(win, gc,
                   true,
                   evState.getRobotPoints(),
                   4);

  bool drew_one = false;
  for(int robot_idx=0; !drew_one && robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx]) {
      drew_one = true;
      drawEVVisRadial(win,gc,robot_idx);
    }
  }
}

void
UI::drawEVVisBall(int robot_id,GdkWindow *win,GdkGC *gc)
{
  static const int pt_size = 9;
  
  gdk_gc_set_foreground(gc,&Colors[ColorOrange]);

  vector2d ball_loc;
  if(DataControl && DataControl->allRobotViewData) {
    if(DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.confidence > 0.4) {
      vector2i screen_pt;

      ball_loc.x = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.x;
      ball_loc.y = DataControl->allRobotViewData->robots[robot_id].visionObjs.ball.loc.y;

      screen_pt = evState.egoToScreen(ball_loc);

      gdk_draw_arc(win,gc,true,
                   screen_pt.x - pt_size/2,
                   screen_pt.y - pt_size/2,pt_size,pt_size,0,64*360);
    }
  }
}

int RObjectIdxToColor[NUM_ROBJECTS] = {
  ColorLightGrey  , /* ROBJECT_VISIBLE_START     */
  ColorLightGrey  , /* ROBJECT_VISIBLE_END       */
  ColorBrightGreen, /* ROBJECT_FIELD_CLEAR_START */
  ColorBrightGreen, /* ROBJECT_FIELD_CLEAR_END   */
  ColorWhite      , /* ROBJECT_WALL              */
  ColorBlack      , /* ROBJECT_ROBOT             */
  ColorRed        , /* ROBJECT_RED_ROBOT         */
  ColorBlue       , /* ROBJECT_BLUE_ROBOT        */
  ColorOrange     , /* ROBJECT_BALL              */
  ColorCyan       , /* ROBJECT_CYAN_GOAL         */
  ColorYellow     , /* ROBJECT_YELLOW_GOAL       */
  ColorLightGrey    /* ROBJECT_STRIPE            */
};

void
UI::drawEVVisRadial(GdkWindow *win,GdkGC *gc,int robot_id) {
  RadialObjectMap *vis_map=&DataControl->allRobotViewData->robots[robot_id].visMap;

  //for(int robj_idx=0; robj_idx<NUM_ROBJECTS; robj_idx++) {
  //  int color=RObjectIdxToColor[robj_idx];
  //  gdk_gc_set_foreground(gc,&Colors[color]);
  //  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
  //    double angle = RadialObjectMap::idxToAngle(ang_idx);
  //    RadialObjectReading *ror = &vis_map->robjects[robj_idx][ang_idx];
  //    if(ror->confidence > 0.5)
  //      drawEVRadialPoint(win,gc,angle,ror->distance);
  //  }
  //}

  if(DataControl->getEVVisRadMapActive()) {
    for(int robj_idx=0; robj_idx<NUM_ROBJECTS; robj_idx++) {
      int color=RObjectIdxToColor[robj_idx];
      gdk_gc_set_foreground(gc,&Colors[color]);
      RadialObjectMap *vis_map=&DataControl->allRobotViewData->robots[robot_id].visMap;
      drawEVRadialObjectData<RadialObjectMap,RadialObjectReading>(win,gc,vis_map,robot_id,robj_idx);
    }
  }
}

template<class RadialMap, class RadialObject>
void
UI::drawEVRadialObjectData(GdkWindow *win,GdkGC *gc,RadialMap *rad_map,int robot_id,int robj_idx) {
  static const int pt_size = 5;
  static GdkPoint draw_points[NUM_ANGLES+1];
  static bool visible[NUM_ANGLES];

  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
    double angle = rad_map->idxToAngle(ang_idx);
    vector2d ego_pt;
    GVector::vector2d<int> screen_pt;
    RadialObject *ro = NULL;
    
    ro = rad_map->query(robj_idx,ang_idx);
    if(ro && ro->confidence > 0.5) {
      ego_pt.set(cos(angle),sin(angle));
      ego_pt *= ro->distance;
      screen_pt = evState.egoToScreen(ego_pt);
    
      visible[ang_idx] = true;
      draw_points[ang_idx].x = screen_pt.x;
      draw_points[ang_idx].y = screen_pt.y;
    } else {
      visible[ang_idx] = false;
    }
  }

  // draw connecting lines
  bool drawing = false;
  int start_ang_idx = 0;
  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
    if(visible[ang_idx] && !drawing) {
      start_ang_idx = ang_idx;
      drawing = true;
    }
    if(!visible[(ang_idx+1)%NUM_ANGLES] && drawing) {
      gdk_draw_lines(win,gc,&draw_points[start_ang_idx],ang_idx-start_ang_idx+1);
      drawing = false;
    }
  }
  if(drawing) {
    draw_points[NUM_ANGLES] = draw_points[0];
    gdk_draw_lines(win,gc,&draw_points[start_ang_idx],NUM_ANGLES-start_ang_idx+1);
  }

  // draw dots
  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
    if(visible[ang_idx]) {
      gdk_draw_arc(win,gc,true,
                   draw_points[ang_idx].x-pt_size/2,
                   draw_points[ang_idx].y-pt_size/2,pt_size,pt_size,0,64*360);  
    }
  }
}

void
UI::drawEVRadialPoint(GdkWindow *win,GdkGC *gc,double angle,double distance) {
  static const int draw_size = 5;

  vector2d ego_pt;
  GVector::vector2d<int> screen_pt;

  ego_pt.set(cos(angle),sin(angle));
  ego_pt *= distance;
  screen_pt = evState.egoToScreen(ego_pt);

  gdk_draw_arc(win,gc,true,screen_pt.x-draw_size/2,screen_pt.y-draw_size/2,draw_size,draw_size,0,64*360);  
}

void
UI::setSizeEgoView(gint width, gint height) {
  evState.setSize(width, height);
}

extern const int ImageXSize;
extern const int ImageYSize;
extern const int BadColor;

void
UI::drawVisRLEImg(int robot_id,GdkWindow *win,GdkGC *gc) {
  GdkGCValues gc_values;
  gdk_gc_get_values(gc,&gc_values);

  sem_wait(&GuiDataSemaphore);

  uchar *rle_img;
  rle_img = DataControl->allRobotViewData->robots[robot_id].rleImg;

  int x_scale,y_scale;
  int x_offset,y_offset;
  int x_size,y_size;
  x_scale  = vrleState.getXScale();
  y_scale  = vrleState.getYScale();
  x_offset = vrleState.getXOffset();
  y_offset = vrleState.getYOffset();
  x_size   = vrleState.getXSize();
  y_size   = vrleState.getYSize();

  gdk_gc_set_foreground(gc,&Colors[StartRLEColors+BadColor]);
  gdk_draw_rectangle(win,gc,TRUE,0,0,                          x_size,y_offset);
  gdk_draw_rectangle(win,gc,TRUE,0,y_offset+ImageYSize*y_scale,x_size,y_offset+1);
  gdk_draw_rectangle(win,gc,TRUE,0,                          y_offset,x_offset  ,ImageYSize*y_scale);
  gdk_draw_rectangle(win,gc,TRUE,x_offset+ImageXSize*x_scale,y_offset,x_offset+1,ImageYSize*y_scale);

  int x,y;
  int cur_color,start_x;
  y=0;
  while(y<ImageYSize) {
    cur_color = -1;
    start_x = 0;
    x=0;
    while(x<ImageXSize) {
      uchar pix_color;
      pix_color = rle_img[y*ImageXSize+x];
      if(cur_color == -1 || cur_color == pix_color) {
        cur_color = pix_color;
        x++;
      } else {
        gdk_gc_set_foreground(gc,&Colors[StartRLEColors+cur_color]);
        gdk_draw_rectangle(win,gc,TRUE,x_offset+start_x*x_scale,y_offset+y*y_scale,(x-start_x)*x_scale,y_scale);
        cur_color = pix_color;
        start_x = x;
        x++;
      }
    }
    gdk_gc_set_foreground(gc,&Colors[StartRLEColors+cur_color]);
    gdk_draw_rectangle(win,gc,TRUE,x_offset+start_x*x_scale,y_offset+y*y_scale,(x-start_x)*x_scale,y_scale);
    y++;
  }

  sem_post(&GuiDataSemaphore);

  gdk_gc_set_foreground(gc,&gc_values.foreground);
  gdk_gc_set_background(gc,&gc_values.background);
}

void
UI::drawVisRLE(GdkWindow *win,GdkGC *gc) {
  drawVisRLEAllImg(win,gc);
}

void
UI::drawVisRLEAllImg(GdkWindow *win,GdkGC *gc) {
  bool drew_one = false;

  for(int robot_idx=0; !drew_one && robot_idx<RVDMaxRobots; robot_idx++) {
    if(DataControl->allRobotViewData->robotsActive[robot_idx]) {
      drew_one = true;
      drawVisRLEImg(robot_idx,win,gc);
    }
  }
}

void
UI::setSizeVisRLE(gint width, gint height) {
  vrleState.setSize(width, height);
}