/**
 * 
 */
package papabench.scj.commons.data.impl;

import static papabench.scj.commons.conf.AirframeParametersConf.CARROT;
import static papabench.scj.commons.conf.AirframeParametersConf.NOMINAL_AIRSPEED;
import papabench.scj.autopilot.conf.LateralFlightMode;
import papabench.scj.autopilot.conf.VerticalFlightMode;
import papabench.scj.autopilot.data.Position3D;
import papabench.scj.autopilot.modules.AutopilotStatus;
import papabench.scj.autopilot.modules.Estimator;
import papabench.scj.autopilot.modules.Navigator;
import papabench.scj.utils.LogUtils;
import papabench.scj.utils.MathUtils;

/**
 * Generalized implementation of navigator commands.
 * 
 * FIXME AbstractFLightPlan should inherit from this method!
 * NOTE: this file is partly based on Paparazzi code (method prefix nav) and partly on Papabench code.
 * 
 * @author Michal Malohlava
 * 
 */
public abstract class NavigatorCommands {
		
	protected abstract Navigator navigator();
	protected abstract Estimator estimator();
	protected abstract AutopilotStatus status();
	protected abstract Position3D WP(int n);
	protected abstract int getLastWPNumber();
	protected abstract Position3D getLastPosition();
	
	private float carrot = 0;

	/**
	 * Set the climb control to auto-throttle with the specified
	 * pitchpre-command
	 */
	protected final void navVerticalAutoThrottleMode(float pitch) {
		// FIXME v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \
//		navigator().setDesiredPitch(pitch);
		status().setVerticalFlightMode(VerticalFlightMode.AUTO_GAZ);
		status().setPitch(pitch);
	}

	/**
	 * Set the climb control to auto-pitch with the specified throttle
	 * pre-command
	 */
	protected final void navVerticalAutoPitchMode(int throttle) {
		// _ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH;
		// nav_throttle_setpoint = _throttle;
		navigator().setAutoPitch(true);
		navigator().setDesiredGaz(throttle);
	}

	/**
	 * Set the vertical mode to altitude control with the specified altitude
	 * setpoint and climb pre-command.
	 */
	protected final void navVerticalAltitudeMode(float alt, float preClimb) {
		// v_ctl_mode = V_CTL_MODE_AUTO_ALT;
		status().setVerticalFlightMode(VerticalFlightMode.AUTO_ALTITUDE);
		navigator().setDesiredAltitude(alt);
		navigator().setPreClimb(preClimb);
	}

	/** Set the vertical mode to climb control with the specified climb setpoint */
	protected final void navVerticalClimbMode(float climb) {
		// v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
		// v_ctl_climb_setpoint = _climb;
		status().setVerticalFlightMode(VerticalFlightMode.AUTO_CLIMB); // AUTO_CLIMB -> navigator has to obtain values
		status().setClimb(climb);

	}

	/** Set the vertical mode to fixed throttle with the specified setpoint */
	protected final void navVerticalThrottleMode(int throttle) {
		// v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
		// nav_throttle_setpoint = _throttle;
		status().setVerticalFlightMode(VerticalFlightMode.AUTO_GAZ);
		navigator().setDesiredGaz(throttle);
	}

	protected final void navHeading(float course) {
		// lateral_mode = LATERAL_MODE_COURSE;
		// h_ctl_course_setpoint = _course;
		status().setLateralFlightMode(LateralFlightMode.COURSE);
		navigator().setDesiredCourse(course);		
	}

	protected final void navAttitude(float roll) {
		// lateral_mode = LATERAL_MODE_ROLL;
		// h_ctl_roll_setpoint = _roll;
		status().setLateralFlightMode(LateralFlightMode.ROLL);		
		status().setRoll(roll);
	}
	
	/**
	 *  Decide if the UAV is approaching the current waypoint.
	 *  Computes dist2_to_wp and compare it to square carrot.
	 *  Return true if it is smaller. Else computes by scalar products if 
	 *  uav has not gone past waypoint.
	 *  Return true if it is the case.
	 */
	protected final boolean navApproachingFrom(int toWP, int fromWP, float approachingTime) {
		return navApproachingFrom(WP(toWP), WP(fromWP), approachingTime);		
	}
	protected final boolean navApproachingFrom(Position3D toWP, Position3D fromWP, float approachingTime) {
		float pwX = toWP.x - estimator().getPosition().x;
		float pwY = toWP.y - estimator().getPosition().y;
		
		float dist2ToWP = pwX*pwX + pwY*pwY;
		float minDist = approachingTime * estimator().getHorizontalSpeed().module;
		
		if (dist2ToWP < minDist*minDist) {
			return true;
		}
		
		float scalarProduct = MathUtils.scalarProduct(toWP, fromWP, estimator().getPosition());
		
		return (scalarProduct < 0.);						
	}
	
