package framework.AI;

import java.util.LinkedList;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import framework.model3D.Position3D;

public class Plan {
	private LinkedList<Location> path;		// 計画内容を表すパス
	private int currentLoc = 0;		// パス上の現在の Location 

	/**
	 * 複数の通過点を持つ計画
	 * @param locationPath
	 */
	public Plan(LinkedList<Location> locationPath) {
		path = locationPath;
		currentLoc = locationPath.size() - 1;
	}
	
	/**
	 * スタートとゴールをダイレクトに結ぶ計画
	 * @param start
	 * @param goal
	 */
	public Plan(Location start, Location goal) {
		path = new LinkedList<Location>();
		path.add(goal);
		path.add(start);
		currentLoc = 1;
	}

	/**
	 * 現在の Location を取得する
	 * @return　現在の Location, すでにゴールに着いているときは null を返す
	 */
	public Location getCurrentLocation() {
		if (currentLoc <= 0) return null;
		return path.get(currentLoc);
	}

	/**
	 * 次の Location を取得する
	 * @return　次の Location, すでにゴールに着いているときは null を返す
	 */
	public Location getNextLocation() {
		if (currentLoc <= 0) return null;
		return path.get(currentLoc - 1);
	}

	/**
	 * 現在の座標値を元に、現在の Location を更新する
	 * @param position　現在の座標値
	 * @return　更新した --- true, 以前のまま --- false
	 */
	public boolean updateCurrentLocation(Position3D position) {
		Vector3d toCurrentPosition = position.getVector3d();
		Location curLocation = getCurrentLocation();
		if (curLocation == null) return true;
		toCurrentPosition.sub(curLocation.getCenter());
		double distanceToCurrentPosition = toCurrentPosition.length();
		Vector3d toNextLocation = new Vector3d(getNextLocation().getCenter());
		toNextLocation.sub(curLocation.getCenter());
		double distanceToNextLocation = toNextLocation.length();
		if (distanceToCurrentPosition >= distanceToNextLocation) {
			// 次の Location を通り過ぎた場合
			currentLoc--;
			return true;
		}
		return false;
	}

}