/*
 * 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.Collections;
import java.util.LinkedList;
import java.util.List;
import java.util.Set;

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;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.route.Route;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.route.RouteSensor.RouteSegmentListener;

/**
 * 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.
 * 
 * TODO add segment handling
 * 
 * Segment handling is only done if there is a segment listener registered for
 * the respective client.
 * 
 * @author Bjoern Richerzhagen
 * @version 1.0, Aug 9, 2017
 */
public class RouteImpl implements Route {

	private PositionVector start;

	private PositionVector destination;

	private PointList pointList;

	private final List<PositionVector> positionsList;

	private final List<RouteSegmentImpl> routeSegments;

	private final Set<RouteSegmentListener> segmentListeners;

	private int currentIndex;

	private int currentSegmentIndex;

	private int routeLength;

	private int routeSegmentsLength;

	/**
	 * 
	 * @param start
	 * @param destination
	 * @param pointList
	 * @param segmentListeners
	 * @param routeSegments
	 */
	public RouteImpl(PositionVector start,
			PositionVector destination, PointList pointList,
			Set<RouteSegmentListener> segmentListeners,
			List<RouteSegmentImpl> routeSegments) {
		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();
		this.segmentListeners = segmentListeners;
		this.routeSegments = routeSegments;
		this.routeSegmentsLength = routeSegments.size();
	}

	/**
	 * 
	 * @param start
	 * @param destination
	 * @param pointList
	 */
	public RouteImpl(PositionVector start,
			PositionVector destination, PointList pointList) {
		this(start, destination, pointList, Collections.emptySet(),
				Collections.emptyList());
	}

	public PositionVector getStart() {
		return start;
	}

	public PositionVector getDestination() {
		return destination;
	}

	@Override
	public Location getTargetLocation() {
		return destination;
	}

	@Override
	public boolean hasSegmentInformation() {
		return routeSegmentsLength != 0;
	}

	@Override
	public RouteSegment getCurrentSegment() {
		if (!hasSegmentInformation()) {
			throw new UnsupportedOperationException(
					"To access segments within a Route, you need to enable the respective functionality in the local movement strategy (usually: calculateRouteSegments=true)");
		}
		return routeSegments.get(currentSegmentIndex);
	}

	@Override
	public List<RouteSegment> getSegmentsAhead() {
		if (!hasSegmentInformation()) {
			throw new UnsupportedOperationException(
					"To access segments within a Route, you need to enable the respective functionality in the local movement strategy (usually: calculateRouteSegments=true)");
		}
		return Collections.unmodifiableList(routeSegments
				.subList(currentSegmentIndex, routeSegmentsLength));
	}

	@Override
	public List<RouteSegment> getSegmentsBehind() {
		if (!hasSegmentInformation()) {
			throw new UnsupportedOperationException(
					"To access segments within a Route, you need to enable the respective functionality in the local movement strategy (usually: calculateRouteSegments=true)");
		}
		return Collections.unmodifiableList(routeSegments
				.subList(0, currentSegmentIndex));
	}

	/**
	 * 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;
			}
		}

		/*
		 * TODO Segment handling (also inform listeners!)
		 */

		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;
	}

}