package lejos.robotics.proposal;

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

/*
 * 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.
 */

/**
 * A PoseProvider that keeps track of coordinates using dead reckoning, by monitoring Pilot movements.
 * 
 * Question: What about a robot that is helped along with a Compass? Should the compass go in a DeadReckoner
 *  constructor, or a Pilot constructor? The PoseProvider seems more logical but Lawrie wants it to only
 *  accept a Pilot. 
 * 
 * Provisional name: DeadReckonerPoseProvider
 * Alternate names:  DeadReckonerPoseProvider, DeadReckoner, OrienteeringPoseProvider, OdometryPoseProvider 
 * 
 */
public class DeadReckonerPoseProvider implements MoveListener, PoseProvider {
	private float x = 0, y = 0, heading = 0;
	private float angle0, distance0;
	MovementProvider mp;
	boolean current = true;
	
	/**
	 * Internally, the constructor  listens to movements from the Pilot. This allows it to keep
	 * track of all vector movements made.
	 * 
	 * @param mp the movement provider
	 */
	public DeadReckonerPoseProvider(MovementProvider mp) {
		mp.addMoveListener(this);
	}
		
	public Pose getPose() {
		if (!current) updatePose(mp.getMovement());
		return new Pose(x,y,heading);
	}

	public void movementStarted(Movement event, MovementProvider mp) {
	    angle0 = 0;
	    distance0 = 0;
	    current = false;
	    this.mp = mp;;
	}

	public void movementStopped(Movement event, MovementProvider mp) {
		updatePose(event);
	}
	
	/*
	 * Update the pose with the movement that has occurred since the 
	 * movementStarted even
	 */
	private void updatePose(Movement event) {
		float angle = event.getAngleTurned() - angle0;
		float distance = event.getDistanceTraveled() - distance0;
        double dx = 0, dy = 0;
        double headingRad = (Math.toRadians(heading));
        //MovementType type = event.getMovementType();
        
        if (Math.abs(angle) > .5) { // rotate or arc
            double turnRad = Math.toRadians(angle);
            double radius = event.getArcRadius(); // zero for rotate
            if (radius != 0) {
            	dy = radius * (Math.cos(headingRad) - Math.cos(headingRad + turnRad));
            	dx = radius * (Math.sin(headingRad + turnRad) - Math.sin(headingRad));
            }
        } else if (Math.abs(distance) > .01) { // travel
            dx = (distance) * (float) Math.cos(headingRad);
            dy = (distance) * (float) Math.sin(headingRad);
        }
        
        heading = normalize(heading + angle); // keep angle between -180 and 180
        x += dx;
        y += dy;
        angle0 = event.getAngleTurned();
        distance0 = event.getDistanceTraveled();
        current = !event.isMoving();		
	}

    /*
     * returns equivalent angle between -180 and +180
     */
    private float normalize(float angle) {
        float a = angle;
        while (a > 180) {
            a -= 360;
        }
        while (a < -180) {
            a += 360;
        }
        return a;
    }
    
    public void setPosition(Point p) {
    	x = p.x;
    	y = p.y;
    	current = true;
    }
    
    void setHeading(float heading) {
    	this.heading = heading;
    	current = true;
    }
}