/*
 * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
 *
 * This file is part of PeerfactSim.KOM.
 * 
 * PeerfactSim.KOM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 * 
 * PeerfactSim.KOM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

package de.tud.kom.p2psim.impl.topology.movement.local;

import java.util.LinkedList;
import java.util.List;

import com.graphhopper.util.PointList;
import com.graphhopper.util.shapes.GHPoint3D;

import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
import de.tud.kom.p2psim.impl.topology.PositionVector;

/**
 * This class contains one trajectory from a start to a destination. It saves
 * the respective points (coordinates) along the path with a given movement
 * speed and also (if enabled) the assigned IDs of the roads and walkways that
 * are traversed.
 * 
 * @author Bjoern Richerzhagen
 * @version 1.0, Aug 9, 2017
 */
public class RealWorldMovementPoints {

	private PositionVector start;

	private PositionVector destination;

	private PointList pointList;
	
	private final List<PositionVector> positionsList;

	private int currentIndex;
	
	private int routeLength;

	public RealWorldMovementPoints(PositionVector start,
			PositionVector destination, PointList pointList) {
		this.start = start;
		this.destination = destination;
		this.pointList = pointList;
		this.positionsList = new LinkedList<>();
		// Create Position vectors ONCE
		for (GHPoint3D temp : pointList) {
			positionsList.add(RealWorldStreetsMovement.transformGPSWindowToOwnWorld(temp.getLat(), temp.getLon()));
		}
		routeLength = positionsList.size();
	}
	
	public PositionVector getStart() {
		return start;
	}

	public PositionVector getDestination() {
		return destination;
	}
	
	/**
	 * Update the internal location with the given movement speed
	 * 
	 * @param speed
	 * @param realPosition
	 * @return
	 */
	public PositionVector updateCurrentLocation(SimLocationActuator comp, double speed, double tolerance) {
		List<PositionVector> sublist = positionsList.subList(currentIndex, routeLength);
		PositionVector newPosition = null;
		for (PositionVector candidate : sublist) {
			newPosition = comp.getRealPosition().moveStep(candidate, speed);
			if (candidate
					.distanceTo(comp.getRealPosition()) < tolerance) {
				currentIndex++;
			} else {
				break;
			}
		}
		return newPosition;
	}
	
	@Override
	public String toString() {
		return "\n\tfrom "+start.toString()+" to "+destination.toString()+ "\n\tstep: "+currentIndex + " length: "+routeLength;
	}

	public PointList getPointList() {
		return pointList;
	}

	public int getCurrentIndex() {
		return this.currentIndex;
	}
}