package roomba.localization.camloc.travelgraph;

import roomba.localization.camloc.RoombaPosition;

import java.util.Vector;

public class Path{
    
    Vector edges = new Vector();

    public Path(){
    }

    /**
     * Edges should be added in reverse order.  For example, if a path
     * should travel through edges A then B then C, C should be added
     * to the path first
     **/
    public void addEdge(PathEdge edge){
	edges.add(0,edge);
    }

    public double getLastHeading(){
	if (edges.size() > 0){
	    return ((PathEdge)edges.get(0)).getHeading();
	} else{
	    return 0.0;
	}
    }

    public double getTotalDistance(){
	double distance = 0.0;
	for (int i=0;i<edges.size();i++){
	    distance += ((PathEdge)edges.get(i)).getDistance();
	}
	return distance;
    }

    public RoombaPosition getFinalPosition(RoombaPosition initialPosition){
	RoombaPosition currentPosition = new RoombaPosition
	    (initialPosition.getX(),
	     initialPosition.getY(),
	     initialPosition.getHeading(),
	     initialPosition.getConfidence());

	for (int i=0;i<edges.size();i++){
	    PathEdge edge = (PathEdge)edges.get(i);
	    double newHeading = initialPosition.getHeading() + edge.getHeading();
	    while (newHeading > 2 * Math.PI){
		newHeading -= 2 * Math.PI;
	    }
	    while (newHeading < 0){
		newHeading += 2 * Math.PI;
	    }
	    
	    //System.out.println("!!! *Heading=" + newHeading);

	    currentPosition.setHeading(newHeading);
	    
	    currentPosition.setX(currentPosition.getX() + 
				 edge.getDistance() * Math.cos(newHeading));
	    currentPosition.setY(currentPosition.getY() + 
				 edge.getDistance() * Math.sin(newHeading));

	    //System.out.println("!!! *Pos=" + currentPosition.getX() + "," + currentPosition.getY());
	}

	
	return currentPosition;
    }

}