/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.lunatecs316.frc2013.subsystems;

import org.lunatecs316.frc2013.RobotMap;
import org.lunatecs316.frc2013.lib.Potentiometer;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 * @author domenicpaul
 */
public class Pickup extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    // Belt states
    public static final int BELTS_ON_FORWARD = 0;
    public static final int BELTS_ON_REVERSE = 1;
    public static final int BELTS_OFF = 2;
    
    private Jaguar angleMotor = new Jaguar(RobotMap.pickupAngleMotor);
    private Victor beltMotor = new Victor(RobotMap.pickupBeltMotor);
    private Potentiometer anglePot = new Potentiometer(RobotMap.pickupAnglePot);
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    /**
     * Set the state of the pickup belts
     * @param state - BELTS_ON_FORWARD(0), BELTS_ON_REVERSE(1), BELTS_OFF(2)
     */
    public void setBeltState(int state) {
        switch (state) {
            case BELTS_ON_FORWARD:
                beltMotor.set(1.0);
                break;
            case BELTS_ON_REVERSE:
                beltMotor.set(-1.0);
                break;
            case BELTS_OFF:
                beltMotor.set(0.0);
                break;
            default:
                System.err.println("Error: invalid belt state");
                beltMotor.set(0.0);
                break;
        }
    }
    
    /**
     * Raise the pickup arm
     */
    public void raise() {
        angleMotor.set(1.0);
    }
    
    /**
     * Lower the pickup arm
     */
    public void lower() {
        angleMotor.set(-1.0);
    }
    
    /**
     * Stop the pickup arm
     */
    public void stop() {
        angleMotor.set(0.0);
    }
}