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

import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;

import de.tud.kom.p2psim.api.uav.MulticopterModel;
import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
import de.tud.kom.p2psim.impl.energy.components.StatelessActuatorComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;

/**
 * A model for the Intel Aero Ready-to-Fly multicopter UAV. 
 * 
 * @author Julian Zobel
 * @version 20.01.2022
 */
public class IntelAeroRTF implements MulticopterModel {

	private double mass; // kg
	private double areaTop; // m^2
	private double areaFront; // m^2
	private double dragCoefficient;
	private double maximumPitchAngleAllowed; // ° max angle
	private double maximumVerticalVelocityAllowed; // m/s
	
	private double optimalCurveVelocity;	
	private double optimalVelocity = -1;
	
	private double maximumTurnAngle; // 90° per second turn angle
	
	private StatelessActuatorComponent motor;
	
	public IntelAeroRTF(double massTotal, 
			double areaTop, double areaFront, double UAVDragCoefficient, double maximumPitchAngleAllowed, 
			double maximumDescentVelocityAllowed, double maximumTurnAngle, double optimalCurveVelocity) {
		this.mass = massTotal;
		this.areaTop = areaTop;
		this.areaFront = areaFront;
		this.dragCoefficient = UAVDragCoefficient;
		this.maximumPitchAngleAllowed = maximumPitchAngleAllowed;
		this.maximumVerticalVelocityAllowed = maximumDescentVelocityAllowed;
		this.maximumTurnAngle = maximumTurnAngle;
		this.optimalCurveVelocity = optimalCurveVelocity;
	}
	
	/**
	 * Estimate the pitch angle (angle of attack) required to get the target velocity. 
	 * Angle precision is 1/4 degree. 
	 * 
	 * @param velocity
	 * @return
	 */
	@Override
	public double pitchAngleForHorizontalVelocity(double velocity) {
		
		int low = 0;
		int high = Integer.MAX_VALUE;
		
		double vsquared = (velocity * velocity);
		
		for(int i = 0; i <= ((int) Math.toDegrees(maximumPitchAngleAllowed)); i++) {
			
			double v2 = 2 * hoverThrust() * Math.tan(Math.toRadians(i)) / bodyDrag(Math.toRadians(i), new PositionVector(1,0,0));
						
			if(v2 > vsquared && i < high) {				
				high = i;
			}
			else if(v2 < vsquared && i >= low) {
				low = i;				
			}
			else if(v2 == vsquared ) {
				return Math.toRadians(i);
			}
			
		}
		
		if(high < Integer.MAX_VALUE) {
			double lo = low;
			double hi = high;
			
			double nearest = -1;
			double nearestDiff = Double.MAX_VALUE;
			
			double step = (hi - lo) / 4;
			
			for(int i = 0; i < 4; i++) {
			
				double d = lo + i * step;
				
				double v2 = 2 * hoverThrust() * Math.tan(Math.toRadians(d)) / bodyDrag(Math.toRadians(d), new PositionVector(1,0,0));
				
				double diff = Math.abs(((velocity * velocity) - v2));
				
				if(diff < nearestDiff || (lo == 0 && i == 1)) {
					nearestDiff = diff;
					nearest = d;
				}			
			}
				
			return Math.toRadians(nearest);
		}
			
		return maximumPitchAngleAllowed;	
	}
	
	@Override
	public double thrustForHorizontalVelocity(double velocity) {
		if(velocity == 0) {
			return hoverThrust();
		}
		else if(velocity > horizontalMaxVelocity()) {
			return -1;
		}
		else if(velocity < horizontalMinVelocity()) {
			return -1;
		}
		else {
			double estimateAngle = pitchAngleForHorizontalVelocity(velocity);
			double estimatedDrag = forwardDrag(velocity, estimateAngle);					
			double requiredThrust = Math.sqrt(hoverThrust() * hoverThrust() + estimatedDrag * estimatedDrag);
			return requiredThrust;
		}
	}
	
	@Override
	public double horizontalMinVelocity() {
		return Math.sqrt(2 * hoverThrust() * Math.tan(Math.toRadians(0.25)) / bodyDrag(Math.toRadians(0.25), new PositionVector(1,0,0)));
	}

	@Override
	public double powerConsumptionWatt(double velocity) {
		
		if(velocity == 0) {
			return motor.estimatePowerConsumptionWatt(hoverThrust());
		}
		else if(velocity > horizontalMaxVelocity()) {
			return -1;
		}
		else if(velocity < horizontalMinVelocity()) {
			return -1;
		}
		else {			
			double requiredThrust = thrustForHorizontalVelocity(velocity);
			double wattage = motor.estimatePowerConsumptionWatt(requiredThrust);		
			return wattage;
		}				
	}
	
