/*
 * 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.ActuatorComponent;
import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
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;

/**
 * Local movement for UAVs. 
 * This simple movement logic uses straight forward movement with the set speed (up to a max).
 * Energy consumption is based on state actuators.
 * 
 * @author Julian Zobel
 * @version 1.1, 14.01.2020
 */
public class SimpleMulticopterMovement implements UAVMovementModel  {

	private UAVTopologyComponent topologyComponent;	
	private double velocity;	
	private double targetVelocity;
	
	private double maximumHorizontalVelocity;
	private double maximumVerticalVelocity;
	
	// TODO currently not used
	private double minimumHorizontalVelocity;
	private double minimumVerticalVelocity;
	
	private double optimalHorizontalVelocity;
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  
		
	private StatefulActuatorComponent motor;
		
	public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent, double maximumHorizontalVelocity, double optimalHorizontalVelocity,
			double maximumVerticalVelocity, double minimumHorizontalVelocity, double minimumVerticalVelocity) {
		this.topologyComponent = topologyComponent;		
		this.maximumHorizontalVelocity = maximumHorizontalVelocity;
		this.optimalHorizontalVelocity = optimalHorizontalVelocity;
		this.maximumVerticalVelocity = maximumVerticalVelocity;
		this.minimumHorizontalVelocity = minimumHorizontalVelocity;
		this.minimumVerticalVelocity = minimumVerticalVelocity;
	}	
	
	@Override
	public void move(long timeBetweenMovementOperations) {
				
		if(motor.isOn() && !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 < velocity)
			{						
				route.removeFirst();
				
				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
				velocity = 0;
				motor.useActuator(0);
				locationReached(targetPosition);
				return;
			}
			
			double timefactor = timeBetweenMovementOperations / Time.SECOND;
			
			velocity = Math.min(distanceToTargetPosition, targetVelocity);					
						
			PositionVector direction = new PositionVector(targetPosition);
			direction.subtract(currentPosition);
			direction.normalize();
			direction.multiplyScalar(velocity * timefactor);

			PositionVector newPosition = new PositionVector(currentPosition);
			newPosition.add(direction);		
			
			if(velocity <= optimalHorizontalVelocity) {
				motor.useActuator(1.0);
			}
			else {
				motor.useActuator(2.0);
			}
					
					
			topologyComponent.updateCurrentLocation(newPosition);					
		}
		else if(motor.isOn()) {
			
			if(velocity != 0) {
				throw new UnsupportedOperationException("no route but speed not 0?");
			}
			
			PositionVector position = new PositionVector(topologyComponent.getRealPosition());	
			
			if(position.getAltitude() == 0) {
				motor.turnOff();
			}
			else {
				motor.useActuator(0);
			}
		}
	}
		
	@Override
	public void setTargetVelocity(double v_pref) {
		this.targetVelocity = v_pref;		
	}
	
	@Override
	public double getCurrentVelocity() {
		return velocity;
	}

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

	@Override
	public double estimatePowerConsumptionWatt(double velocity) {
		
		double wattage = 0;
		
		if(velocity == 0) {
			wattage = motor.estimateEnergyConsumptionWatt(0);
		}
		else if( velocity <= optimalHorizontalVelocity){
			wattage = motor.estimateEnergyConsumptionWatt(1);
		}
		else {
			wattage = motor.estimateEnergyConsumptionWatt(2);
		}
		
		return wattage;
	}

	@Override
	public void setMotorControl(ActuatorComponent motor) {
		this.motor = (StatefulActuatorComponent) motor;
	}

	@Override
	public double getVerticalAscentMaxVelocity() {
		return maximumVerticalVelocity;
	}

	@Override
	public double getHorizontalMaxVelocity() {
		return maximumHorizontalVelocity;
	}
	
	@Override	
	public double getHorizontalMinVelocity() {
		return 0;
	}
	
	/**
	 * Factory for this movement model
	 * 
	 * @author Julian Zobel
	 * @version 1.0, 14.01.2020
	 */
	public static class Factory implements AerialMovementModelFactory {
		
		private double maximumHorizontalVelocity = 15;
		
		public void setMaximumHorizontalVelocity(double maximumHorizontalVelocity) {
			this.maximumHorizontalVelocity = maximumHorizontalVelocity;
		}
		
		private double optimalHorizontalVelocity = 10;
		
		public void setOptimalHorizontalVelocity(double optimalHorizontalVelocity) {
			this.optimalHorizontalVelocity = optimalHorizontalVelocity;
		}		

		private double maximumVerticalVelocity = 5;		
		
		public void setMaximumVerticalVelocity(double maximumVerticalVelocity) {
			this.maximumVerticalVelocity = maximumVerticalVelocity;
		}
		
		private double minimumHorizontalVelocity = 15;
		
		public void setMinimumHorizontalVelocity(double minimumHorizontalVelocity) {
			this.minimumHorizontalVelocity = minimumHorizontalVelocity;
		}
		
		private double minimumVerticalVelocity = 5;		
		
		public void setMinimumVerticalVelocity(double minimumVerticalVelocity) {
			this.minimumVerticalVelocity = minimumVerticalVelocity;
		}
		
		public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) {
			return new SimpleMulticopterMovement(topologyComponent, maximumHorizontalVelocity, optimalHorizontalVelocity,
					maximumVerticalVelocity, minimumHorizontalVelocity, minimumVerticalVelocity);
		}			
	}

	@Override
	public double estimateOptimalSpeed() {
		return optimalHorizontalVelocity;
	}
}