public class Arm {
    private CANJaguar m_shoulder;
    private CANJaguar m_elbow;
    private DriverStationEnhancedIO m_enhancedIO;
    private int m_shoulderWaldoChannel;
    private int m_elbowWaldoChannel;
    private double m_shoulderValue = 0;
    private double m_elbowValue = 0;
    
    // This is correct. The down value is greater than the up value on the Waldo.
    private static final double kDownShoulderVoltage = 2.7;
    private static final double kUpShoulderVoltage = 1.3;
    private static final double kRetractedElbowVoltage = 2.8;
    private static final double kExtendedElbowVoltage = 0.9;
    public static final double kDownShoulderPosition = 0.09;
    public static final double kUpShoulderPosition = 0.55;
    public static final double kRetractedElbowPosition = 0.084;
    public static final double kExtendedElbowPosition = 0.665;

    public static final double kElbowMinToBeCollecting = .4;
    public static final double kShoulderMinWhenElbowInCollecting = 0.155;

    private DriverStation m_ds = DriverStation.getInstance();

    public Arm(int shoulderCANDevice, int elbowCANDevice, int shoulderWaldoChannel, int elbowWaldoChannel) {
        try {
            m_shoulder = new CANJaguar(shoulderCANDevice, CANJaguar.ControlMode.kPosition);
            m_shoulder.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_shoulder.setPID(1000, 0, 100);
            m_shoulder.configPotentiometerTurns(1);
            m_shoulder.configSoftPositionLimits(kUpShoulderPosition, kDownShoulderPosition);
            m_shoulder.enableControl();

            m_elbow = new CANJaguar(elbowCANDevice, CANJaguar.ControlMode.kPosition);
            m_elbow.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_elbow.setPID(700, 0, 50);
            m_elbow.configPotentiometerTurns(1);
            m_elbow.configSoftPositionLimits(kExtendedElbowPosition, kRetractedElbowPosition);
            m_elbow.enableControl();
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Init Error!");
        }
        m_enhancedIO = DriverStation.getInstance().getEnhancedIO();
        m_shoulderWaldoChannel = shoulderWaldoChannel;
        m_elbowWaldoChannel = elbowWaldoChannel;
    }

    private void getValues() {
        try {
            m_shoulderValue = m_enhancedIO.getAnalogIn(m_shoulderWaldoChannel);
            m_shoulderValue -= kDownShoulderVoltage;
            m_shoulderValue /= (kUpShoulderVoltage - kDownShoulderVoltage);
            if (m_shoulderValue < 0) {
                m_shoulderValue = 0;
            }
            if (m_shoulderValue > 1) {
                m_shoulderValue = 1;
            }

            m_elbowValue = m_enhancedIO.getAnalogIn(m_elbowWaldoChannel);
            m_elbowValue -= kRetractedElbowVoltage;
            m_elbowValue /= (kExtendedElbowVoltage - kRetractedElbowVoltage);
            if (m_elbowValue < 0) {
                m_elbowValue = 0;
            }
            if (m_elbowValue > 1) {
                m_elbowValue = 1;
            }
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Pos Read Error!");
        }
    }

    public void update() {
        getValues();
        double dMotorValueShoulder = m_shoulderValue;
        dMotorValueShoulder *= (kUpShoulderPosition - kDownShoulderPosition);
        dMotorValueShoulder += kDownShoulderPosition;

        double dMotorValueElbow = m_elbowValue;
        dMotorValueElbow *= (kExtendedElbowPosition - kRetractedElbowPosition);
        dMotorValueElbow += kRetractedElbowPosition;

        // Make sure that when the elbow is extended to collect that the shoulder stays
        // up enough to prevent the forarm from hitting the front of the robot.
        if ((dMotorValueElbow > kElbowMinToBeCollecting) && (dMotorValueShoulder < kShoulderMinWhenElbowInCollecting)){
            dMotorValueShoulder = kShoulderMinWhenElbowInCollecting;
        }

        try {
            m_shoulder.setX(dMotorValueShoulder);
            m_elbow.setX(dMotorValueElbow);
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Arm Pos Error!");
        }
    }
    
    /*
     *
     * @param shoulderPosition The position value for the shoulder in motor counts.
     * @param elbowPosition The position value for the elbow in motor counts.
     */
    public void setAutonomousArmPositions(double shoulderPosition, double elbowPosition) {
        if (m_ds.isAutonomous()) {
            // Make sure the shoulder position is in range.
            if (shoulderPosition > kUpShoulderPosition) {
                shoulderPosition = kUpShoulderPosition;
            } else if (shoulderPosition < kDownShoulderPosition) {
                shoulderPosition = kDownShoulderPosition;
            }
            // Make sure the elbow position is in range.
            if (elbowPosition > kExtendedElbowPosition) {
                elbowPosition = kExtendedElbowPosition;
            } else if (elbowPosition < kRetractedElbowPosition) {
                elbowPosition = kRetractedElbowPosition;
            }

            // Set the positions.
            try {
                m_shoulder.setX(shoulderPosition);
                m_elbow.setX(elbowPosition);
            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println("CAN Auto Arm Pos Error!");
            }
        }
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {
        try {
            SmartDashboard.log(m_shoulder.getPosition(), "Shoulder Position");
            SmartDashboard.log(m_elbow.getPosition(), "Elbow Position");
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Get Arm Pos Error!");
        }
    }
}