# == Things for auto-hover:
#
#   Control hover height/vertical speed using throttle.
#
#   Control hover forwards speed/position using elevators.
#
#   Control hover sideways speed/position using ailerons.
#
#   Control hover heading/rotation speed using rudder.
#
#   Control AoA using nozzle angle.
#
#   Make semi-auto conventional landing using autopilot to control vertical
#   speed.
#
#   Make nozzles always vertical to allow Farley climb.
#
#
# == Units:
#
#   User-supplied values are generally in ft, fps, or kts.
#
#   All internal values are in SI units unless specified otherwise.
#
#
# == Properties
#
#   === Hover vertical
#       /controls/auto-hover/y-mode:
#           off
#               Auto-hover height control off.
#           height
#               Control altitude - see /controls/auto-hover/y-height-target.
#           speed
#               Control vertical speed - see /controls/auto-hover/y-speed-target.
#       
#       /controls/auto-hover/y-height-target
#           Target height in ft.
#       
#       /controls/auto-hover/y-speed-target
#           Target vertical speed in fps (+ve upwards).
#
#   === Hover horizontal
#       /controls/auto-hover/x-mode
#           off
#               Auto-hover sideways control off.
#           speed
#               Control sideways speed.
#           target
#               Control horizontal position.
#       
#       /controls/auto-hover/x-speed-target
#           Target sideways speed in kts (+ve to right).
#       
#       /controls/auto-hover/z-mode
#           off
#               Auto-hover forwards control off.
#           speed
#               Control forwards speed.
#           target
#               Control absolute position.
#       
#       /controls/auto-hover/z-speed-target
#           Target forwards speed in kts.
#       
#       /controls/auto-hover/xz-target-latitude
#       /controls/auto-hover/xz-target-longitude
#           Target position.
#       
#       /controls/auto-hover/x-airground-mode:
#       /controls/auto-hover/z-airground-mode:
#           'air'
#               Control air speed.
#           'ground'
#               Control ground speed (default).
#
#   === Hover rotation
#   
#       /controls/auto-hover/rotation-mode
#           ''
#               Rotation control off.
#           speed pid
#           heading pid
#               Let PID controllers control rotation.
#           speed
#               Maintain rotation speed.
#           heading
#               Maintain heading heading.
#       
#       /controls/auto-hover/rotation-speed-target
#           Target rotation speed in degrees/sec.
#   
#       /controls/auto-hover/rotation-heading-target
#           Target heading in degrees.
#
#
#   === Vspeed Land
#
#       /controls/auto-hover/vspeed-land
#           ''
#               disabled.
#           <anything else>
#               enabled.
#
#
#   === AoA nozzles
#
#       /controls/auto-hover/aoa-nozzles-target
#           Target AoA or 0.
#
#       Implemented as PID controller - see ../Systems/Autopilot.xml
#

printf( 'autohover loading...');


# List of all windows that we have created. Used for gui style changes, or if
# we are reloaded.
#
var windows = [];
var windows_cleanup = func() {
    foreach( var w; windows) {
        window_close( w);
    }
    windows = [];
}

# Wrap setlistener() so that we keep a list of all listeners, to allow reload.
#
# Note doesn't cope with unregistering of listeners.
#
var setlistener_original = setlistener;
var listeners = [];
var setlistener = func( property, fn, startup=0, runtime=1) {
    var handle = setlistener_original(property, fn, startup, runtime);
    append(listeners, handle);
    return handle;
}
var listeners_cleanup = func() {
    foreach( var i; listeners) {
        #printf("listeners_cleanup: i=%s", i);
        # Hack: don't fail if <i> has already been removed. Really need to
        # write a wrapper for removelistener() so we keep track of things.
        call( func removelistener(i),  nil, var err=[]);
    }
    listeners = [];
}


# Wrapper for maketimer() that allows reload.
#
var timers = [];
var maketimer_original = maketimer;
maketimer = func( t, fn) {
    handle = maketimer_original( t, fn);
    append( timers, handle);
    return handle;
}
var timers_cleanup = func() {
    foreach( var i; timers) {
        i.stop();
    }
    timers = [];
}


var reload = func() {
    printf("reload(): reloading autohover...");
    printf("reload(): cancelling listeners...");
    listeners_cleanup();
    printf("reload(): cancelling timers...");
    timers_cleanup();
    printf("reload(): closing all windows...");
    windows_cleanup();
    setlistener = setlistener_original;
    maketimer = maketimer_original;
    var path = getprop("/sim/aircraft-dir") ~ "/Nasal/autohover.nas";
    printf("reloading: '%s'", path);
    io.load_nasal(path, "autohover");
    printf("reload(): reloading autohover completed.");
    gui.popupTip( 'Autohover reloaded', 5);
}


# Some constants for conversion between imperial/metric etc.
#
var knots2si = 1852.0/3600;     # Knots to metres/sec.
var ft2si = 12 * 2.54 / 100;    # Feet to metres
var lbs2si = 0.45359237;        # Pounds to kilogrammes.
var gravity = 9.81;             # Gravity in m/s/s.


var clip_abs = func(x, max) {
    if (x > max)    return max;
    if (x < -max)   return -max;
    return x;
}

var window_close = func( window) {
    window.clear();
    window.close();
    
    # The above sometimes only partially works; doing it again using a timer
    # seems to improve things...
    #
    var timer = maketimer( 0, func { window.close() });
    timer.singleShot = 1;
    timer.start();
    
}

# Sets property if it is currently nil. Useful to allow override from
# command-line with --prop:<name>=<value>.
#
var set_if_nil = func( node, value) {
    v = node.getValue();
    if (v == nil) {
        node.setValue( value);
    }
}

var g_replay_state_prop = props.globals.getNode( '/sim/replay/replay-state', 1);

# This governs whether we use PID controllers for final control of xz and
# rotation.
#
# Note that we (yet) have a PID controller for vertical speed.
#
var g_prop_pid = props.globals.getNode( 'controls/auto-hover/pid', 1);
set_if_nil( g_prop_pid, 'false');


# We generate extra information about wheels on the ground. We disable control
# when the first wheel touches the ground. We enable control when the first
# wheel leaves the ground.
#
var g_wow_any_prop = props.globals.getNode( 'gear/wow_any', 1);
var g_wow_all_prop = props.globals.getNode( 'gear/wow_all', 1);
var g_wow_n_prop = props.globals.getNode( 'gear/wow_n', 1);     # Number of wheels on ground.
var g_wow_dn_prop = props.globals.getNode( 'gear/wow_dn', 1);   # Most recent change to wow_n.

var wow_fn = func {
    wow0 = getprop( 'gear/gear[0]/wow') or 0;
    wow1 = getprop( 'gear/gear[1]/wow') or 0;
    wow2 = getprop( 'gear/gear[2]/wow') or 0;
    wow3 = getprop( 'gear/gear[3]/wow') or 0;
    var n = 0;
    if (wow0)   n += 1;
    if (wow1)   n += 1;
    if (wow2)   n += 1;
    if (wow3)   n += 1;
    var prev_n = g_wow_n_prop.getValue();
    if (prev_n == nil) {
        prev_n = 0;
    }
    g_wow_any_prop.setValue( wow0 or wow1 or wow2 or wow3);
    g_wow_all_prop.setValue( wow0 and wow1 and wow2 and wow3);
    g_wow_n_prop.setValue( n);
    if (n != prev_n) {
        g_wow_dn_prop.setValue( n - prev_n);
    }
}

var wow_start = func() {
    setlistener("gear/gear[0]/wow", wow_fn, 1, 0);
    setlistener("gear/gear[1]/wow", wow_fn, 1, 0);
    setlistener("gear/gear[2]/wow", wow_fn, 1, 0);
    setlistener("gear/gear[3]/wow", wow_fn, 1, 0);
}


var set_font = func(w) {
    w.font = getprop("/sim/gui/selected-style/fonts/message-display/name");
    w.fontsize = getprop("/sim/gui/selected-style/fonts/message-display/size");
    #printf("w.font=%s w.fontsize=%s", view.str(w.font), view.str(w.fontsize));
}
    
var set_fonts = func() {
    foreach(var w; windows) {
        set_font(w);
        w._redraw_();
    }
}

setlistener("/sim/gui/current-style", set_fonts);

var make_window = func(x, y) {
    var w = screen.window.new(
        x,
        y,
        1,      # num of lines.
        9999,   # timeout
        );
    w.bg = [0,0,0,.5]; # black alpha .5 background
    w.fg = [1, 1, 1, 0.5];
    # Setting font here doesn't appear to make any difference.
    set_font(w);
    append(windows, w);
    return w;
}

