UAVTopologyComponent.java

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

import java.util.LinkedList;
import java.util.Set;
import de.tud.kom.p2psim.api.common.SimHost;
import de.tud.kom.p2psim.api.energy.Battery;
import de.tud.kom.p2psim.api.energy.ComponentType;
import de.tud.kom.p2psim.api.energy.EnergyModel;
import de.tud.kom.p2psim.api.network.SimNetInterface;
import de.tud.kom.p2psim.api.topology.Topology;
import de.tud.kom.p2psim.api.topology.TopologyComponent;
import de.tud.kom.p2psim.api.topology.movement.MovementModel;
import de.tud.kom.p2psim.api.topology.movement.SimUAVLocationActuator;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.api.topology.placement.PlacementModel;
import de.tud.kom.p2psim.impl.energy.RechargeableBattery;
import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
import de.tud.kom.p2psim.impl.energy.models.AbstractEnergyModel;
import de.tud.kom.p2psim.impl.topology.placement.UAVBasePlacement;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Monitor;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.overlay.OverlayComponent;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.IAttractionPoint;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.BaseConnectionCallback;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.BatteryReplacementCallback;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.communication.UAVToBaseInterface;

/**
 * Topology component extension providing a broader topology functionality for UAVs
 * 
 * @author Julian Zobel
 * @version 1.0, 06.09.2018
 */
public class UAVTopologyComponent extends AbstractTopologyComponent implements SimUAVLocationActuator {
	
	public enum UAVstate {OFFLINE, BASE_CONNECTION, BASE_CONNECTION_READY, ACTION, RETURN, CRASHED}
	
	private UAVMovementModel movement;
	
	private OverlayComponent uavOverlayComponent;
	
	protected PositionVector direction;
	
	private ActuatorComponent actuator;
	private RechargeableBattery battery;
	
	private UAVstate state = UAVstate.OFFLINE;
	private PositionVector baseLocation;
	
	private UAVToBaseInterface controllerInterface;
	
	/**
	 * Create a TopologyComponent for the current host.
	 *
	 * @param host
	 * @param topology
	 * @param movementModel
	 */
	public UAVTopologyComponent(SimHost host, Topology topology,
			MovementModel movementModel, PlacementModel placementModel, boolean registerAsInformationProviderInSiS) {
		super(host, topology, movementModel, placementModel, registerAsInformationProviderInSiS);		
		direction = new PositionVector(0,-1,0);				
	}

	@Override
	public void initialize() {		
		super.initialize();
		
		try {
			actuator = getHost().getComponent(EnergyModel.class)
					.getComponent(ComponentType.ACTUATOR, ActuatorComponent.class);	
			movement.setMotorControl(actuator);
		} catch (ComponentNotAvailableException e) {
			System.err.println("No Acutator Energy Component was found!");
		}
		
		try {
			battery = (RechargeableBattery) getHost().getComponent(AbstractEnergyModel.class).getBattery();
				
		} catch (ComponentNotAvailableException e) {
			System.err.println("No Battery Component was found!");
		}
		
		// retrieve base location
		baseLocation = UAVBasePlacement.base.position.clone();	
		
	}
	
	private void setState(UAVstate newState) {
		this.state = newState;
		
		// TODO analyzer
		if(Monitor.hasAnalyzer(UAVStatisticAnalyzer.class)) {
			Monitor.getOrNull(UAVStatisticAnalyzer.class).uavSwitchedStates(this, newState);
		}
	}
			
	public void setUAVComponent(OverlayComponent uavOverlayComponent) {
		this.uavOverlayComponent = uavOverlayComponent;
	}
	
	public OverlayComponent getUAVComponent() {
		return uavOverlayComponent;
	}

	@Override
	public double getMinMovementSpeed() {
		return movement.getHorizontalMinVelocity();
	}

	@Override
	public double getMaxMovementSpeed() {
		return movement.getHorizontalMaxVelocity();
	}

	@Override
	public double getMovementSpeed() {
		return movement.getCurrentVelocity();
	}

	@Override
	public void setMovementSpeed(double speed) {
		movement.setTargetVelocity(speed);
	}

	@Override
	public boolean isActive() {
		if(actuator.isOn()) {
			return true;
		} else {
			if(state == UAVstate.ACTION || state == UAVstate.RETURN) {
				this.deactivate();
			}
			return false;
		}
	}

