# This is a script that controls a target aircraft for various target/tracking
# tasks.


#print("Target Lead script loading ...");

# script defaults (configurable if you like)
default_update_period = 0.05;
default_goal_range_nm = 0.2;
default_target_root = "/ai/models/aircraft[0]";
default_target_task = "straight";

# master enable switch
lead_target_enable = 0;

# update period
update_period = default_update_period;

# goal range to acheive when following target
goal_range_nm = 0;

# Target property tree root
target_root = "";

# Target task (straight, turns, pitch, both)
target_task = default_target_task;
base_alt = 3000;
tgt_alt = base_alt;
tgt_hdg = 20;
tgt_pitch = 0;
tgt_roll = 0;
tgt_speed = 280;
tgt_lat_mode = "roll";
tgt_lon_mode = "alt";
next_turn = 0;
next_pitch = 0;
last_time = 0;


# Calculate new lon/lat given starting lon/lat, and offset radial, and
# distance.  NOTE: starting point is specifed in radians, distance is
# specified in meters (and converted internally to radians)
# ... assumes a spherical world.
# @param orig lon specified in polar coordinates
# @param orig lat specified in polar coordinates
# @param course offset radial
# @param dist offset distance
# @return destination point in polar coordinates

# define some global constants
SG_METER_TO_NM = 0.0005399568034557235;
SG_NM_TO_RAD   = 0.00029088820866572159;
SG_EPSILON     = 0.0000001;
SGD_PI         = 3.14159265358979323846;
SGD_2PI        = 6.28318530717958647692;
SGDTR          = SGD_PI / 180.0;
fmod = func(x, y) { x - y * int(x/y) }
asin = func(y) { math.atan2(y, math.sqrt(1-y*y)) }

CalcGClonlat = func( lon_rad, lat_rad, hdg_rad, dist_m ) {
    result = [0, 0];

    # sanity check
    while ( hdg_rad < 0 ) { hdg_rad += SGD_2PI; }
    while ( hdg_rad >= SGD_2PI ) { hdg_rad -= SGD_2PI; }

    print("lon = ", lon_rad, " lat = ", lat_rad, " hdg = ", hdg_rad, " dist = ",
          dist_m);

    # lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
    # IF (cos(lat)=0)
    #   lon=lon1      // endpoint a pole
    # ELSE
    #   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
    # ENDIF

    dist_rad = dist_m * SG_METER_TO_NM * SG_NM_TO_RAD;

    result[1] = asin( math.sin(lat_rad) * math.cos(dist_rad) +
                      math.cos(lat_rad) * math.sin(dist_rad) *
                      math.cos(hdg_rad) );

    if ( math.cos(result[1]) < SG_EPSILON ) {
        result[0] = lon_rad;      # endpoint a pole
    } else {
        result[0] = 
            fmod(lon_rad - asin( (math.sin(hdg_rad) *
                                 math.sin(dist_rad)) /
                                 math.cos(result[1]) )
                      + SGD_PI, SGD_2PI) - SGD_PI;
    }

    print("new lon = ", result[0], " new lat = ", result[1]);

    return result;
}


# Initialize target tracking
LeadTargetInit = func {
    # seed the random number generator with time
    srand();

    lead_target_enable = getprop("/autopilot/lead-target/enable");
    if ( lead_target_enable == nil ) {
        lead_target_enable = 0;
        setprop("/autopilot/lead-target/enable", lead_target_enable);
    }

    target_task = getprop("/autopilot/lead-target/task");
    if ( target_task == nil ) {
        target_task = default_target_task;
        setprop("/autopilot/lead-target/task", target_task);
    }

    update_period = getprop("/autopilot/lead-target/update-period");
    if ( update_period == nil ) {
        update_period = default_update_period;
        setprop("/autopilot/lead-target/update-period", update_period);
    }

    goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
    if ( goal_range_nm == nil ) {
        goal_range_nm = default_goal_range_nm;
        setprop("/autopilot/lead-target/goal-range-nm", goal_range_nm);
    }

    target_root = getprop("/autopilot/lead-target/target-root");
    if ( target_root == nil ) {
        target_root = default_target_root;
        setprop("/autopilot/lead-target/target-root", target_root);
    }
}
settimer(LeadTargetInit, 0);


