package org.texastorque.texastorque2015.subsystem;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.texastorque.texastorque2015.constants.Constants;
import org.texastorque.torquelib.controlLoop.TorquePID;

public class Arms extends Subsystem {

    public static double UP = 0.0;
    public static double HORIZONTAL = 0.0;
    public static double DOWN = 0.0;

    //Solenoid states
    private boolean armsOpen;
    private boolean punchOut;

    //pid
    private double setPointAngle;

    private double leftAngle;
    private double leftMotorSpeed;
    private TorquePID leftPID;

    private double rightAngle;
    private double rightMotorSpeed;
    private TorquePID rightPID;

    public Arms() {
        leftPID = new TorquePID();
        rightPID = new TorquePID();
    }

    @Override
    public void init() {
    }

    @Override
    public void run() {
        armsOpen = input.isArmOpen();
        punchOut = input.isPunchOut();

        leftAngle = feedback.getLeftTiltAngle();
        rightAngle = feedback.getRightTiltAngle();

        if (!input.isOverride()) {

            setPointAngle = input.getTiltAngle();

            leftPID.setSetpoint(setPointAngle);
            rightPID.setSetpoint(setPointAngle);

            leftMotorSpeed = leftPID.calculate(leftAngle);
            rightMotorSpeed = rightPID.calculate(rightAngle);
        } else {
            leftMotorSpeed = input.getTiltOverrideMotorSpeed();
            rightMotorSpeed = input.getTiltOverrideMotorSpeed();
        }

        if (outputEnabled) {
            output.setArmsOpen(armsOpen);
            output.setPunchOut(punchOut);
            output.setTiltMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
        }
    }

    @Override
    public void loadParams() {
        UP = Constants.TiltUpAngle.getDouble();
        HORIZONTAL = Constants.TiltHorizontalAngle.getDouble();
        DOWN = Constants.TiltDownAngle.getDouble();

        leftPID.setPIDGains(Constants.TiltP.getDouble(),
                Constants.TiltI.getDouble(),
                Constants.TiltD.getDouble());
        rightPID.setPIDGains(Constants.TiltP.getDouble(),
                Constants.TiltI.getDouble(),
                Constants.TiltD.getDouble());
    }

    @Override
    public void pushToDashboard() {
        SmartDashboard.putBoolean("ArmsOpen", armsOpen);
        SmartDashboard.putBoolean("PunchOut", punchOut);
        SmartDashboard.putNumber("LeftTiltMotorSpeed", leftMotorSpeed);
        SmartDashboard.putNumber("RightTiltMotorSpeed", rightMotorSpeed);
        SmartDashboard.putNumber("TiltSetpoint", setPointAngle);
    }

    @Override
    public String getLogNames() {
        return "ArmsOpen, PunchOut, TiltMotorSpeed, ";
    }

    @Override
    public String getLogValues() {
        return armsOpen + ", " + punchOut + ", " + leftMotorSpeed + ", ";
    }

}

@@ -734,68 +734,236 @@
     
-leftPID = new TorquePID();%0A        rightPID = new TorquePID(
+//Limit motor output on the left arm so that the right arm will not get behind.%0A        leftPID = new TorquePID();%0A        leftPID.setMaxOutput(0.8);%0A        %0A        rightPID = new TorquePID();%0A        rightPID.setMaxOutput(1.0
 );%0A 
@@ -1126,24 +1126,82 @@
 unchOut();%0A%0A
+        //Get the current angles from the Potentiometers.%0A
         left
@@ -1324,16 +1324,65 @@
 de()) %7B%0A
+            //The angle we want to arms to go to.
 %0A       
@@ -1417,32 +1417,191 @@
 etTiltAngle();%0A%0A
+            //Set the left arm to go to the desired angle, and the right arm to follow it.%0A            //This should keep the two sides level with each other.%0A
             left
@@ -1657,39 +1657,35 @@
 PID.setSetpoint(
-setPoin
+lef
 tAngle);%0A%0A      

Change arm pid to keep the two sides level