/*
 * 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 org.apache.commons.math3.geometry.Vector;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;

import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.StatelessMotorComponent;
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;

public class MulticopterMovement implements UAVMovementModel {

		
	private UAVTopologyComponent topologyComponent;

	private PositionVector direction;
	private double currentAngleOfAttack;
	private double currentSpeed;
	
	private double preferredSpeed;
	
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  // TODO callback interface
		
	private StatelessMotorComponent motor;
	
	
	private double mass = 1.465; // kg
	private final double airdensity = 1.2255; // kg/m^3
	private final double gravity = 9.807; // m/s^2
	private double A_top = 0.245; // m^2
	private double A_front = 0.04; // m^2
	private double dragCoefficient = 0.5;
	private double maxPitchAngle = Math.toRadians(45); // 45° max angle
	private double descentVelocityMax = 5; // m/s
	private double maxTurnAngle = Math.toRadians(90); // 90° per second turn angle
	
	
	public MulticopterMovement(UAVTopologyComponent topologyComponent) {
		this.topologyComponent = topologyComponent;
		this.direction = new PositionVector(0,0);
		
	}
	
	
	/*
	 * 
	 */
	
	public double verticalDescentMaxThrust() {		
		// m * g - 0.5 * p * C * A * v^2
		return hoverThrustRequired() - 0.5 * bodyDrag(0, new PositionVector(0,0,1)) * descentVelocityMax * descentVelocityMax;
	}
	
	public double verticalAscentMaxAcceleration() {
		return (motor.getMaxThrust() - hoverThrustRequired()) / mass;
	}
	
	public double verticalAscentMaxVelocity() {		
		double maxThrust = motor.getMaxThrust();		
		return Math.sqrt(2.0 * (maxThrust - hoverThrustRequired()) / bodyDrag(0, new PositionVector(0,0,1)));		
	}
	
	
	public double hoverThrustRequired()  {		
		return mass * gravity;		
	}
	
	
	public double horizontalMaxVelocity() {
		
		double horizontalThrust = horizontalMaxThrust();
				
		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maxPitchAngle, new PositionVector(1,0,0)));
				
		return maxVelocity;
	}
	
	public double horizontalMaxThrust() {
		
		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
		double stableAltitudeMaximumTotalThrust = hoverThrustRequired() / Math.cos(maxPitchAngle);
		
		// fraction of total thrust in horizonal (forward) direction with °angle
		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maxPitchAngle);
		
		return maximumHorizontalThrustStableAltitude;
	}

	public double bodyDrag(double angleRadians, PositionVector direction) {
		return airdensity * dragCoefficient * areaExposedToDrag(angleRadians, direction);
	}
	
	public double areaExposedToDrag(double angleRadians, PositionVector direction) {
			
		Vector2D v = new Vector2D(Math.abs(direction.getX()) + Math.abs(direction.getY()), Math.abs(direction.getZ()));
		v = v.normalize();
		
		double areaExposedFront = v.getX() * (Math.sin(angleRadians) * A_top + Math.cos(angleRadians) * A_front);
		double areaExposedTop = v.getY() * (Math.cos(angleRadians) * A_top + Math.sin(angleRadians) * A_front);
		
		return areaExposedFront + areaExposedTop;
	}
	
	/*
	 * F_drag [N] = 0.5 * p * C_drag * A * v^2
	 */
	public double currentDrag() {
		return 0.5 * bodyDrag(currentAngleOfAttack, direction) * currentSpeed * currentSpeed;
	}
	
	
	
	/*
	 *
	 */
	
	@Override
	public void setMotorControl(StatelessMotorComponent motor) {
		this.motor = motor;
	}

	@Override
	public void move(long timeBetweenMovementOperations) {
		
		if(motor.isOn() && !route.isEmpty()) {
			PositionVector position = new PositionVector(topologyComponent.getRealPosition());			
			
			double thrust = 0;
			direction = new PositionVector(0,0,1);
			
			double distz = 100 - position.getZ();
			
			if(distz < 100) {
				
				double time = ((double) timeBetweenMovementOperations) / ((double) Time.SECOND);
				
				// ASCENSION TO SAFETY HEIGHT
				thrust = motor.requestThrust(motor.getMaxThrust());
												
				double acc = ((thrust - hoverThrustRequired()) / mass ) - currentDrag();
				double speed = 0;
				
				if(acc < 0.1) {
					// normal flight
					speed = verticalAscentMaxVelocity();
				}
				else {
					// acceleration
					speed = currentSpeed + acc * time; 
				}
				
				
				System.out.println("ACCELERATION : " + acc);			
				System.out.println("SPEED : " + speed);
				
				//double dist = 0.5 * acc * (timeBetweenMovementOperations / Time.SECOND) * (timeBetweenMovementOperations / Time.SECOND);
				double dist = currentSpeed * time;
				
				//double safetydistance = speed
				
				//System.out.println();
				
				position.add(new PositionVector(0,0,dist));
				
				System.out.println("NEW POSITION : " + position);

				topologyComponent.updateCurrentLocation(position);
			}
			else {
				thrust = motor.requestThrust(hoverThrustRequired());
				topologyComponent.updateCurrentLocation(position);
			}
			
			
			
			
//			PositionVector directionToTarget = new PositionVector(route.getFirst());
//			directionToTarget.subtract(position);
//			
//			double distanceToTargetPosition = directionToTarget.getLength();
			
							
//			// 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(position); 
//				currentSpeed = 0;
//			
//				locationReached(position);
//				return;
//			}
//			
//			double timefactor = timeBetweenMovementOperations / Time.SECOND;
//			
//			currentSpeed = Math.min(distanceToTargetPosition, preferredSpeed);
//				
//			direction.normalize();
//			direction.multiplyScalar(currentSpeed * timefactor);
//
//			PositionVector newPosition = new PositionVector(position);
//			newPosition.add(direction);		
//			
//			topologyComponent.updateCurrentLocation(newPosition);
			
		}
		
		//System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);		
	}

	
	@Override
	public void setPreferredSpeed(double v_pref) {
		this.preferredSpeed = v_pref;		
	}
	
	@Override
	public double getCurrentSpeed() {
		return currentSpeed;
	}

	/**
	 * 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 minimumVelocity() {
		return 0;
	}


	@Override
	public double estimatePowerConsumption(double velocity) {
		// TODO Auto-generated method stub
		return 0;
	}



}