<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Guidance="Tethys/Guidance" 
       xmlns:Sensor="Tethys/Sensor"
       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/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
                           Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Sensor http://okeanids.mbari.org/tethys/Xml/Sensor.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="FDI_nominal">

    <DefineArg Name="InsertDepth"><Description>
        Depth to settle.
    </Description><Units:meter/><Value>4</Value></DefineArg>

    <DefineArg Name="InsertSettleTime"><Description>
        Time to settle.
    </Description><Units:minute/><Value>4</Value></DefineArg>

    <DefineArg Name="AllowableMassDeadband"><Description>
        How close to get to the specified mass position before considering it
        close enough. This goes into a mission-level Break condition because
        Guidance:Pitch.massPosition never evaluates to satisfied.
    </Description><Units:millimeter/><Value>0.1</Value></DefineArg>

    <DefineArg Name="MassDefault"><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="ExperimentMassDefault"><Units:millimeter/><Value>0</Value></DefineArg>

    <DefineArg Name="FwdLocation"><Description>
        Move here on the fwd stroke.
    </Description><Units:millimeter/><Value>35</Value></DefineArg>

    <DefineArg Name="AftLocation"><Description>
        Move here on the backward stroke.
    </Description><Units:millimeter/><Value>-18</Value></DefineArg>

    <DefineArg Name="HomeLocation"><Description>
        Move here at the beginning and end of each experiment.
    </Description><Units:millimeter/><Value>0</Value></DefineArg>

    <DefineArg Name="Loops"><Description>
        Number of experiment iterations.
    </Description><Units:count/><Value>135</Value></DefineArg>

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

    <DefineArg Name="NeedCommsTime"><Description>
        How often to surface for commumications
    </Description><Units:minute/><Value>90</Value></DefineArg>

    <DefineArg Name="MaxDepth"><Description>
        Maximum depth for the entire mission.
    </Description><Units:meter/><Value>40</Value></DefineArg>

    <DefineArg Name="MinOffshore"><Description>
        Minimum distance offshore for the entire mission.
    </Description><Units:kilometer/><Value>2</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <Timeout Duration="MissionTimeout"/>

    <Insert Filename="Insert/NeedComms.xml" Id="NeedComms"/>

    <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="NeedCommsTime"/></Assign>

    <Insert Filename="Insert/StandardEnvelopes.xml"/>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinAltitude"/><Arg Name="MinAltitude"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MaxDepth"/><Arg Name="MaxDepth"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinOffshore"/><Arg Name="MinOffshore"/></Assign>

    <Guidance:SetSpeed>
        <Parallel/>
        <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
    </Guidance:SetSpeed>

    <Guidance:Pitch>
        <Parallel/>
        <Setting><Guidance:Pitch.depth/><Arg Name="InsertDepth"/></Setting>
    </Guidance:Pitch>

    <Syslog Severity="Info">Waiting for the vehicle to settle. Depth = <Universal:depth/><Units:meter/></Syslog>

    <Guidance:Wait>
        <Sequence/>
        <Setting><Guidance:Wait.duration/><Arg Name="InsertSettleTime"/></Setting>
    </Guidance:Wait>

    <Aggregate Id="HomeToZero">

        <Sequence/>

        <Assign><Parallel/><Control:VerticalControl.massDefault/><Arg Name="ExperimentMassDefault"/></Assign>

        <Syslog>"Homing"</Syslog>

        <Guidance:Pitch>
            <Sequence/>
            <Timeout Duration="P1M">
                <Syslog Severity="Error">Timed out moving mass shifter.</Syslog>
            </Timeout>
            <Break>
                <Abs><Universal:platform_mass_position/><Sub><Arg Name="HomeLocation"/></Sub></Abs>
                <Lt><Arg Name="AllowableMassDeadband"/></Lt>
            </Break>
            <Setting><Guidance:Pitch.massPosition/><Arg Name="HomeLocation"/></Setting>
        </Guidance:Pitch>

        <Aggregate Id="PowerFDI">

            <Syslog>"Powering"</Syslog>

            <ReadData Strategy="MinError">
                <Parallel/>
                <Sensor:FDI.powerFDI/>
            </ReadData>

            <Syslog>"Waiting for payload to boot"</Syslog>

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

            <Aggregate Id="Experiment">

                <Sequence Repeat="Loops"/>

                <Syslog>"Experiment beginning: Idling for 10 seconds"</Syslog>

                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Units:second/><Value>10</Value></Setting>
                </Guidance:Wait>

                <Aggregate Id="RunFDI">

                    <Sequence/>

                    <Syslog>"Starting FDI"</Syslog>

                    <ReadData Strategy="MinError">
                        <Parallel/>
                        <Sensor:FDI.runFDI/>
                    </ReadData>

                    <Syslog>"Waiting"</Syslog>

                    <Guidance:Wait>
                        <Sequence/>
                        <Setting><Guidance:Wait.duration/><Units:second/><Value>10</Value></Setting>
                    </Guidance:Wait>

                    <Syslog>"Sending Mass Shifter fwd"</Syslog>

                    <Guidance:Pitch>
                        <Sequence/>
                        <Timeout Duration="P1M">
                            <Syslog Severity="Error">Timed out moving mass shifter.</Syslog>
                        </Timeout>
                        <Break>
                            <Abs><Universal:platform_mass_position/><Sub><Arg Name="FwdLocation"/></Sub></Abs>
                            <Lt><Arg Name="AllowableMassDeadband"/></Lt>
                        </Break>
                        <Setting><Guidance:Pitch.massPosition/><Arg Name="FwdLocation"/></Setting>
                    </Guidance:Pitch>

                    <Syslog>"Sending Mass Shifter back"</Syslog>

                    <Guidance:Pitch>
                        <Sequence/>
                        <Timeout Duration="P2M">
                            <Syslog Severity="Error">Timed out moving mass shifter.</Syslog>
                        </Timeout>
                        <Break>
                            <Abs><Universal:platform_mass_position/><Sub><Arg Name="AftLocation"/></Sub></Abs>
                            <Lt><Arg Name="AllowableMassDeadband"/></Lt>
                        </Break>
                        <Setting><Guidance:Pitch.massPosition/><Arg Name="AftLocation"/></Setting>
                    </Guidance:Pitch>

                    <Syslog>"Sending Mass Shifter to 0"</Syslog>

                    <Guidance:Pitch>
                        <Sequence/>
                        <Timeout Duration="P1M">
                            <Syslog Severity="Error">Timed out moving mass shifter.</Syslog>
                        </Timeout>
                        <Break>
                            <Abs><Universal:platform_mass_position/><Sub><Arg Name="HomeLocation"/></Sub></Abs>
                            <Lt><Arg Name="AllowableMassDeadband"/></Lt>
                        </Break>
                        <Setting><Guidance:Pitch.massPosition/><Arg Name="HomeLocation"/></Setting>
                    </Guidance:Pitch>

                    <Syslog>"Stopping FDI"</Syslog>

                </Aggregate>

            </Aggregate>

<!--    
	 <Syslog>"Leaving payload powered for 30 min."</Syslog>
	 <Guidance:Wait>
	   <Sequence/>
	   <Setting><Guidance:Wait.duration/><Units:minute/><Value>30</Value></Setting>
	 </Guidance:Wait>
	 -->

        </Aggregate>

    </Aggregate>

</Mission>