# update target task
do_target_task = func {
    time = getprop("/sim/time/elapsed-sec");
    dt = time - last_time;
    last_time = time;

    target_task = getprop("/autopilot/lead-target/task");

    if ( target_task == "straight" ) {
        tgt_lat_mode = "roll";
        tgt_lon_mode = "alt";
        tgt_roll = 0;
        tgt_alt = base_alt;
    } else {
        if ( target_task == "turns" ) {
            next_turn -= dt;
            if ( next_turn < 0 ) {
                if ( tgt_roll > 0 ) {
                    # new roll 
                    tgt_roll = -30.0;
                } else {
                    # new roll 
                    tgt_roll = 30.0;
                }
                # next turn in 15 seconds
                next_turn = 15.0;

                # roll to zero if tgt_speed < 50 so we don't spin in mid air
                if ( tgt_speed < 50 ) { tgt_roll = 0; }
            }
            tgt_lat_mode = "roll";
        } elsif ( target_task == "turns-rand" or target_task == "both-rand" ) {
            next_turn -= dt;
            if ( next_turn < 0 ) {
                if ( tgt_roll > 0 ) {
                    # new roll between [-45 - -15]
                    tgt_roll = -45.0 + rand() * 30.0;
                } else {
                    # new roll between [15 - 45]
                    tgt_roll = rand() * 30.0 + 15.0;
                }
                # next turn between 5 - 15 seconds
                next_turn = rand() * 10.0 + 5.0;

                # roll to zero if tgt_speed < 50 so we don't spin in mid air
                if ( tgt_speed < 50 ) { tgt_roll = 0; }
            }
            tgt_lat_mode = "roll";
        }
        if ( target_task == "pitch" or target_task == "both" ) {
            next_pitch -= dt;
            if ( next_pitch < 0 ) {
                if ( tgt_alt > base_alt ) {
                    # new alt base - 1000
                    tgt_alt = base_alt - 1000.0;
                } else {
                    # new alt base + 1000
                    tgt_alt = base_alt + 1000.0;
                }
                # next pitch in 15 seconds
                next_pitch = 15.0;

                # ??? (fixme?)
                # pitch to zero if tgt_speed < 50 so we don't porpoise in
                # mid air
                # if ( tgt_speed < 50 ) { tgt_pitch = 0; }
            }
            tgt_lon_mode = "alt";
        } elsif ( target_task == "pitch-rand" or target_task == "both-rand" ) {
            next_pitch -= dt;
            if ( next_pitch < 0 ) {
                if ( tgt_alt > base_alt ) {
                    # new alt between [2000 - 2500]
                    tgt_alt = base_alt - 1000.0 + rand() * 500.0;
                } else {
                    # new alt between [3500 - 4000]
                    tgt_alt = base_alt + 500.0 + rand() * 500.0;
                }
                # next pitch between 5 - 15 seconds
                next_pitch = rand() * 10.0 + 5.0;
                print("--> alt = ", tgt_alt, " next in ", next_pitch, " secs");

                # ??? (fixme?)
                # pitch to zero if tgt_speed < 50 so we don't porpoise in
                # mid air
                # if ( tgt_speed < 50 ) { tgt_pitch = 0; }
            }
            tgt_lon_mode = "alt";
        } elsif ( target_task == "lazy-eights" ) {
            tgt_lat_mode = "roll";
            tgt_lon_mode = "alt";
            next_turn -= dt;

            if ( next_turn < 0 ) {
		next_turn = 32.0;
	    }
	    if ( next_turn > 16.0 ) {
		# rolling right
		tgt_roll = 30.0;
	    } else {
		# rolling left
		tgt_roll = -30.0;
	    }
	    # roll to zero if tgt_speed < 50 so we don't spin in mid air
	    if ( tgt_speed < 50 ) { tgt_roll = 0; }

	    if ( next_turn > 24.0 ) {
		# climbing
		tgt_alt = base_alt + 1000.0;
	    } elsif ( next_turn > 16.0 ) {
		# decending to original altitude
		tgt_alt = base_alt;
	    } elsif ( next_turn > 8.0 ) {
		# climbing
		tgt_alt = base_alt + 1000.0;
	    } else {
		# decending to original altitude
		tgt_alt = base_alt;
	    }
	}
        # fixed altitude if in turns mode
        if ( target_task == "turns" or target_task == "turns-rand" ) {
            tgt_lon_mode = "alt";
            tgt_alt = base_alt;
        }
        # zero roll if in pitch mode
        if ( target_task == "pitch" or target_task == "pitch-rand" ) {
            tgt_lat_mode = "roll";
            tgt_roll = 0;
        }
    }

    tgt_lat_prop = sprintf("%s/controls/flight/lateral-mode",
                           target_root );
    setprop(tgt_lat_prop, tgt_lat_mode);

    tgt_lon_prop = sprintf("%s/controls/flight/longitude-mode",
                           target_root );
    setprop(tgt_lon_prop, tgt_lon_mode);


    if ( tgt_lat_mode == "roll" ) {
        tgt_roll_prop = sprintf("%s/controls/flight/target-roll", target_root );
        setprop(tgt_roll_prop, tgt_roll);
    } else {
        tgt_hdg_prop = sprintf("%s/controls/flight/target-hdg", target_root );
        setprop(tgt_hdg_prop, tgt_hdg);
    }

    if ( tgt_lon_mode == "alt" ) {
        tgt_alt_prop = sprintf("%s/controls/flight/target-alt", target_root );
        setprop(tgt_alt_prop, tgt_alt);
    } else {
        tgt_pitch_prop = sprintf("%s/controls/flight/target-pitch",
                                 target_root );
        setprop(tgt_pitch_prop, tgt_pitch);
    }
}


