package org.usfirst.frc.team4272.robot2015;

import org.usfirst.frc.team4272.robotlib.Toggler;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	private HwRobot robot = new HwRobot();
	private HwOI oi;
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {

    }

    public void autonomousInit() {
    	
    }
    public void autonomousPeriodic() {

    }

    private Toggler grabButton = new Toggler();
    private Toggler pushButton = new Toggler();
    private Toggler ledsButton = new Toggler();
    public void teleopInit() {
    	//robot.lDrive.reset();
    	//robot.rDrive.reset();
    	//robot.winch.reset();
    	//robot.lDrive.enable();
    	//robot.rDrive.enable();
    	//robot.winch.enable();
    	robot.grab.setEnabled(true);
    	robot.push.setEnabled(true);
    	oi = new HwOI();
    	grabButton.update(false);
    	pushButton.update(false);
    	ledsButton.update(false);
    }
    public void teleopPeriodic() {
        /* Drive */
    	robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY()));
    	robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY()));
    	
    	/* Winch */
    	double w = oi.xbox.getAxis(Axis.LY);
    	if (Math.abs(w) < 0.1) { w = 0; }
    	robot.winchM.pidWrite(-w);
    	
    	/* left intake */
    	if (oi.xbox.getButton(Button.LB)) {
    		robot.lIntake.set(Relay.Value.kReverse);
    	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
    		robot.lIntake.set(Relay.Value.kForward);
    	} else {
    		robot.lIntake.set(Relay.Value.kOff);
    	}
    	/* right intake */
    	if (oi.xbox.getButton(Button.RB)) {
    		robot.rIntake.set(Relay.Value.kReverse);
    	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
    		robot.rIntake.set(Relay.Value.kForward);
    	} else {
    		robot.rIntake.set(Relay.Value.kOff);
    	}
    	
    	/* grab */
    	robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A)));
    	robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B)));
    	if (ledsButton.update(oi.xbox.getButton(Button.RT))) {
    		robot.lights.set(Relay.Value.kForward);
    	} else {
    		robot.lights.set(Relay.Value.kOff);
    	}
    }
    
    public void disabledInit() {
    	//robot.lDrive.reset();
    	//robot.rDrive.reset();
    	//robot.winch.reset();
    	robot.grab.setEnabled(false);
    	robot.push.setEnabled(false);
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    
    }
    
}