/*
 * 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 jakarta.persistence.Column;
import jakarta.persistence.Entity;
import jakarta.persistence.Table;

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.Monitor;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;

/**
 * Complex thrust based movement model using specific models of a multicopter UAV for calculation.
 * 
 * TODO Altitude changes while moving are not possible.
 * 
 * @author Julian Zobel
 * @version 1.0, 20.02.2022
 */
public class StateMulticopterMovement implements UAVMovementModel  {
	
	public enum MobilityState { GROUND, HOVER, ASCEND, DESCEND, TARGET}
	private enum IntraState { STOP, ACCELERATION, CONSTANT, DECELERATION, TURN, CURVE, ADJUST}
	
	private MobilityState state = MobilityState.GROUND; 
	private IntraState intrastate = IntraState.STOP;
	
	private final double MARGIN = Math.pow(10, -2); // defines which resolution is enough to arrive at a target
	private UAVTopologyComponent topologyComponent;	
	private MulticopterModel uav;
	
	private double currentAngleOfAttack;	
	private double cruiseVelocity = -1;		
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  
			
	private PositionVector currentTargetPosition = null;
	private PositionVector positionVector = null;
	private PositionVector directionVector = new PositionVector(0,0,0);
	private PositionVector velocityVector = new PositionVector(0,0,0);
	private PositionVector accelerationVector = new PositionVector(0,0,0);
	
	/**
	 * Constructor.
	 * 
	 * @param topologyComponent
	 * @param uav
	 */
	public StateMulticopterMovement(UAVTopologyComponent topologyComponent, MulticopterModel uav) {
		this.topologyComponent = topologyComponent;	
		this.uav = uav;
	}

	
	@Override
	public void move(long timeBetweenMovementOperations) {
		
		// required for initial resolving this components position and direction
		if(positionVector == null) {
			positionVector = new PositionVector(topologyComponent.getRealPosition());
			directionVector = new PositionVector(topologyComponent.getCurrentDirection());
		}
		
		// UAV only moves when the motor is online and there is a valid target position available.
		if(uav.isOn() && !route.isEmpty()) {
			assert positionVector.distanceTo(topologyComponent.getRealPosition()) == 0;
			
			if(currentTargetPosition == null) {
				currentTargetPosition = route.getFirst();
			}
			else {
				
				// perform the next step based on the current state.
				switch( state) {
					case ASCEND: 
						timeBetweenMovementOperations = ascend(timeBetweenMovementOperations);		
						break;
					case DESCEND:						
						timeBetweenMovementOperations = descend(timeBetweenMovementOperations);	
						break;
					case HOVER:
					case GROUND:
						if(positionVector.distanceTo(currentTargetPosition) == 0) {						
							this.locationReached(currentTargetPosition);
						}
						else {						
							// new target, check if altitude must be adapted, otherwise approach
							if(positionVector.getAltitude() < currentTargetPosition.getAltitude()) {
								this.state = MobilityState.ASCEND;
							}
							else if(positionVector.getAltitude() > currentTargetPosition.getAltitude())  {						
								this.state = MobilityState.DESCEND;
							}
							else {
								this.state = MobilityState.TARGET;
							}						
						}			
						break;
					case TARGET:
						timeBetweenMovementOperations = approachTarget(timeBetweenMovementOperations);
						break;
					
					default: throw new UnsupportedOperationException("Wrong State");
				}
				update(); // location & direction
			}
			
			// if the performed movement calculations did not use all available time,
			// we need to perform more calculations...
			if(timeBetweenMovementOperations != 0) {
				move(timeBetweenMovementOperations);
				return;
			}			
		}
		// when there is no target position but the motor is online, the UAV hovers (while in the air) or stays on the ground.
		else if(uav.isOn()) {
			
			if(velocityVector.getLength() != 0) {
				throw new UnsupportedOperationException("[StateMulticopterMovement] no route but speed not 0?");
			}					
			
			if(positionVector.getAltitude() == 0) {
				ground(timeBetweenMovementOperations);
			}
			else {
				hover(timeBetweenMovementOperations);
			}
			
			update();
		}
	}
	
	
	
	///////////////////
	// TARGET FLIGHT
	///////////////////
	
