import lejos.nxt.*;
import lejos.util.*;
import lejos.robotics.subsumption.*;
import lejos.robotics.navigation.*;

/**
  trying a FIRST lejos.robotics.subsumption/
  Behavior-based robotics example:

  this is an implementation of the Behavior interface,
  BehaviorForward - a going-forward behavior
  (better be a LOW-priority behavior, because it "thinks"
  it should ALWAYS be in control...!)

  @author Bagnall - based on course textbook Chapter 14 example,
          p. 277, AND on tutorial at 
     http://www.lejos.org/nxt/nxj/tutorial/Behaviors/BehaviorProgramming.htm
  @author adapted by Sharon Tuttle
  @version 2015-03-05
*/

public class BehaviorForward implements Behavior
{
    // data fields

    private DifferentialPilot robot;
    private boolean suppressed;

    // currently, this short delay is 200 ms / .2 second

    private static final int SHORT_DELAY = 200;

    /**
        create an instance of this forward-behavior

        @param pilot the DifferentialPilot to exhibit this 
                     forward-behavior
    */

    public BehaviorForward(DifferentialPilot pilot)
    {
        this.robot = pilot;
        this.suppressed = false;
    }

    /**
        this forward-behavior ALWAYS wants to take control!
        (it better be a LOWER-priority behavior...!)
        ...BUT it delays a SHORT_DELAY amount before saying so
        
        @return if this forward-behavior should take control
    */

    public boolean takeControl()
    {
        Delay.msDelay(SHORT_DELAY);
        return true;
    }

    /**
        when this forward-behavior is suppressed, the robot should
	 stop moving forward -- we're using a boolean that will
        interact with the action method in a way we hope is
	 reasonable...
    */

    public void suppress()
    {
        this.suppressed = true;
    }

    /**
        when the arbitrator calls this for forward-behavior, 
        IF the robot is NOT currently moving, it should 
        start moving forward

        note that when this is first called, that means
	 the arbitrator called it because it was the
	 highest-priority behavior whose takeControl()
	 method returned true -- so, it is definitely NOT
	 suppressed right now, so this sets suppressed
	 boolean to false. 

        (BUT it might yield control and
        allow the arbitrator to call its suppress() method,
	 which would set suppressed to true again...)
    */

    public void action()
    {
        suppressed = false;

        if (!robot.isMoving())
	{
            robot.forward();
        }

        // the idea here is that this action method
        //    should give the Arbitratar a chance to "break in" --
        //    Thread.yield (Java 7 API) gives "a hint to the
        //    scheduler that the current thread [a thread of
        //    execution, like a little lightweight process]
        //    is willing to yield its current use of a processor"

        while ( !suppressed )
	{
            Thread.yield();
        }
        
        // clean up?

        robot.stop();
    }
}