/*
 * 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.api.uav.MulticopterModel;
import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tud.kom.p2psim.impl.util.db.metric.CustomMeasurement;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
import jakarta.persistence.Column;
import jakarta.persistence.Entity;
import jakarta.persistence.Table;

/**
 * Simplified thrust-based local movement model based on the Intel Aero UAV. 
 * 
 * The movement logic uses only straight forward movement with the maximum speed available. 
 * 
 * TODO Acceleration
 * TODO Movement model for plane-like UAVs 
 * 
 * @author Julian Zobel
 * @version 1.0, 11.09.2018
 */
public class MulticopterMovement implements UAVMovementModel  {
	
	private UAVTopologyComponent topologyComponent;
	
	private double velocity;
	
	private double targetVelocity;	
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
			
	private MulticopterModel uav;
		
	
	public MulticopterMovement(UAVTopologyComponent topologyComponent, MulticopterModel uav) {
		this.topologyComponent = topologyComponent;	
		this.uav = uav;
	}
	
	boolean first = true;
	
	@Override
	public void move(long timeBetweenMovementOperations) {
				
		if(uav.isOn() && !route.isEmpty()) {
			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
						
			PositionVector target = route.getFirst();
			
			double distanceToTargetPosition = position.distanceTo(target);
			
			
			// If target point is reached within a 1 meter margin, we remove that point from the list 
			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < velocity)
			{						
				target = route.removeFirst();
								
				if(route.isEmpty()) {	
					
					// go to hover mode					
					topologyComponent.updateCurrentLocation(target.clone()); 
					
					velocity = 0;		
					uav.thrust(uav.hoverThrust(), timeBetweenMovementOperations);
					
					PositionVector direction = topologyComponent.getCurrentDirection().clone();
					direction.setEntry(2, 0);
					topologyComponent.updateCurrentDirection(direction);
					
					locationReached(topologyComponent.getCurrentLocation());
					return;
				}
				else {
					
					// get to speed	
					if(targetVelocity > 0 && targetVelocity < getMaximumVelocity()) {
						
						
						uav.thrust(uav.thrustForHorizontalVelocity(targetVelocity), timeBetweenMovementOperations);
						velocity = targetVelocity;
					}
					else {
						uav.thrust(uav.thrustForHorizontalMaxVelocity(), timeBetweenMovementOperations);
						velocity = getMaximumVelocity();
					}							
			
					long timeUntilReachedLocation = (long) (distanceToTargetPosition / velocity) * Time.SECOND;
					
					target = route.getFirst();					
					PositionVector directionToTarget = new PositionVector(target);
					directionToTarget.subtract(position);
					
					double timefactor = timeUntilReachedLocation / Time.SECOND;
					
					directionToTarget.normalize();
					topologyComponent.updateCurrentDirection(directionToTarget.clone());
					directionToTarget.multiplyScalar(velocity * timefactor);
					
					PositionVector newPosition = new PositionVector(position);
					newPosition.add(directionToTarget);		
					
					topologyComponent.updateCurrentLocation(newPosition);
					
					if(timeUntilReachedLocation < timeBetweenMovementOperations) {
						this.move(timeBetweenMovementOperations - timeUntilReachedLocation);					
					}
					
				}			
				
			}
			else {
				double timefactor = timeBetweenMovementOperations / Time.SECOND;
				
				// get to speed	
				if(targetVelocity > 0 && targetVelocity < getMaximumVelocity()) {
					uav.thrust(uav.thrustForHorizontalVelocity(targetVelocity), timeBetweenMovementOperations);
					velocity = targetVelocity;
				}
				else {
					uav.thrust(uav.thrustForHorizontalMaxVelocity(), timeBetweenMovementOperations);
					velocity = getMaximumVelocity();
				}				
				
				PositionVector directionToTarget = new PositionVector(target);
				directionToTarget.subtract(position);
				
				directionToTarget.normalize();
				
				if(directionToTarget.getX() != 0 || directionToTarget.getY() != 0) {
					topologyComponent.updateCurrentDirection(directionToTarget.clone());
				}				
				
				directionToTarget.multiplyScalar(velocity * timefactor);

				PositionVector newPosition = new PositionVector(position);
				newPosition.add(directionToTarget);		
				
				topologyComponent.updateCurrentLocation(newPosition);
			}
			
		}
		else if(uav.isOn()) {
			
			if(velocity != 0) {
				throw new UnsupportedOperationException("[MulticopterMovement] no route but speed not 0?");
			}
					
			PositionVector position = new PositionVector(topologyComponent.getRealPosition());	
			
			if(position.getAltitude() == 0) {
				uav.thrust(0, timeBetweenMovementOperations);
			}
			else {
				uav.thrust(uav.hoverThrust(), timeBetweenMovementOperations);
			}
			
			topologyComponent.updateCurrentLocation(position);
		}
	}
	
	/*
	 * 
	 */
	
	@Override
	public void setMotorControl(ActuatorComponent motor) {
		uav.setMotor(motor);	
	}
	
	@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 getMinimumVelocity() {
		return uav.horizontalMinVelocity();
	}
	

	@Override
	public double optimalSpeed() {
		return uav.optimalSpeed();
	}

	@Override
	public double getMaximumVelocity() {
		return uav.horizontalMaxVelocity();
	}


	@Override
	public double powerConsumptionWatt(double velocity) {
		return uav.powerConsumptionWatt(velocity);
	}
	
	public MulticopterModel getUAVModel() {
		return this.uav;
	}
	
		
	/**
	 * Factory for this movement model
	 * 
	 * @author Julian Zobel
	 * @version 1.0, 14.01.2020
	 */
	public static class Factory implements AerialMovementModelFactory {
	
		private MulticopterModel.Factory factory;	
		
		public void setUAVModel(MulticopterModel.Factory uavFactory) {
			this.factory = uavFactory;
		}

		public UAVMovementModel createComponent(UAVTopologyComponent topologyComponent) {
			return new MulticopterMovement(topologyComponent, factory.createComponent());
		}		
	}
	
	// For evaluation only!
	@Entity
	@Table(name = "energyModel")
	public static class EnergyModelPropertyMeasurement extends CustomMeasurement {

		/*
		 * For DB performance reasons, we only store up to 15 characters of the
		 * topic (longer topics are truncated)
		 */
		@Column(nullable = false, name = "[velocity]")
		final float velocity;

		@Column(nullable = false, name = "[thrust]")
		final float thrust;

		@Column(nullable = false, name = "[ampere]")
		final float ampere;

		@Column(nullable = false, name = "[watt]")
		final float watt;

		@Column(nullable = false, name = "[wattPerMeter]")
		final float wattperdistance;


		public EnergyModelPropertyMeasurement(double velocity, double thrust, double ampere, double watt) {
			/*
			 * Store all relevant fields
			 */
			this.velocity = (float) velocity;
			this.thrust = (float) thrust;
			this.ampere = (float) ampere;
			this.watt = (float) watt;

			if (velocity == 0) {
				this.wattperdistance = -1;
			} else
				this.wattperdistance = this.watt / this.velocity;

		}
		
		public static EnergyModelPropertyMeasurement getPropoertyMeasurement(MulticopterMovement m, double velocity) {		
			// FIXME only useful with UAVs that use 14.8V
			double amp = m.getUAVModel().powerConsumptionWatt(velocity) / 14.8;			
			return new EnergyModelPropertyMeasurement(velocity, m.getUAVModel().thrustForHorizontalVelocity(velocity), amp, m.getUAVModel().powerConsumptionWatt(velocity));
		}

	}

}
