package com.frc2879.newcomen.subsystems;

import com.ctre.CANTalon;
import com.ctre.CANTalon.FeedbackDevice;
import com.frc2879.newcomen.commands.DriveMecanum;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
 *
 */
public class Drivetrain extends Subsystem {
	
	public static int TALONS_FRONT_LEFT = 2;
	public static int TALONS_FRONT_RIGHT = 3;
	public static int TALONS_REAR_LEFT = 4;
	public static int TALONS_REAR_RIGHT = 5;
	
	public static int ENC_CODES_PER_REV = 2048;
	
	public CANTalon[] talons;
	RobotDrive robotDrive;
	 
    // Put methods for controlling this subsystem
    // here. Call these from Commands.


    public Drivetrain() {
        super("Drivetrain");
        //SmartDashboard.putData(this);
        System.out.println("drivetrain init");
        initTalonConfig();

    }
    

    public void initTalonConfig() {

	    talons = new CANTalon[] {
	    		new CANTalon(TALONS_FRONT_LEFT), new CANTalon(TALONS_FRONT_RIGHT),
	            new CANTalon(TALONS_REAR_LEFT), new CANTalon(TALONS_REAR_RIGHT)};
	    
	    talons[MotorType.kFrontLeft.value].setInverted(true);
	    talons[MotorType.kFrontRight.value].setInverted(true);
	    talons[MotorType.kRearLeft.value].setInverted(false);
	    talons[MotorType.kRearRight.value].setInverted(false);
	    
	    for (CANTalon t: talons) {
            t.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
            t.enableBrakeMode(false);
            t.setFeedbackDevice(FeedbackDevice.QuadEncoder);
            t.configEncoderCodesPerRev(ENC_CODES_PER_REV);
            t.setEncPosition(0);
            t.set(0);
        }
	    robotDrive = new RobotDrive(talons[MotorType.kFrontLeft.value], talons[MotorType.kFrontRight.value], 
	    							talons[MotorType.kRearLeft.value], talons[MotorType.kRearRight.value]);
	    robotDrive.setSafetyEnabled(false);
	}

    public void initDefaultCommand() {

        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    	
    	setDefaultCommand(new DriveMecanum());
    	System.out.println("drivetrain default command set");
    }
    public void stop() {
        robotDrive.stopMotor();
    }

	public  RobotDrive getRobotDrive() {
		// TODO Auto-generated method stub
		return robotDrive  ;
	}
	
	
	/**
	 * Returns the Talon with the given ID
	 * @param i  The ID of the Talon
	 * @return  The <code>CANTalon</code> object for the given ID
	 */
	public CANTalon getTalon(int id) {
		return talons[id];
	}
	
	public CANTalon getTalon(MotorType mtype) {
		return talons[mtype.value];
	}
	
	public double[] mecanumSpeeds_Cartesian(double x, double y, double rotation) {
		double xIn = x;
		double yIn = -y; // account for reversed y from joystick
		
		double[] wheelSpeeds = new double[4];
		wheelSpeeds[MotorType.kFrontLeft.value] = xIn + yIn + rotation;
	    wheelSpeeds[MotorType.kFrontRight.value] = -xIn + yIn - rotation;
	    wheelSpeeds[MotorType.kRearLeft.value] = -xIn + yIn + rotation;
	    wheelSpeeds[MotorType.kRearRight.value] = xIn + yIn - rotation;
	    return wheelSpeeds;
	}
	
	public void setTalons(double[] speeds) {
		talons[MotorType.kFrontLeft.value].set(MotorType.kFrontLeft.value);
	    talons[MotorType.kFrontRight.value].set(MotorType.kFrontRight.value);
	    talons[MotorType.kRearLeft.value].set(MotorType.kRearLeft.value);
	    talons[MotorType.kRearRight.value].set(MotorType.kRearRight.value);
	}
	
	public void customMecanumDrive_Cartesian(double x, double y, double rotation) {
		setTalons(mecanumSpeeds_Cartesian(x, y, rotation));
	}
	
}
    
@@ -3193,13 +3193,13 @@
 e%5B%5D 
-speed
+value
 s) %7B
@@ -3232,32 +3232,39 @@
 Left.value%5D.set(
+values%5B
 MotorType.kFront
@@ -3265,32 +3265,33 @@
 kFrontLeft.value
+%5D
 );%0A%09    talons%5BM
@@ -3314,32 +3314,39 @@
 ight.value%5D.set(
+values%5B
 MotorType.kFront
@@ -3352,24 +3352,25 @@
 tRight.value
+%5D
 );%0A%09    talo
@@ -3395,32 +3395,39 @@
 Left.value%5D.set(
+values%5B
 MotorType.kRearL
@@ -3431,24 +3431,25 @@
 arLeft.value
+%5D
 );%0A%09    talo
@@ -3483,16 +3483,23 @@
 ue%5D.set(
+values%5B
 MotorTyp
@@ -3516,16 +3516,17 @@
 ht.value
+%5D
 );%0A%09%7D%0A%09%0A

Fix mistake in setTalons