package cz.cuni.amis.pogamut.usar2004.communication.messages.usarinfomessages;
   
   import java.util.*;
   import javax.vecmath.*;
   import cz.cuni.amis.pogamut.base.communication.worldview.event.*;
   import cz.cuni.amis.pogamut.base.communication.translator.event.*;
   import cz.cuni.amis.pogamut.base3d.worldview.object.*;
   import cz.cuni.amis.pogamut.usar2004.communication.messages.*;
   import cz.cuni.amis.pogamut.usar2004.communication.messages.datatypes.CustomTypes.*;
  import cz.cuni.amis.pogamut.usar2004.communication.messages.datatypes.*;
  
  /**
   *
   * Sensor messages contain the sensor data. Sensor message Every sensor message
   * starts with “SEN”. After it is an optional Time segment, {Time float}, that
   * reports the current time in seconds in the virtual world. Whether the Time
   * segment will appear or not is decided by the sensor’s ‘bWithTimeStamp’
   * variable. For details information, please read section 10 in USARSim manual
   * v3.1.3. Please note that the Sensor message parameters depend on the type of
   * sensor that the information is being reported.
   *
   * Corresponding GameBots message is SEN.
   *
   */
  public class SensorMessage extends GBEvent implements IWorldEvent, IWorldChangeEvent
  {
      public SensorMessage(String Type, double Time, String Name, double Duration, double FOV, int Fix, double Loudness, double Prob, double Resolution, int Satellites, String Status, String Gas, double Density, boolean Visible, double Dist, double Radius, double Corona)
      {
          this.Type = Type;
          this.Time = Time;
          this.Name = Name;
          this.Duration = Duration;
          this.FOV = FOV;
          this.Fix = Fix;
          this.Loudness = Loudness;
          this.Prob = Prob;
          this.Resolution = Resolution;
          this.Satellites = Satellites;
          this.Status = Status;
          this.Gas = Gas;
          this.Density = Density;
          this.Visible = Visible;
          this.Radius = Radius;
          this.Dist = Dist;
          this.Corona = Corona;
  
          //range-type, name-range..,
          //laser-type,name,resolution,fov,range(...)
          //odometry-type,name,pose(x,y,theta)
          //gps-type, name, latitude(int,double,char), longitude, fix, satelites
          //ins-type, name location, orientation...(?)
          //encoder-type, (name,tick)...
          //touch-type, (name,touch)...
          //rfid-type, TAG-from type, name, id, location...
          //victiom-type, name, partname, location...
          //humanmotion-type, name, prob
          //sound-type, name, loudness, duration
      }
      /**
       * Example how the message looks like - used during parser tests.
       */
      public static final String PROTOTYPE = "SEN {Type text} {Time 0} {Name text} {Duration 0} {FOV 0} {Fix 0} {Loudness 0} {Prob 0} {Resolution 0} {Satellites 0} {Status text}";
      /////// Properties BEGIN
      protected String Type = null;
  
      /**
       * Type describes the sensor type. It will be one of the following values:
       * "RFID”, “RangeScanner”, “IR”, "IRScanner", "Tachometer", "Helper",
       * "Sonar", "Odometry", "GPS", "INS", "INU", "Encoder", "Touch",
       * "VictSensor","HumanMotion", "Sound" or "GroundTruth".
       *
       * @return Type of the sensor.
       */
      public String getType()
      {
          return Type;
      }
      protected double Time = 0;
  
      /**
       * Timestamp form the UT since server start in seconds. Note that time is
       * optional parameter.
       *
       * @return Returns seconds elapsed from the start of the server.
       */
      public double getTime()
      {
          return Time;
      }
      protected String Name = null;
  
      /**
       * Name of the sensor.
       *
       * @return Returns name of the sensor
       */
      public String getName()
      {
          return Name;
     }
     //Range Sensor - Type, Name/Range...
     //Sensor name/Range in meters
     protected Map<String, Double> RangeRanges = new HashMap<String, Double>();
 
     /**
      * Map indexed by string is used to collect data of range sensor. ‘Dtring’
      * is the sensor name, ‘Double’ is the range value in meters.
      *
      * @return Returns Map of ranges from range sensor(Sonar).
      */
     public Map<String, Double> getRangeRanges()
     {
         return RangeRanges;
     }
 
     /**
      * Used by Yylex to fill the RangeRanges Map.
      *
      * @param Name Name of the range sensor.
      * @param Range Valued of the range sensor.
      */
     public void addRange(String Name, Double Range)
     {
         RangeRanges.put(Name, Range);
     }
     //Laser Sensor - Type, Name, Resolution, FOV, Range(...)
     protected double Resolution = 0;
 
     /**
      * It is the sensor’s resolution in radians. With FOV, we can calculate the
      * number of the data in the Range segment.
      *
      * @return Returns resolution of the sensor.
      */
     public double getResolution()
     {
         return Resolution;
     }
     //Sensors field of view in radians
     protected double FOV = 0;
 
     /**
      * FOV is the sensor’s field of view in radians.
      *
      * @return Returns field of view in radians.
      */
     public double getFOV()
     {
         return FOV;
     }
     protected List<Double> LaserRanges = new ArrayList<Double>();
 
     /**
      * LaserRanges is a series of range values in meters.
      *
      * @return Returns List of double values representing distance in meters.
      */
     public List<Double> getLaserRanges()
     {
         return LaserRanges;
     }
     //Odometry sensor - Type, Name, Pose(x,y,theta)
     //estimated robots position relative to the start position in meters(first two components) and theta - the head angle in radians(relative to the start position)
     protected OdometryPose OdoPose = new OdometryPose();
 
     /**
      * OdoPose is the estimated robot position relative to the start point in
      * meters. ‘theta’ is the head angle in radians relative to the start
      * orientation.
      *
      * @return Returns OdometryPose value.
      */
     public OdometryPose getOdoPose()
     {
         return OdoPose;
     }
     //GPS sensor - Type, Name, Latitude(int,double,char), Longitude, Fix, Satellites
     protected Latitude Latitude = new Latitude();
 
     /**
      * ‘int’, ’double’, ’char’ provide the latitude degree, minute (as a
      * decimal), and cardinal description (i.e. ‘N’ or ‘S’), respectively. There
      * are only two possible values for the ‘char’ parameter: ‘N’ for North and
      * ‘S’ for South.
      *
      * @return Returns Latitude value of the sensor.
      */
     public Latitude getLatitude()
     {
         return Latitude;
     }
     protected Longitude Longitude = new Longitude();
 
     /**
      * ‘int’, ’float’, ’char’ provide the longitude degree, minute (as a
      * decimal), and cardinal description (i.e. ‘E’ or ‘W’), respectively. There
      * are only two possible values for the ‘char’ parameter: ‘E’ for East and
      * ‘W’ for West.
      *
      * @return Returns Longitude value of the sensor.
      */
     public Longitude getLongitude()
     {
         return Longitude;
     }
     protected int Fix = 0;
 
     /**
      * ‘int’ indicates whether or not a position was acquired. The fix is the
      * same as the GGA format. Namely, a value of 0 means that the GPS sensor
      * failed to acquire a position and a value of 1 means that a position was
      * acquired.
      *
      * @return Returns 0 - failed to acquire a position or 1 - success.
      */
     public int getFix()
     {
         return Fix;
     }
     //Number of satellites availible
     protected int Satellites = 0;
 
     /**
      * ‘int’ gives the number of satellites tracked by the GPS sensor. This
      * number is an inexplicit source of accuracy. The more satellites are
      * tracked, the higher the position accuracy.
      *
      * @return Returns number of active GPS satellite connections.
      */
     public int getSatellites()
     {
         return Satellites;
     }
     //INS Sensor - Type, Name, Location..., Orientation...
     //a.k.a. The Inertial Navigation Sensor is a sensor that provides estimated location and orientation of the vehicle,
     //based on measurements of angular velocity and linear acceleration relative to the vehicle current pose
     protected List<Location> Locations = new ArrayList<Location>();
 
     /**
      * List of Locations used by RFID sensor to determine where are various RFID
      * Tags or by Victiom sensor to determine locations of bodys. Also used by
      * INS sensor to determine robots location
      *
      * @return Returns list of Locations.
      */
     public List<Location> getLocations()
     {
         return Locations;
     }
     protected List<Rotation> Orientations = new ArrayList<Rotation>();
 
     /**
      * INS sensor entry in USARSim manual v3.1.3 suggests that it should need
      * more than one record of Orienation, but the experience was different.
      * Still we use list of Rotations to support multiple entry. Mainly used for
      * determination of robots Orientation.
      *
      * @return Returns List of Rotations.
      */
     public List<Rotation> getOrientations()
     {
         return Orientations;
     }
     //Encoder sensor - Type, Name/Tick...
     //Sensor name/tick count
     protected Map<String, Integer> EncoderTicks = new HashMap<String, Integer>();
 
     /**
      * String is the sensor name. Integer is the tick count. Example:
      *
      * SEN {Type Encoder} {Name ECLeft Tick -61} {Name ECRight Tick -282} {Name
      * ECTilt Tick 0} {Name ECPan Tick 0}
      *
      * @return Returns Map of Encoder couples String-Integers.
      */
     public Map<String, Integer> getEncoderTicks()
     {
         return EncoderTicks;
     }
     //Touch sensor - Type, Name/Bool...
     //Sensor name/touch count
     protected Map<String, Boolean> Touches = new HashMap<String, Boolean>();
 
     /**
      * String is the sensor name. Boolean indicates whether the sensor is
      * touching something. Value ‘True’ means the sensor is touching something.
      *
      * @return Returns map of touch names and values.
      */
     public Map<String, Boolean> getTouches()
     {
         return Touches;
     }
     //RFID sensor - Type, Name, ID..., Location... //tag I would say, is the ID
     //RFID tags are simulated by implementing the class USARBot.RFIDTag class in the Unreal Editor when editing a map.
     //This class contains an integer id and a bool bSingleshot variable that determines whether the tag is a single shot
     //tag or a multi shot tag. They are deployed by placing them in UnrealEd. If the tag's ID is set to -1(default),
     //then the tag id will be set to a unique value automatically. Other values for the id will not be changed. If the
     //tags are within the MaxRange of the RFID sensor mounted on the robot then the server sends the following message:
     protected List<Integer> IDs = new ArrayList<Integer>();
 
     /**
      * Used by RFID sensor. RFID tags are simulated by implementing the class
      * USARBot.RFIDTag class in the Unreal Editor when editing a map. This class
      * contains an integer id and a boolean bSingleshot variable that determines
      * whether the tag is a single shot tag or a multi shot tag. They are
      * deployed by placing them in UnrealEd. If the tag’s id is set to -1 (the
      * default), then the tag id will be set to a unique value automatically.
      * Other values for the id will not be changed. If the tags are within the
      * MaxRange of the RFID sensor mounted on the robot then the server sends
      * the following message to the client:
      *
      * @return Returns a list of RFID IDs.
      */
     public List<Integer> getIDs()
     {
         return IDs;
     }
     //Victim sensor - Type, Name, PartName..., Location...
     protected List<String> PartNames = new ArrayList<String>();
 
     /**
      * PartName is the name of the victim part that was discovered by the
      * sensor. It can be one of 7 values: “Head”, “Arm”, “Hand”, “Chest”,
      * “Pelvis”, “Leg”, and “Foot”. Please note that the sensor does not
      * differentiate between real victim’s part and false alarms. It is up to
      * the controller to perform this task.
      *
      * @return Returns a list of victim body part names.
      */
     public List<String> getPartNames()
     {
         return PartNames;
 
     }
     //Added after testing - not in the documentation!
     protected String Status = null;
 
     /**
      * Status is not in USARSim manual v3.1.3. And was added to full support of
      * Victim sensor.
      *
      * @return Returns the status of Victim sensor.
      */
     public String getStatus()
     {
         return Status;
     }
     //Human Motion Detector - Type, Name, Prob
     //Probability that it is actual human motion detected
     protected double Prob = 0;
 
     /**
      * Prob is the probability of it’s human motion.
      *
      * @return Returns the probability of Human motion detector.
      */
     public double getProb()
     {
         return Prob;
     }
     //Sound sensor - Type, Name, Loudness, Duration
     protected double Loudness = 0;
 
     /**
      * Loudnes from the sound sensor.
      *
      * @return Returns loundness of a sound.
      */
     public double getLoudness()
     {
         return Loudness;
     }
     protected double Duration = 0;
 
     /**
      * Duration of a sound from the sound sensor.
      *
      * @return Returns a drutaion of a sound.
      */
     public double getDuration()
     {
         return Duration;
     }
     //Tachometer - new type not documented - Type, Name, Velocity, Position
     protected Point4d Velocity = null;
 
     /**
      * Velocity of the robot measured by Tachometer. Not in the USARSim manual
      * v3.1.3.
      *
      * @return Returns velocity from Tachometer.
      */
     public Point4d getVelocity()
     {
         return Velocity;
     }
     protected Point4d Position = null;
 
     /**
      * Position of the robot measured by Tachometer. Not in the USARSim manual
      * v3.1.3.
      *
      * @return Returns position from Tachometer.
      */
     public Point4d getPosition()
     {
         return Position;
     }
     //added after trying DogBot-
     protected Velocity Acceleration = null;
 
     /**
      * Acceleration from Acceleration Sensor. Not in USARSim manual v3.1.3.
      *
      * @return Returns acceleration from accel sensor.
      */
     public Velocity getAcceleration()
     {
         return Acceleration;
     }
     //added after trying StereoP2AT
     protected String Gas = null;
 
     /**
      * Gas sensor measures type and density. Default value is CO2. There is
      * Resolution and Range information offered by GetConf. Not in USARSim
      * manual v3.1.3.
      *
      * @return Returns type of a gas measured by Gas sensor.
      */
     public String getGas()
     {
         return Gas;
     }
     protected double Density = 0;
 
     /**
      * Gas sensor measures type and density. There is Resolution and Range
      * information offered by GetConf. Not in USARSim manual v3.1.3.
      *
      * @return Returns density of a gas measured by Gas sensor.
      */
     public double getDensity()
     {
         return Density;
     }
     //added along with implementing AIBO - ERS sample bot. this is new undocumented sensor for tracking soccer ball from aibo camera
     protected boolean Visible = false;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns wether the soccer ball is visible or not.
      */
     public boolean isVisible()
     {
         return Visible;
     }
     protected Point2d Pos2D = null;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns position on the camera of the soccer ball.
      */
     public Point2d getPos2D()
     {
         return Pos2D;
     }
     protected Location Pos3D = null;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns location of the soccer ball in the environment.
      */
     public Location getPos3D()
     {
         return Pos3D;
     }
     protected double Radius = 0;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns radius of the soccer ball.
      */
     public double getRadius()
     {
         return Radius;
     }
     protected double Dist = 0;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns distance of the soccer ball from the robot.
      */
     public double getDistance()
     {
         return Dist;
     }
     protected double Corona = 0;
 
     /**
      * AIBO - ERS robot uses tracking ball sensor, that recognises a soccer ball
      * from camera. Not in USARSim manual v3.1.3.
      *
      * @return Returns corona of the soccer ball.
      */
     public double getCorona()
     {
         return Corona;
     }
 
     /**
      * Cloning constructor.
      */
     public SensorMessage(SensorMessage original)
     {
         this.EncoderTicks.putAll(original.EncoderTicks);
         this.Type = original.Type;
         this.Time = original.Time;
         this.Name = original.Name;
         this.Duration = original.Duration;
         this.FOV = original.FOV;
         this.Fix = original.Fix;
         this.Loudness = original.Loudness;
         this.Prob = original.Prob;
         this.Resolution = original.Resolution;
         this.Satellites = original.Satellites;
         this.IDs.addAll(original.IDs);
         this.LaserRanges.addAll(original.LaserRanges);
         this.Locations.addAll(original.Locations);
         this.Longitude = original.Longitude;
         this.Latitude = original.Latitude;
         this.OdoPose = original.OdoPose;
         this.Orientations.addAll(original.Orientations);
         this.PartNames.addAll(original.PartNames);
         this.Position = original.Position;
         this.RangeRanges.putAll(original.RangeRanges);
         this.Touches.putAll(original.Touches);
         this.Velocity = original.Velocity;
         this.Acceleration = original.Acceleration;
         this.Status = original.Status;
         this.Density = original.Density;
         this.Gas = original.Gas;
         this.Visible = original.Visible;
         this.Pos2D = original.Pos2D;
         this.Pos3D = original.Pos3D;
         this.Radius = original.Radius;
         this.Dist = original.Dist;
         this.Corona = original.Corona;
     }
 
     /**
      * Used by Yylex to create empty message then to fill it's protected fields
      * (Yylex is in the same package).
      */
     public SensorMessage()
     {
     }
 
     @Override
     public String toString()
     {
         StringBuilder buf = new StringBuilder();
         //"SEN {Type text} {Name 0} {Duration 0} {FOV 0} {Fix 0} {Loudness 0} {Prob 0} {Resolution 0} {Satellites 0}";
         buf.append(super.toString() + " | "
                 + "Type = "
                 + String.valueOf(Type) + " | "
                 + "Time = "
                 + String.valueOf(Time) + " | "
                 + "Name = "
                 + String.valueOf(Name) + " | "
                 + "Duration = "
                 + String.valueOf(Duration) + " | "
                 + "FOV = "
                 + String.valueOf(FOV) + " | "
                 + "Fix = "
                 + String.valueOf(Fix) + " | "
                 + "Loudness = "
                 + String.valueOf(Loudness) + " | "
                 + "Prob = "
                 + String.valueOf(Prob) + " | "
                 + "Resolution = "
                 + String.valueOf(Resolution) + " | "
                 + "Satellites = "
                 + String.valueOf(Satellites) + " | "
                 + "Status = "
                 + String.valueOf(Status) + " | "
                 + "Gas = "
                 + String.valueOf(Gas) + " | "
                 + "Density = "
                 + String.valueOf(Density) + " | "
                 + "Radius = "
                 + String.valueOf(Radius) + " | "
                 + "Corona = "
                 + String.valueOf(Corona) + " | "
                 + "Visible = "
                 + String.valueOf(Visible) + " | "
                 + "Dist = "
                 + String.valueOf(Dist) + " | ");
 
         if(!RangeRanges.isEmpty())
         {
             Iterator it = RangeRanges.entrySet().iterator();
             while(it.hasNext())
             {
                 Map.Entry en = (Map.Entry) it.next();
                 buf.append(" ").append(en.getKey()).append(" ").append(en.getValue()).append(",");
             }
             buf.append(" | ");
         }
 
 
         if(!Touches.isEmpty())
         {
             Iterator it = Touches.entrySet().iterator();
             while(it.hasNext())
             {
                 Map.Entry en = (Map.Entry) it.next();
                 buf.append(" ").append(en.getKey()).append(" = ").append(en.getValue()).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(Velocity != null)
         {
             buf.append("Velocity = ").append(String.valueOf(Velocity.w)).append(String.valueOf(Velocity.x)).append(String.valueOf(Velocity.y)).append(String.valueOf(Velocity.z)).append(" | ");
         }
 
         if(Acceleration != null)
         {
             buf.append("Acceleration = ").append(String.valueOf(Acceleration.x)).append(String.valueOf(Acceleration.y)).append(String.valueOf(Acceleration.z)).append(" | ");
         }
 
         if(!IDs.isEmpty())
         {
             for(Integer i : IDs)
             {
                 buf.append("ID = ").append(i.toString()).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(!LaserRanges.isEmpty())
         {
             buf.append("LaserRange = ");
             for(Double i : LaserRanges)
             {
                 buf.append(i.toString()).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(!Locations.isEmpty())
         {
             for(Location i : Locations)
             {
                 buf.append("Location = ").append(i.x).append(" ").append(i.y).append(" ").append(i.z).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(!Orientations.isEmpty())
         {
             for(Rotation i : Orientations)
             {
                 buf.append("Orientation = ").append(i.yaw).append(" ").append(i.roll).append(" ").append(i.pitch).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(Longitude != null)
         {
             buf.append("Longitude = ").append(String.valueOf(Longitude.getDegree())).append(" ").append(String.valueOf(Longitude.getMinute())).append(" ").append(String.valueOf(Longitude.getCardinal() + " | "));
         }
 
         if(Latitude != null)
         {
             buf.append("Latitude = ").append(String.valueOf(Latitude.getDegree())).append(" ").append(String.valueOf(Latitude.getMinute())).append(" ").append(String.valueOf(Latitude.getCardinal() + " | "));
         }
 
         if(OdoPose != null)
         {
             buf.append("OdoPose = ").append(String.valueOf(OdoPose.getX())).append(" ").append(String.valueOf(OdoPose.getY())).append(" ").append(String.valueOf(OdoPose.getTheta() + " | "));
         }
 
         if(!PartNames.isEmpty())
         {
             for(String i : PartNames)
             {
                 buf.append("PartNames = ").append(i).append(" ");
             }
             buf.append(" | ");
         }
 
 
         if(Position != null)
         {
             buf.append("Position = ").append(String.valueOf(Position.w)).append(String.valueOf(Position.x)).append(String.valueOf(Position.y)).append(String.valueOf(Position.z)).append(" | ");
         }
 
         if(Pos3D != null)
         {
             buf.append("Pos3D = ").append(String.valueOf(Pos3D.x)).append(String.valueOf(Pos3D.y)).append(String.valueOf(Pos3D.z)).append(" | ");
         }
         if(Pos2D != null)
         {
             buf.append("Pos2D = ").append(String.valueOf(Pos2D.x)).append(String.valueOf(Pos2D.y)).append(" | ");
         }
 
         return buf.toString();
 
     }
 
     /**
      * Gets all properties and values to create a HTML formated string;
      *
      * @return Returns all properties in HTML format
      */
     public String toHtmlString()
     {
         StringBuilder buf = new StringBuilder();
         buf.append(super.toString()
                 + "<b>Type</b> : "
                 + String.valueOf(Type)
                 + " <br/> "
                 + "<b>Time</b> : "
                 + String.valueOf(Time)
                 + " <br/> "
                 + "<b>Name</b> : "
                 + String.valueOf(Name)
                 + " <br/> "
                 + "<b>Duration</b> : "
                 + String.valueOf(Duration)
                 + " <br/> "
                 + "<b>FOV</b> : "
                 + String.valueOf(FOV)
                 + " <br/> "
                 + "<b>Fix</b> : "
                 + String.valueOf(Fix)
                 + " <br/> "
                 + "<b>Loudness</b> : "
                 + String.valueOf(Loudness)
                 + " <br/> "
                 + "<b>Prob</b> : "
                 + String.valueOf(Prob)
                 + " <br/> "
                 + "<b>Resolution</b> : "
                 + String.valueOf(Resolution)
                 + " <br/> "
                 + "<b>Satellites</b> : "
                 + String.valueOf(Satellites)
                 + " <br/> "
                 + "<b>Status</b> : "
                 + String.valueOf(Status)
                 + " <br/> "
                 + "<b>Gas</b> : "
                 + String.valueOf(Gas)
                 + " <br/> "
                 + "<b>Density</b> : "
                 + String.valueOf(Density)
                 + " <br/> "
                 + "<b>Dist</b> : "
                 + String.valueOf(Dist)
                 + " <br/> "
                 + "<b>Radius</b> : "
                 + String.valueOf(Radius)
                 + " <br/> "
                 + "<b>Corona</b> : "
                 + String.valueOf(Corona)
                 + " <br/> "
                 + "<b>Visible</b> : "
                 + String.valueOf(Visible)
                 + " <br/> ");
 
         if(!RangeRanges.isEmpty())
         {
             Iterator it = RangeRanges.entrySet().iterator();
             while(it.hasNext())
             {
                 Map.Entry en = (Map.Entry) it.next();
                 buf.append(" ").append(en.getKey()).append(" ").append(en.getValue()).append(" <br/> ");
             }
         }
 
         if(!Touches.isEmpty())
         {
             Iterator it = Touches.entrySet().iterator();
             while(it.hasNext())
             {
                 Map.Entry en = (Map.Entry) it.next();
                 buf.append(" ").append(en.getKey()).append(" = ").append(en.getValue()).append(" <br/> ");
             }
         }
 
         if(Velocity != null)
         {
             buf.append("<b>Velocity</b> : ").append(String.valueOf(Velocity.w)).append(String.valueOf(Velocity.x)).append(String.valueOf(Velocity.y)).append(String.valueOf(Velocity.z)).append(" <br/> ");
         }
 
         if(Acceleration != null)
         {
             buf.append("<b>Acceleration</b> : ").append(String.valueOf(Acceleration.x)).append(String.valueOf(Acceleration.y)).append(String.valueOf(Acceleration.z)).append(" <br/> ");
         }
 
         if(!IDs.isEmpty())
         {
             for(Integer i : IDs)
             {
                 buf.append("<b>ID</b> : ").append(i.toString()).append(" <br/>  ");
             }
         }
 
         if(!LaserRanges.isEmpty())
         {
             buf.append("<b>LaserRange</b> : ");
             for(Double i : LaserRanges)
             {
                 buf.append(i.toString()).append(" <br/>  ");
             }
         }
 
         if(!Locations.isEmpty())
         {
             for(Location i : Locations)
             {
                 buf.append("<b>Location</b> : ").append(i.x).append(" ").append(i.y).append(" ").append(i.z).append(" <br/>  ");
             }
         }
 
         if(!Orientations.isEmpty())
         {
             for(Rotation i : Orientations)
             {
                 buf.append("<b>Orientation</b> : ").append(i.yaw).append(" ").append(i.roll).append(" ").append(i.pitch).append(" <br/>  ");
             }
         }
         if(Longitude != null)
         {
             buf.append("<b>Longitude</b> : ").append(String.valueOf(Longitude.getDegree())).append(" ").append(String.valueOf(Longitude.getMinute())).append(" ").append(String.valueOf(Longitude.getCardinal() + " <br/> "));
         }
 
         if(Latitude != null)
         {
             buf.append("<b>Latitude</b> : ").append(String.valueOf(Latitude.getDegree())).append(" ").append(String.valueOf(Latitude.getMinute())).append(" ").append(String.valueOf(Latitude.getCardinal() + " <br/>  "));
         }
 
         if(OdoPose != null)
         {
             buf.append("<b>OdoPose</b> : ").append(String.valueOf(OdoPose.getX())).append(" ").append(String.valueOf(OdoPose.getY())).append(" ").append(String.valueOf(OdoPose.getTheta() + " <br/> "));
         }
 
         if(!PartNames.isEmpty())
         {
             for(String i : PartNames)
             {
                 buf.append("<b>PartNames</b> : ").append(i).append(" <br/> ");
             }
         }
 
         if(Position != null)
         {
             buf.append("<b>Position</b> : ").append(String.valueOf(Position.w)).append(String.valueOf(Position.x)).append(String.valueOf(Position.y)).append(String.valueOf(Position.z)).append(" <br/> ");
         }
         if(Pos3D != null)
         {
             buf.append("<b>Pos3D</b> : ").append(String.valueOf(Pos3D.x)).append(String.valueOf(Pos3D.y)).append(String.valueOf(Pos3D.z)).append(" <br/> ");
         }
         if(Pos2D != null)
         {
             buf.append("<b>Pos2D</b> : ").append(String.valueOf(Pos2D.x)).append(String.valueOf(Pos2D.y)).append(" <br/> ");
         }
 
         return buf.toString();
     }
 }