# Returns text describing target vs actual.
var make_text = func(prefix_long, prefix_small, actual, target, units, fmt) {
    if (display_none()) {
        return '';
    }
    
    if (display_small()) {
        var target_text = sprintf(fmt, target);
        return sprintf('%s%s %s', prefix_small, target_text, units);
    }
    
    var target_text = sprintf(fmt, target);
    var actual_text = sprintf(fmt, actual);
    if (target == nil) {
        var delta_text = '';
    }
    else {
        var delta_text = sprintf(fmt, actual - target);
    }
    delta_text = string.trim(delta_text);
    c = left(delta_text, 1);
    if (c == '-') {
        delta_text = right(delta_text, size(delta_text) - 1);
    }
    else if ( c == '+') {
        delta_text = right(delta_text, size(delta_text) - 1);
    }
    else {
        c = '+';
    }
    t = sprintf('%s%s %s. (%s %s => %s %s)', prefix_long, target_text, units, c, delta_text, actual_text, units,);
    return t;
}

#
var make_text2 = func(
        prefix_long, prefix_small,
        actual, target, units, fmt,
        other, other_value, other_units, other_fmt,
        ) {
    if (display_none()) {
        return '';
    }
    
    var text = '';
    if (other) {
        var other_value_text = sprintf(other_fmt, other_value);
        if (display_small()) {
            text = sprintf('%s%s %s', prefix_small, other_value_text, other_units);
        }
        else {
            text = sprintf('%s%s %s', prefix_long, other_value_text, other_units);
            text = text ~ ': ' ~ make_text('', '', actual, target, units, fmt);
        }
    }
    else {
        text = make_text(prefix_long, prefix_small, actual, target, units, fmt);
    }
    return text;
}

# A hack to allow us to modify behaviour at high speeds. Doesn't really work
# yet.
#
var speed_correction = func() {
    var s = props.globals.getValue('/velocities/equivalent-kt') * knots2si;
    s -= 70 * knots2si;
    if (s < 0) return 0;
    var sc = math.sqrt( s / (600*knots2si));
    return sc;
}

var auto_hover_aoa_nozzles_window       = make_window( 20, 175);
var auto_hover_xz_target_prime_window   = make_window( 20, 150);
var auto_hover_height_window            = make_window( 20, 125);
var auto_hover_z_window                 = make_window( 20, 100);
var auto_hover_x_window                 = make_window( 20, 75);
var auto_hover_rotation_window          = make_window( 20, 50);

var display = func() {
    var v = props.globals.getValue('/controls/auto-hover/display');
    if (v == nil) {
        v = 0;
        props.globals.setValue('/controls/auto-hover/display', v);
    }
    return v;
}

var auto_hover_dsplay_cycle = func(delta) {
    var d = display();
    d += delta;
    d = math.mod(d, 3);
    props.globals.setValue('controls/auto-hover/display', d);
}

var display_none = func() {
    return display() == 0;
}

var display_small = func() {
    return display() == 1;
}

var display_large = func() {
    return display() == 2;
}

props.globals.setValue('/controls/auto-hover/z-speed-error', 0);

var auto_hover_target_pos = nil;


# Distance from aircraft origin (the tip of the boom) and midpoint between
# the two main wheels. Used to allows us to place wheel midpoint at target
# position.
#
# See Harrier-GR3.xml for offsets of two main gears - these are offsets x=-4.91
# and x=-8.25.
#
# [Why is aircraft fore/aft called 'z' in properties, but 'x' in model?]
#
var z_offset = 6.58;


