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

import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.Map;

import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;

public class MulticopterMovement implements UAVMovementModel {

	//public static enum FlightState { OFF, HOVER, 
	
	private UAVTopologyComponent topologyComponent;

	private final double maxCruiseSpeed;
	private final double minCruiseSpeed;
	private double preferredCruiseSpeed;
	private double currentSpeed;
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
		
	private StatelessMotorComponent motor;
	
	public MulticopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
		this.topologyComponent = topologyComponent;
		this.maxCruiseSpeed = maxCruiseSpeed;
		this.minCruiseSpeed = minCruiseSpeed;
		this.preferredCruiseSpeed = this.maxCruiseSpeed;
					
	}
	
	@Override
	public void setMotorControl(StatelessMotorComponent motor) {
		this.motor = motor;
	}

	@Override
	public void move(long timeBetweenMovementOperations) {
		
		if(topologyComponent.isActive() && !route.isEmpty()) {
			PositionVector currentPosition= topologyComponent.getRealPosition();
			
			PositionVector targetPosition = route.getFirst();
			Double distanceToTargetPosition = targetPosition.distanceTo(currentPosition);
							
			// If target point is reached within a 1 meter margin, we remove that point from the list 
			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < currentSpeed)
			{						
				route.removeFirst();
			//	topologyComponent.getActuatorEnergyComponent().useActuator(0.5);
				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
				currentSpeed = 0;
			//	topologyComponent.getActuatorEnergyComponent().useActuator(0); // now hover
				locationReached(targetPosition);
				return;
			}
			
			double timefactor = timeBetweenMovementOperations / Time.SECOND;
			
			currentSpeed = Math.min(distanceToTargetPosition, preferredCruiseSpeed);					
						
			PositionVector direction = new PositionVector(targetPosition);
			direction.subtract(currentPosition);
			direction.normalize();
			direction.multiplyScalar(currentSpeed * timefactor);

			PositionVector newPosition = new PositionVector(currentPosition);
			newPosition.add(direction);		
			
			//topologyComponent.getActuatorEnergyComponent().useActuator(1);
			topologyComponent.updateCurrentLocation(newPosition);
			
		}
		
		//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);		
	}

	
	@Override
	public void setPreferredCruiseSpeed(double v_pref) {
		this.preferredCruiseSpeed = v_pref;		
	}

	@Override
	public double getMaxCruiseSpeed() {
		return maxCruiseSpeed;
	}

	@Override
	public double getMinCruiseSpeed() {
		return minCruiseSpeed;
	}

	@Override
	public double getCurrentSpeed() {
		return currentSpeed;
	}

	/**
	 * Trigger the callback function, if there is a valid callback 
	 * 
	 * @param position
	 */
	private void locationReached(PositionVector position) {
		if(locationCallbacks.containsKey(position)) {
			locationCallbacks.get(position).reachedLocation();
		}
	}
	
	@Override
	public void setTargetLocation(PositionVector target,
			ReachedLocationCallback reachedLocationCallback) {
		route.clear();
		route.add(target);
		if(reachedLocationCallback != null)
			locationCallbacks.put(target, reachedLocationCallback);
		
	}

	@Override
	public void setTargetLocationRoute(LinkedList<PositionVector> route,
			ReachedLocationCallback reachedLocationCallback) {
		this.route.clear();
		this.route.addAll(route);
		if(reachedLocationCallback != null)
			locationCallbacks.put(route.getLast(), reachedLocationCallback);
	}

	@Override
	public void addTargetLocation(PositionVector target,
			ReachedLocationCallback reachedLocationCallback) {
		route.add(target);
		if(reachedLocationCallback != null)
			locationCallbacks.put(target, reachedLocationCallback);
	}

	@Override
	public LinkedList<PositionVector> getTargetLocations() {
		
		LinkedList<PositionVector> copy = new LinkedList<>();
		for (PositionVector pv : route) {
			copy.add(pv.clone());		
		}
		
		return copy;
	}

	@Override
	public void removeTargetLocations() {
		route.clear();
		locationCallbacks.clear();
	}



}