<?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>120</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>