# Class for setting a control (e.g. an elevator) whose derivative (wrt to time)
# is proportional to third derivative (wrt time) of a target value (typically a
# speed).
#
# Params:
#
#   name
#       Name to use inside property names.
#       We read/write various properties, such as:
#           /controls/auto-hover/<name>-mode
#           /controls/auto-hover/<name>-airground-mode
#           /controls/auto-hover/<name>-speed-target
#           /controls/auto-hover/<name>-speed-target-delta
#   name_user
#   name_user_small
#       Human-friendly names to use in displays.
#   period
#       Delay in seconds between updates.
#   mode_name
#       Name of property that controls whether we use air speed or ground
#       speed. Typically modified by the user.
#   airspeed_get
#   groundspeed_get
#       Callables that returns the airspeed/groundspeed that we are trying to
#       control. This is a callable, rather than a property name, so that we
#       can be used with things that aren't available directly but must instead
#       be calculated, e.g. lateral airspeed.
#   control_name
#       Name of property that we modify in order to control the speed, e.g. the
#       elevator property.
#   control_smoothing
#       Sets the smoothing we apply to <control> to crudely model the delay in
#       it affecting <speed>.
#   t_deriv
#       Time in seconds over which we notionally expect our corrections to take
#       affect. This determines the size of changes to <control>.
#   window
#       We write status messages to this window.
#
# E.g. when hovering, forward speed is controlled by pitch. Forwards
# acceleration is roughly proportional to pitch angle (measured downward), and
# the derivative of pitch angle (wrt time) is roughly proportional to elevator
# value.
#
# So if D foo is dfoo/ft:
#
#   D speed = pitch
#   D pitch = elevator - elevator0
#
# and:
#
#   DD speed = elevator - elevator0
#
# We don't know elevator0 (it's the neutral elevator position), so we
# differentiate again to get something we can implement:
#
#   DDD speed = D elevator
#
# For each iteration, we calculate the current DDD speed. And we also determine
# a target DDD speed, based on the target speed or accelaration, and use this
# to increment/decrement the elevator.
#
# There is usually a delay between changing the elevator and seeing the affect
# on pitch and then on accelaration. We model this by using a slightly smoothed
# and delayed 'effective elevator', determined by the <control_smoothing>
# param.
#
var auto_hover_speed = {
    new: func(
            name,
            name_user,
            name_user_small,
            period,
            airspeed_get,
            groundspeed_get,
            control_name,
            control_smoothing,
            t_deriv,
            window,
            debug=0,
            ) {
        var me = { parents:[auto_hover_speed]};
        
        me.prop_mode                    = props.globals.getNode( sprintf( 'controls/auto-hover/%s-mode', name), 1);
        me.prop_airground_mode          = props.globals.getNode( sprintf( 'controls/auto-hover/%s-airground-mode', name), 1);
        me.prop_speed_target            = props.globals.getNode( sprintf( 'controls/auto-hover/%s-speed-target', name), 1);
        me.prop_accel_target            = props.globals.getNode( sprintf( 'controls/auto-hover/%s-accel-fpss-target', name), 1);
        me.prop_target_latitude         = props.globals.getNode( 'controls/auto-hover/xz-target-latitude', 1);
        me.prop_target_longitude        = props.globals.getNode( 'controls/auto-hover/xz-target-longitude', 1);
        me.prop_z_target_distance_ft    = props.globals.getNode( 'controls/auto-hover/z-target-distance-ft', 1);
        me.prop_x_target_distance_ft    = props.globals.getNode( 'controls/auto-hover/x-target-distance-ft', 1);
        me.prop_orientation_heading     = props.globals.getNode( 'orientation/heading-deg', 1);
        me.prop_control                 = props.globals.getNode( control_name);
        me.prop_pid                     = props.globals.getNode( sprintf( 'controls/auto-hover/%s-pid', name), 1);
        
        if (name == 'x') {
            me.prop_roll = props.globals.getNode( 'orientation/roll-deg', 1);
            me.prop_roll_rate = props.globals.getNode( sprintf( 'orientation/roll-rate-degps', 1));
        }
        if (name == 'z') {
            me.prop_pitch = props.globals.getNode( 'orientation/pitch-deg', 1);
            me.prop_nozzles = props.globals.getNode( 'controls/engines/engine/mixture', 1);
        }
        
        me.name = name;
        me.name_user = name_user;
        me.name_user_small = name_user_small;
        me.period = period;
        me.airspeed_get = airspeed_get;
        me.groundspeed_get = groundspeed_get;
        me.control_smoothing = control_smoothing;
        me.t_deriv = t_deriv;
        me.window = window;
        me.debug = debug;
        
        me.speed_prev = 0;
        me.accel_prev = 0;
        
        me.mode = 'off';
        set_if_nil( me.prop_mode, me.mode);
        
        set_if_nil( me.prop_airground_mode, 'ground');
        set_if_nil( me.prop_speed_target, '');
        me.prop_pid.setValue( 'false');
        
        me.control = 0;
        me.control_smoothed = 0;
        
        me.timer = maketimer(0, func { me.do()});
        me.timer.singleShot = 1;
        
        return me;
    },
    
    start: func() {
        setlistener( me.prop_mode, func() { me.listener()}, 1);
        setlistener( g_wow_any_prop, func() { me.listener()}, 1);
        setlistener( g_wow_all_prop, func() { me.listener()}, 1);
        setlistener( g_replay_state_prop, func() { me.listener()}, 1);
        setlistener( g_prop_pid, func() { me.listener()}, 1);
    },
    
    listener: func() {
        var mode_prev = me.mode;
        me.mode = me.prop_mode.getValue();
        if ( g_replay_state_prop.getValue()
                or g_wow_all_prop.getValue()
                or (g_wow_any_prop.getValue() and g_wow_dn_prop.getValue() > 0)
                ) {
            me.mode = 'off';
        }
        
        if (mode_prev == 'off' and me.mode != 'off') {
            me.control_smoothed = 0;
            me.timer.restart( 0);
        }
        else if (mode_prev != 'off' and me.mode == 'off') {
            me.timer.stop();
            window_close( me.window);
        }
        
        me.prop_pid.setValue( me.mode != 'off' and g_prop_pid.getValue());
    },
        
    do: func() {
        
        var mode = me.mode;
        
        if (mode == 'off' or mode == 'ground speed') {
            printf( '*** did not expect mode=%s ', mode);
            return;
        }
        
        # We are active.
        if (0 and me.debug) printf("me=%s mode=%s me.mode_prev=%s", me, mode, me.mode_prev);
        
        var airground_mode = me.prop_airground_mode.getValue();
        if (airground_mode == 'air')            speed = me.airspeed_get();
        else if (airground_mode == 'ground')    speed = me.groundspeed_get();
        else {
            printf('unrecognised airground_mode: %s', airground_mode);
            return;
        }
        
        var speed_target = me.prop_speed_target.getValue();
        if (speed_target == nil or speed_target == '') {
            speed_target = speed;
            me.prop_speed_target.setValue( speed_target / knots2si);
        }
        
        if (mode == 'speed' or mode == 'target') {

            if (me.speed_prev == nil) {
                me.speed_prev = speed;
            }
            var accel = (speed - me.speed_prev) / me.period;

            if (me.accel_prev == nil)   me.accel_prev = accel;

            var daccel = (accel - me.accel_prev) / me.period;

            me.speed_prev = speed;
            me.accel_prev = accel;

            var target_distance = nil;
            
            if (mode == 'target') {
                var target_lat = me.prop_target_latitude.getValue();
                var target_lon = me.prop_target_longitude.getValue();
                var pos_target = geo.Coord.new();
                pos_target.set_latlon(target_lat, target_lon);
                var pos_current = geo.aircraft_position();
                var target_bearing = pos_current.course_to( pos_target);
                var target_distance = pos_current.distance_to( pos_target);
                var aircraft_heading = me.prop_orientation_heading.getValue();
                var relative_bearing = target_bearing - aircraft_heading;
                var relative_bearing_rad = relative_bearing * math.pi / 180;
                
                # Find target distance relative to the direction we are
                # controlling. I think this may be slightly wrong - sometimes
                # the distance increases even though our actual speed is
                # in the correct direction; maybe we need to factor in the
                # the attitude to convert our speed into strict horizontal
                # direction to match our target?
                #
                if (me.name == 'z') {
                    # We are controlling forwards speed.
                    target_distance = target_distance * math.cos(relative_bearing_rad);
                    
                    # Aircraft origin is tip of front boom. Correct so that
                    # we aim to place the target halfway between the two
                    # main gears.
                    target_distance += z_offset;
                    me.prop_z_target_distance_ft.setValue( target_distance / ft2si);
                }
                else if (me.name == 'x') {
                    # We are controlling sideways speed. 
                    target_distance = target_distance * math.sin(relative_bearing_rad);
                    me.prop_x_target_distance_ft.setValue( target_distance / ft2si);
                }
                else {
                    printf('*** unrecognised me.name=%s', me.name);
                    return;
                }
                var t_distance = 22.0;
                var speed_target = target_distance / t_distance;
                
                var speed_max = 30 * knots2si;
                if (me.name == 'x')         speed_max = 20 * knots2si;
                else if (speed_target < 0)  speed_max = 20 * knots2si;
                speed_target = clip_abs(speed_target, speed_max);
                
                var speed_target_kts = speed_target / knots2si;
                var accel_target = (speed_target - speed) / me.t_deriv;
                
                if (math.sgn(accel_target) != math.sgn(target_distance) and target_distance != 0) {
                    # If we are deaccelarating, target a constant accel that
                    # would bring us stationary at the target position. This
                    # appears to work better than trying to track speed_target.
                    accel_target = -speed*speed / 2 / target_distance;  
                    #printf('fixing accel_target=%.2f', accel_target);
                }
            }
            else {

                var speed_target_kts = me.prop_speed_target.getValue();
                if (speed_target_kts == '') {
                    # No target speed set, so use current speed.
                    speed_target_kts = speed / knots2si;
                }

                var speed_target = speed_target_kts * knots2si;
                var accel_target = (speed_target - speed) / me.t_deriv;
            }

            if (1) {

                # Don't try to accelerate too much - can end up over-pitching.
                var accel_target_max = 0.5 + 0.1 * math.sqrt(abs(speed));
                accel_target = clip_abs(accel_target, accel_target_max);
                var text = sprintf('auto-hover: %s:', me.name_user);
                var highlight = 0;
            }
            
            # Try to avoid excessive pitch or roll, e.g. if vertical speed is
            # high, sideways speed control doesn't work.
            #
            if (0) {
                if (me.name == 'x') {
                    var angle_delta = me.prop_roll.getValue();
                }
                else {
                    var pitch = me.prop_pitch.getValue() - 8;
                    var nozzles = me.prop_nozzles.getValue() - 0.18;
                    var angle_delta = pitch - nozzles;
                }
                if ( math.abs(angle_delta) > 5) {
                    printf( '%s: angle_delta=%.1f', me.name, angle_delta);
                    var accel_target_old = accel_target;
                    if (angle_delta > 0)    accel_target -= (angle_delta - 5) * 0.1;
                    else                    accel_target -= (angle_delta + 5) * 0.1;
                    if (me.name == 'z') accel_target = -accel_target;
                    printf( '%s: override: angle_delta=%.1f accel_target_old=%.1f accel_target=%.1f',
                            me.name, angle_delta, accel_target_old, accel_target);
                }
            }
            
            me.prop_accel_target.setValue( accel_target / ft2si);
            
            if (display_none()) {
                me.window.close();
            }
            else {
                if (target_distance == nil) {
                    var target_distance_ft = 0;
                }
                else {
                    var target_distance_ft = target_distance / ft2si;
                }
                text = make_text2(
                        me.name_user,
                        me.name_user_small,
                        speed / knots2si,
                        speed_target_kts,
                        'kts',
                        '%+.2f',
                        mode == 'target',
                        target_distance_ft,
                        'ft',
                        '%+.1f',
                        );
                if (highlight) {
                    fg = me.window.fg;
                    me.window.fg = [1, 0, 0, 0.5];
                    me.window.write(text);
                    me.window.fg = fg;
                }
                else {
                    me.window.write(text);
                }
            }

            # Set daccel_target by looking at accel target vs actual, using
            # me.t_deriv to scale.
            var daccel_target = (accel_target - accel) / me.t_deriv;

            var e = me.control_smoothing;
            var correction = speed_correction();
            var daccel_max = 2 * (1 - correction);
            daccel_target = daccel_target * (1 - correction);
            e = e + ( (1-e) * correction);
            daccel_target = clip_abs(daccel_target, daccel_max);
            if (me.control_smoothed == nil) {
                me.control = me.prop_control.getValue();
                me.control_smoothed = me.control;
            }
            
            var control_actual = me.prop_control.getValue();
            me.control_smoothed = (1-e) * me.control_smoothed + 0 * e * me.control + e * control_actual;

            # Derivative of <control> is proportional to <daccel_target>. We
            # assume that <me.t_deriv> has scaled things; though might be
            # better to use daccel_target * me.period here?
            var control_new = me.control_smoothed + daccel_target;

            if (me.debug) {
                printf(
                        'speed: %+.5f target=%+.5f. accel: %+.5f target=%+.5f max=.%+.5f daccel: %+.5f target=%+.5f. control: raw=%+.5f smoothed=%+.5f => new=%+.5f',
                        speed,
                        speed_target,
                        accel,
                        accel_target,
                        accel_target_max,
                        daccel,
                        daccel_target,
                        me.control,
                        me.control_smoothed,
                        control_new,
                        );
            }

            # We store the control setting in our state so that we overwrite
            # any changes by the user.
            me.control = control_new;
            if (g_prop_pid.getValue() != 'true') {
                me.prop_control.setValue( me.control);
            }
        }
        
        me.timer.restart( me.period);
    },
};


