<?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="DockingTankTest">

   <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="MissionTimeout"><Description>
        Maximum length of mission
    </Description><Units:minute/><Value>15</Value></DefineArg>
        
    <DefineArg Name="Speed"><Description>
        Thruster speed for docking.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="LatchingSpeed"><Description>
        Thruster speed while closing the latch (ideally slower) than docking.
    </Description><Units:meter_per_second/><Value>0.5</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>70</Value></DefineArg>  

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

    <DefineArg Name="LatchCloseTimeout"><Description>
        Time to wait while the latch closes.
    </Description><Units:second/><Value>10</Value></DefineArg> 
    
    <DefineArg Name="DockTimeout"><Description>
        Time to wait on the dock.
    </Description><Units:second/><Value>15</Value></DefineArg>

    <DefineArg Name="DetachTimeout"><Description>
        Time to wait while performing 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="ElevatorDefault"><Description>
        Static setting for elevator.
    </Description><Units:degree/><Value>-4.0</Value></DefineArg>
    
    <DefineArg Name="RudderDefault"><Description>
        Static setting for rudder.
    </Description><Units:degree/><Value>3.0</Value></DefineArg>


    <DefineOutput Name="DiveMode"><Description>
        Mission variable (don't change). The mission sets this variable to
        switch between dive modes.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DoingComms"><Description>
        Mission variable (don't change). The mission will run in this to mode
        when running surface comms.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="LatchSequence"><Description>
        Mission variable (don't change). The mission will run in this mode to
        initiate the debouncing latch sequence.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="ArmClose"><Description>
        Mission variable (don't change). The mission will run in this mode when
        surface comms are done to dive the vehicle to DVL bottom range.
    </Description><Units:count/><Value>2</Value></DefineOutput>
    
    <DefineOutput Name="WaitOnDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        maintatin depth while the vehicle is latched on the dock.
    </Description><Units:count/><Value>3</Value></DefineOutput>

    <DefineOutput Name="DetachFromDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        drift away from the dock after is unlatched.
    </Description><Units:count/><Value>4</Value></DefineOutput>

    <DefineOutput Name="FloatUp"><Description>
        Mission variable (don't change). The mission will run in this mode to
        float back to the surface rather than drive.
    </Description><Units:count/><Value>5</Value></DefineOutput>
    
    <Timeout Duration="MissionTimeout"/>

    <Aggregate Id="TankDocking">

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

        <!-- set up the vertical control -->
        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Arg Name="DockingDepth"/></Setting>
            <Setting><Guidance:Pitch.elevatorAngle/><Arg Name="ElevatorDefault"/></Setting>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Pitch>

        <!-- 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="LatchSequence">
            <Description>
                Vehicle hit cable and now needs to debounce.
            </Description>
            <Syslog Severity="Important">Initiating latching sequence. Waiting for debounce</Syslog>
            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="LatchSequence"/></Eq>
            </While>
                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
                </Guidance:SetSpeed>  
            <Aggregate Id="dive">
                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="LatchTimeout"/></Setting>
                </Guidance:Wait>                    
                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="ArmClose"/></Assign>
            </Aggregate>
        </Aggregate>


        <Aggregate Id="ArmClose">
            <Description>
                Change speed and close arm.
            </Description>
            <Syslog Severity="Important">Closing and changing speed.</Syslog>
            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="ArmClose"/></Eq>
            </While>
                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Arg Name="LatchingSpeed"/></Setting>
                </Guidance:SetSpeed>
          <!--  <Aggregate Id="dive">-->
                <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>   
                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="LatchCloseTimeout"/></Setting>
                </Guidance:Wait>
                
                <Aggregate Id="assign">
            	      <When>
                        <Dock:Dock.dock_cable_present/>
                        <Eq><True/></Eq>
                        <And><Servo:DockingServo.armAngle/><Lt><Units:degree/><Value>-12</Value></Lt></And>
                    </When> 
                    <Syslog Severity="Important">Latch confirmed closed.</Syslog>
                    <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="WaitOnDock"/></Assign>
                </Aggregate>
                <!-- If we didn't change the mode above, let's detach -->
                <Syslog Severity="Important">Could not confirm latch closed. Detaching.</Syslog>
                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="DetachFromDock"/></Assign>
            <!--</Aggregate>-->
        </Aggregate>
        

        <Aggregate Id="DockHold">
            <Description>
                Wait on dock after vehicle latches.
            </Description>
            <Syslog Severity="Important">Holding on dock.</Syslog>
            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="WaitOnDock"/></Eq>
            </While>
            <Aggregate Id="dive">
                <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>         	

		<!--                <ReadData Id="PowerRedLights">
               	    <Parallel/>
                    <Sensor:PowerOnly.sampleLoad3/>
                </ReadData>
-->

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

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="DetachFromDock"/></Assign>                
                
            </Aggregate>

        </Aggregate>

        <Aggregate Id="Detach">
            <Description>
                Reverse off the dock.
            </Description>   
            <Syslog Severity="Important">Detaching From Dock.</Syslog>     	
            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="DetachFromDock"/></Eq>
            </While>
            <Aggregate Id="dive">
                <!-- 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> 
                
                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="FloatUp"/></Assign>            
            </Aggregate>
        </Aggregate>

    
        <Aggregate Id="Float Up"> 
            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="FloatUp"/></Eq>
            </While>
            <Syslog Severity="Important">Floating Up.</Syslog>
                   
            <!-- keep open while floating up  --> 
            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</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>     
           
           <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="DoingComms"/></Assign>
        </Aggregate>        





        <Aggregate Id="Wait for loading">
        	  <Syslog Severity="Info">Waiting to load for <Arg Name="WaitForLoadTimeout"/><Units:second/></Syslog>
           
            <Guidance:Buoyancy>
                <Parallel/>
                <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
            </Guidance:Buoyancy>
                	  
            <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>
            
            <Break>
                <Arg Name="DiveMode"/><Gt><Arg Name="DoingComms"/></Gt>
            </Break>
   
            <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>
                        <And><Not><Arg Name="DiveMode"/><Eq><Arg Name="LatchSequence"/></Eq></Not></And>
                    </When>
                    <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="LatchSequence"/></Assign>
                </Aggregate> 

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

        </Aggregate>

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

</Mission>