	@Override
	public boolean activate() {		
		if(actuator.turnOn()) {
			this.setState(UAVstate.ACTION);			
			return true;
		}
		else {
			return false;
		}
	}

	@Override
	public boolean deactivate() {
		actuator.turnOff();
				
		if(this.position.getAltitude() != 0) {
			this.setState(UAVstate.CRASHED);		
						
			uavOverlayComponent.shutdown();	
			shutdownCommunication();
			
		} else {
			this.setState(UAVstate.OFFLINE);				
		}				
		
		return true;
	}

	@Override
	public PositionVector getCurrentLocation() {
		return position.clone();
	}

	@Override
	public double getCurrentBatteryLevel() {
		return battery.getCurrentPercentage();
	}
	
	@Override
	public double getCurrentBatteryEnergy() {
		return battery.getCurrentEnergy();
	}

	public RechargeableBattery getBattery() {
		return battery;
	}
	
	@Override
	public double getMaximumBatteryCapacity() {
		return battery.getMaximumEnergy();
	}
	
	@Override
	public UAVMovementModel getUAVMovement() {
		return movement;
	}

	@Override
	public void setUAVMovement(UAVMovementModel uavMovement) {
		this.movement = uavMovement;		
	}

	@Override
	public Set<IAttractionPoint> getAllAttractionPoints() {
		throw new UnsupportedOperationException();
	}

	@Override
	public void setTargetLocation(PositionVector targetLocation,
			ReachedLocationCallback cb) {
		if(!isActive()) return;
		
		movement.setTargetLocation(new PositionVector(targetLocation), cb);		
	}

	@Override
	public void addTargetLocation(PositionVector targetLocation,
			ReachedLocationCallback cb) {
		if(!isActive()) return;
		
		movement.addTargetLocation(new PositionVector(targetLocation), cb);
	}

	@Override
	public void setTargetLocationRoute(LinkedList<PositionVector> route,
			ReachedLocationCallback cb) {
		LinkedList<PositionVector> positionvectorlist = new LinkedList<>();
		for (Location loc : route) {
			positionvectorlist.add(new PositionVector(loc));
		}
		movement.setTargetLocationRoute(positionvectorlist, cb);
	}

	@Override
	public void removeAllTargetLocations() {
		movement.removeTargetLocations();
	}

	@Override
	public void setTargetAttractionPoint(IAttractionPoint targetAttractionPoint) {
		throw new UnsupportedOperationException();		
	}

	@Override
	public IAttractionPoint getCurrentTargetAttractionPoint() {
		throw new UnsupportedOperationException();
	}

	@Override
	public LinkedList<PositionVector> getTargetLocations() {
		return movement.getTargetLocations();
	}

	public UAVstate getUAVState() {
		return state;
	}
	
	private PositionVector getOverBaseLocation() {
		PositionVector locationOverBase = baseLocation.clone();
		locationOverBase.setAltitude(10);
		
		return locationOverBase;
	}
	
	@Override
	public void returnToBase(double velocity, ReachedLocationCallback cb) {					
		movement.setTargetVelocity(velocity);
		movement.setTargetLocation(getOverBaseLocation(), cb);	
	}

	@Override
	public boolean landAtBase(BaseConnectionCallback cb) {
		if(overBaseLocation()) {
			this.setState(UAVstate.RETURN);		
			
			ReachedLocationCallback landedCallback = new ReachedLocationCallback() {
				
				@Override
				public void reachedLocation() {
					deactivate();
					connectToBase(cb);				
				}
			};
			
			movement.setTargetVelocity(5);
			movement.setTargetLocation(baseLocation, landedCallback);
			
			return true;
		}
		else return false;
	}	
		
	@Override
	public boolean startFromBase(ReachedLocationCallback cb) {
		if(this.getCurrentLocation().equals(this.getBaseLocation())
				&& state == UAVstate.BASE_CONNECTION_READY 
				&& activate()) {
			disconnectFromBase();
						
			movement.setTargetVelocity(movement.getVerticalAscentMaxVelocity());
			movement.setTargetLocation(getOverBaseLocation(), cb);
			
			return true;
		}
		else return false;
		
	}
	