var z_speed = nil;
if (z_speed == nil) {
z_speed = auto_hover_speed.new(
        name: 'z',
        name_user: 'autohover: forwards: ',
        name_user_small: '^',
        period: 0.1,
        airspeed_get: func () {
            # Forwards air-speed.
            return props.globals.getValue('/velocities/equivalent-kt') * knots2si;
        },
        groundspeed_get: func () {
            # We return the forwards ground-speed. Note that uBody-fps is
            # speed along longitudal axis of aircraft, which is different from
            # forwards ground speed if attitude is not zero deg.
            #
            # curiously, wBody-fps is speed downwards when aircraft is
            # horizontal, hence no minus in the w*sin(attitude) term.
            #
            var w = props.globals.getValue('/velocities/wBody-fps');
            var u = props.globals.getValue('/velocities/uBody-fps');
            var attitude_deg = props.globals.getValue('/orientation/pitch-deg');
            var attitude = attitude_deg * math.pi / 180;
            var ret = u * math.cos(attitude) + w * math.sin(attitude);
            return ret * ft2si;
        },
        control_name: '/controls/flight/elevator',
        control_smoothing: 0.015,
        t_deriv: 3.5,
        window: auto_hover_z_window,
        debug: 0,
        );
}
# There are a few properties that look like they give us
# the airspeed, but they don't all behave as we want.
# /instrumentation/airspeed-indicator/indicated-speed-kt seems to jump
# every few seconds. /velocities/airspeed-kt doesn't go negative when we go
# backwards.
#
# /velocities/equivalent-kt seems to behave ok

# There doesn't appear to be a property for lateral airspeed, so we calculate
# it from forward airspeed and side-slip angle.

var equivalent_kt_prop = props.globals.getNode( 'velocities/equivalent-kt', 1);
var side_slip_rad = props.globals.getNode( 'orientation/side-slip-rad', 1);
var vBody_fps = props.globals.getNode( 'velocities/vBody-fps', 1);

var x_speed = auto_hover_speed.new(
        name: 'x',
        name_user: 'autohover: sideways: ',
        name_user_small: '>',
        period: 0.1,
        airspeed_get: func () {
            # Lateral (+ve to right) air-speed.
            var z_speed_kt = equivalent_kt_prop.getValue();
            var z_speed = z_speed_kt * knots2si;
            var angle = side_slip_rad.getValue();
            var x_speed = z_speed * math.tan(angle);
            return x_speed;
        },
        groundspeed_get: func () {
            # Lateral (+ve to right) ground-speed.
            #return vbody_smooth();
            return vBody_fps.getValue() * ft2si;
        },
        control_name: '/controls/flight/aileron',
        control_smoothing: 0.015,
        t_deriv: 3.5,
        window: auto_hover_x_window,
        );




# Class for handling rudder when hovering.
#
var auto_hover_rotation = {
    new: func( window, debug=0) {
        var me = { parents:[auto_hover_rotation]};
        
        me.prop_mode                    = props.globals.getNode( 'controls/auto-hover/rotation-mode', 1);
        me.prop_rotation_heading_target = props.globals.getNode( 'controls/auto-hover/rotation-heading-target', 1);
        me.prop_rotation_speed_target   = props.globals.getNode( 'controls/auto-hover/rotation-speed-target', 1);
        me.prop_rudder                  = props.globals.getNode( 'controls/flight/rudder', 1);
        me.prop_target                  = props.globals.getNode( 'controls/auto-hover/rotation-speed-target', 1);
        me.prop_pid                     = props.globals.getNode( 'controls/auto-hover/rotation-pid', 1);
        
        me.period = 0.1;
        me.control_smoothing = 0.015;
        me.t_deriv = 3.5;
        me.window = window;
        me.debug = debug;
        
        me.mode = 'off';
        set_if_nil( me.prop_mode, me.mode);
        me.prop_pid.setValue( 'false');
        
        me.speed_prev = 0;
        me.accel_target = 0;
        me.accel_prev = 0;
        
        me.prop_target.setValue(0);
        
        me.control = 0;
        me.control_smoothed = 0;
        me.timer = maketimer( 0, func { me.do()});
        me.timer.singleShot = 1;
        
        return me;
    },
    
    start: func() {
        setlistener( me.prop_mode, func() { me.listener()}, 1);
        setlistener( g_wow_any_prop, func() { me.listener()}, 1);
        setlistener( g_wow_all_prop, func() { me.listener()}, 1);
        setlistener( g_replay_state_prop, func() { me.listener()}, 1);
    },
    
    listener: func() {
        var mode_prev = me.mode;
        me.mode = me.prop_mode.getValue();
        
        if (g_replay_state_prop.getValue()
                or g_wow_all_prop.getValue()
                or (g_wow_any_prop.getValue() and g_wow_dn_prop.getValue() > 0)
                ) {
            me.mode = 'off';
        }
        
        if (mode_prev == 'off' and me.mode != 'off') {
            # Starting from scratch; preset some state appropriately.
            me.control = me.prop_rudder.getValue();
            me.control_smoothed = me.control;
            me.timer.restart(0);
            if (g_prop_pid.getValue() == 'true') {
                me.prop_pid.setValue( 'true');
            }
        }
        if (mode_prev != 'off' and me.mode == 'off') {
            me.timer.stop();
            window_close( me.window);
            me.prop_pid.setValue( 'false');
        }
    },
        
    do: func() {
        var active = 1;
        var mode = me.mode;
        
        var heading_target = nil;
        var heading = nil;
        var speed_target = nil;

        if (mode == 'speed') {
            var speed_target = me.prop_rotation_speed_target.getValue();
        }

        if (mode == 'heading' or mode == 'heading pid') {
            var heading_target = me.prop_rotation_heading_target.getValue();
            var heading = auto_hover_rotation_heading_deg.getValue();
        }

        if (mode == 'heading') {
            var heading_delta = heading_target - heading;
            if (heading_delta > 180)    heading_delta -= 360;
            if (heading_delta < -180)   heading_delta += 360;
            var speed_target = heading_delta / me.t_deriv;
        }

        var speed = auto_hover_rotation_yaw_rate_degps.getValue();

        if (mode == 'heading' or mode == 'speed') {
            # Figure out how to get to speed_target.
            var accel = (speed - me.speed_prev) / me.period;

            me.speed_prev = speed;
            me.accel_prev = accel;

            var accel_target = (speed_target - speed) / me.t_deriv;

            # Don't try to accelerate too much - can end up unstable.
            var accel_target_max = 0.2;

            var correction = speed_correction();
            accel_target_max = 0.2 * (1 - correction);

            accel_target = clip_abs(accel_target, accel_target_max);
        }

        var text = make_text2(
                'auto-hover: rotation: ',
                'R ',
                speed,
                speed_target,
                'deg/s',
                '%+.1f',
                mode == 'heading' or mode == 'heading pid',
                heading_target,
                'deg',
                '%.1f',
                );
        if (display_none()) {
            me.window.close();
        }
        else {
            me.window.write(text);
        }

        if (mode == 'heading' or mode == 'speed') {
            var e = me.control_smoothing;
            e = e + (1-e) * correction;
            var control_actual = me.prop_rudder.getValue();
            me.control_smoothed = (1-e) * me.control_smoothed + 0 * e * me.control + e * control_actual;

            var control_new = me.control_smoothed + accel_target;

            if (me.debug) {
                printf(
                        'speed: %+.5f target=%+.5f. accel: %+.5f target=%+.5f. control: raw=%+.5f smoothed=%+.5f => new=%+.5f',
                        speed,
                        speed_target,
                        accel,
                        accel_target,
                        me.control,
                        me.control_smoothed,
                        control_new,
                        );
            }

            # We store the control setting here so that we can overwrite any
            # changes by the user.
            me.control = control_new;
            if (g_prop_pid.getValue() == 'true') {
                me.prop_target.setValue( speed_target);
            }
            else {
                me.prop_rudder.setValue(me.control);
            }
        }
        
        me.timer.restart( me.period);
    },
};

var auto_hover_rotation_yaw_rate_degps  = props.globals.getNode( 'orientation/yaw-rate-degps', 1);
var auto_hover_rotation_heading_deg     = props.globals.getNode( 'orientation/heading-deg', 1);

var auto_hover_rotation = auto_hover_rotation.new(
        window: auto_hover_rotation_window,
        debug: 0,
        );

var rotation_use_pid = props.globals.getNode( 'controls/auto-hover/rotation-use-pid', 1);

var auto_hover_rotation_current_heading = func() {
    props.globals.setValue(
            '/controls/auto-hover/rotation-heading-target',
            props.globals.getValue('orientation/heading-deg', 0),
            );
    if (rotation_use_pid.getValue()) {
        props.globals.setValue('/controls/auto-hover/rotation-mode', 'heading pid');
    }
    else {
        props.globals.setValue('/controls/auto-hover/rotation-mode', 'heading');
    }
}

