#!/usr/bin/python3
# AP_FLAKE8_CLEAN

"""Check standalone SITL startup and the MAVLink controller input path."""

import argparse
import os
import subprocess
import tempfile
import time
from contextlib import closing
from pathlib import Path

from pymavlink import mavutil

VEHICLES = {
    "arducopter": "+",
    "arducopter-heli": "heli",
    "arduplane": "plane",
    "ardurover": "rover",
    "ardusub": "vectored",
    "antennatracker": "tracker",
    "blimp": "blimp",
}

PARAMETER_FILES = {
    "arducopter": "default_params/copter.parm",
    "arducopter-heli": "default_params/copter-heli.parm",
    "arduplane": "models/plane.parm",
    "ardurover": "default_params/rover.parm",
    "ardusub": "default_params/sub.parm",
    "blimp": "default_params/blimp.parm",
}

DEFAULT_PARAMETERS = {
    "arducopter": ("FRAME_CLASS", 1),
    "arducopter-heli": ("FRAME_CLASS", 6),
    "arduplane": ("AIRSPEED_CRUISE", 22),
    "ardurover": ("CRUISE_SPEED", 5),
    "ardusub": ("BATT_MONITOR", 4),
    "blimp": ("FRAME_CLASS", 1),
}

MAV_TYPES = {
    "arducopter": mavutil.mavlink.MAV_TYPE_QUADROTOR,
    "arducopter-heli": mavutil.mavlink.MAV_TYPE_HELICOPTER,
    "arduplane": mavutil.mavlink.MAV_TYPE_FIXED_WING,
    "ardurover": mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
    "ardusub": mavutil.mavlink.MAV_TYPE_SUBMARINE,
    "antennatracker": mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER,
    "blimp": mavutil.mavlink.MAV_TYPE_AIRSHIP,
}


def receive(link, process, message_type, timeout):
    deadline = time.monotonic() + timeout
    while time.monotonic() < deadline:
        if process.poll() is not None:
            raise RuntimeError(f"SITL exited with status {process.returncode}")
        message = link.recv_match(type=message_type, blocking=True, timeout=1)
        if message is not None:
            return message
    raise RuntimeError(f"No {message_type} message received in {timeout} seconds")


def check_controller(link, process):
    link.mav.request_data_stream_send(
        link.target_system, link.target_component,
        mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS, 10, 1,
    )
    deadline = time.monotonic() + 15
    while time.monotonic() < deadline:
        link.mav.heartbeat_send(
            mavutil.mavlink.MAV_TYPE_GCS,
            mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0,
            mavutil.mavlink.MAV_STATE_ACTIVE,
        )
        link.mav.rc_channels_override_send(
            link.target_system, link.target_component,
            1600, 1500, 1000, 1500, 0, 0, 0, 0,
        )
        if process.poll() is not None:
            raise RuntimeError(f"SITL exited with status {process.returncode}")
        message = link.recv_match(type="RC_CHANNELS", blocking=True, timeout=1)
        if message is not None and message.chan1_raw == 1600 and message.chan3_raw == 1000:
            print("arducopter: MAVLink RC input received while disarmed", flush=True)
            return
    raise RuntimeError("SITL did not report the injected RC channel values")


def check_defaults(link, process, vehicle):
    if vehicle not in DEFAULT_PARAMETERS:
        return
    name, expected = DEFAULT_PARAMETERS[vehicle]
    deadline = time.monotonic() + 15
    while time.monotonic() < deadline:
        if process.poll() is not None:
            raise RuntimeError(f"SITL exited with status {process.returncode}")
        link.mav.param_request_read_send(link.target_system, link.target_component, name.encode(), -1)
        message = link.recv_match(type="PARAM_VALUE", blocking=True, timeout=1)
        if message is not None and message.param_id == name:
            if abs(message.param_value - expected) > 0.001:
                raise RuntimeError(f"{name}={message.param_value}, expected preset value {expected}")
            print(f"{vehicle}: preset {name}={expected}", flush=True)
            return
    raise RuntimeError(f"No value received for {name}")


def check_vehicle(binary_dir, data_dir, vehicle, instance):
    binary = binary_dir / vehicle
    if vehicle == "blimp" and not binary.exists():
        binary = binary_dir / "ardupilot-blimp"
    binary = binary.resolve(strict=True)
    with tempfile.TemporaryDirectory(prefix=f"sitl-{vehicle}-") as directory:
        log_path = Path(directory) / "sitl.log"
        with log_path.open("w") as log, closing(mavutil.mavlink_connection(
            "udpin:127.0.0.1:0", source_system=255,
        )) as link:
            port = link.port.getsockname()[1]
            command = [
                str(binary), "--model", VEHICLES[vehicle],
                "--instance", str(instance), "--speedup", "2",
                "--serial0", f"udpclient:127.0.0.1:{port}",
            ]
            if vehicle in PARAMETER_FILES:
                preset = (data_dir / PARAMETER_FILES[vehicle]).resolve(strict=True)
                command.extend(["--defaults", str(preset)])
            process = subprocess.Popen(command, cwd=directory, stdout=log, stderr=subprocess.STDOUT)
            try:
                heartbeat = receive(link, process, "HEARTBEAT", 60)
                if heartbeat.autopilot != mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA:
                    raise RuntimeError(f"Unexpected autopilot identifier {heartbeat.autopilot}")
                if heartbeat.type != MAV_TYPES[vehicle]:
                    raise RuntimeError(f"{vehicle} reported vehicle type {heartbeat.type}, expected {MAV_TYPES[vehicle]}")
                link.target_system = heartbeat.get_srcSystem()
                link.target_component = heartbeat.get_srcComponent()
                print(f"{vehicle}: heartbeat received in a fresh session", flush=True)
                check_defaults(link, process, vehicle)
                if vehicle == "arducopter":
                    if heartbeat.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
                        raise RuntimeError("Fresh Copter simulation unexpectedly started armed")
                    check_controller(link, process)
            except Exception:
                log.flush()
                print(log_path.read_text(errors="replace"), flush=True)
                raise
            finally:
                if process.poll() is None:
                    process.terminate()
                try:
                    process.wait(timeout=10)
                except subprocess.TimeoutExpired:
                    process.kill()
                    process.wait()


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--binary-dir", type=Path, default=Path("/usr/bin"))
    parser.add_argument(
        "--data-dir", type=Path, default=Path("/usr/share/ardupilot/Tools/autotest"),
        help="directory containing the installed default_params and models",
    )
    parser.add_argument("--vehicle", choices=VEHICLES, action="append")
    args = parser.parse_args()
    # Keep test instance ports separate from the ordinary interactive instance.
    first_instance = 100 + os.getpid() % 1000
    for index, vehicle in enumerate(args.vehicle or VEHICLES):
        check_vehicle(args.binary_dir, args.data_dir, vehicle, first_instance + index)


if __name__ == "__main__":
    main()
