#ifndef WIN32

#include <iostream>
#include <string>
#include <sstream>
#include <errno.h>
//#include "SSLVision.h"

#include <messages_robocup_ssl_geometry.pb.h>
#include <messages_robocup_ssl_wrapper.pb.h>
#include <messages_robocup_ssl_detection.pb.h>

#include "Platform/Win32Linux/UdpSocket.h"
#include "Tools/Team.h"

#include "Representations/Sensing/InertiaMatrix.h"
#include "Tools/Debugging/DebugDrawings3D.h"


void SSLVision::init()
{
  sock = new UdpSocket();
  sock->setBlocking(false);
  if(!sock->bind("224.5.23.2", 10002))
  {
    perror("Bind failed.");
    ASSERT(false);
  }

  if(!sock->joinMulticast("224.5.23.2"))
  {
    std::cout << "could not set multicast address" << std::endl;
  }

  lastOdometry = theOdometryData;
  robotPoseValidity = 0;
}


  for(int i = 0; i < num; ++i)
  {
    const SSL_DetectionRobot& robot = teamYellow ? frame->robots_yellow(i) : frame->robots_blue(i);
    if(robotid == (int)robot.robot_id())
    {
      networkRobotPose.translation.x = robot.x();
      networkRobotPose.translation.y = robot.y();
      if(robot.has_orientation())
        networkRobotPose.rotation = robot.orientation();

      DEBUG_RESPONSE("module:SSLVision:rotateField",
                     {
                       networkRobotPose.translation.rotate(pi);
                       networkRobotPose.rotation = normalize(networkRobotPose.rotation+pi);
                     } );

      robotPoseValidity = 1.;
      found = true;
    }
  }

void printRobotInfo(const SSL_DetectionRobot & robot) {
    printf("CONF=%4.2f ", robot.confidence());
    if (robot.has_robot_id()) {
        printf("ID=%3d ",robot.robot_id());
    } else {
        printf("ID=N/A ");
    }
    printf(" HEIGHT=%6.2f POS=<%9.2f,%9.2f> ",robot.height(),robot.x(),robot.y());
    if (robot.has_orientation()) {
        printf("ANGLE=%6.3f ",robot.orientation());
    } else {
        printf("ANGLE=N/A    ");
    }
    printf("RAW=<%8.2f,%8.2f>\n",robot.pixel_x(),robot.pixel_y());
}