package Carmen;

import java.util.*;
import IPC.*;
/** Carmen class for  a Robot */
public class Robot {
  /** Event Handler receives this and can set its fields */
  public static class BaseResetMessage extends Message {
    private static final String CARMEN_BASE_RESET_COMMAND_NAME = 
      "carmen_base_reset_command";
    private static final String CARMEN_BASE_RESET_COMMAND_FMT = 
      "{double,string}";
      
    public void publish() {
      publish(CARMEN_BASE_RESET_COMMAND_NAME, 
	      CARMEN_BASE_RESET_COMMAND_FMT, this);
    }
  }


  /** Event Handler receives this and can set its fields */
  public static class RobotVelocityMessage extends Message {
    public double tv, rv;

    private static final String CARMEN_ROBOT_VELOCITY_NAME = 
      "carmen_robot_vel";
    private static final String CARMEN_ROBOT_VELOCITY_FMT = 
    "{double,double,double,string}";

    /** Event Handler receives this and can set its fields */
    RobotVelocityMessage(double tv, double rv) {
      this.tv = tv;
      this.rv = rv;
    }

    public void publish() {
      publish(CARMEN_ROBOT_VELOCITY_NAME, CARMEN_ROBOT_VELOCITY_FMT, this);
    }
  }


  /** Event Handler receives this and can set its fields */
  public static class RobotVectorMoveMessage extends Message {
    public double distance, theta;

    private static final String CARMEN_ROBOT_VECTOR_MOVE_NAME = 
      "carmen_robot_vector_move";
    private static final String CARMEN_ROBOT_VECTOR_MOVE_FMT = 
      "{double,double,double,string}";
    
    RobotVectorMoveMessage(double distance, double theta) {
      this.distance = distance;
      this.theta = theta;
    }

    public void publish() {
      publish(CARMEN_ROBOT_VECTOR_MOVE_NAME, CARMEN_ROBOT_VECTOR_MOVE_FMT, 
	      this);
    }
  }

  /** Application module calls this class method to direct the robot
   * at stated translational velocity (m/s) and rotational velocity (r/s).
   * This results in a message via IPC to the Orc_daemon then out to the 
   * robot base. To view how the message is processed in more detail, see
   * the Carmen c-code base in subdirectory orc_daemon file orclib.c and
   * procedure: carmen_base_direct_set_velocity 
   */
  public static void setVelocity(double tv, double rv) {
    RobotVelocityMessage velocityMessage = new RobotVelocityMessage(tv, rv);
    velocityMessage.publish();
  }

  /**
   * <p>Application module calls this class method to direct the robot to
   * proceed in a vector stated by distance and orientation.</p>
   *
   * <p>If <code>distance</code> is zero, and the robot is capable of turning
   * in place (TBD: and otherwise?), then the robot will just rotate CCW by
   * <code>theta</code> radians <i>relative to current orientation in world
   * frame</i>.</p>
   *
   * <p>Otherwise, define a point P with polar coordinates
   * (<code>distance</code>, <code>theta</code>) <i>relative to current robot
   * frame</i>, aim the robot along the vector from its current position
   * towards P (first), then drive to P, then stop when the current orientation
   * is within 5*pi/180 radians and current position along the vector is within
   * <code>robot_min_approach_dist</code> (a configurable Carmen parameter) of
   * P.</p>
   *
   * <p>The motion happens asynchronously (i.e. this method does not block, and
   * when it returns the motion will likely not yet be complete).  There is no
   * semantic way to determine that the motion has completed, though Carmen
   * does eventually disable its internal feedback controllers which drive the
   * motion, according to the termination conditions above.  If you need to
   * know when the motion has completed, you can watch the odometry for the
   * same conditions.</p>
   *
   * <p>Explicitly calling {@link #setVelocity} while any
   * <code>moveAlongVector()</code> is ongoing tacitly cancels the latter.</p>
   *
   * @param distance the distance to move in meters (see above).  Negative
   * input is handled correctly.
   * @param theta the angle to rotate in radians.  TBD is negative handled
   * correctly?
   **/
   public static void moveAlongVector(double distance, double theta) {
    RobotVectorMoveMessage vectorMessage = 
      new RobotVectorMoveMessage(distance, theta);
    vectorMessage.publish();
  }

/** Application module calls this class method to reset the 
   * robot base to the initialized state.
   */
   public static void resetRobotBase() {
    BaseResetMessage resetMessage = new BaseResetMessage();
    resetMessage.publish();
  }


  public static void initialize(String moduleName, String SBCName)
  {
    IPC.connectModule(moduleName, SBCName);
  }

  public static void dispatch()
  {
    IPC.dispatch();
  }

  // Dispatch but only for as long as timeoutMSecs 

  public static int listen (long timeoutMSecs) {
    return IPC.listen(timeoutMSecs);
  }

}