/*
* 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 .
*
*/
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.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 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 route = new LinkedList<>();
private Map 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;
}
@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(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.updateCurrentLocation(targetPosition, 0.5); // 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.updateCurrentLocation(newPosition, 1);
}
//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " + topologyComponent);
}
/**
* 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 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 getTargetLocations() {
LinkedList copy = new LinkedList<>();
for (PositionVector pv : route) {
copy.add(pv.clone());
}
return copy;
}
@Override
public void removeTargetLocations() {
route.clear();
locationCallbacks.clear();
}
}