	protected final void navGotoWaypoint(int wp) {
//		  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
		  flyToWP(WP(wp));
	}
	
	protected final void navSegment(Position3D startWP, Position3D endWP) {
		navRouteXY(startWP, endWP);
	}
	
	protected final void navRouteXY(Position3D startWP, Position3D endWP) {
		float legX = endWP.x - startWP.x;
		float legY = endWP.y - startWP.y;
		float leg2 = Math.max(legX*legX+legY*legY,1f);
		float navLegProgress = ((estimator().getPosition().x - startWP.x)*legX + (estimator().getPosition().y - startWP.y)*legY) / leg2;
		float navLegLength = (float) Math.sqrt(leg2);
		
		/* distance of CARROT */
		float carrot = CARROT * NOMINAL_AIRSPEED;
		
//		navLegProgress += Math.max(carrot/navLegLength, 0);
//		navInSegment = true;
//		navSegment.1 = startWP;
//		navSegment.2 = endWP;
//		
//		horizontal_mode = ROUTE;
//		
//		flyToXY(startWP.x+navLegProgress*legX + navShift*legY/navLegLength, startWP.y+navLegProgress*legY-navShift*legX/navLegLength);
		
	}
	
	protected final void killThrottle() {		
	}
	
	protected final boolean approaching(int wp) {
		return approaching(WP(wp));
	}
	
	protected final boolean approaching(Position3D wp) {
		return approaching(wp, getLastPosition());
	}
	
	protected final boolean approaching(Position3D toWP, Position3D fromWP) {
		LogUtils.log(this, "Approaching to=" + toWP + " from="+fromWP);
		float pwX = toWP.x - estimator().getPosition().x;
		float pwY = toWP.y - estimator().getPosition().y;
		
		float dist2ToWP = pwX*pwX + pwY*pwY;
		
		carrot = CARROT * estimator().getHorizontalSpeed().module;
		carrot = carrot < 40 ? 40 : carrot; // FIXME what is number 10 (40 in original version)? replace by constant with user friendly name
		
		if (dist2ToWP < carrot*carrot) {
			return true;
		}
		
		float scalarProduct = MathUtils.scalarProduct(toWP, fromWP, estimator().getPosition());

		return (scalarProduct < 0.);
	}
	
	protected final void flyToWP(int wp) {
		flyToWP(WP(wp));		
	}
	
	protected final void flyToWP(Position3D wp) {
		flyToXY(wp.x, wp.y);		
	}
	
	// WARNING - this method is implemented according to PapaBench code (it is not compatible with paparazzi)
	protected final void flyToXY(float x, float y) {
		
		navigator().setDesiredPosition(x,y);		
		navigator().setDesiredCourse((float) (Math.PI/2 - Math.atan2(y-estimator().getPosition().y, x-estimator().getPosition().x)));
		LogUtils.log(this, "FlyToWP: (" + x + ", "+ y + "), course = " + Math.toDegrees(navigator().getDesiredCourse()));
	}
	
	protected final void routeTo(int fromWP, int toWP) {
		routeTo(WP(fromWP), WP(toWP));		
	}
	
	protected final void routeTo(Position3D fromWP, Position3D toWP) {
		float legX = toWP.x - fromWP.x;
		float legY = toWP.y - fromWP.y;
		float leg2 = Math.max(legX*legX+legY*legY,1f);
		float alpha = ((estimator().getPosition().x - fromWP.x) * legX + (estimator().getPosition().y - fromWP.y)*legY) / leg2;
		alpha = Math.max(alpha, 0);
		float leg = (float) Math.sqrt(leg2);
		alpha += Math.max(carrot/2, 0f ); /* carrot computed in approaching() */
		alpha = Math.min(1, alpha);
		
		flyToXY(fromWP.x + alpha*legX, fromWP.y + alpha*legY);
	}
}
