# drive APMrover2 in SITL
from __future__ import print_function
import os
import shutil

import pexpect
from pymavlink import mavutil

from common import *
from pysim import util

# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))


HOME = mavutil.location(33.810313, -118.393867, 0, 185)
homeloc = None


def wait_ready_to_arm(mavproxy):
    # wait for EKF and GPS checks to pass
    mavproxy.expect('IMU0 is using GPS')

def arm_sub(mavproxy, mav):
    wait_ready_to_arm(mavproxy);
    for i in range(8):
        mavproxy.send('rc %d 1500\n' % (i+1))

    mavproxy.send('arm throttle\n')
    mavproxy.expect('ARMED')

    print("SUB ARMED")
    return True

def dive_mission(mavproxy, mav, filename):
    """Dive a mission from a file."""
    global homeloc

    mavproxy.send('rc 3 1600\n')
    mavproxy.send('rc 6 1600\n')
    mavproxy.send('rc 7 1550\n')

    if not wait_distance(mav, 50, accuracy=7, timeout=100):
        return False
    
    mavproxy.send('rc 4 1550\n')
    
    if not wait_heading(mav, 0):
        return False
    
    mavproxy.send('rc 4 1500\n')
    
    if not wait_distance(mav, 50, accuracy=7, timeout=100):
        return False
    
    mavproxy.send('rc 4 1550\n')
    
    if not wait_heading(mav, 0):
        return False
    
    mavproxy.send('rc 4 1500\n')
    mavproxy.send('rc 6 1500\n')
    mavproxy.send('rc 7 1100\n')
    
    if not wait_distance(mav, 75, accuracy=7, timeout=100):
        return False
    
    mavproxy.send('rc 7 1500\n')
    
    wait_mode(mav, 'MANUAL')
    print("Mission OK")
    return True


def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
    """Dive ArduSub in SITL.

    you can pass viewerip as an IP address to optionally send fg and
    mavproxy packets too for local viewing of the mission in real time
    """
    global homeloc

    options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
    if viewerip:
        options += " --out=%s:14550" % viewerip
    if use_map:
        options += ' --map'

    home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
    sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=10)
    mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
    mavproxy.expect('Received [0-9]+ parameters')

    # setup test parameters
    mavproxy.send("param load %s/default_params/sub.parm\n" % testdir)
    mavproxy.expect('Loaded [0-9]+ parameters')
    mavproxy.send('param set FS_GCS_ENABLE 0\n')
    mavproxy.send("param set LOG_REPLAY 1\n")
    mavproxy.send("param set LOG_DISARMED 1\n")
    time.sleep(3)

    # reboot with new parameters
    util.pexpect_close(mavproxy)
    util.pexpect_close(sitl)

    sitl = util.start_SITL(binary, model='vectored', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
    mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
    mavproxy.expect('Telemetry log: (\S+)')
    logfile = mavproxy.match.group(1)
    print("LOGFILE %s" % logfile)

    buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog")
    print("buildlog=%s" % buildlog)
    if os.path.exists(buildlog):
        os.unlink(buildlog)
    try:
        os.link(logfile, buildlog)
    except Exception:
        pass

    mavproxy.expect('Received [0-9]+ parameters')

    util.expect_setup_callback(mavproxy, expect_callback)

    expect_list_clear()
    expect_list_extend([sitl, mavproxy])

    print("Started simulator")

    # get a mavlink connection going
    try:
        mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
    except Exception as msg:
        print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
        raise
    mav.message_hooks.append(message_hook)
    mav.idle_hooks.append(idle_hook)

    failed = False
    e = 'None'
    try:
        print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
        mav.wait_heartbeat()
        print("Waiting for GPS fix")
        mav.wait_gps_fix()

        homeloc = mav.location()
        print("Home location: %s" % homeloc)
        if not arm_sub(mavproxy, mav):
            print("Failed to ARM")
            failed = True
        if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub1.txt")):
            print("Failed mission")
            failed = True
        
        mavproxy.send('disarm\n');
        
        # wait for disarm
        mav.motors_disarmed_wait()
        
        if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")):
            print("Failed log download")
            failed = True
    except pexpect.TIMEOUT as e:
        print("Failed with timeout")
        failed = True

    mav.close()
    util.pexpect_close(mavproxy)
    util.pexpect_close(sitl)

    valgrind_log = util.valgrind_log_filepath(binary=binary, model='sub')
    if os.path.exists(valgrind_log):
        os.chmod(valgrind_log, 0o644)
        shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log"))

    if failed:
        print("FAILED: %s" % e)
        return False
    return True