// Only drawn in 2d at the moment
void LocalizationGL::drawRelativeObjects(WorldObjectBlock* gtObjects, WorldObjectBlock* beliefObjects, RobotStateBlock* robotState) {
  if (beliefObjects == NULL || robotState == NULL){
    //cout << "no wo or robotstate to draw seen objects" << endl;
    return;
  }

  WorldObject* wo;
  WorldObject* self = &(gtObjects->objects_[robotState->WO_SELF]);

  for (int i = 0; i < NUM_WORLD_OBJS; i++){
    wo = &(beliefObjects->objects_[i]);
    // if seen, get vision dist and bearing
    if (wo->seen){
      // use vision distance and bearing to figure out image loc
      AngRad orient = (self->orientation + wo->visionBearing);
      Point2D obsLocFd (wo->visionDistance, orient, POLAR);
      obsLocFd += self->loc;
      Vector3<float> start(self->loc.x, self->loc.y, 250);
      Vector3<float> end(obsLocFd.x, obsLocFd.y,250);
      if (wo->isOwnGoal() && wo->isGoalPost()) {
        drawObservationLine(start,end,Colors::Blue);
        objectsGL.drawPost(obsLocFd,0.25);
      } else if (wo->isOwnGoal() && !wo->isGoalPost()) {
        drawObservationLine(start,end,Colors::Blue);
        objectsGL.drawGoal(obsLocFd,0.25);
      } else if (wo->isBall()) {
        drawObservationLine(start,end,Colors::Orange);
        objectsGL.drawBall(obsLocFd,0.5);
      } else if (wo->isBeacon()) {
        vector<RGB> colors;
        switch(wo->type) {
          case WO_BEACON_BLUE_YELLOW: colors = { Colors::Blue, Colors::Yellow }; break;
          case WO_BEACON_YELLOW_BLUE: colors = { Colors::Yellow, Colors::Blue }; break;
          case WO_BEACON_BLUE_PINK: colors = { Colors::Blue, Colors::Pink }; break;
          case WO_BEACON_PINK_BLUE: colors = { Colors::Pink, Colors::Blue }; break;
          case WO_BEACON_PINK_YELLOW: colors = { Colors::Pink, Colors::Yellow }; break;
          case WO_BEACON_YELLOW_PINK: colors = { Colors::Yellow, Colors::Pink }; break;
        }
        drawObservationLine(start,end,Colors::White);
        objectsGL.drawBeacon(obsLocFd, colors[0], colors[1],0.25);
      /*
      } else if (wo->isIntersection() && !wo->isUnknownIntersection()){
        drawObservationLine(start,end,Colors::White);
        objectsGL.drawIntersection(obsLocFd,0.5);
      } else if (wo->isLine() || wo->isUnknownLine()) {
        Point2D closest = wo->visionLine.relativeToGlobal(self->loc, self->orientation).getPointOnSegmentClosestTo(self->loc);
        drawObservationLine(start,Vector3<float>(closest.x, closest.y, 250),Colors::White);
        objectsGL.drawLinePoint(closest,0.5);
        //draw the line segment
        // unknown - black
        if (wo->isUnknownLine())
          basicGL.colorRGBAlpha(Colors::Black,1.0);
        // known - red
        else
          basicGL.colorRGBAlpha(Colors::Red,1.0);
        Point2D sP=wo->visionPt1;
        Point2D eP=wo->visionPt2;
        sP=sP.relativeToGlobal(self->loc,self->orientation);
        eP=eP.relativeToGlobal(self->loc,self->orientation);
        basicGL.drawLine(sP,eP,2.0);
      } else if (wo->isUnknownIntersection()) {
        drawObservationLine(start,end,Colors::Pink);
        objectsGL.drawIntersection(obsLocFd,0.5);
      } else if (wo->isCenterCircle()){
        // draw center cirlce
        drawObservationLine(start,end,Colors::White);
        objectsGL.drawCenterCircle(obsLocFd,0.5);
      } else if (wo->isUnknownPenaltyCross() || wo->isKnownPenaltyCross()){
        // draw penalty cross
        drawObservationLine(start,end,Colors::White);
        objectsGL.drawPenaltyCross(obsLocFd,0.5);
      */
      }
    }
  }
}
// Only drawn in 2d at the moment
void LocalizationGL::drawRelativeObjects(WorldObjectBlock* worldObjects, RobotStateBlock* robotState) {
  if (worldObjects == NULL || robotState == NULL){
    //cout << "no wo or robotstate to draw seen objects" << endl;
    return;
  }

  WorldObject* wo;
  WorldObject* self = &(worldObjects->objects_[robotState->WO_SELF]);

  for (int i = 0; i < NUM_WORLD_OBJS; i++){
    wo = &(worldObjects->objects_[i]);
    // if seen, get vision dist and bearing
    if (wo->seen){
      // use vision distance and bearing to figure out image loc
      AngRad orient = (self->orientation + wo->visionBearing);
      Point2D obsLocFd (wo->visionDistance, orient, POLAR);
      obsLocFd += self->loc;
      Vector3<float> start(self->loc.x, self->loc.y, 250);
      Vector3<float> end(obsLocFd.x, obsLocFd.y,250);
      if (wo->isGoal() && wo->isGoalPost()) {
        drawObservationLine(start,end,basicGL.yellowRGB);
        objectsGL.drawYellowPost(obsLocFd,0.25);
      } else if (wo->isGoal() && !wo->isGoalPost()) {
        drawObservationLine(start,end,basicGL.yellowRGB);
        objectsGL.drawYellowGoal(obsLocFd,0.25);
      } else if (wo->isUnknownIntersection()) {
        end.z=0.0;
        drawObservationLine(start,end,basicGL.pinkRGB);
        objectsGL.drawIntersection(obsLocFd,0.5);
      } else if (wo->isIntersection() && !wo->isUnknownIntersection()){
        end.z=0.0;
        drawObservationLine(start,end,basicGL.whiteRGB);
        objectsGL.drawIntersection(obsLocFd,0.5);
      } else if (wo->isLine() || wo->isUnknownLine()) {
        end.z=0.0;
        drawObservationLine(start,end,basicGL.whiteRGB);
        objectsGL.drawLinePoint(obsLocFd,0.5);
        //draw the line segment
        // unknown - black
        if (wo->isUnknownLine())
          basicGL.colorRGBAlpha(basicGL.blackRGB,1.0);
        // known - red
        else
          basicGL.colorRGBAlpha(basicGL.redRGB,1.0);
        Point2D sP=wo->visionPt1;
        Point2D eP=wo->visionPt2;
        sP=sP.relativeToGlobal(self->loc,self->orientation);
        eP=eP.relativeToGlobal(self->loc,self->orientation);
        basicGL.drawLine(sP,eP,2.0);
      } else if (wo->isBall()) {
        end.z=BALL_RADIUS;
        drawObservationLine(start,end,basicGL.orangeRGB);
        objectsGL.drawBall(obsLocFd,0.5);
      } else if (wo->isCenterCircle()){
        // draw center cirlce
        end.z = 0.0;
        drawObservationLine(start,end,basicGL.whiteRGB);
        objectsGL.drawCenterCircle(obsLocFd,0.5);
      } else if (wo->isUnknownPenaltyCross() || wo->isKnownPenaltyCross()){
        // draw penalty cross
        end.z = 0.0;
        drawObservationLine(start,end,basicGL.whiteRGB);
        objectsGL.drawPenaltyCross(obsLocFd,0.5);
      }
    }
  }
}