var auto_hover_rotation_change = func(delta) {
    var mode_prop = props.globals.getNode('/controls/auto-hover/rotation-mode', 1);
    var mode = mode_prop.getValue();
    var speed_target_prop = props.globals.getNode('/controls/auto-hover/rotation-speed-target', 1);
    
    if (mode == 'off') {
        mode = 'speed';
        mode_prop.setValue(mode);
        speed_target_prop.setValue('');
    }
    
    if (mode == 'speed') {
        var speed_target = speed_target_prop.getValue();
        if (speed_target == '') {
            speed_target = 0;
        }
        else {
            speed_target += delta;
        }
        speed_target_prop.setValue( speed_target);
    }
    else {
        var heading_target_prop = props.globals.getNode('/controls/auto-hover/rotation-heading-target', 1);
        var heading_target = heading_target_prop.getValue();
        if (heading_target == nil) {
            return;
        }
        heading_target += delta;
        heading_target = math.round( heading_target, math.abs(delta));
        heading_target = math.mod( heading_target, 360);
        heading_target_prop.setValue( heading_target);
    }
}

var auto_hover_rotation_off = func() {
    props.globals.setValue('/controls/auto-hover/rotation-mode', 'off');
    props.globals.setValue('/controls/flight/rudder', 0);
}

# Class for controlling height when hovering.
#
#   window
#       We write status messages to this window.
#
var auto_hover_height = {
    new: func( window) {
        var me = { parents:[auto_hover_height]};
        
        me.prop_altitude_ft         = props.globals.getNode( 'position/altitude-ft', 1);
        me.prop_down_accel_fps_sec  = props.globals.getNode( 'accelerations/ned/down-accel-fps_sec', 1);
        me.prop_gear_agl_m          = props.globals.getNode( 'position/gear-agl-m', 1);
        me.prop_mode                = props.globals.getNode( 'controls/auto-hover/y-mode', 1);
        me.prop_throttle            = props.globals.getNode( 'controls/engines/engine/throttle', 1);
        me.prop_thrust_lbs          = props.globals.getNode( 'engines/engine/thrust-lbs', 1);
        me.prop_vertical_speed_fps  = props.globals.getNode( 'velocities/vertical-speed-fps', 1);
        me.prop_y_height_target     = props.globals.getNode( 'controls/auto-hover/y-height-target', 1);
        me.prop_y_speed_target      = props.globals.getNode( 'controls/auto-hover/y-speed-target', 1);
        
        me.it = 0;
        me.critical_active = 0;
        me.period = 0.25;
        me.window = window;
        
        me.mode = 'off';
        set_if_nil(me.prop_mode, me.mode);
        
        me.timer = maketimer( 0, func { me.do()});
        me.timer.singleShot = 1;
        
        return me;
    },
    
    start: func() {
        setlistener( me.prop_mode, func() { me.handle_mode_change()}, 1);
        setlistener( 'gear/wow_any', func() { me.handle_mode_change()}, 1);
        setlistener( g_replay_state_prop, func() { me.handle_mode_change()}, 1);
    },
    
    handle_mode_change: func() {
        var mode_prev = me.mode;
        me.mode = me.prop_mode.getValue();
        var wow_any = getprop( 'gear/wow_any');
        var in_replay = g_replay_state_prop.getValue();
        if (wow_any or in_replay) {
            me.mode = 'off';
        }
        
        if (mode_prev == 'off' and me.mode != 'off') {
            # Starting auto-hover. Need to initialise smoothed variables to
            # current values:
            #printf( 'starting');
            me.speed_prev = nil;
            me.accel_prev = nil;
            me.control_smoothed = nil;
            me.throttle_smoothed = me.prop_throttle.getValue();
            me.accel_max_smoothed = nil;
            
            # auto-hover AoA will disable (see Systems/Autopilot.xml) but we
            # need to turn off our AoA display explicitly here if it was previously active.
            
            auto_hover_aoa_nozzles_off();
            me.timer.restart( 0);
        }
        
        if (mode_prev != 'off' and me.mode == 'off') {
            me.timer.stop();
            window_close( me.window);
        }
    },
    
    do: func () {
        var mode = me.mode;
                
        me.it += 1;

        var accel_fpss = -1 * me.prop_down_accel_fps_sec.getValue();
        var accel = accel_fpss * ft2si;

        var speed_fps = me.prop_vertical_speed_fps.getValue();
        var speed = speed_fps * ft2si;
        # Current vertical speed in m/s.

        var height_ft = me.prop_altitude_ft.getValue();
        var height = height_ft * ft2si;
        # Current height in metres.

        var thrust_lb = me.prop_thrust_lbs.getValue();
        if (thrust_lb == nil) {
            # This can occur if we startup very early.
            #printf("*** changing thrust_lb from nil to 0");
            thrust_lb = 0;
        }
        var thrust = thrust_lb * lbs2si * gravity;
        # Current engine thrust in Newtons. We assume this is pointed directly
        # downwards, but actually it's probably ok if this isn't the case -
        # we'll scale things automatically when we modify the throttle.

        if (thrust > 0) {
            var throttle = me.prop_throttle.getValue();

            var speed_target = nil;
            var height_target_ft = nil;
            
            # Find target vertical speed:
            #
            if (mode == 'height') {

                # Calculate desired vertical speed by comparing target height and
                # current height.

                var t = 3;
                # This is vaguely a time in seconds over which we will try to reach
                # target height.

                var height_target_ft = me.prop_y_height_target.getValue();
                height_target = height_target_ft * ft2si;
                speed_target = (height_target - height) / t;
                # Our target vertical speed. Approaches zero as we reach target
                # height.

                # Restrict vertical speed to avoid problems if we are a long way
                # from the target height.
                var speed_max_fps = 200;
                var speed_max = speed_max_fps * ft2si;
                if (speed_target > speed_max)   speed_target = speed_max;
                if (speed_target < -speed_max)  speed_target = -speed_max;
            }
            else
            {
                # Use target vertical speed directly.
                var speed_target_fps = me.prop_y_speed_target.getValue();
                if (speed_target_fps == nil) {
                    printf( "*** /controls/auto-hover/y-speed-target is nil");
                }
                if (speed_target_fps != nil) {
                    speed_target = speed_target_fps * ft2si;
                }
            }

            # Decide on a target vertical acceleration to get us to the target
            # vertical speed.
            #
            # We know current acceleration and current thrust, and we know how
            # these are related (thrust - mass*g = mass*accel), and we assume
            # that thrust is proportional to throttle, so this will allow us to
            # adjust the throttle in an appropriate way.

            var t = 8;
            # This is vaguely a time in seconds over which we will try to reach
            # the target speed.

            var accel_target = (speed_target - speed) / t;

            var throttle_smoothed_e = 0.4;
            me.throttle_smoothed = me.throttle_smoothed * (1-throttle_smoothed_e) + throttle * throttle_smoothed_e;
            #
            # We use a smoothed throttle value to crudely model the engine's slow
            # response to throttle changes.
            #
            # Would be nice to do this more accurately - e.g. maybe we could
            # model the engine by tracking the throttle over time, and
            # calculate the effective average thrust over the period of time
            # that the current vertical acceleration was measured.

            var override_text_1 = '';
            var height_above_ground = me.prop_gear_agl_m.getValue();
            
            if (speed >= -7*ft2si and speed_target > -6*ft2si) {
                #printf( 'doing nothing because speed=%.2f fps', speed/ft2si);
            }
            else if (speed < 0 and throttle >= 0.1 and accel != -gravity and height_above_ground > 0) {
            
                # See whether we need to override accel_target to avoid
                # crashing into ground.
                #
                # We don't bother to try to correct for vertical air
                # resistence, though this means we may slightly overestimate
                # the maximum deaccelaration we can achieve.
                
                
                # Look at height a couple of seconds from now, to crudely
                # correct for engine response time.
                var height_above_ground2 = height_above_ground + speed * 2;
                if (height_above_ground2 < 0)   height_above_ground2 = 1;

                var speed_slow = -2 * ft2si;
                
                var speed2 = speed;
                var speed3 = speed + 3 * accel;
                if (speed3 < speed2)    speed2 = speed3;

                accel_critical = (speed2*speed2 - speed_slow*speed_slow) / 2 / height_above_ground2;
                # This is the constant accelaration that would give a
                # smooth landing, given our current height and vertical
                # speed.

                var thrust_max = thrust * 1.0 / me.throttle_smoothed;
                # Thrust seems to be exactly proportional to throttle.
                
                var air_resistance = 82 * -speed;
                # Not sure this is significant.

                var mass = (thrust + air_resistance) / (gravity + accel);
                accel_max = thrust_max / mass - gravity;
                # This is the maximum vertical acceleration that we can achieve.
                #
                # Unfortunately this isn't an accurate or stable measure. Maybe
                # we need to take vertical air resistance into account?

                # Our calculation for accel_max is noisy, so we smooth it
                # here. In theory it should be stable, changing only as fuel
                # load decreases and/or engine power is affected by altitude.
                var accel_max_smoothed_e = 0.1;
                if (me.accel_max_smoothed == nil) {
                    me.accel_max_smoothed = accel_max;
                }
                else {
                    me.accel_max_smoothed = me.accel_max_smoothed * (1-accel_max_smoothed_e)
                            + accel_max*accel_max_smoothed_e;
                }
                
                var safety_hack = 0.5;
                var agl_hack = 50*ft2si;

                var speed_critical = speed_target;
                if (me.accel_max_smoothed > 0) {
                    speed_critical = -math.sqrt( 2 * me.accel_max_smoothed*safety_hack * height_above_ground);
                }
                # speed_critical is speed we would be doing at this altitude in
                # perfect fast descent.

                var override = 0;
                if (accel_critical > me.accel_max_smoothed * safety_hack) {
                    #printf('accel_critical > me.accel_max_smoothed * safety_hack');
                    override = 1;
                    me.critical_active = me.it;
                }
                else if (speed2 < speed_critical*0.75) {
                    #printf('speed2 < speed_critical*0.75');
                    override = 1;
                    me.critical_active = me.it;
                }
                else if (speed2 < speed_critical*0.5 and height_above_ground/ft2si < 100) {
                    #printf('speed2 < speed_critical*0.5 and height_above_ground/ft2si < 100');
                    override = 1;
                    me.critical_active = me.it;
                }
                else if (height_above_ground < agl_hack and speed_target < 2*speed_slow) {
                    #printf('height_above_ground < agl_hack and speed_target < 2*speed_slow');
                    override = 1;
                    me.critical_active = me.it;
                }
                else if (me.critical_active and me.it < me.critical_active + 5/me.period) {
                    #printf('continuity');
                    override = 1;
                }
                else {
                    me.critical_active = 0;
                }

                if (override) {
                    # We need to override accel_target, because vertical
                    # acceleration required is near the maximum we can do.
                    accel_target = accel_critical;

                    # Increment accel_target a little if it's a lot bigger
                    # than the existing accel, to crudely compensate for
                    # the delay in large changes to throttle.
                    if (accel_target > accel) {
                        accel_target = accel_target + 2 * (accel_target - accel);
                    }

                    override_text_1 = ' * override *';

                    if (height_above_ground < agl_hack and speed2 >= speed_slow*2) {
                        # Switch mode to maintain current height when we are
                        # slow and near ground.
                        #
                        # [Trying to land causes problems - we tend to bounce
                        # and get unstable (unless we kill the engine, but not
                        # sure we should automate that).
                        #
                        me.prop_mode.setValue( 'height');
                        me.prop_y_height_target.setValue( height_ft);
                        
                        # Reset target vertical speed to zero in case user
                        # re-enables it.
                        me.prop_y_speed_target.setValue( 0);
                        override_text_1 = ' #';
                    }
                }

                if (0) {
                    # Detailed diagnostics about override.
                    #
                    var override_text_2 = sprintf(
                            '%s override it=%s agl=%.1f ft speed=[%.1f fps 2=%.1f fps critical=%.1f fps] fps throttle=[%.2f smoothed=%.2f] thrust=[%.1f max=%.1f] air_resistance=%.1f mass=%.1f kg accel=[%.2f fps/s critical=%.2f fps/s max=%.2f fps/s target=%.2f fps/s]',
                            override_text_1,
                            me.it,
                            height_above_ground / ft2si,
                            speed / ft2si,
                            speed2 / ft2si,
                            speed_critical / ft2si,
                            throttle,
                            me.throttle_smoothed,
                            thrust,
                            thrust_max,
                            air_resistance,
                            mass,
                            accel / ft2si,
                            accel_critical / ft2si,
                            me.accel_max_smoothed / ft2si,
                            accel_target / ft2si,
                            );
                    printf( "%s", override_text_2);
                }
            }
            
            var thrust_target = thrust * (accel_target + gravity) / (accel + gravity);
            
            var throttle_target = 0;
            if (me.throttle_smoothed == 0 and thrust_target > thrust) {
                # This is a hack to ensure we can increase thrust if it is
                # currently zero.
                throttle_target = thrust_target / thrust * 0.1;
            }
            else {
                throttle_target = thrust_target / thrust * me.throttle_smoothed;
            }
            
            # Limit throttle to range 0..1.
            if (throttle_target < 0)    throttle_target = 0;
            if (throttle_target > 1)    throttle_target = 1;

            if (0) {
                # Detailed diagnostics.
                height_target = -1;
                printf('auto-hover height mode=%.1f: height=[%.1f (%.1fft) target=%.1fm (%.1fft)] vel=[%.2f target=%.2f] accel=[%.2f target=%.2f] throttle=[%.2f smoothed=%.2f] thrust=[%.1f target=%.1f] throttle=[%.1f target=%.1f]',
                        mode,
                        height,
                        height / ft2si,
                        height_target,
                        height_target / ft2si,
                        speed,
                        speed_target,
                        accel,
                        accel_target,
                        throttle,
                        me.throttle_smoothed,
                        thrust,
                        thrust_target,
                        100*throttle,
                        100*throttle_target,
                        );
            }

            if (0) {
                # Brief diagnostics.
                if (mode == 'height') {
                    printf('auto-hover: height: target=%.1f ft, actual=%.1f ft. vertical speed=%.2f fps.',
                            height_target / ft2si,
                            height / ft2si,
                            speed / ft2si,
                            );
                }
                else {
                    printf('auto-hover: vertical speed: target=%.2f fps actual=%.2f fps.',
                            speed_target / ft2si,
                            speed / ft2si,
                            );
                }
            }

            # Update throttle:
            me.prop_throttle.setValue( throttle_target);
            
            var text = make_text2(
                    'autohover: vertical: ',
                    'V ',
                    speed_fps,
                    speed_target / ft2si,
                    'fps',
                    '%+.1f',
                    mode == 'height',
                    height_target_ft,
                    'ft',
                    '%.1f',
                    );
            
            if (override_text_1 != '') {
                if (display_large()) {
                    override_text_1 = sprintf('%s throttle=%.2f speed_critical=%.1f fps',
                            override_text_1,
                            throttle_target,
                            speed_critical / ft2si,
                            );
                    text = text ~ override_text_1;
                }
                else if (display_small()) {
                    text = text ~ ' *';
                }
            }
            if (display_none()) {
                me.window.close();
            }
            else {
                me.window.write(text);
            }
        }
        me.timer.restart( me.period);
    },
};

