package cz.cuni.amis.pogamut.usar2004.communication.messages.datatypes;
   
   import java.util.ArrayList;
   import java.util.List;
   
   /**
    *
    * LINK parameter gives the link number that will be described by the next two
    * parameters (Value and Torque). Please note that you will have as many as
   * these parameters as you have links. *
   *
   * VALUE parameter has two possible meanings. If the link being described is a
   * prismatic joint, double gives the distance (in meters) from the original
   * position of the link. If the link being described is a revolute joint, it
   * gives a sector(probably).
   *
   * The TORQUE parameter gives the current torque of the link being described. *
   * All of the above are collected into one data structure below this text.
   *
   * @author vejmanm
   *
   */
  public class LinkStateSet
  {
      private List<Integer> links = new ArrayList<Integer>();
      private List<Double> values = new ArrayList<Double>();
      private List<Double> torques = new ArrayList<Double>();
  
      public LinkStateSet()
      {
      }
  
      public void add(int l, double v, double t)
      {
          links.add(l);
          values.add(v);
          torques.add(t);
      }
  
      public void add(LinkState ls)
      {
          links.add(ls.getLink());
          values.add(ls.getValue());
          torques.add(ls.getTorque());
      }
  
      public void addLink(int l)
      {
          links.add(l);
      }
  
      public void addValue(double v)
      {
          values.add(v);
      }
  
      public void addTorque(double t)
      {
          torques.add(t);
      }
  
      public List<LinkState> GetLinkStateList()
      {
          try
          {
              if(links.size() != values.size() || links.size() != torques.size())
              {
                  throw new Exception("Could not convert! Arrays are of different sizes!");
              }
  
              List<LinkState> returnee = new ArrayList<LinkState>();
              for(int i = 0; i < links.size(); i++)
              {
                  LinkState l = new LinkState(links.get(i), values.get(i), torques.get(i));
                  returnee.add(l);
              }
              return returnee;
          }
          catch(Exception e)
          {
              return null;
          }
      }
  
      public boolean isEmpty()
      {
          return links.isEmpty();
      }
  }