import lejos.navigation.Pilot;
import lejos.navigation.TachoNavigator;
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.TouchSensor;
import lejos.subsumption.Arbitrator;
import lejos.subsumption.Behavior;

/**
 * Team Hadaga's COMP 2910 Spring 2008 program for the Power Missions table.
 * 
 * We chose to implement some behaviours at the Power table.
 *
 * @version 2.0
 * @author Team Hadaga
 */
public class PowerMissions {

    /**
     * Controls distance travelled.
     */    
    private static final int WHEEL_DIAMETER = 56;
    
    /**
     * Distance from centre of left wheel to centre of right wheel.  Controls
     * turn radius and centre point.
     */    
    private static final int TRACK_WIDTH = 135;
    
    /**
     * Used by the motor to determine torque power.
     */    
    private static final int MAX_POWER = 100;
 
    /**
     * Used by the TachoNavigator to determine speed.
     */    
    private static final int ONE_REV = 360;
    
    /**
     * Used by the TachoNavigator to determine speed.
     */    
    private static final int TWO_REVS = 720;

    /**
     * Used by the TachoNavigator to determine speed.
     */    
    private static final int MAX_REVS = 900;
    
    /**
     * Records number of completed missions.
     */    
    public static int COMPLETED_MISSIONS = 0;
    
    /**
     * Flags whether robot is in base.
     */    
    public static boolean IN_BASE = true;
 
    /**
     * Controls robot movements with differential steering.
     */
    public static Pilot pilot;
    
    /**
     * Tracks robot position, direction angle, and uses a Pilot object to
     * control the robot's movements.
     */
    public static TachoNavigator robot;
    
    /**
     * Controls which behavior should be active.
     */
    public static Arbitrator arbitrator;
    
    /**
     * An array of Behavior objects that will be passed to the Arbitrator.
     */    
    private static Behavior [] bArray;
    
    /**
     * The main method drives the mission.
     * 
     * @param args
     */
    public static void main(String[] args)
    {
        addBehaviors();
        createArbitrator();
        setMotorPower(MAX_POWER, MAX_POWER);
        regulateSpeed(true, true);
        setAcceleration(true, true);
        createNavigator();
        setSpeed(TWO_REVS); // 720 degrees per second
        
        // Robot begins at origin, facing due north
        setPosition(0, 0, 90);
        arbitrator.start();
    }
    
    /**
     * Populates an array of Behavior objects.
     */      
    private static void addBehaviors()
    {
        Behavior b1 = new LeaveBase(); // This class is in the same file
        Behavior b2 = new DriveForward(); // This class is in the same file        
        Behavior b3 = new MakeContact(); // This class is in the same file
        Behavior b4 = new Plough(); // This class is in the same file
        bArray = new Behavior [] {b1, b2, b3, b4};         
    }

    /**
     * Instantiates Arbitrator with an array of Behaviors.
     */      
    private static void createArbitrator()
    {
        arbitrator = new Arbitrator(bArray); 
    }

    /**
     * Instantiates Pilot and passes Pilot to TachoNavigator.
     */     
    private static void createNavigator()
    {
        pilot = new Pilot(WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.B);
        robot = new TachoNavigator(pilot);
    }

    /**
     * Turns speed regulation on.
     */       
    private static void regulateSpeed(boolean A, boolean B)
    {
        Motor.A.regulateSpeed(A);
        Motor.B.regulateSpeed(B);
    }

    /**
     * Sets smooth acceleration.
     */       
    private static void setAcceleration(boolean A, boolean B)
    {
        Motor.A.smoothAcceleration(A);
        Motor.B.smoothAcceleration(B);
    }

    /**
     * Sets power rating for motors.
     */     
    private static void setMotorPower(int A, int B)
    {
        Motor.A.setPower(A);
        Motor.B.setPower(B);
    }

    /**
     * Sets robot position and direction for TachoNavigator.
     */     
    private static void setPosition(int X, int Y, int Z)
    {
        robot.setPosition(X, Y, Z);
    }

    /**
     * Sets robot speed in wheel revolutions per second.
     */     
    private static void setSpeed(int revolutions)
    {
        robot.setSpeed(revolutions);
    }
}

/**
 * Behaviour implemented to push scoring objects across Power field.
 * 
 * @version 2.0
 * @author Team Hadaga
 */
class Plough implements Behavior
{
    /**
     * Instantiates object/
     */ 
    public Plough() {}

    /**
     * Determines combination of variables that cause Arbitrator to assert
     * this Behaviour.
     */     
    public boolean takeControl()
    {
        // Implemented when missions completed equals 2 and robot in base.
        return ((PowerMissions.COMPLETED_MISSIONS > 0)
            && PowerMissions.IN_BASE);
    }