# reset target aircraft position and heading relative to the "ownship".
reset_target_aircraft = func {
    print("Reseting target aircraft position");

    my_lon = getprop("/position/longitude-deg");
    my_lat = getprop("/position/latitude-deg");
    my_alt = getprop("/position/altitude-ft");
    my_hdg = getprop("/orientation/heading-deg");
    my_spd = getprop("/velocities/airspeed-kt");

    result = CalcGClonlat( my_lon*SGDTR, my_lat*SGDTR, -my_hdg*SGDTR,
                           0.5/SG_METER_TO_NM );

    target_root = getprop("/autopilot/lead-target/target-root");

    target_prop = sprintf("%s/position/longitude-deg", target_root );
    setprop(target_prop, result[0]/SGDTR);

    target_prop = sprintf("%s/position/latitude-deg", target_root );
    setprop(target_prop, result[1]/SGDTR);

    target_prop = sprintf("%s/position/altitude-ft", target_root );
    setprop(target_prop, my_alt);

    target_prop = sprintf("%s/orientation/true-heading-deg", target_root );
    setprop(target_prop, my_hdg);

    target_prop = sprintf("%s/velocities/true-airspeed-kt", target_root );
    setprop(target_prop, my_spd);

    base_alt = my_alt;
}


# If enabled, update our AP target values based on the target range,
# bearing, and speed
LeadTargetUpdate = func {
    lead_target_enable = props.globals.getNode("/autopilot/lead-target/enable");
    update_period = getprop("/autopilot/lead-target/update-period");

    if ( lead_target_enable.getBoolValue() ) {
        # refresh user configurable values
        goal_range_nm = getprop("/autopilot/lead-target/goal-range-nm");
        target_root = getprop("/autopilot/lead-target/target-root");

        # force radar debug-mode on (forced radar calculations even if
        # no radar instrument and ai aircraft are out of range
        setprop("/instrumentation/radar/debug-mode", 1);

        # update target task
        do_target_task();

        my_hdg = getprop("/orientation/heading-deg");

        alt = getprop("/position/altitude-ft");
        if ( alt == nil ) { alt = 0; }
    
        speed = getprop("/velocities/airspeed-kt");
        if ( speed == nil ) { speed = 0; }
    
        range_prop = sprintf("%s/radar/range-nm", target_root );
        range = getprop(range_prop);
        if ( range == nil ) {
            print("bad property path: ", range_prop);
            return;
        }
        if ( range > 3.0 ) {
            # if range is greater than threshold, then reset the target
            # aircraft back in range/view.
            reset_target_aircraft();
        }
    
        h_offset_prop = sprintf("%s/radar/h-offset", target_root );
        h_offset = getprop(h_offset_prop);
        if ( h_offset == nil ) {
            print("bad property path: ", h_offset_prop);
            return;
        }

        if ( h_offset > -90 and h_offset < 90 ) {
            # in front of us
            range_error = goal_range_nm - range;
        } else {
            # behind us
            range_error = goal_range_nm + range;
        }
        if ( range_error < 0 ) {
            # slow the target down
            tgt_speed = speed + range_error * 300.0;
        } else {
            # speed the target up agressively
            tgt_speed = speed + range_error * 2000.0;
        }
        if ( tgt_speed < 0 ) { tgt_speed = 0; }

        # print("speed = ", speed, " range err = ", range_error,
        #       " target spd = ", tgt_speed);

        speed_prop = sprintf("%s/controls/flight/target-spd", target_root );
        setprop( speed_prop, tgt_speed );
    }

    # last thing to do before we return from this function
    registerTimer();
}


select_task_dialog = func {
    dialog.load();  # load every time?
    dialog.open();
}


var dialog = nil;
settimer(func {
    dialog = gui.Dialog.new("/sim/gui/dialogs/NTPS/config/dialog", "gui/dialogs/NTPS_target_task.xml");
}, 0);


# timer handling to cause our update function to be called periodially
registerTimer = func {
    settimer(LeadTargetUpdate, update_period );
}
registerTimer();