<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Guidance="Tethys/Guidance"
       xmlns:Units="Tethys/Units"
       xmlns:Universal="Tethys/Universal"
       xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
       xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                           Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="joystick_backseat">

    <Description>
        This mission drives the vehicle on the surface at a speed
        and heading as commanded by a beackseat computer.

        The mission monitors the time between speed updates and
        stops the vehicle if SpeedUpdateTimeout expires.
    </Description>

<!-- Mission-wide settings -->

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission
    </Description><Units:hour/><Value>2</Value></DefineArg>

    <DefineArg Name="SpeedUpdateTimeout"><Description>
        Max time allowed between speed command updates.
    </Description><Units:second/><Value>15</Value></DefineArg>

    <DefineArg Name="HorizontalCommandMode"><Description>
        Mission horizontal command mode. 0 = Heading command (default). 1 = Rudder angle command.
    </Description><Units:count/><Value>0</Value></DefineArg>

    <DefineArg Name="ElevatorAngle"><Description>
        Holds elevator angle to keep vehicle pitched up on the surface.
    </Description><Units:degree/><Value>-7</Value></DefineArg>

<!-- Mission args do not change these. -->

    <DefineOutput Name="Speed"><Description>
        Commanded vehicle speed.
    </Description><Units:meter_per_second/><Value>0</Value></DefineOutput>

    <DefineOutput Name="Heading"><Description>
        Commanded vehicle heading. Mission will init Heading to current vehicle heading.
    </Description><Units:degree/><Value>0</Value></DefineOutput>

    <DefineOutput Name="RudderAngle"><Description>
        Commanded vehicle RudderAngle. Only acitve when mission is in rudder commands mode.
    </Description><Units:degree/><Value>0</Value></DefineOutput>

    <DefineOutput Name="MassPosition"><Description>
        Commanded vehicle mass position. Mission will init MassPosition to current vehicle default.
    </Description><Control:VerticalControl.massDefault/></DefineOutput>

    <DefineOutput Name="HeadingMode"><Description>
        Heading angle command mode enumaration.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="RudderMode"><Description>
        Rudder angle command mode enumaration.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="ElapsedSinceMissionStarted"><Units:minute/><Value>0</Value></DefineOutput>


<!-- Missions should always start with a timeout. -->

    <Timeout Duration="MissionTimeout"/>

<!-- Init Heading at current vehicle heading. -->

    <Assign><Sequence/><Arg Name="Heading"/><Universal:platform_orientation/></Assign>

<!-- Add guidance update directives high in the stack. -->

    <Aggregate Id="SpeedTimeout">

        <When>
            <Elapsed><Arg Name="Speed"/></Elapsed>
            <Ge><Arg Name="SpeedUpdateTimeout"/></Ge>
        </When>

        <Assign><Sequence/><Arg Name="Speed"/><Units:meter_per_second/><Value>0</Value></Assign>

        <Syslog Severity="Fault">Speed timeout reached. Stopping vehicle (speed <Arg Name="Speed"/><Units:meter_per_second/>).</Syslog>

    </Aggregate>

    <Aggregate Id="UpdateSpeed">

        <When>
            <Elapsed><CustomUri Uri="_.speedCmd"/></Elapsed>
            <Lt><Elapsed><Arg Name="Speed"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="Speed"/><CustomUri Uri="_.speedCmd"/></Assign>

        <Syslog Severity="Info">Updating vehicle speed <Arg Name="Speed"/><Units:meter_per_second/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateCommandMode">

        <When>
            <Elapsed><CustomUri Uri="_.horizontalCmdMode"/></Elapsed>
            <Lt><Elapsed><Arg Name="HorizontalCommandMode"/></Elapsed></Lt>
            <And>
                <CustomUri Uri="_.horizontalCmdMode"/><Eq><Arg Name="HeadingMode"/></Eq>
                <Or><CustomUri Uri="_.horizontalCmdMode"/><Eq><Arg Name="RudderMode"/></Eq></Or>
            </And>
        </When>

        <Assign><Sequence/><Arg Name="HorizontalCommandMode"/><CustomUri Uri="_.horizontalCmdMode"/></Assign>

        <Syslog Severity="Info">Updating horizontal command mode <Arg Name="HorizontalCommandMode"/><Units:count/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateHeading">

        <When>
            <Elapsed><CustomUri Uri="_.headingCmd"/></Elapsed>
            <Lt><Elapsed><Arg Name="Heading"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="Heading"/><CustomUri Uri="_.headingCmd"/></Assign>

        <Syslog Severity="Info">Updating heading <Arg Name="Heading"/><Units:degree/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateRudder">

        <When>
            <Elapsed><CustomUri Uri="_.rudderAngleCmd"/></Elapsed>
            <Lt><Elapsed><Arg Name="RudderAngle"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="RudderAngle"/><CustomUri Uri="_.rudderAngleCmd"/></Assign>

        <Syslog Severity="Info">Updating rudder angle <Arg Name="RudderAngle"/><Units:degree/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateMassPosition">

        <When>
            <Elapsed><CustomUri Uri="_.massPositionCmd"/></Elapsed>
            <Lt><Elapsed><Arg Name="MassPosition"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="MassPosition"/><CustomUri Uri="_.massPositionCmd"/></Assign>

        <Syslog Severity="Info">Updating mass position <Arg Name="MassPosition"/><Units:millimeter/></Syslog>

    </Aggregate>

