/*
 * 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.HashMap;
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.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.simengine.Simulator;
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.component.sensor.location.Location;

/**
 * Local movement logic specifically designs the movement for multicopter UAVs. 
 * This simple movement logic uses straight forward movement with the maximum speed available.
 * 
 * @author Julian Zobel
 * @version 1.0, 11.09.2018
 */
public class SimpleMutlicopterMovement implements UAVMovementModel  {

	private UAVTopologyComponent topologyComponent;

	private final double maxCruiseSpeed;
	private final double minCruiseSpeed;
	private double preferredCruiseSpeed;
	private double currentSpeed;
	
	private LinkedList<Location> waypoints = new LinkedList<>();
	private Map<Location, ?> locationCallbacks = new HashMap<>();  // TODO callback interface
		
	
	public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
		this.topologyComponent = topologyComponent;
		this.maxCruiseSpeed = maxCruiseSpeed;
		this.minCruiseSpeed = minCruiseSpeed;
		this.preferredCruiseSpeed = this.maxCruiseSpeed;
				
		waypoints.add(new PositionVector(100,100,100));
		waypoints.add(new PositionVector(600,600,15));
		waypoints.add(new PositionVector(200,500,70));
	}
		
	@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;
	}

	@Override
	public void move(long timeBetweenMovementOperations) {
		
		if(!waypoints.isEmpty() && topologyComponent.isActive() ) {
			PositionVector currentPosition= topologyComponent.getRealPosition();
			
			Location targetLocation = waypoints.getFirst();
			PositionVector targetPosition = new PositionVector(targetLocation);				
			Double distanceToTargetPosition = targetPosition.distanceTo(currentPosition);
							
			// If target point is reached within a 1 meter margin, we remove that point from the list 
			if(distanceToTargetPosition < 1 || distanceToTargetPosition < currentSpeed)
			{
				waypoints.add(waypoints.removeFirst());
				topologyComponent.updateCurrentLocation(targetPosition, 0);
				currentSpeed = 0;
				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.updateCurrentLocation(newPosition, 1);
			
		}
		
		
		// System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);		
	}


	
}