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

   <Description>
        1.) Wait timeout for loading in launcher.
        2.) Turn on prop and arm for docking.
        3.) Wait timeout for either cable present or timeout. 
        4.) Cable present == latch for timeout then release, back up and end mission.
        5.) No cable present == speed 0 and end mission.
        6.) end mission == float up at 0 speed
    </Description>
    
    <DefineArg Name="Speed"><Description>
        Thruster speed for docking.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineOutput Name="DockingDepth"><Description>
        How deep in the tank do you want to swim?
    </Description><Units:meter/><Value>1.22</Value></DefineOutput>    
        
    <DefineArg Name="WaitForLoadTimeout"><Description>
        Time to wait prior to turning on the prop.
    </Description><Units:second/><Value>90</Value></DefineArg>    

    <DefineArg Name="LaunchTimeout"><Description>
        Time to wait with the prop on driving to the dock.
    </Description><Units:second/><Value>60</Value></DefineArg>  

    <DefineArg Name="LatchTimeout"><Description>
        Time to wait before closing the latch after seeing the cable.
    </Description><Units:second/><Value>15</Value></DefineArg> 

    <DefineArg Name="DetachTimeout"><Description>
        Time to wait for dock detach.
    </Description><Units:second/><Value>7</Value></DefineArg>      

    <DefineArg Name="FloatTimeout"><Description>
        How long to wait for the vehicle to float to the surface.
    </Description><Units:second/><Value>120</Value></DefineArg>

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Set to NaN mm for active
        mass position
    </Description><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="ElevtorDeadband"><Description>
        Degree of rounding in elevator command output value
    </Description><Units:arcdeg/><Value>0.1</Value></DefineArg>

    <DefineArg Name="kpPitchElevatorGain"><Description>
        proportional gain
    </Description><Units:none/><Value>0.25</Value></DefineArg>

    <DefineArg Name="kiPitchElevatorGain"><Description>
        integral gain
    </Description><Units:reciprocal_second/><Value>0.008</Value></DefineArg>

    <DefineArg Name="kdPitchElevatorGain"><Description>
        derivative gain
    </Description><Units:second/><Value>0.05</Value></DefineArg>    

    <DefineArg Name="RudderDefault"><Description>
        Static setting for rudder.
    </Description><Units:degree/><Value>3.0</Value></DefineArg>

    <Timeout Duration="P15M"/>

    <Aggregate Id="TankDocking">

        <!-- Be neutral the entire time -->   
        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.massPosition/><Arg Name="MassDefault"/></Setting>
            <Setting>:<Guidance:Pitch.pitch/><Units:degree/><Value>0</Value></Setting>
            <Setting><Guidance:Pitch.depth/><Arg Name="DockingDepth"/></Setting>
        </Guidance:Pitch>

        <!-- Set up the elevator gains and deadband -->
        <Assign><Parallel/><Control:VerticalControl.elevDeadband/><Arg Name="ElevtorDeadband"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kpPitchElevator/><Arg Name="kpPitchElevatorGain"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kiPitchElevator/><Arg Name="kiPitchElevatorGain"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kdPitchElevator/><Arg Name="kdPitchElevatorGain"/></Assign>

        <!-- set up the horizontal control -->           
        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Arg Name="RudderDefault"/></Setting>
        </Guidance:Point>

        <!-- Let's fire up the DockingServo component here just so it's ready to go -->   
        <ReadData Strategy="MinError">
        	  <Parallel/>
            <Dock:Dock.docking_state/>
        </ReadData>
        
        <!-- Start with the arm closed if it isn't already -->  
        <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>             


        <Aggregate Id="Wait for loading">
        	  <Syslog Severity="Info">Waiting to load for <Arg Name="WaitForLoadTimeout"/><Units:second/></Syslog>
            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="WaitForLoadTimeout"/></Setting>
            </Guidance:Wait>             
        </Aggregate>


        <Aggregate Id="LaunchAndDock">
            <Timeout Duration="LaunchTimeout">
                <Syslog Severity="Important">Timed out while driving to the dock.</Syslog>
            </Timeout>
   
            <Syslog Severity="Info">Prop on and arming!</Syslog>
            <Guidance:SetSpeed>
                <Parallel/>
                <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
            </Guidance:SetSpeed>
               
            <!-- Arm the....arm. Hmm....  --> 
            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>


                <Aggregate Id="Latch">
                <When>
                    <Dock:Dock.dock_cable_present/>
                    <Eq><True/></Eq>
                </When>

                    <Guidance:Wait>
                        <Sequence/>
                        <Setting><Guidance:Wait.duration/><Arg Name="LatchTimeout"/></Setting>
                    </Guidance:Wait>                
                
                    <!-- close the arm cut the prop --> 
                    <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>            	
                    <Guidance:SetSpeed>
                        <Parallel/>
                        <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
                    </Guidance:SetSpeed>                
                </Aggregate>
        	
            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="LaunchTimeout"/></Setting>
            </Guidance:Wait> 

        </Aggregate>


        <Aggregate Id="Detach">
            <!-- open to detach  --> 
            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>

            <!-- And reverse -->   
            <Guidance:SetSpeed>
                <Parallel/>
                <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>-1</Value></Setting>
            </Guidance:SetSpeed>  
        
            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="DetachTimeout"/></Setting>
            </Guidance:Wait> 
            
        </Aggregate>

    </Aggregate>
    
    
    <Aggregate Id="Float Up">    
        <!-- close  --> 
        <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>

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

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
        </Guidance:SetSpeed>          
        
        <Guidance:Wait>
            <Sequence/>
            <Setting><Guidance:Wait.duration/><Arg Name="FloatTimeout"/></Setting>
        </Guidance:Wait>     

    </Aggregate>        

</Mission>