var height = auto_hover_height.new(
        window: auto_hover_height_window,
        );


# Functions for use by Harrier-GR3-keyboard.xml.
#

# Set height target to current height.
var auto_hover_y_current = func() {
    var height_ft = props.globals.getValue('/position/altitude-ft');
    props.globals.setValue('/controls/auto-hover/y-height-target', height_ft);
    props.globals.setValue('/controls/auto-hover/y-mode', 'height');
}

# Set target vertical speed target.
var auto_hover_y_speed_set = func(speed) {
    props.globals.setValue('/controls/auto-hover/y-speed-target', speed);
    props.globals.setValue('/controls/auto-hover/y-mode', 'speed');
}

# Adjust vertical speed target.
var auto_hover_y_speed_delta = func(delta) {
    var mode = props.globals.getValue('/controls/auto-hover/y-mode');
    if (mode == 'off') {
        auto_hover_y_speed_set( 0);
    }
    else if (mode == 'speed') {
        speed = props.globals.getValue('/controls/auto-hover/y-speed-target');
        if (speed == nil)   speed = 0;
        speed += delta;
        speed = math.round( speed, math.abs(delta));
        props.globals.setValue('/controls/auto-hover/y-speed-target', speed);
    }
    else if (mode == 'height') {
        var y_height_target = props.globals.getValue('/controls/auto-hover/y-height-target');
        y_height_target += delta;
        y_height_target = math.round( y_height_target, math.abs(delta));
        props.globals.setValue('/controls/auto-hover/y-height-target', y_height_target);
    }
    else {
        printf( "unrecognised y-mode: %s", mode);
    }
}

# Turn vertical control off.
var auto_hover_y_off = func() {
    props.globals.setValue('/controls/auto-hover/y-mode', 'off');
}


