/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package mechanism;

import actuator.IMotor;
import core.GRTLoggedProcess;
import event.SwitchEvent;
import event.SwitchListener;
import sensor.GRTSwitch;

/**
 *
 * @author calvin, gerberduffy
 */
public class Wedge extends GRTLoggedProcess implements SwitchListener {
    private IMotor motor;
    private GRTSwitch limitUp;
    private GRTSwitch limitDown;
    private int targetState;
    private static final int UP = 0;
    private static final int DOWN = 1;
    private static final int DISABLED = 2;
    
    public Wedge(IMotor motor, GRTSwitch limitUp, GRTSwitch limitDown, String name){
        super(name);
        this.motor = motor;
        this.limitDown = limitDown;
        this.limitUp = limitUp;
    }

    public void startListening(){
        limitUp.addSwitchListener(this);
        limitDown.addSwitchListener(this);
    }
    
    public void stopListening(){
        limitUp.removeSwitchListener(this);
        limitDown.removeSwitchListener(this);
    }
    
    public void switchStateChanged(SwitchEvent e) {
        //if upper limit switch is tripped and we are going up...
        if((e.getSource() == limitUp) && (targetState == UP)){
            
            //if we are at upper limit whent trying to raise:
            if(e.getState() == GRTSwitch.PRESSED)       //If the upper switch is pressed, stop raising
                stopWedge();
        }
        
        //Otherwise, if we are going down and there is a change in the lower limit switch
        else if((e.getSource() == limitDown) && (targetState == DOWN)){
            if(e.getState() == GRTSwitch.PRESSED)
                stopWedge();
        }
    }
    
    public void raiseWedge(){
        //Set our goal to go up
        targetState = UP;
        
        //If the upper limit has not been reached, keep raising
        System.out.println("Limit Up State: " + limitUp.getState(GRTSwitch.KEY_STATE));
        if(limitUp.getState(GRTSwitch.KEY_STATE) != GRTSwitch.PRESSED){
            
            System.out.println("Raising wedge");

            motor.setSpeed(1.0);
        }
    }

    
    /**
     * 
     */
    public void lowerWedge(){
        
        //We want to go down now
        targetState = DOWN;
        
        System.out.println("Limit Down State: " + limitDown.getState(GRTSwitch.KEY_STATE));
        //Set motor speed to go down if we have not reached the lower limit
        if(limitDown.getState(GRTSwitch.KEY_STATE) != GRTSwitch.PRESSED){
            motor.setSpeed(-1.0);
            
            System.out.println("Lowering wedge");
        }
    }
    
    public void stopWedge(){
        targetState = DISABLED;
        motor.setSpeed(0.0);
    }

    
}

@@ -815,172 +815,46 @@
     
-%7D%0A%0A    public void startListening()%7B%0A        limitUp.addSwitchListener(this);%0A        limitDown.addSwitchListener(this);%0A    %7D%0A    %0A    public void stopListening()%7B
+    limitDown.addSwitchListener(this);
 %0A   
@@ -866,22 +866,19 @@
 limitUp.
-remove
+add
 SwitchLi
@@ -899,55 +899,10 @@
     
-    limitDown.removeSwitchListener(this);%0A    
 %7D
+%0A
 %0A   
@@ -1998,33 +1998,32 @@
        %7D%0A    %7D%0A%0A
- 
    %0A    /**%0A    

Change Wedge to start listening automatically