	/**
	 * Approach the current target position.
	 * 
	 * @param time
	 * @return
	 */
	private long approachTarget(long time) {
		
		if(state != MobilityState.TARGET) {
			Monitor.log(getClass(), Monitor.Level.ERROR, "Wrong State", state);
			return time;
		}
		
		switch(intrastate) {
			case STOP:
			case ADJUST:
				// not directly in front of the UAV... turn or curve
				if(horizontalAngleToPosition(currentTargetPosition) != 0) {
					if(velocityVector.getLength() == 0) {
						intrastate = IntraState.TURN;
					}
					else {
						intrastate = IntraState.CURVE;
					}
				}
				else {
					intrastate = IntraState.ACCELERATION;
				}			
				break;			
			case ACCELERATION:
				time = horizontalAcceleration(time);
				break;
			case CONSTANT:
				time = horizontalFlight(time);
				break;
			case DECELERATION:
				time = horizontalDeceleration(time);
				break;
			case CURVE: 
				time = circularMovement(time);		
				break;
			case TURN:
				time = hoverTurn(time);
				break;
			default: throw new UnsupportedOperationException("Wrong State");			
		}
		
		if(time > 0 && intrastate != IntraState.STOP) {
			time = approachTarget(time);
		}
		
		return time;
	}
	

	/**
	 * Accelerate the UAV, straight level flight.
	 * 
	 * @param time
	 * @return
	 */
	private long horizontalAcceleration(long time) {
		// acceleration
		assert velocityVector.getZ() == 0;
		assert horizontalAngleToPosition(currentTargetPosition) == 0;
		
		double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
		
		// acceleration is thrust from motors minus drag from speed
		// rough approximation for velocity induced drag when accelerating (half of maximum)	
		double a_acc = uav.horizontalMaxAcceleration();
		double v0 = velocityVector.getLength();				
		double vMax = flightVelocity();			
									
		double accTime = (vMax - v0) / a_acc;
		double accDist = 0.5 * a_acc * accTime * accTime + v0 * accTime;
		
		if(route.size() == 1) {
			// deceleration is thrust from motors plus drag from speed
			double a_dec = uav.horizontalMaxDeceleration();
			double decTime = vMax / a_dec;		
			double decDist = -0.5 * a_dec * decTime * decTime + vMax * decTime;
					
			// need to limit acceleration time / maximum velocity
			if(accDist + decDist > distanceToTarget) {
			  	accTime = limitAccelerationTimeForShortDistances(a_acc, a_dec, distanceToTarget, v0);		
			}
		}
					
		long accelerationPhaseTime = (long) (accTime * Time.SECOND);
		
		if(accelerationPhaseTime > time) {
			accelerationPhaseTime = time;
		}
		else {
			intrastate = IntraState.CONSTANT;
		}
		
		accTime = (double) accelerationPhaseTime / Time.SECOND;
		accDist = 0.5 * a_acc * accTime * accTime + v0 * accTime;
		double vint = Math.min(vMax, (a_acc * accTime + v0));
		
		// prevent rounding errors at vMax
		if(Math.abs(vint - vMax) < MARGIN) {
			vint = vMax;
		}			
	
		directionVector.normalize(); // should be valid anyways?	
		positionVector = positionVector.plus(directionVector.scale(accDist));					
		velocityVector = directionVector.scale(vint);	
		currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(vint);
		accelerationVector = directionVector.scale(a_acc);
		
		// accelerate towards target
		uav.thrust(uav.thrustForHorizontalMaxVelocity(), accelerationPhaseTime);
		
		// remaining time
		return time - accelerationPhaseTime;			
	}
	
	/**
	 * Horizontal straight level flight.
	 * 
	 * @param time
	 * @return
	 */
	private long horizontalFlight(long time) {
		assert velocityVector.getZ() == 0;
		
		if(horizontalAngleToPosition(currentTargetPosition) != 0) {
			Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
					"(Horizontal Flight) The Multicopter is not properly aligned to the target. Deviation: " + Math.toDegrees(horizontalAngleToPosition(currentTargetPosition))+"°", positionVector);	
		}	
				