<!-- Power the Backseat payload. -->

    <Guidance:BackseatDriver>
        <Parallel/>
    </Guidance:BackseatDriver>

<!-- Keep the vehilce on the surface. -->

    <Aggregate Id="Surface">
        <While>
            <Universal:depth/><Ge><Control:VerticalControl.surfaceThreshold/></Ge>
        </While>

        <Guidance:GoToSurface>
            <Progression/>
        </Guidance:GoToSurface>
    </Aggregate>

<!-- Start mission sequence. -->

    <Aggregate Id="JoystickControl">

        <Sequence/>

        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyDefault/></Setting>
        </Guidance:Buoyancy>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
        </Guidance:SetSpeed>

        <Guidance:Mass>
            <Parallel/>
            <Setting><Guidance:Mass.position/><Arg Name="MassPosition"/></Setting>
        </Guidance:Mass>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.elevatorAngle/><Arg Name="ElevatorAngle"/></Setting>
        </Guidance:Pitch>

        <Aggregate Id="CmdHeading">

            <While>
                <Arg Name="HorizontalCommandMode"/><Eq><Arg Name="HeadingMode"/></Eq>
            </While>

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.forceUpdate/><True/></Setting>
                <Setting><Guidance:Point.heading/><Arg Name="Heading"/></Setting>
            </Guidance:Point>

        </Aggregate>

        <Aggregate Id="CmdRudder">

            <While>
                <Arg Name="HorizontalCommandMode"/><Eq><Arg Name="RudderMode"/></Eq>
            </While>

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.forceUpdate/><True/></Setting>
                <Setting><Guidance:Point.rudderAngle/><Arg Name="RudderAngle"/></Setting>
            </Guidance:Point>

        </Aggregate>

        <Aggregate Id="CheckIn">

            <Sequence Repeat="288"/>

            <ReadDatum Id="Read_GPS">
                <Timeout Duration="P5M"/>
                <Universal:time_fix/>
            </ReadDatum>

            <ReadDatum Id="Read_Iridium">
                <Universal:platform_communications/>
            </ReadDatum>

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Units:minute/><Value>5</Value></Setting>
            </Guidance:Wait>

            <Assign><Sequence/><Arg Name="ElapsedSinceMissionStarted"/><Elapsed><Universal:mission_started/></Elapsed></Assign>

            <Syslog Severity="Important">Joystick mission has been running for <Arg Name="ElapsedSinceMissionStarted"/><Units:minute/></Syslog>

        </Aggregate>

    </Aggregate>

</Mission>