	/*
	 * 
	 */	
	// Thrust required to hold the maximum allowed descent velocity
	@Override
	public double verticalDescentThrust(double velocity) {		
		return hoverThrust() - verticalDrag(velocity);
	}
	
	// Drag (upward lift) induced on the frame by moving with the maximum allowed descent velocity
	//  0.5 * p * C * A * v^2
	private double verticalDrag(double velocity) {
		assert velocity <= maximumVerticalVelocityAllowed;
		assert velocity > 0;
		
		return dragInducedByMovement(0, new PositionVector(0,0,1), velocity);
		
	}
	
	@Override
	public double verticalDescentMaxAcceleration() {
		return GRAVITY - verticalDrag(maximumVerticalVelocityAllowed) / mass;
	}
	
	@Override
	public double verticalDescentMaxVelocity() {
		double technicalMaximum = Math.sqrt(2.0 * (hoverThrust()) / bodyDrag(0, new PositionVector(0,0,-1)));
		return Math.min(technicalMaximum, maximumVerticalVelocityAllowed);
	}
	
	@Override
	public double verticalAscentMaxAcceleration() {
		return (motor.getMaxThrust() - hoverThrust()) / mass;
	}
	
	@Override
	public double verticalAscentMaxVelocity() {			
		double technicalMaximum = Math.sqrt(2.0 * (motor.getMaxThrust() - hoverThrust()) / bodyDrag(0, new PositionVector(0,0,1)));
		return Math.min(technicalMaximum, maximumVerticalVelocityAllowed);
	}	
	
	@Override
	public double verticalAscentThrust(double velocity) {		
		return hoverThrust() + verticalDrag(velocity);
	}
	
	
	@Override
	public double hoverThrust()  {		
		return mass * GRAVITY;		
	}
	
	@Override
	public double horizontalMaxVelocity() {		
		double horizontalThrust = horizontalComponentMaxThrust();				
		double maxVelocity = Math.sqrt( (2.0 * horizontalThrust) / bodyDrag(maximumPitchAngleAllowed, new PositionVector(1,0,0)));				
		return maxVelocity;
	}
		
	private double horizontalComponentMaxThrust() {		
		// hoverthrust / cos => amount of thrust in horizonal direction with °angle		
		double stableAltitudeMaximumTotalThrust = thrustForHorizontalMaxVelocity(); 
		// fraction of total thrust in horizonal (forward) direction with °angle
		double maximumHorizontalThrustStableAltitude = stableAltitudeMaximumTotalThrust * Math.sin(maximumPitchAngleAllowed);		
		return maximumHorizontalThrustStableAltitude;
	}
	
	/**
	 * Acceleration is thrust from motors minus drag from speed
	 * Drag: rough approximation for velocity induced drag when accelerating (half of maximum velocity drag)	
	 */
	@Override
	public double horizontalMaxAcceleration() {
		return horizontalComponentMaxThrust() - forwardDrag(horizontalMaxVelocity() / 2, maximumPitchAngleAllowed)  / mass;
	}
	
	/**
	 * Acceleration is thrust from motors minus drag from speed
	 * Drag: rough approximation for velocity induced drag when accelerating (half of maximum velocity drag)	
	 */
	@Override
	public double horizontalMaxDeceleration() {
		return horizontalComponentMaxThrust() + forwardDrag(horizontalMaxVelocity() / 2, maximumPitchAngleAllowed)  / mass;
	}

	@Override
	public double thrustForHorizontalMaxVelocity() {
		return hoverThrust() / Math.cos(maximumPitchAngleAllowed);
	}
	
	private double bodyDrag(double angleRadians, PositionVector direction) {
		return AIRDENSITY * dragCoefficient * areaExposedToDrag(angleRadians, direction);
	}
	
