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

import java.rmi.UnexpectedException;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;

import de.tud.kom.p2psim.api.scenario.ConfigurationException;
import de.tud.kom.p2psim.api.topology.movement.MovementModel;
import de.tud.kom.p2psim.api.topology.movement.SimEnergyConsumingLocationActuator;
import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
import de.tud.kom.p2psim.impl.simengine.Simulator;
import de.tud.kom.p2psim.impl.topology.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Event;
import de.tudarmstadt.maki.simonstrator.api.EventHandler;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;

/**
 * Movement model for controllable multicopter UAVs. Does linear and direct movement between target locations, with hovering on the last.
 * 
 * @author Julian Zobel
 * @version 1.0, 07.02.2018
 */
public class ControllableMulticopterUAVMovement implements MovementModel, EventHandler{

	protected Map<SimEnergyConsumingLocationActuator, Location> positions = new LinkedHashMap<>();	
	protected Map<SimEnergyConsumingLocationActuator, LinkedList<Location>> targets = new LinkedHashMap<>();
	
	protected long timeBetweenMoveOperation = Simulator.SECOND_UNIT;
		
	public ControllableMulticopterUAVMovement() {
		initialize();
		Event.scheduleImmediately(this, null, 0);
	}
	
	@Override
	public void eventOccurred(Object content, int type) {
		move();
	}

	protected void initialize() {
		// empty		
	}
	
	@Override
	public void placeComponent(SimLocationActuator actuator) {
		// not used here I guess
	}
 
	@Override
	public void changeTargetLocations(SimLocationActuator actuator, LinkedList<Location> targetLocations) {
		targets.put((SimEnergyConsumingLocationActuator)actuator, new LinkedList<Location>(targetLocations));
	}
	
	@Override
	public void changeTargetLocation(SimLocationActuator actuator, Location targetLocation) throws UnsupportedOperationException {
		LinkedList<Location> emptyTargetList = new LinkedList<Location>();
		emptyTargetList.add(targetLocation);
		targets.put((SimEnergyConsumingLocationActuator)actuator, emptyTargetList);		
	}
	
	
	@Override
	public void removeTargetLocations(SimLocationActuator actuator)
	{
		targets.remove(actuator);
	}
	
	@Override
	public void addTargetLocation(SimLocationActuator actuator, Location targetLocation) {		
			targets.get(actuator).add(targetLocation);		
	}
	
	/**
	 * Returns the first target location contained in the list
	 */
	@Override
	public Location getTargetLocation(SimLocationActuator actuator) {		
		return targets.get(actuator).getFirst().clone();
	}  
	
	@Override
	public List<Location> getAllTargetLocations(SimLocationActuator actuator)
			throws UnsupportedOperationException {
		return new LinkedList<>(targets.get(actuator));
	}

	@Override
	public void addComponent(SimLocationActuator actuator) {
		actuator.updateCurrentLocation(actuator.getRealPosition());
		positions.put((SimEnergyConsumingLocationActuator)actuator, actuator.getRealPosition());		
		
		LinkedList<Location> emptyTargetList = new LinkedList<>();
		targets.put((SimEnergyConsumingLocationActuator)actuator, emptyTargetList);
	}

	@Override
	public void setTimeBetweenMoveOperations(long time) {
		if (time > 0) {
			this.timeBetweenMoveOperation = time;
		} else {
			throw new ConfigurationException(
					"Time is negative for the MOVE operations");
		}
	}
	
	protected void move() {
		for (Entry<SimEnergyConsumingLocationActuator, LinkedList<Location>> entry : targets.entrySet()) {
			
			SimEnergyConsumingLocationActuator actuator = entry.getKey();
						
			if(actuator.isAvailable() )
			{				
				if(entry.getValue().isEmpty())
				{					
					actuator.holdPosition();
					continue;
				}
				
				Location targetLocation = entry.getValue().getFirst();	
				PositionVector targetPosition = new PositionVector(targetLocation);				
				Double distanceToTargetPosition = targetPosition.distanceTo(actuator.getRealPosition());
								
				// If target point is reached within a 1 meter margin, we remove that point from the list 
				if(distanceToTargetPosition < 1)
				{
					entry.getValue().removeFirst();
					positions.put(actuator, targetPosition);
					actuator.updateCurrentLocation(targetPosition);
					actuator.reachedLocation(targetLocation);
					if(entry.getValue().isEmpty()) {
						actuator.holdPosition();							
					}	
					
					continue;
				}
				
				if(actuator.setMoving()) {
					Double movementSpeed = Math.min(distanceToTargetPosition, actuator.getMovementSpeed());						
					PositionVector newPosition = actuator.getRealPosition().moveStep(targetPosition, movementSpeed);	
					
					actuator.updateCurrentLocation(newPosition);
					
					positions.put(actuator, newPosition);		
				}
			}
			
		}

		Event.scheduleWithDelay(timeBetweenMoveOperation, this, null, 1);
	}

}
