<?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_masstest">

    <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="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>-20</Value></DefineArg>

    <DefineArg Name="HomeLocation"><Description>
        Home location of the mass shifter.
    </Description><Units:millimeter/><Value>0</Value></DefineArg>

    <DefineArg Name="Loops"><Units:count/><Value>15</Value></DefineArg>

<Aggregate Id="PowerFDI">
  <Sequence/>

    <Syslog>"Powering"</Syslog>
    <ReadData Strategy="MinError">
      <Parallel/>
      <Sensor:FDI.powerFDI/>
    </ReadData>
    
    <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>

    <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>"Waiting"</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>30</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>
        
    <Guidance:Wait>
      <Sequence/>
      <Setting><Guidance:Wait.duration/><Units:minute/><Value>30</Value></Setting>
    </Guidance:Wait>
    
  </Aggregate>
  
</Mission>
