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

    <Description>
        Home to target and, when within ContactDistance, perform a hockey stop (rudder hard over, speed 0) and
        wait for settle time to expire at ContactDepth. Ideally this mission would be used in conjunction with
        the scheduler in order to kick off another mission underwater once the settle time has expired. 
    </Description>

<!-- Define mission arguments -->

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

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

    <DefineArg Name="ContactLabel"><Description>
        The acoustic address of the asset to be tracked
    </Description><Units:count/><Value>9</Value></DefineArg>
    
    <DefineArg Name="ContactDepth"><Description>
        The depth of the asset to be tracked
    </Description><Units:meter/><Value>10</Value></DefineArg>

    <DefineArg Name="ContactDistance"><Description>
        How close to get before stopping 
    </Description><Units:meter/><Value>50</Value></DefineArg>

    <DefineArg Name="TrackingUpdatePeriod"><Description>
        How long to wait between acoustic queries.
    </Description><Units:second/><Value>5</Value></DefineArg>

    <DefineArg Name="NumberOfPings"><Description>
        Number of pings requested each time.
    </Description><Units:count/><Value>1</Value></DefineArg>

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

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

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

    <DefineArg Name="SettleTime"><Description>
        Wait this long to settle after stopping the prop
    </Description><Units:minute/><Value>10</Value></DefineArg>

    <DefineArg Name="CenterLatitude"><Description>
        The latitude of the center of the circle. Fill this in to specify a
        start position, it will be overwritten once the vehicle receives an
        acoustic signal from the contact.
    </Description><Units:degree/><Value>36.797</Value></DefineArg>

    <DefineArg Name="CenterLongitude"><Description>
        The longitude of the center of the circle. Fill this in to specify a
        start position, it will be overwritten once the vehicle receives an
        acoustic signal from the contact.
    </Description><Units:degree/><Value>-121.847</Value></DefineArg>
    
    <DefineArg Name="CircleRadius"><Description>
        Radius of circle around acoustic contact during wait time
    </Description><Units:meter/><Value>25</Value></DefineArg>

    <DefineArg Name="CircleMaxError"><Description>
        If this distance away from the circle, drive straight towards (or away
        from the center). Otherwise, try to reduce distance from the ideal
        circle.
    </Description><Units:meter/><Value>10</Value></DefineArg>

    <DefineArg Name="CircleTurnToPort"><Description>
        If true, vehicle turns to the left around the center point. If false,
        vehicle turns to the right.
    </Description><False/></DefineArg>
           
<!-- Internal variables that you should not change -->

    <DefineOutput Name="CenterTouched"><Description>
        Flag to indicate center lon/lat was updated.
    </Description><False/></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

    <Insert Filename="Insert/Science.xml">
        <Description>
            Most missions will run the science sensors. If you don't place this
            aggregate above NeedComms, science instruments get turned off on the
            last upcast and while floating on the surface.
        </Description>
    </Insert>

<!-- Safety nets and surfacing behaviors -->

    <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>

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

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

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>


    <!-- Add track acoustic contact directive high in the stack. -->
    <Estimation:TrackAcousticContact>
        <Parallel/>
        <Setting><Estimation:TrackAcousticContact.contactLabelSetting/><Arg Name="ContactLabel"/></Setting>
        <Setting><Estimation:TrackAcousticContact.contactDepthSetting/><Arg Name="ContactDepth"/></Setting>
        <Setting><Estimation:TrackAcousticContact.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
        <Setting><Estimation:TrackAcousticContact.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
    </Estimation:TrackAcousticContact>

    <Aggregate Id="UpdateCenter">

        <When>
            <Elapsed><Estimation:TrackAcousticContact.range_to_contact/></Elapsed>
            <Lt><Elapsed><Arg Name="CenterTouched"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="CenterLatitude"/><Estimation:TrackAcousticContact.contact_latitude/></Assign>

        <Assign><Sequence/><Arg Name="CenterLongitude"/><Estimation:TrackAcousticContact.contact_longitude/></Assign>

        <Assign><Sequence/><Arg Name="CenterTouched"/><True/></Assign>
        
    </Aggregate>

     
<!-- Start mission sequence. -->

    <Aggregate Id="Home">
       <Description>
           Head towards the target.
       </Description>

        <Break>
            <Universal:acoustic_contact_range/>
            <Lt><Arg Name="ContactDistance"/></Lt>
        </Break>
               
        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Pitch>
        
        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy>

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

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

        <Guidance:Circle>
            <Parallel/>
            <Setting><Guidance:Circle.latitude/><Arg Name="CenterLatitude"/></Setting>
            <Setting><Guidance:Circle.longitude/><Arg Name="CenterLongitude"/></Setting>
            <Setting><Guidance:Circle.radius/><Arg Name="CircleRadius"/></Setting>
            <Setting><Guidance:Circle.maxError/><Arg Name="CircleMaxError"/></Setting>
            <Setting><Guidance:Circle.turnToPort/><Arg Name="CircleTurnToPort"/></Setting>
        </Guidance:Circle>
                        
    </Aggregate>


    <Aggregate Id="Hockey Stop">
        
        <Syslog Severity="Important">Hockey stop! Range: <Universal:acoustic_contact_range/><Units:meter/></Syslog>
        
        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
        </Guidance:SetSpeed>

        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Units:degree/><Value>13</Value></Setting>
        </Guidance:Point>
                
        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Arg Name="ContactDepth"/></Setting>
        </Guidance:Pitch>

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

    </Aggregate>
        
</Mission>