	private boolean overBaseLocation() {
		if(this.getCurrentLocation().getX() == this.getBaseLocation().getX() 
				&& this.getCurrentLocation().getY() == this.getBaseLocation().getY()) {
			return true;
		}
		else return false;
	}
	
		
	private void batteryReplacement(BatteryReplacementCallback cb) {
		
		if(state != UAVstate.BASE_CONNECTION) {
			throw new UnsupportedOperationException("Cannot recharge if not connected to base!");
		}
		
		this.setState(UAVstate.OFFLINE);
		
		BaseTopologyComponent base = UAVBasePlacement.base;
		base.getCharger().charge(this, cb);
	}

	public void setControllerInterface(UAVToBaseInterface controllerInterface) {
		this.controllerInterface = controllerInterface;			
		this.controllerInterface.setDeploymentArea(topology.getWorldDimensions());
		connectToBase(null);
	}

	/**
	 * Connect to the base and initiate a recharge
	 * 
	 * @param cb
	 */
	private void connectToBase(BaseConnectionCallback cb) {
		BaseTopologyComponent base = UAVBasePlacement.base;
		base.connectUAVToBase(controllerInterface);	
		
		if(cb != null) {			
			cb.successfulConnection();		
		}
				
		this.setState(UAVstate.BASE_CONNECTION);		
		shutdownCommunication();
		
		// recharge
		if(battery.getCurrentPercentage() <= 75) {
			batteryReplacement(new BatteryReplacementCallback() {

				@Override
				public void rechargeComplete() {
					setState(UAVstate.BASE_CONNECTION_READY);
					if(cb != null) {
						cb.readyForTakeoff();
					}
				}
			});
		}
		else {
			this.setState(UAVstate.BASE_CONNECTION_READY);		
			if(cb != null) {
				cb.readyForTakeoff();
			}
		}
	}

	
	private void disconnectFromBase() {
		startCommunication();
		
		BaseTopologyComponent base = UAVBasePlacement.base;
		base.disconnectUAVFromBase(controllerInterface);		
	}

	private void shutdownCommunication() {
		for (SimNetInterface net : getHost().getNetworkComponent().getSimNetworkInterfaces()) 
			net.goOffline();
	}

	private void startCommunication() {
		for (SimNetInterface net : getHost().getNetworkComponent().getSimNetworkInterfaces()) 
			net.goOnline();
	}

	@Override
	public PositionVector getCurrentDirection() {
		return direction;
	}

	@Override
	public void updateCurrentDirection(PositionVector direction) {
		this.direction.set(direction);
	}
	
	@Override
	public double estimatePowerConsumptionWatt(double velocity) {
		return movement.estimatePowerConsumptionWatt(velocity);
	}

	@Override
	public double estimateFlightDistance(double velocity, double batterylevel, double batterythreshold) {
				
		assert batterylevel > batterythreshold;
		assert batterylevel <= 1.0 && batterylevel >= 0.0;
		assert batterythreshold <= 1.0 && batterythreshold >= 0.0;
		
		double availableEnergy = (battery.getMaximumEnergy() * (batterylevel - batterythreshold)) / Battery.uJconverison; // since battery energy is in uJ, conversion in J is required		
		double powerconsumption = estimatePowerConsumptionWatt(velocity); // J/s (or Watt)				
		double distance = (availableEnergy / powerconsumption) * velocity; // d = (E/P)* v [m]
	
		return distance;
	}
	
	@Override
	public PositionVector getBaseLocation() {
		return baseLocation.clone();
	}

	
	public static class Factory implements TopologyComponentFactory {

		@Override
		public TopologyComponent createTopologyComponent(SimHost host,
				Topology topology, MovementModel movementModel,
				PlacementModel placementModel,
				boolean registerAsInformationProviderInSiS) {
			return new UAVTopologyComponent(host, topology, movementModel, placementModel, registerAsInformationProviderInSiS);
		}
		
	}


	@Override
	public UAVstate getState() {
		return state;
	}

	@Override
	public void updateCurrentLocation(Location location) {		
		super.updateCurrentLocation(location);
		
		if(!actuator.isOn() && (state == UAVstate.ACTION || state == UAVstate.RETURN)) {
			this.deactivate();			
		}
		
	}

}