		double v = flightVelocity();
		if(v != velocityVector.getLength() && Math.abs(v - velocityVector.getLength()) > MARGIN) {				
					Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
							"The Multicopter velocity is not within the margin to the target velocity. Deviation: " +Math.abs(v - velocityVector.getLength())+"m/s", velocityVector);		
		}
				
		double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
		double constDist = distanceToTarget;
		double constTime = constDist / v;
		
		boolean decelerationRequired = decelerationAtNextTargetRequired();
		
		// if last given point, need to break!
		if(decelerationRequired) {
			// rough approximation for velocity induced drag when accelerating (half of maximum)			
			double a_dec = uav.horizontalMaxDeceleration();
			
			// calculate brake time
			double decTime = v / a_dec;		
			double decDist = -0.5 * a_dec * decTime * decTime + v * decTime;
			
			// substract the deceleration distance for breaking
			constTime = (constDist - decDist) / v;
		}
					
		long constantPhaseTime = (long) (constTime * Time.SECOND);
		
		if(constantPhaseTime > time) {
			constantPhaseTime = time;
		}
		else {	
			// reached end of phase
			if(!decelerationRequired) {			
				intrastate = IntraState.ADJUST;
			}
			else {
				intrastate = IntraState.DECELERATION;
			}
		}
		
		constTime = (double) constantPhaseTime / Time.SECOND;
		constDist = constTime * v;
		
		if(constDist > distanceToTarget) {
			throw new UnsupportedOperationException("Multicopter Movement: The approximated step distance exceeds the distance to the target");
		}
				
		directionVector.normalize(); // should be valid anyways?	
		positionVector = positionVector.plus(directionVector.scale(constDist));
		velocityVector = directionVector.scale(v);	
		currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(v);
		accelerationVector = directionVector.scale(0);
		
		if(intrastate == IntraState.ADJUST) {
			if(Math.abs(positionVector.distanceTo(currentTargetPosition)) < MARGIN) {
				positionVector = currentTargetPosition.clone();
				locationReached(currentTargetPosition);										
			}		
		}
		
		// thrust includes energy calculation; 
		uav.thrust(uav.thrustForHorizontalVelocity(v), constantPhaseTime);
					
		// remaining time
		return time - constantPhaseTime;	
	}
	
	/**
	 * Reduce flight speed, straight level flight.
	 * 
	 * @param time
	 * @return
	 */
	private long horizontalDeceleration(long time) {
		assert velocityVector.getZ() == 0;		
		
		if(horizontalAngleToPosition(currentTargetPosition) != 0) {
			Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
					"(Horizontal Deceleration) The Multicopter is not properly aligned to the target. Deviation: " + Math.toDegrees(horizontalAngleToPosition(currentTargetPosition))+"°", positionVector);	
		}		
		
		double v = velocityVector.getLength();
		
		// rough approximation for velocity induced drag when accelerating (half of maximum)			
		double a_dec = uav.horizontalMaxDeceleration();
					
		// calculate brake time
		double decTime = v / a_dec;		
		double decDist = -0.5 * a_dec * decTime * decTime + v * decTime;
		
		long decelerationtPhaseTime = (long) (decTime * Time.SECOND);
		
		if(decelerationtPhaseTime > time) {
			decelerationtPhaseTime = time;
		}
		
		decTime = (double) decelerationtPhaseTime / Time.SECOND;
		decDist = -0.5 * a_dec * decTime * decTime + v * decTime;
		double vint = (v - decTime * a_dec);
		
		if(vint < Math.pow(10, -3)) {
			vint = 0;
		}
		
		if(decDist > positionVector.distanceTo(currentTargetPosition)) {
			throw new UnsupportedOperationException("Multicopter Movement: The approximated step distance exceeds the distance to the target");
		}
		
		directionVector.normalize(); // should be valid anyways?	
		positionVector = positionVector.plus(directionVector.scale(decDist));
		velocityVector = directionVector.scale(vint);			
		accelerationVector = directionVector.scale(-a_dec);
		
		// thrust includes energy calculation; accelerate against flight direction
		uav.thrust(uav.thrustForHorizontalMaxVelocity(), decelerationtPhaseTime);
		
		if(Math.abs(positionVector.distanceTo(currentTargetPosition)) <  MARGIN) {
			// reached end of phase
			intrastate = IntraState.STOP;
			positionVector = currentTargetPosition.clone();
			velocityVector = directionVector.scale(0);
			accelerationVector = directionVector.scale(0);
			
			this.state = MobilityState.HOVER;
		}		
		
		// remaining time
		return time - decelerationtPhaseTime;
	}
	
	/**
	 * Turn the front towards the target position while hovering. 
	 * 
	 * @param time
	 * @return
	 */
	private long hoverTurn(long time) {
		double rotation = horizontalAngleToPosition(currentTargetPosition);	
		double turnTime = Math.abs(rotation) / uav.getMaximumTurnAngle();						
		long turnPhaseTime = (long) (turnTime * Time.SECOND);
			
		if(turnPhaseTime > time) {
			turnPhaseTime = time;
		}
		else {
			// reached end of phase
			intrastate = IntraState.STOP;
		}
		
		// limit the turn rotation angle with limited time
		turnTime = (double) turnPhaseTime / Time.SECOND;
		rotation = rotation * ( turnTime / (Math.abs(rotation) / uav.getMaximumTurnAngle()));					
		
		// x' = x cos θ − y sin θ
		// y' = x sin θ + y cos θ
		double xn = directionVector.getX() * Math.cos(rotation) - directionVector.getY() * Math.sin(rotation);
		double yn = directionVector.getX() * Math.sin(rotation) + directionVector.getY() * Math.cos(rotation);
	
		directionVector = new PositionVector(xn, yn);
		directionVector.normalize();
		
		// thrust includes energy calculation; 
		uav.thrust(uav.hoverThrust(), turnPhaseTime);
				
		return time - turnPhaseTime;	
	}
	
	/**
	 * Fly a leveled turn towards the current target, such that the UAV faces directly towards the target at the end of the circular movement.
	 * 
	 * TODO Dubin's curves?
	 * 
	 * @param time
	 * @return
	 */
	private long circularMovement(long time) {
				
		double curveVelocity = Math.min(flightVelocity(), uav.curveVelocity());
	
		// estimation for the turn radius based on the available, free thrust potential of the motors 		
		double turnRadius =  uav.getCurveRadius(curveVelocity);
		
		// calculate turn circle
		double angleToTarget = horizontalAngleToPosition(currentTargetPosition);

		PositionVector directionToCircleCenter = null;
		PositionVector circleCenter = null;		
				
		if(angleToTarget == 0) {
			throw new UnsupportedOperationException("Angle cannot be zero!");
		}
		else if(angleToTarget < 0) {
			// left					
			directionToCircleCenter = new PositionVector(directionVector.getY(), -directionVector.getX());
			directionToCircleCenter.normalize();
			directionToCircleCenter.multiplyScalar(turnRadius);
			circleCenter = positionVector.plus(directionToCircleCenter);
		}
		else if(angleToTarget > 0) {
			//right			
			directionToCircleCenter = new PositionVector(-directionVector.getY(), directionVector.getX());
			directionToCircleCenter.normalize();
			directionToCircleCenter.multiplyScalar(turnRadius);
			circleCenter = positionVector.plus(directionToCircleCenter);
		}
	
		PositionVector tangentPoint = calculateCircleTangentExitPoint(circleCenter, turnRadius, currentTargetPosition, angleToTarget);
		
		try {
		// calculate circular path from the current location to the destination tangent point
		double circleAngle = 2 * Math.asin(positionVector.distanceTo(tangentPoint) / (2 * turnRadius));
		double circleDistance = circleAngle * turnRadius;		
		
		// this movement takes some time, which cannot exceed the available time
		double constTime = circleDistance / curveVelocity;							
		long constantPhaseTime = (long) (constTime * Time.SECOND);
		
		if(constantPhaseTime > time) {
			constantPhaseTime = time;
		}
		
		// re-calculate to only move within the available time
		constTime = (double) constantPhaseTime / Time.SECOND;
		circleDistance = constTime * curveVelocity;			
		circleAngle = circleDistance / turnRadius;
		
		// movement to the left: requires to negate the angle (has no sign!), positive for right, negative for left.
		if(angleToTarget < 0) {
			circleAngle = -circleAngle;
		}
	
		// new position on the circle
		double positionAngle = circleCenter.bearingTo(positionVector);			
		double circleLocationX = circleCenter.getX() + turnRadius * Math.cos(positionAngle + circleAngle);
		double circleLocationY = circleCenter.getY() + turnRadius * Math.sin(positionAngle + circleAngle);
		positionVector = new PositionVector(circleLocationX, circleLocationY, positionVector.getZ());
						
		currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(curveVelocity);
	
		// direction vector is the tangent at the new position
		// x' = x cos θ − y sin θ
		// y' = x sin θ + y cos θ
		double xn = directionVector.getX() * Math.cos(circleAngle) - directionVector.getY() * Math.sin(circleAngle);
		double yn = directionVector.getX() * Math.sin(circleAngle) + directionVector.getY() * Math.cos(circleAngle);
		
			
		directionVector = new PositionVector(xn, yn);
		directionVector.normalize();		
		
		velocityVector = directionVector.scale(curveVelocity);		
		accelerationVector = directionToCircleCenter.scale(uav.getCurveCentripedalAcceleration(curveVelocity));
		
		if(Math.abs(positionVector.distanceTo(tangentPoint)) < MARGIN) {	
			// reached end of phase
			positionVector = tangentPoint.clone();
			directionVector = currentTargetPosition.minus(positionVector);
			directionVector.normalize();			
			intrastate = IntraState.ADJUST;
		}	
		
		// thrust includes energy calculation; accelerate against flight direction
		uav.thrust(uav.thrustForHorizontalMaxVelocity(), constantPhaseTime);
		
		// remaining time
		return time - constantPhaseTime;
		}
		catch(Exception e) {
			System.out.println();
			return 0;
		}
	}
	
	
	/**
	 * Static hovering
	 * 
	 * @param time
	 */
	private void hover(long time) {
		this.state = MobilityState.HOVER;
		uav.thrust(uav.hoverThrust(), time);
	}
	
	/**
	 * Static on the ground, requires idle motor motion. 
	 * 
	 * @param time
	 */
	private void ground(long time) {
		this.state = MobilityState.GROUND;
		uav.thrust(uav.minThrust(), time);
	}
	
	
	/////////////
	// UP & DOWN (VERTICAL)
	////////////
		
	/**
	 * Vertical movement, upwards in a straight line.
	 * 
	 * @param time
	 * @return
	 */
	private long ascend(long time) {
		assert time > 0;
		assert positionVector.getAltitude() < currentTargetPosition.getAltitude();	
		assert this.state == MobilityState.ASCEND;		
				
		double verticalDistance = currentTargetPosition.getAltitude() - positionVector.getAltitude();
		
		switch(intrastate) {
			case STOP: 
				intrastate = IntraState.ACCELERATION;	
				break;
			case ACCELERATION: 
				time = verticalAcceleration(time, verticalDistance, uav.verticalAscentMaxAcceleration(),  uav.verticalDescentMaxAcceleration(), uav.verticalAscentMaxVelocity(), true);			
				break;
			case CONSTANT:
				time = verticalFlight(time, verticalDistance, uav.verticalDescentMaxAcceleration(), true);
				break;
			case DECELERATION:
				time = verticalDeceleration(time, verticalDistance, uav.verticalDescentMaxAcceleration(), true);
				break;
			default: throw new UnsupportedOperationException("wrong state");
		}		
		
		if(time > 0 && intrastate != IntraState.STOP) {
			time = ascend(time);
		}
		
		return time;		
	}
	
	/**
	 * Vertical movement, downwards in a straight line.
	 * 
	 * @param time
	 * @return
	 */
	private long descend(long time) {
		assert time > 0;
		assert positionVector.getAltitude() > currentTargetPosition.getAltitude();	
		assert this.state == MobilityState.DESCEND;
		
		double verticalDistance = positionVector.getAltitude() - currentTargetPosition.getAltitude();
		
		switch(intrastate) {
			case STOP: 
				intrastate = IntraState.ACCELERATION;	
				break;
			case ACCELERATION:			
				time = verticalAcceleration(time, verticalDistance, uav.verticalDescentMaxAcceleration(),  uav.verticalAscentMaxAcceleration(), uav.verticalDescentMaxVelocity(), false);			
				break;
			case CONSTANT:			
				time = verticalFlight(time, verticalDistance, uav.verticalAscentMaxAcceleration(), false);
				break;
			case DECELERATION:
				time  = verticalDeceleration(time, verticalDistance, uav.verticalAscentMaxAcceleration(), false);
				break;
			default: throw new UnsupportedOperationException("wrong state");
		}
		
		if(time > 0 && intrastate != IntraState.STOP) {
			time = descend(time);
		}
		
		return time;		
	}
	
	
	/**
	 * Accelerate vertically, either downwards by lowering the thrust or upwards by increasing thrust.
	 * 
	 * @param time
	 * @param distance
	 * @param acceleration
	 * @param deceleration
	 * @param verticalMaximumVelocity
	 * @param upwards
	 * @return
	 */
	private long verticalAcceleration(long time, double distance, double acceleration, double deceleration, double verticalMaximumVelocity, boolean upwards) {		
		double v0 = Math.abs(velocityVector.getZ());				
	
		double accTime = (verticalMaximumVelocity - v0) / acceleration;
		double accDist = 0.5 * acceleration * accTime * accTime + v0 * accTime;
		
		double decTime = verticalMaximumVelocity / deceleration;		
		double decDist = -0.5 * deceleration * decTime * decTime + verticalMaximumVelocity * decTime;
		
		// when the distance is short, we need to limit acceleration time / maximum velocity
		if(accDist + decDist > distance) {
			double A = 0.5 * acceleration + (acceleration * acceleration)/(2 * deceleration);
			double B = v0 + (v0 * acceleration) / deceleration;
			double C = (v0 * v0) / deceleration - distance;
			
			double t1 = (-B + Math.sqrt(B * B - 4 * A * C))/(2*A);				
			
			accTime = t1;						
		}
		
		// limit the phase time by the available time
		long accelerationPhaseTime = (long) (accTime * Time.SECOND);
		
		if(accelerationPhaseTime > time) {
			accelerationPhaseTime = time;
		}
		else {
			// reached end of phase
			intrastate = IntraState.CONSTANT;
		}
				
		accTime = (double) accelerationPhaseTime / Time.SECOND;
		accDist = 0.5 * acceleration * accTime * accTime + v0 * accTime;
		double vint = (acceleration * accTime + v0);
		
		if(upwards) {
			positionVector.setZ(positionVector.getAltitude() + accDist);
			velocityVector.setZ(vint);
			accelerationVector.setZ(acceleration);
			
			// thrust includes energy calculation; 
			uav.thrust(uav.maxThrust(), accelerationPhaseTime);
		}
		else {
			positionVector.setZ(positionVector.getAltitude() - accDist);
			velocityVector.setZ(-vint);
			accelerationVector.setZ(-acceleration);
			
			// thrust includes energy calculation; 
			uav.thrust(uav.minThrust(), accelerationPhaseTime);
		}		
		
		// remaining step time
		return time - accelerationPhaseTime;						
	}
	
	/**
	 * Decelerate the current vertical movement, either by more thrust when moving downwards, 
	 * or by reducing thrust when moving upwards.
	 * 
	 * @param time
	 * @param distance
	 * @param deceleration
	 * @param upwards
	 * @return
	 */
	private long verticalDeceleration(long time, double distance, double deceleration, boolean upwards) {
		double v0 = Math.abs(velocityVector.getZ());
		
		// calculate brake time
		double decTime = v0 / deceleration;		
		double decDist = -0.5 * deceleration * decTime * decTime + v0 * decTime;

		long decelerationtPhaseTime = (long) (decTime * Time.SECOND);

		if(decelerationtPhaseTime > time) {
			decelerationtPhaseTime = time;
		}
		else {
			// reached end of phase
			intrastate = IntraState.STOP;
		}

		decTime = (double) decelerationtPhaseTime / Time.SECOND;
		decDist = -0.5 * deceleration * decTime * decTime + v0 * decTime;
		double vint = (v0 - decTime * deceleration);

		if(upwards) {
			positionVector.setZ(positionVector.getAltitude() + decDist);
			velocityVector.setZ(vint);
			accelerationVector.setZ(-deceleration);
			uav.thrust(uav.minThrust(), decelerationtPhaseTime);
		}
		else {
			positionVector.setZ(positionVector.getAltitude() - decDist);
			velocityVector.setZ(-vint);
			accelerationVector.setZ(deceleration);
			uav.thrust(uav.maxThrust(), decelerationtPhaseTime);
		}

		// reached the position (within margin)
		if(Math.abs(currentTargetPosition.getAltitude() - positionVector.getAltitude()) < MARGIN) {
			intrastate = IntraState.STOP;
			positionVector.setZ(currentTargetPosition.getAltitude());
			velocityVector.setZ(0);
			accelerationVector.setZ(0);
			
			this.state = MobilityState.HOVER;
		}		
					
		return time - decelerationtPhaseTime;
	}
	
	/**
	 * Constant speed on vertical flight. 
	 * 
	 * @param time
	 * @param distance
	 * @param deceleration
	 * @param upwards
	 * @return
	 */
	private long verticalFlight(long time, double distance, double deceleration, boolean upwards) {
		double v0 = Math.abs(velocityVector.getZ());
		
		// calculate brake time
		double decTime = v0 / deceleration;		
		double decDist = -0.5 * deceleration * decTime * decTime + v0 * decTime;
		
		double constDist = distance - decDist;
		double constTime = constDist / v0;
		
		long constantPhaseTime = (long) (constTime * Time.SECOND);
		
		if(constantPhaseTime > time) {
			constantPhaseTime = time;
		}
		else {
			// reached end of phase
			intrastate = IntraState.DECELERATION;
		}
		
		constTime = (double) constantPhaseTime / Time.SECOND;
		constDist = constTime * v0;
		
		if(upwards) {
			positionVector.setZ(positionVector.getAltitude() + constDist);			
			accelerationVector.setZ(0);
			uav.thrust(uav.verticalAscentThrust(v0), constantPhaseTime);
		}
		else {
			positionVector.setZ(positionVector.getAltitude() - constDist);			
			accelerationVector.setZ(0);
			uav.thrust(uav.verticalDescentThrust(v0), constantPhaseTime);		
		}
			
		return time - constantPhaseTime;	
	}
	
	
	
	
	///////////
	///////////

	/**
	 * Calculate the horizontal angle to a given target position from the current UAVs position. 
	 * Angle is given w.r.t. the current UAV direction.
	 * 
	 * @return >0 -> right; <0 -> left  
	 */
	private double horizontalAngleToPosition(PositionVector target) {
		PositionVector directionToTarget = target.minus(positionVector);
		directionToTarget.normalize();				
				
		double dot1 = directionVector.getX() * directionToTarget.getX() + directionVector.getY() * directionToTarget.getY(); //dot product between [x1, y1] and [x2, y2]
		double det1 = directionVector.getX() * directionToTarget.getY() - directionVector.getY() * directionToTarget.getX(); // determinant
		double angle2 = Math.atan2(det1, dot1);  // atan2(y, x) or atan2(sin, cos)
									
		if(Math.abs(angle2) < Math.pow(10, -3)) {
			return 0;
		}
		else {
			return angle2;		
		}
	}
	
	/**
	 * Calculate the maximum length of the acceleration phase for limited distances to prevent overshooting the target.
	 * 
	 * @param acceleration
	 * @param deceleration
	 * @param distanceToTarget
	 * @param v0
	 * @return
	 */
	private double limitAccelerationTimeForShortDistances(double acceleration, double deceleration, double distanceToTarget, double v0) {
		double A = 0.5 * acceleration + (acceleration * acceleration)/(2 * deceleration);
		double B = v0 + (v0 * acceleration) / deceleration;
		double C = (v0 * v0) / deceleration - distanceToTarget;
		
		return (-B + Math.sqrt(B * B - 4 * A * C))/(2*A); 
	}
	
	/**
	 * Depending on the current state and available route waypoints, a deceleration may not be required at the currently approached target waypoint.
	 * @return
	 */
	private boolean decelerationAtNextTargetRequired() {
		
		if(route == null || route.size() == 0) {
			throw new UnsupportedOperationException("Cannot be possible");
		}
		
		if(route.size() == 1) {
			return true;
		}
			
		switch(state) {
			case HOVER:
			case GROUND:
				// not really interesting at this point!
				return true;
			//
			case ASCEND:			
				if(route.get(0).getAltitude() < route.get(1).getAltitude()) {
					return false;
				}
				break;
			//
			case DESCEND:
				if(route.get(0).getAltitude() > route.get(1).getAltitude()) {
					return false;
				}
				break;
			case TARGET:
				if(route.get(0).getAltitude() == route.get(1).getAltitude()) {
					return false;
				}
				break;			
		}				
		
		return true;
	}
	
	/**
	 * Based on a given circle, a given target, and the angle of the UAV on the circle to the target,
	 * calculate the tangent point on the circle that the UAV must enter. 
	 * 
	 * @param circleCenter
	 * @param turnRadius
	 * @param target
	 * @param angleToTarget
	 * @return
	 */
	private PositionVector calculateCircleTangentExitPoint(PositionVector circleCenter, double radius, PositionVector target, double angleToTarget) {
		// Calculate the tangent points from the target Position on the turn circle: Defines exit points for circle movement.
		// https://stackoverflow.com/a/49987361		
		double b = Math.sqrt(Math.pow(target.getX() - circleCenter.getX(), 2) + Math.pow(target.getY() - circleCenter.getY(), 2));
		 // angle theta
		double th = Math.acos(radius / b);
		// direction angle of point P from C
		double d = Math.atan2(target.getY() - circleCenter.getY(), target.getX() - circleCenter.getX());  
		double d1 = d + th;  // direction angle of point T1 from C
		double d2 = d - th;  // direction angle of point T2 from C

		double T1x = circleCenter.getX() + radius * Math.cos(d1);
		double T1y = circleCenter.getY() + radius * Math.sin(d1);
		double T2x = circleCenter.getX() + radius * Math.cos(d2);
		double T2y = circleCenter.getY() + radius * Math.sin(d2);
		
		PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ());
		PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ());
		
		// pick the other one, if one is the current location
		if(T1.equals(positionVector)) {
			return T2;
		}
		else if(T2.equals(positionVector)) {
			return T1;
		}
				
		// now define which point to use for exiting	
		double aT1 = horizontalAngleToPosition(T1);
		double aT2 = horizontalAngleToPosition(T2);
							
		PositionVector tangentPoint = null;
				
		// left -> neg angles
		if(angleToTarget < 0) {
			assert aT1 < 0 || aT2 < 0;
			
			if(aT1 < 0 && aT2 > 0) {
				tangentPoint = T1;
			}
			else if(aT1 > 0 && aT2 < 0) {
				tangentPoint = T2;
			}
			else if(aT1 < 0 && aT2 < 0) {
				tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2;
			}				
		}
		// right
		else if(angleToTarget > 0) {
			assert aT1 > 0 || aT2 > 0;
			
			if(aT1 < 0 && aT2 > 0) {
				tangentPoint = T2;
			}
			else if(aT1 > 0 && aT2 < 0) {
				tangentPoint = T1;
			}
			else if(aT1 > 0 && aT2 > 0) {
				tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2;
			}	
		}
		else if(angleToTarget == 0) {
			throw new AssertionError();
		}
		
		if(tangentPoint == null) {
			throw new AssertionError();
		}
		
		return tangentPoint;
	}
	
	
	//////////
	//////////	
	
	
	// Put the current position and direction to the topology component
	private void update() {		
		topologyComponent.updateCurrentLocation(new PositionVector(positionVector));
		topologyComponent.updateCurrentDirection(new PositionVector(directionVector));
	}
	
	/**
	 * Trigger the callback function, if there is a valid callback 
	 * 
	 * @param position
	 */
	private void locationReached(PositionVector position) {
		
		assert position == currentTargetPosition;
		try {
			assert position == route.getFirst();
		}
		catch (AssertionError e) {
			// TODO: handle exception
			System.out.println(position);
			System.out.println();
		}
		
		// remove current target position when reached			
		route.removeFirst();
		if(!route.isEmpty()) {
			currentTargetPosition = route.getFirst();
		}
		else {
			currentTargetPosition = null;
		}
		
		if(locationCallbacks.containsKey(position)) {
			locationCallbacks.get(position).reachedLocation();
		}
	}
	
	@Override
	public void removeTargetLocations() {		
		Monitor.log(getClass(), Monitor.Level.INFO, "All targets removed, abort current action", 0);
		
		intrastate = IntraState.STOP;
		currentTargetPosition = null;
		route.clear();
		locationCallbacks.clear();
	}		
	

	/**
	 * Flight velocity for the multicopter. Either a target velocity is set, which must 
	 * be within the margins of the available velocities of the UAV, or the optimal velocity is returned.
	 * 
	 * @return
	 */
	private double flightVelocity() {
		if(cruiseVelocity > 0 && cruiseVelocity < uav.horizontalMaxVelocity()) {
			return cruiseVelocity;
		}
		else {
			cruiseVelocity = -1;
			return uav.optimalSpeed();
		}
	}
	
	//////////
	// GETTERS AND SETTERS
	//////////	
	
	@Override
	public void setMotorControl(ActuatorComponent motor) {		
		uav.setMotor(motor);	
	}
	
	@Override
	public void setTargetVelocity(double v_pref) {
		this.cruiseVelocity = v_pref;		
	}
	
	@Override
	public double getCurrentVelocity() {
		return velocityVector.getLength() ;
	}	
	
	@Override
	public void setTargetLocation(PositionVector target,
			ReachedLocationCallback reachedLocationCallback) {
		
		if(currentTargetPosition != null || this.route.size() > 0 || intrastate != IntraState.STOP) {
			removeTargetLocations();
		}		
		route.add(target);
		if(reachedLocationCallback != null)
			locationCallbacks.put(target, reachedLocationCallback);		
	}

	@Override
	public void setTargetLocationRoute(LinkedList<PositionVector> route,
			ReachedLocationCallback reachedLocationCallback) {
		if(currentTargetPosition != null || this.route.size() > 0 || intrastate != IntraState.STOP) {
			removeTargetLocations();
		}
		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;
	}

	public double getAngleOfAttack() {
		return currentAngleOfAttack;
	}
		
	@Override
	public double getMinimumVelocity() {
		return 0;
	}


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


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


	@Override
	public double optimalSpeed() {
		return uav.optimalSpeed();
	}
	
	/**
	 * 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 StateMulticopterMovement(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(StateMulticopterMovement m, double velocity, double thrust, double watt) {		
			// FIXME 
			double amp = watt / 14.8;			
			return new EnergyModelPropertyMeasurement(velocity, thrust, amp, watt);
		}

	}
}