# Adjust AoA target.
var auto_hover_aoa_nozzles_change = func(delta) {
    target = props.globals.getValue('/controls/auto-hover/aoa-nozzles-target');
    if (target == nil) {
        target = props.globals.getValue('/orientation/alpha-deg');
        target = math.round(target, 0.5);
    }
    target = target + delta;
    props.globals.setValue('/controls/auto-hover/aoa-nozzles-target', target);
    if (display_none()) {
    }
    else {
        if (display_small()) {
            var text = sprintf('aoa=%.1f', target)
        }
        else {
            var text = sprintf('auto-hover aoa nozzles: target=%.1f', target);
        }
        auto_hover_aoa_nozzles_window.write(text);
    }
}

# Arrange to turn AoA off as soon as any wheel touches ground.
var aoa_listener = func() {
    if (getprop( 'gear/wow_any')) {
        auto_hover_aoa_nozzles_off();
    }
}        
var aoa_listener_start = func() {
    var listener = setlistener( 'gear/wow_any', aoa_listener);
    
}

# Turn AoA nozzles off.
var auto_hover_aoa_nozzles_off = func() {
    props.globals.getNode('/controls/auto-hover/aoa-nozzles-target').clearValue();
    auto_hover_aoa_nozzles_window.close();
}


var auto_hover_xz_target_lat_old = nil;
var auto_hover_xz_target_lon_old = nil;
props.globals.setValue('/controls/auto-hover/xz-target', '');


# Handle xz target, e.g. in response to Alt-.
var auto_hover_xz_target_set = func( lat, lon) {
    props.globals.setValue('/controls/auto-hover/xz-target', '');
    props.globals.setValue('/controls/auto-hover/xz-target-latitude', lat);
    props.globals.setValue('/controls/auto-hover/xz-target-longitude', lon);
    props.globals.setValue('/controls/auto-hover/x-mode', 'target');
    props.globals.setValue('/controls/auto-hover/z-mode', 'target');
    printf('new xz_target: lat=%s lon=%s', lat, lon);
    auto_hover_xz_target_prime_window.close();
}

var auto_hover_xz_target_click_listener = nil;

var auto_hover_xz_target_end = func() {
    removelistener( auto_hover_xz_target_click_listener);
    auto_hover_xz_target_click_listener = nil;
    auto_hover_xz_target_prime_window.close();
}

var auto_hover_xz_target_click_handler = func() {
    var lat = getprop("/sim/input/click/latitude-deg");
    var lon = getprop("/sim/input/click/longitude-deg");
    auto_hover_xz_target_set(lat, lon);
    auto_hover_xz_target_end();
}

var auto_hover_xz_target_click = func() {
    if (auto_hover_xz_target_click_listener) {
        # Cancel if Alt-, entered twice.
        auto_hover_xz_target_end();
    }
    else {
        auto_hover_xz_target_click_listener = setlistener(
                props.globals.getNode( '/sim/signals/click'),
                auto_hover_xz_target_click_handler,
                0,
                );
        auto_hover_xz_target_prime_window.write('auto-hover: horizontal: next click sets target position...');
    }
}

var auto_hover_xz_target_current = func() {
    var pos = geo.aircraft_position();
    var aircraft_heading = props.globals.getValue('/orientation/heading-deg');
    # Use midpoint between the main gears:
    pos.apply_course_distance( aircraft_heading, -z_offset);
    auto_hover_xz_target_set(pos.lat(), pos.lon())
}


# Control vertical speed near ground to give smooth landing.
#
# We define a relationship between height and ideal vertical speed
# (negative) that gives a rapid but smooth landing. If we are active we set
# /autopilot/settings/vertical-speed-fpm to the ideal vertical speed for the
# current height above the ground.
#
# If we are inactive, we start being active if we are descending faster than
# ideal.
#
# In practise this means that we take control when the aircraft's trajectory
# intersects with our ideal tragectory, so in theory we do the right thing
# regardless of whether the approach is steep or shallow.
#
# Note that we don't know anything about where the runway is.
#

var vspeed_land_class = {
    new: func() {
        var me = { parents:[vspeed_land_class]};
        
        me.timer = maketimer( 0, func { me.fn() }, 0);
        me.timer.singleShot = 1;
        
        me.active = 0;

        # Next two allow crude correction of runway slope.
        me.t_prev = 0;
        me.agl_ft_prev = 0;

        # If 1, we activated autopilot vertical speed control.
        me.autopilot_own = 0;

        me.state_prop               = props.globals.getNode('/controls/auto-hover/vspeed-land', 1);

        me.v0_prop                  = props.globals.getNode('/controls/auto-hover/vspeed-land-v0', 1);
        me.vscale_prop              = props.globals.getNode('/controls/auto-hover/vspeed-land-vscale', 1);
        me.vpower_prop              = props.globals.getNode('/controls/auto-hover/vspeed-land-vpower', 1);

        me.description_prop         = props.globals.getNode('/controls/auto-hover/vspeed-land-description', 1);
        me.agl_ft_prop              = props.globals.getNode('/position/gear-agl-ft', 1);
        me.fps_prop                 = props.globals.getNode('/velocities/vertical-speed-fps', 1);
        me.autopilot_altitude_prop  = props.globals.getNode('/autopilot/locks/altitude', 1);
        me.autopilot_vspeed_prop    = props.globals.getNode('/autopilot/settings/vertical-speed-fpm', 1);

        # Set defaults.
        me.state = '';
        me.v0_prop.setValue(1.5);      # Target vertical speed on touchdown in fps.
        me.vscale_prop.setValue(1.2);  # Higher values give faster descent.
        me.vpower_prop.setValue(0.4);  # Higher values give more sudden levelling out near ground.
        
        return me;
    },
    
    start: func() {
        setlistener( me.state_prop, func() { me.listener() }, 1);
    },
    
    # Returns target vertical speed appropriate for a given height above the
    # ground.
    #
    agl_ft_to_vspeed_fps: func( agl_ft) {
        v0 = me.v0_prop.getValue();
        vscale = me.vscale_prop.getValue();
        vpower = me.vpower_prop.getValue();

        if (agl_ft < 0) {
            agl_ft = 0;
        }
        return - (v0 + vscale * math.pow( agl_ft, vpower));
    },

    listener: func() {
        var old_state = me.state;
        me.state = me.state_prop.getValue();
        if (!old_state and me.state) {
            me.timer.restart(0);
        }
        else if ( old_state and !me.state) {
            me.active = 0;
            # If we are not active, turn off vertical-speed-hold if we turned it on.
            #
            if (me.autopilot_own == 1) {
                me.autopilot_altitude_prop.setValue('');
                me.autopilot_own = 0;
            }
            me.timer.stop();
            me.description_prop.setValue('');
            
            # If we are not active, turn off vertical-speed-hold if we turned it on.
            #
            if (me.autopilot_own == 1) {
                me.autopilot_altitude_prop.setValue('');
                me.autopilot_own = 0;
            }
        }
    },
    
    fn: func() {

        var dt = 0.5;

        if (me.state == nil)    me.state = '';
        var description = '';

        if (me.state == '') {
            me.active = 0;
        }
        else {
            description = 'L';
            # Make sure we get called back soon.
            dt = 0.05;
            var agl_ft = me.agl_ft_prop.getValue();
            if (agl_ft == nil) {    # can be nil if --enable-freeze.
                agl_ft = 0;
            }

            if (agl_ft > 0) {
                # We are above the ground.

                var vspeed_fps = me.fps_prop.getValue();
                var vspeed_fps_target = me.agl_ft_to_vspeed_fps(agl_ft);

                # Turn on activation if we are descending faster than
                # vspeed_fps_target.
                if (me.active == 0) {
                    if (vspeed_fps < vspeed_fps_target or me.state == 'force') {
                        # Start.
                        #
                        # For this first iteration, we set autopilot vertical speed
                        # to current vertical speed, in an attempt to minimise
                        # initial pitching around as autopilot PID controller gets
                        # started.
                        #
                        me.active = 1;

                        # Make sure autopilot is in vertical-speed-hold mode. We
                        # remember whether we turned it on so that we can turn it off
                        # when we deactivate.
                        #
                        var vsh = me.autopilot_altitude_prop.getValue();
                        if (vsh != 'vertical-speed-hold') {
                            me.autopilot_altitude_prop.setValue('vertical-speed-hold');
                            me.autopilot_own = 1;
                        }

                        me.autopilot_vspeed_prop.setValue(vspeed_fps * 60);
                        description = sprintf('L +0');
                    }
                }
                else if (me.active == 1) {

                    # We are controlling vertical speed.

                    # Figure out vertical speed correction for runway slope -
                    # we need to modify target vertical speed if runway slopes,
                    # otherwise we can end up not touching down or touching down
                    # too quickly.
                    #
                    var vspeed_fps_correction = 0;
                    var t = systime();
                    if (me.agl_ft_prev != 0
                        and t - me.t_prev < 0.5
                        and agl_ft < 5
                        ) {
                        # Correct for runway slope.
                        var v_fps = (agl_ft - me.agl_ft_prev) / (t - me.t_prev);
                        vspeed_fps_correction = vspeed_fps - v_fps;
                        if (vspeed_fps_correction < -3 or vspeed_fps_correction > 3) {
                            vspeed_fps_correction = 0;
                        }
                    }
                    me.agl_ft_prev = agl_ft;
                    me.t_prev = t;

                    # In a crude attempt to model the delay of changing vertical
                    # speed, we use the anticipated vertical speed in the future.
                    #
                    agl_ft_soon = agl_ft + 1 * vspeed_fps;
                    var vspeed_fps_target = me.agl_ft_to_vspeed_fps(agl_ft_soon) + vspeed_fps_correction;

                    if (0) printf("autohover land: agl_ft=%.1f vspeed_fps=%.1f agl_ft_soon=%.1f. vspeed_fps_target=%.1f vspeed_fps_correction=%.1f. error=%.1f",
                            agl_ft,
                            vspeed_fps,
                            agl_ft_soon,
                            vspeed_fps_target,
                            vspeed_fps_correction,
                            vspeed_fps - vspeed_fps_target,
                            );

                    # Set target vertical speed.
                    me.autopilot_vspeed_prop.setValue(vspeed_fps_target * 60);

                    # Set <description> to indicate that we are in control.
                    var error_percent = 0;
                    if (vspeed_fps_target) {
                        error_percent = 100 * (vspeed_fps - vspeed_fps_target) / abs(vspeed_fps_target);
                    }
                    description = sprintf('L %+.1f%%', error_percent);

                    #description = sprintf('L Active');
                }
            }
            else {
                # We are on on ground.
                me.state_prop.setValue( '');
                return;
            }
        }

        me.description_prop.setValue( description);

        if (me.state) {
            me.timer.restart( dt);
        }
    },
};

