package lejos.robotics.proposal;

/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

import lejos.geom.Point;
import lejos.robotics.MoveListener;
import lejos.robotics.Movement;
import lejos.robotics.MovementProvider;
import lejos.robotics.Pose;



/**
 * Experimental Pose
 * Represents the location and heading(direction angle) of a robot.<br>
 * This class includes  methods for updating the UpdateablePose to track common robot movements.
 * It will updates itself for every move of a Pilot if it registers as a MoveListener.
 * The Pilot must implement the MovementProvider interface.
 * It can report the current pose at any time, even while a move is in progress.
 * @author Roger Glassey
 */
public class UpdateablePose extends Pose implements MoveListener
{
  /**
   * allocate a new UpdateablePose at the origin, heading  = 0:the direction  the positive X axis
   */
public UpdateablePose()
{
  super();
}
/**
 * Allocate a new pose at location (x,y) with specified heading in degrees.
 * @param x
 * @param y
 * @param heading
 */
public UpdateablePose(float x, float y, float heading)
{
  super(x,y,heading);
}

public  void  movementStarted(Movement move, MovementProvider p)
  {
    _current = false;
    _angle0 = 0;
    _distance0 = 0;
    pilot =  p;
  }

	public  void movementStopped(Movement move, MovementProvider p)
    {
      update(move);
    }
/**
 * Updates can occur while the robot is moving.
 * Update of pose required the change in angle and distance since last update.
 * Pilot returns angle and distance since the start of movement.
 * UpdateablePose uses angle0 and distance0 to calculate the change since last update.
 * */
    protected  synchronized void  update(Movement move)
    {

      float angle = move.getAngleTurned();
      float distance = move.getDistanceTraveled();

      Movement.MovementType type = move.getMovementType();
       if(type == Movement.MovementType.ROTATE)
      {
        rotateUpdate(angle -_angle0);
      }
      else if(type == Movement.MovementType.TRAVEL)
      {
        moveUpdate(distance - _distance0);
      }
      else if(type == Movement.MovementType.ARC)
      {
        arcUpdate(distance - _distance0, angle -_angle0) ;
      }
      _current = move.isMoving();
      _angle0 = angle;
      _distance0 = distance;
    }

/**
 * returns the heading (direction angle) of the UpdateablePose
 * @return
 */
public float getHeading()
{ 
  if(!_current) update(pilot.getMovement());
   return _heading ;
}
/**
 * return X coordinate
 * @return
 */
public  float getX()
{
  if(!_current) update(pilot.getMovement());
  return (float) _location.getX();}
/**
 * return Y coordinate
 * @return
 */
public   float getY()
{
  if(!_current) update(pilot.getMovement());
  return (float)_location.getY();
}
/**
 * return the location as a Point
 * @return
 */
public Point getLocation()
{ 
  if(!_current) update(pilot.getMovement());
  return _location;
}

public void setLocation(Point p)
{
  _location = p;
  _current = true;
}
public void setHeading(float heading )
{
  _heading = heading;
  _current = true;
}
public void setPilot(MovementProvider aPilot)
{
  pilot =  aPilot;
}
public boolean isCurrent() { return _current;}

protected  boolean _current = true; //pose is up to date
protected  MovementProvider pilot;
protected float _angle0;
protected float _distance0;


}