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.worldview.object.*;
   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.*;
  
  /**
   *
   * Geometry message reports the robot's Geometry information. Please note that
   * the robot Geometry message parameters depend on the type of robot that you
   * are driving. For example, a robot of type “GroundVehicle” will not have the
   * same Geometry message as a robot of type “AerialVehicle.”
   *
   * Corresponding GameBots message is GEO.
   *
   * NOTE: An effecter can implement more than one opcode and the CONF message
   * returns the configuration information for all effecters on the robotic
   * platform. Therefore the segment with the name value pair indicates a new
   * effecter and the opcode name value pair indicates the opcode the effecter
   * specified in by name implements.
   *
   *
   * NOTE: For revolute joints, if the MinRange parameter is greater than the
   * MaxRange parameter, the joint does not have any constraints.
   */
  public class GeometryMessage extends GBEvent implements IWorldEvent, IWorldChangeEvent
  {
      public GeometryMessage(String Type, String Name, double WheelBase, double WheelRadius, double WheelSeparation, Point3d COG, Vector3d Dimensions)
      {
          this.Type = Type;
          this.Name = Name;
          this.WheelBase = WheelBase;
          this.WheelRadius = WheelRadius;
          this.WheelSeparation = WheelSeparation;
          this.CenterOfGravity = COG;
          this.Dimensions = Dimensions;
      }
      /**
       * Example how the message looks like - used during parser tests.
       */
      public static final String PROTOTYPE = "GEO {Type text} {Name text} {WheelBase 0} {WheelRadius 0} {WheelSeparation 0} {CenterOfGravity 0,0,0} {Dimensions 0,0,0}";
      /////// Properties BEGIN
      protected String Type = null;
  
      /**
       * Type describes the vehicle type. It will be one of the following values:
       * "GroundVehicle”, “LeggedRobot”, “NauticVehicle”, or “AerialVehicle” of
       * "Camera" or some kind of sensor or effecter or "MisPkg".
       *
       * @return Returns type of the geometry message.
       */
      public String getType()
      {
          return Type;
      }
      //Ground Vehicle - Type, Name Dimensions, COG, WheelRadius, WheelSeparation, WheelBase
      protected String Name = null;
  
      /**
       * Name of the subject that this geometry message concerns.
       *
       * @return Returns the name of the Geometry item.
       */
      public String getName()
      {
          return Name;
      }
      protected Vector3d Dimensions = null;
  
      /**
       * Dimension (x,y,z) ‘x’ defines the robot’s length, ‘y’ defines the robot’s
       * width, and ‘z’ describes the robot’s height. Please note that these
       * values are in meters.
       *
       * @return Returns Dimension of the robot.
       */
      public Vector3d getDimensions()
      {
          return Dimensions;
      }
      protected Point3d CenterOfGravity = null;
  
      /**
       * COG(x,y,z) ‘x’, ‘y’, and ‘z’ identify the position of the center of
       * gravity, in meters, calculated from the chassis origin.
       *
       * @return Returns center of gravity of the robot
       */
      public Point3d getCenterOfGravity()
      {
          return CenterOfGravity;
      }
      protected double WheelRadius = 0;
  
     /**
      * The value is the radius of the robot’s wheels, in meters
      *
      * @return Returns wheel radius of the robot.
      */
     public double getWheelRadius()
     {
         return WheelRadius;
     }
     protected double WheelSeparation = 0;
 
     /**
      * The value is the wheel separation, in meters. The wheel separation
      * defines the distance between two wheels along the length (x axis) of the
      * robot’s chassis.
      *
      * @return Returns wheel separation of the robot.
      */
     public double getWheelSeparation()
     {
         return WheelSeparation;
     }
     protected double WheelBase = 0;
 
     /**
      * The value is the wheel base, in meters. The wheel base defines the
      * distance between two wheels along the width (y axis) of the robot’s
      * chassis
      *
      * @return Retursn wheel base of the robot.
      */
     public double getWheelBase()
     {
         return WheelBase;
     }
     //Legged Robot - (still to be expanded) - Type, Name, Dimensions, COG
     //Nautic Vehicle - (still to be expanded) - Type, Name, Dimensions, COG
     //Aerial Vehicle - (still to be expanded) - Type, Name, Dimensions, COG
     //Sensor/effecter - Type Name/Location/Orientation/Mount...
     //Specifies how the sensor is mounted on the robot
     protected List<SensorMount> SenEffGeo = new ArrayList<SensorMount>();
 
     /**
      * Camera, Sensor and effecter mounts are composed from Name, Location,
      * Orientaion and Mount name.
      *
      * Example: GEO {Type Camera} {Name Camera Location 0.0820,0.0002,0.0613
      * Orientation 0.0000,-0.0000,0.0000 Mount CameraTilt}
      *
      * @return Returns a collection of sensor mounts.
      */
     public List<SensorMount> getSenEffGeo()
     {
         return SenEffGeo;
     }
     //Mission Package - Type, Name, Link..., ParentLink..., Location..., Orientation...
     protected List<Integer> Links = new ArrayList<Integer>();
 
     /**
      * For a mission package, the geometry message is expressed in the following
      * format to tell us how the mission package and its elements are
      * ‘installed’ together to the robot.
      *
      * Example: GEO {Type MisPkg} {Name CameraPanTilt} {Link 1} {ParentLink -1}
      * {Location 0.1239,0.0000,-0.2036} {Orientation 3.1415,0.0000,0.0000} {Link
      * 2} {ParentLink 1} {Location 0.0000,0.0000,0.0599} {Orientation
      * 1.5707,0.0000,0.0000}
      *
      * @return Returns List of links.
      */
     public List<Integer> getLinks()
     {
         return Links;
     }
     protected List<Integer> ParentLinks = new ArrayList<Integer>();
 
     /**
      * For a mission package, the geometry message is expressed in the following
      * format to tell us how the mission package and its elements are
      * ‘installed’ together to the robot.
      *
      * Example: GEO {Type MisPkg} {Name CameraPanTilt} {Link 1} {ParentLink -1}
      * {Location 0.1239,0.0000,-0.2036} {Orientation 3.1415,0.0000,0.0000} {Link
      * 2} {ParentLink 1} {Location 0.0000,0.0000,0.0599} {Orientation
      * 1.5707,0.0000,0.0000}
      *
      * @return Returns list of parent links.
      */
     public List<Integer> getParentLinks()
     {
         return ParentLinks;
     }
     protected List<Location> Locations = new ArrayList<Location>();
 
     /**
      * For a mission package, the geometry message is expressed in the following
      * format to tell us how the mission package and its elements are
      * ‘installed’ together to the robot.
      *
      * Example: GEO {Type MisPkg} {Name CameraPanTilt} {Link 1} {ParentLink -1}
      * {Location 0.1239,0.0000,-0.2036} {Orientation 3.1415,0.0000,0.0000} {Link
      * 2} {ParentLink 1} {Location 0.0000,0.0000,0.0599} {Orientation
      * 1.5707,0.0000,0.0000}
      *
      * @return Return list of Locations.
      */
     public List<Location> getLocations()
     {
         return Locations;
     }
     protected List<Rotation> Orientations = new ArrayList<Rotation>();
 
     /**
      * For a mission package, the geometry message is expressed in the following
      * format to tell us how the mission package and its elements are
      * ‘installed’ together to the robot.
      *
      * Example: GEO {Type MisPkg} {Name CameraPanTilt} {Link 1} {ParentLink -1}
      * {Location 0.1239,0.0000,-0.2036} {Orientation 3.1415,0.0000,0.0000} {Link
      * 2} {ParentLink 1} {Location 0.0000,0.0000,0.0599} {Orientation
      * 1.5707,0.0000,0.0000}
      *
      * @return Returns list of Orientations.
      */
     public List<Rotation> getOrientations()
     {
         return Orientations;
     }
 
     /**
      * Cloning constructor.
      */
     public GeometryMessage(GeometryMessage original)
     {
         this.Type = original.Type;
         this.CenterOfGravity = original.CenterOfGravity;
         this.Dimensions = original.Dimensions;
         this.Links.addAll(original.Links);
         this.Locations.addAll(original.Locations);
         this.Name = original.Name;
         this.Orientations.addAll(original.Orientations);
         this.ParentLinks.addAll(original.ParentLinks);
         this.SenEffGeo.addAll(original.SenEffGeo);
         this.WheelBase = original.WheelBase;
         this.WheelRadius = original.WheelRadius;
         this.WheelSeparation = original.WheelSeparation;
     }
 
     /**
      * Used by Yylex to create empty message then to fill it's protected fields
      * (Yylex is in the same package).
      */
     public GeometryMessage()
     {
     }
     /**
      * Here we save the original object for which this object is an update.
      */
     private IWorldObject orig = null;
 
     @Override
     public String toString()
     {
 
         StringBuilder buf = new StringBuilder();
         buf.append(super.toString() + " | "
                 + "Type = "
                 + String.valueOf(Type) + " | "
                 + "Name = "
                 + String.valueOf(Name) + " | "
                 + "WheelBase = "
                 + String.valueOf(WheelBase) + " | "
                 + "WheelRadius = "
                 + String.valueOf(WheelRadius) + " | "
                 + "WheelSeparation = "
                 + String.valueOf(WheelSeparation) + " | ");
         if(Dimensions != null)
         {
             buf.append("Dimensions = ").append(String.valueOf(Dimensions)).append(" | ");
         }
 
 
         if(CenterOfGravity != null)
         {
             StringBuilder append = buf.append("CenterOfGravity = ").append(String.valueOf(CenterOfGravity)).append(" | ");
         }
         if(!Links.isEmpty())
         {
             for(Integer i : Links)
             {
                 buf.append("Link = ").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(!ParentLinks.isEmpty())
         {
             for(Integer i : ParentLinks)
             {
                 buf.append("ParentLink = ").append(i.toString()).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(!SenEffGeo.isEmpty())
         {
             buf.append("Sensor/Effecter Geo: ");
             for(SensorMount i : SenEffGeo)
             {
                 buf.append("Location = ").append(i.getLocation().x).append(",").append(i.getLocation().y).append(",").append(i.getLocation().z).append(" ");
                 buf.append("Name = ").append(i.getName()).append(" ");
                 buf.append("Mount = ").append(i.getMount()).append(" ");
                 buf.append("Orientation = ").append(i.getOrientation().yaw).append(",").append(i.getOrientation().roll).append(",").append(i.getOrientation().pitch).append(" ");
             }
             buf.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>Name</b> : "
                 + String.valueOf(Name)
                 + " <br/> "
                 + "<b>WheelBase</b> : "
                 + String.valueOf(WheelBase)
                 + " <br/> "
                 + "<b>WheelRadius</b> : "
                 + String.valueOf(WheelRadius)
                 + " <br/> "
                 + "<b>WheelSeparation</b> : "
                 + String.valueOf(WheelSeparation)
                 + " <br/> ");
 
         if(Dimensions != null)
         {
             buf.append("<b>Dimensions</b> : ").append(String.valueOf(Dimensions)).append(" <br/> ");
 
         }
 
         if(CenterOfGravity != null)
         {
             buf.append("<b>CenterOfGravity</b> : ").append(String.valueOf(CenterOfGravity)).append(" <br/> ");
         }
 
         if(!Links.isEmpty())
         {
             for(Integer i : Links)
             {
                 buf.append("<b>Link</b> : ").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(!ParentLinks.isEmpty())
         {
             for(Integer i : ParentLinks)
             {
                 buf.append("<b>ParentLink</b> : ").append(i.toString()).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(!SenEffGeo.isEmpty())
         {
             buf.append("<b>Sensor/Effecter Geo</b> : ");
             for(SensorMount i : SenEffGeo)
             {
                 buf.append("<b>Location</b> : ").append(i.getLocation().x).append(",").append(i.getLocation().y).append(",").append(i.getLocation().z).append(" <br/> ");
                 buf.append("<b>Name</b> : ").append(i.getName()).append(" <br/> ");
                 buf.append("<b>Mount</b> : ").append(i.getMount()).append(" <br/> ");
                 buf.append("<b>Orientation</b> : ").append(i.getOrientation().yaw).append(",").append(i.getOrientation().roll).append(",").append(i.getOrientation().pitch).append(" <br/> ");
             }
         }
 
         return buf.toString();
     }
 
     @Override
     public long getSimTime()
     {
         throw new UnsupportedOperationException("Not supported yet.");
     }
 }