vspeed_land = vspeed_land_class.new();


# Sets /autopilot/internal/vertical-speed-text to text describing autopilot
# vertical speed hold error. Useful when investigating autopilot performance.
vertical_speed_hud_prop_altitude                        = props.globals.getNode( 'autopilot/locks/altitude', 1);
vertical_speed_hud_prop_autopilot_vertical_speed_fpm    = props.globals.getNode( 'autopilot/settings/vertical-speed-fpm', 1);
vertical_speed_hud_prop_vertical_speed_fps              = props.globals.getNode( 'velocities/vertical-speed-fps', 1);
vertical_speed_hud_prop_vertical_speed_text             = props.globals.getNode( 'autopilot/internal/vertical-speed-text', 1);
vertical_speed_hud = func() {
    var dt = 0.5;
    if ( vertical_speed_hud_prop_altitude.getValue() == 'vertical-speed-hold') {
        dt = 0.1;
        var target = vertical_speed_hud_prop_autopilot_vertical_speed_fpm.getValue();
        var actual = vertical_speed_hud_prop_vertical_speed_fps.getValue() * 60;
        var text = '0';
        if (target != 0) {
            var error = (actual - target) / abs( target);
            text = sprintf( '%.1f %+.1f%%', target, 100 * (actual - target) / abs( target));
        }
        vertical_speed_hud_prop_vertical_speed_text.setValue( text);
    }
    vertical_speed_hud_timer.restart( dt);
}
vertical_speed_hud_timer = maketimer( 0, vertical_speed_hud);
vertical_speed_hud_timer.singleShot = 1;
#vertical_speed_hud_timer.restart( 0);


# Alter horizontal target speed or target position.
#
#   name: either 'x' or 'z'.
#
#   delta:
#       Change in target speed in kts, or change in target position in ft.
#
xz_delta = func(name, delta) {
    #printf('xz_delta() name=%s delta=%s', name, delta);
    var mode_name = sprintf('/controls/auto-hover/%s-mode', name);
    var mode = props.globals.getValue(mode_name);
    var mode_old = mode;
    if (mode == 'off') {
        mode = 'speed';
        props.globals.setValue(mode_name, mode);
    }
    #printf('mode=%s', mode);
    if (mode == 'speed') {
        # Modify target speed.
        var speed_target_name = sprintf('/controls/auto-hover/%s-speed-target', name);
        var speed_target_kts = props.globals.getValue(speed_target_name, 0);
        if (speed_target_kts == '' or speed_target_kts == nil or mode != mode_old) {
            # No target speed set, so use current speed.
            if (name == 'z') {
                var speed = props.globals.getValue('/velocities/uBody-fps') * ft2si;
            }
            else {
                var speed = props.globals.getValue('/velocities/vBody-fps') * ft2si;
            }
            speed_target_kts = speed / knots2si;
            delta = 0;
        }
        speed_target_kts += delta;
        speed_target_kts = math.round(speed_target_kts, 0.5);
        props.globals.setValue(speed_target_name, speed_target_kts);
    }
    else if (mode == 'target') {
        # Modify target position.
        var target_lat = props.globals.getValue('/controls/auto-hover/xz-target-latitude');
        var target_lon = props.globals.getValue('/controls/auto-hover/xz-target-longitude');
        var pos_target = geo.Coord.new();
        pos_target.set_latlon(target_lat, target_lon);
        var aircraft_heading = props.globals.getValue('/orientation/heading-deg');
        target_delta = delta * ft2si;
        if (name == 'z') {
            pos_target.apply_course_distance(aircraft_heading, target_delta);
        }
        else {
            pos_target.apply_course_distance(aircraft_heading + 90, target_delta);
        }
        props.globals.setValue('/controls/auto-hover/xz-target-latitude', pos_target.lat());
        props.globals.setValue('/controls/auto-hover/xz-target-longitude', pos_target.lon());
        #printf('have changed pos_target to lat=%s lon=%s', pos_target.lat(), pos_target.lon());
    }
    props.globals.setValue(sprintf('/controls/auto-hover/%s-speed-target-delta', name), '');
}

var cycle_use_pid = func() {
    var value = g_prop_pid.getValue();
    if (value == 'true') {
        value = ''
    }
    else {
        value = 'true';
    }
    g_prop_pid.setValue( value);
    gui.popupTip( sprintf( 'Autohover pid=%s', value), 5);
}


# Class for keeping nozzles vertical, regardless of pitch.
#
nozzle_vertical_class = {
    
    new: func() {
        var me = { parents: [nozzle_vertical_class]};
        me.timer = maketimer( 0, func { me.fn()}, 0);
        me.timer.singleShot = 1;
        me.state = 0;
        me.state_prop = props.globals.getNode( '/controls/auto-hover/nozzle_vertical', 1);
        me.pitch_prop = props.globals.getNode( '/orientation/pitch-deg', 1);
        me.nozzle_prop = props.globals.getNode( '/controls/engines/engine/mixture', 1);
        
        setlistener( me.state_prop, func() { me.listener() }, 1);
        setlistener( g_replay_state_prop, func() { me.listener() }, 1);
        setlistener( 'gear/wow_any', func() { me.listener() }, 1);
        
        return me;
    },
    
    listener: func() {
        var old_state = me.state;
        me.state = me.state_prop.getValue();
        if (g_replay_state_prop.getValue()) {
            me.state = 0;
        }
        if ( me.state and !old_state) {
            me.timer.restart(0);
        }
        else if ( !me.state and old_state) {
            me.timer.stop();
        }
    },
    
    fn: func() {
        var pitch = me.pitch_prop.getValue();
        # pitch = 8.14 => nozzles = 0.18
        # pitch = 90 => nozzles = 1
        var nozzles = 0.18 + (1-0.18) * (pitch-8.14) / (90-8.14);
        me.nozzle_prop.setValue( nozzles);
        me.timer.restart(0.5);
    }
};

var nozzle_vertical = nozzle_vertical_class.new();

# We start each separate control only after sim/signals/fdm-initialized is set,
# to avoid problems with un-initialised properties etc.
#
var startup = func( fdm_initialised_node) {
    fdm_initialised = fdm_initialised_node.getValue();
    if (fdm_initialised) {
        printf( 'autohover initialising...');
        wow_start();
        z_speed.start();
        x_speed.start();
        auto_hover_rotation.start();
        height.start();
        vspeed_land.start();
        aoa_listener_start();
        printf( 'autohover initialised');
    }
}

printf( 'autohover loaded');

# Need to call this at the end of this file, so that reloading works - if we
# call it earlier, some vars will be from the old invocation.
#
setlistener( "sim/signals/fdm-initialized", startup, 1);