	private 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) * areaTop + Math.cos(angleRadians) * areaFront );
		double areaExposedTop = v.getY() * (Math.cos(angleRadians) * areaTop + Math.sin(angleRadians) * areaFront);
		
		return areaExposedFront + areaExposedTop;
	}
	
	/*
	 * F_drag [N] = 0.5 * p * C_drag * A * v^2
	 */
	private double dragInducedByMovement(double angleOfAttack, PositionVector direction, double velocity) {
		return 0.5 * bodyDrag(angleOfAttack, direction) * velocity * velocity;
	}
	
	/**
	 * Calculate the drag induced on the UAV with a given velocity and an angle of attack (in radians) moving forward horizontally.
	 * 
	 * @param velocity
	 * @param angleInRadians
	 * @return
	 */
	private double forwardDrag(double velocity, double angleInRadians) {
		return dragInducedByMovement(angleInRadians,  new PositionVector(1,0,0), velocity);
	}
	
	@Override
	public double optimalSpeed() {
		
		if(optimalVelocity == -1) {
			double MIN = Double.MAX_VALUE;		
			double opt = -1;
			
			double min = horizontalMinVelocity();
			double max = horizontalMaxVelocity();
			
			int steps = 50;			
			double stepsize = ( max-min) / steps;
			
			for (int i = 0; i < steps; i++) {
				double speed =  min + i * stepsize;
				double energyPerMeter = powerConsumptionWatt(speed) / speed ;
				
				if(energyPerMeter < MIN) {
					MIN = energyPerMeter;
					opt = speed;
				}
			}
					
			optimalVelocity = opt;
		}
		
		return optimalVelocity;		
	}

	@Override
	public boolean isOn() {
		return motor.isOn();
	}
	
	/////
	@Override
	public void setMotor(ActuatorComponent actuator) {
		assert actuator instanceof StatelessActuatorComponent;
		this.motor = (StatelessActuatorComponent) actuator;
	}
	
	@Override
	public double getMaximumTurnAngle() {
		return maximumTurnAngle;
	}
	
	@Override
	public void thrust(double thrust, long phaseTime) {
		motor.requestThrust(thrust, phaseTime);
	}

	@Override
	public double getCurveRadius(double curveVelocity) {
		return Math.pow(curveVelocity, 2) / getCurveCentripedalAcceleration(curveVelocity);
	}

	@Override
	public double getCurveCentripedalAcceleration(double curveVelocity) {
		return (horizontalComponentMaxThrust() -   thrustForHorizontalVelocity(curveVelocity)) / mass;
	}

	@Override
	public double maxThrust() {
		return motor.getMaxThrust();
	}

	@Override
	public double minThrust() {
		return motor.getMinThrust();
	}
	
	@Override
	public double curveVelocity() {
		return optimalCurveVelocity;
	}
	
	/**
	 * Factory for the Intel UAV
	 * 
	 * @author Julian Zobel
	 * @version 1.0, 14.01.2020
	 */
	public static class Factory implements MulticopterModel.Factory {
		
		// standard values
		private double massTotal = 1.465; // kg		
		private double areaTop = 0.245; // m^2
		private double areaFront = 0.1; // m^2
		private double UAVDragCoefficient = 0.7;
		private double maximumPitchAngleAllowed = Math.toRadians(60); // ° max angle
		private double maximumVerticalVelocityAllowed = 10; // m/s
		private double maximumTurnAngle = Math.toRadians(90); // 90° per second turn angle
		private double optimalCurveVelocity = 8;
		
		public void setMassTotal(double massTotal) {
			this.massTotal = massTotal;
		}

		public void setAreaTop(double areaTop) {
			this.areaTop = areaTop;
		}

		public void setAreaFront(double areaFront) {
			this.areaFront = areaFront;
		}

		public void setUAVDragCoefficient(double uAVDragCoefficient) {
			UAVDragCoefficient = uAVDragCoefficient;
		}

		public void setMaximumPitchAngleAllowed(double maximumPitchAngleAllowed) {
			this.maximumPitchAngleAllowed = Math.toRadians(maximumPitchAngleAllowed);
		}

		public void setMaximumVerticalVelocityAllowed(
				double maximumDecentVelocityAllowed) {
			this.maximumVerticalVelocityAllowed = maximumDecentVelocityAllowed;
		}

		public void setMaximumTurnAngle(double maximumTurnAngle) {
			this.maximumTurnAngle = Math.toRadians(maximumTurnAngle);
		}

		public MulticopterModel createComponent() {
			return new IntelAeroRTF(massTotal, areaTop, areaFront, 
					UAVDragCoefficient, maximumPitchAngleAllowed, maximumVerticalVelocityAllowed, maximumTurnAngle, optimalCurveVelocity);
		}

		public void setOptimalCurveVelocity(double optimalCurveVelocity) {
			this.optimalCurveVelocity = optimalCurveVelocity;
		}		
	}
}