    /**
     * Describes how the robot behaves when this code is asserted.
     */       
    public void action()
    {
        // Delivers Grid/Wind.Turbines/Trees.
        if (PowerMissions.COMPLETED_MISSIONS == 1)
        {
            // Robot is manually set in position.
            PowerMissions.robot.setPosition(-80, 0, 90);
            
            // Delivers Grid/Wind Turbines/Trees.
            PowerMissions.robot.goTo(-40, 780);

            // Returns to base.
            PowerMissions.robot.travel(-200);
            PowerMissions.robot.rotateTo(270);
            PowerMissions.robot.goTo(-100, 0);      
        }
        
        // Robot pauses to be manually repositioned before botton pressed to
        // re-deploy.
        PowerMissions.robot.stop();
        while(Button.waitForPress() != 1)
        {
        }
        
        // Iterates mission trackers.
        PowerMissions.COMPLETED_MISSIONS++;
        PowerMissions.IN_BASE = true;
        
        // Delivers hydro-dam and parks in ocean to complete mission as Wave
        // Turbine.
        if (PowerMissions.COMPLETED_MISSIONS == 2)
        {
            PowerMissions.robot.setPosition(-230, 0,45);
            PowerMissions.robot.goTo(410, 580);
            PowerMissions.robot.travel(-200);
            PowerMissions.robot.rotateTo(180);
            PowerMissions.robot.goTo(0, 200);
            PowerMissions.robot.goTo(-600, 200);
            PowerMissions.robot.goTo(-1480, 200);
            PowerMissions.robot.goTo(-1480, 830);
            System.exit(0);
        }
    }

    /**
     * Describes how the robot reacts when another Behaviour is asserted.
     */      
    public void suppress()
    {
        PowerMissions.robot.stop();
    }
    
}
/**
 * Behavior is implemented when the robot departs from a destination.
 * 
 * @version 2.0
 * @author Team Hadaga
 */
class DriveForward implements Behavior
{
    /**
     * Instantiates object/
     */     
    public DriveForward() {}
    
    /**
     * Determines combination of variables that cause Arbitrator to assert
     * this Behaviour.
     */      
    public boolean takeControl()
    {
        // Implemented when robot leaves base for the first time.
        return (PowerMissions.COMPLETED_MISSIONS == 0
            && !(PowerMissions.IN_BASE));
    }

    /**
     * Describes how the robot behaves when this code is asserted.
     */    
    public void action()
    {
        PowerMissions.robot.forward();
    }

    /**
     * Describes how the robot reacts when another Behaviour is asserted.
     */      
    public void suppress()
    {
        PowerMissions.robot.stop();
    }
}

/**
 * This Behavior is asserted when the robot is at the origin.
 * 
 * @version 1.2
 * @author Team Hadaga
 */
class LeaveBase implements Behavior
{
    /**
     * Instantiates object/
     */       
    public LeaveBase() {}

    /**
     * Determines combination of variables that cause Arbitrator to assert
     * this Behaviour.
     */     
    public boolean takeControl()
    {
        return ((PowerMissions.robot.getX() == 0)
            && (PowerMissions.robot.getY() == 0)
            && PowerMissions.IN_BASE);
    }

    /**
     * Describes how the robot behaves when this code is asserted.
     */     
    public void action()
    {
        PowerMissions.robot.goTo(0, 100);
        
        if (PowerMissions.COMPLETED_MISSIONS == 0) 
        {
            PowerMissions.robot.rotateTo(85);          
        }
        else
        {
            // Can be used to assert Behaviour in other situations.
        }
        PowerMissions.IN_BASE = false;
    }

    /**
     * Describes how the robot reacts when another Behaviour is asserted.
     */      
    public void suppress()
    {
        PowerMissions.robot.stop();
    }
}

/**
 * This Behavior is asserted when the TouchSensor is activated.
 * 
 * @version 1.2
 * @author Team Hadaga
 */
class MakeContact implements Behavior, SensorPortListener
{
    /**
     * Receives pressure data from sensor.
     */    
    TouchSensor touch;

    /**
     * Records state of TouchSensor.
     */    
    boolean hasCollided;

    /**
     * Instantiates object/
     */        
    public MakeContact()
    {
        hasCollided = false;
        SensorPort.S1.addSensorPortListener(this);
    }

    /**
     * Determines combination of variables that cause Arbitrator to assert
     * this Behaviour.
     */         
    public boolean takeControl()
    {
        // Asserts control when SensorPort state changes.
        if ((hasCollided) && PowerMissions.COMPLETED_MISSIONS < 2) {
            // Resets variable value
            hasCollided = false;
            // returns true so Arbitrator can assert Behaviour.
            return true;
        }
        else
        {
            return false;
        }
    }

    /**
     * Describes how the robot behaves when this code is asserted.
     */    
    public void action()
    {
        // Deploys satellites.
        if (PowerMissions.COMPLETED_MISSIONS == 0)
        {
            // Push a little just to be sure Satellite 1 has been deployed.
            PowerMissions.robot.travel(10);
            PowerMissions.robot.travel(-150);

            PowerMissions.robot.rotateTo(270);
            
            // Transcribe an arc to satellite 2.
            PowerMissions.robot.turn(-420, 190);
            
             // Deploy satellite 2.
            PowerMissions.robot.travel(280);
            
            // Return to base.
            PowerMissions.robot.travel(-150);
            PowerMissions.robot.rotateTo(290); // face south
            PowerMissions.robot.goTo(-100, -100); // overshoot trip home a little
        }
        
        // Pauses and waits for button to be pressed before continuing.
        PowerMissions.robot.stop();
        while(Button.waitForPress() != 1)
        {
        }
        
        // Increments match trackers.
        PowerMissions.COMPLETED_MISSIONS++;
        PowerMissions.IN_BASE = true;
    }

    /**
     * Describes how the robot reacts when another Behaviour is asserted.
     */      
    public void suppress()
    {
        PowerMissions.robot.stop();
    }

    /**
     * Monitors and reports pressure applied against TouchSensor.
     */     
    public void stateChanged(SensorPort touch, int oldValue, int newValue)
    {
        // If the sensor is pressed with a 900/1024 pressure
        if (newValue < 900)
        {
            // Asserts takeControl()
            hasCollided = true;
        }
    }
}