package org.usfirst.frc.team4915.stronghold.subsystems;
import java.util.Arrays;
import java.util.List;

import org.usfirst.frc.team4915.stronghold.Robot;
import org.usfirst.frc.team4915.stronghold.RobotMap;
import org.usfirst.frc.team4915.stronghold.commands.DriveTrain.ArcadeDrive;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class DriveTrain extends Subsystem {

<<<<<<< HEAD
    // Constructor for SpeedControllers: frontLeftMotor, rearLeftMotor,
    // frontRightMotor, rearRightMotor
    public static RobotDrive robotDrive =
            new RobotDrive(RobotMap.leftBackMotor, RobotMap.rightBackMotor);
    public double joystickThrottle;

    private double lastTopSpeed=-1.;
    // motors
    public static List<CANTalon> motors =
            Arrays.asList(RobotMap.leftFrontMotor, RobotMap.leftBackMotor, RobotMap.rightFrontMotor, RobotMap.rightBackMotor);
=======
    public final static double DEFAULT_SPEED_MAX_OUTPUT = 100.0;         // 100.0 == ~13 ft/sec interpolated from observations
    public final static double MAXIMUM_SPEED_MAX_OUTPUT = 150.0;         // 150.0 == ~20 ft/sec interpolated from observations

    public static RobotDrive robotDrive =
            new RobotDrive(RobotMap.leftMasterMotor, RobotMap.rightMasterMotor);

    private double maxSpeed = 0;
>>>>>>> 81e0108... position mode with autonomous drives striaght!

    @Override
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        System.out.println("ArcadeDrive getControlModes: " +
        			RobotMap.leftMasterMotor.getControlMode() + "  " +
        			RobotMap.rightMasterMotor.getControlMode());
        setDefaultCommand(new ArcadeDrive());

        robotDrive.setSafetyEnabled(true);
        robotDrive.stopMotor();
    }    

    public void arcadeDrive(double driveYstick, double driveXstick) {
        System.out.println("Arcade drive y: " + driveYstick + ", x " + driveXstick);
        robotDrive.arcadeDrive(driveYstick, driveXstick);
        System.out.println("Arcade drive get speed = " + RobotMap.leftMasterMotor.getSpeed());
    }
    
    public void resetEncoders(){
        RobotMap.leftMasterMotor.setEncPosition(0);
        RobotMap.rightMasterMotor.setEncPosition(0);
    }
       
    /*
     * Methods to get/set maximum top speed for our robot 
     */
    public double getMaxOutput() {

        if (maxSpeed == 0) {   /* not initialized, yet */
            return DEFAULT_SPEED_MAX_OUTPUT;
        }
        else {
            return maxSpeed;
        }
    }


    public void setMaxOutput(double maxOutput) {

        if (maxOutput > MAXIMUM_SPEED_MAX_OUTPUT) {
            maxSpeed = MAXIMUM_SPEED_MAX_OUTPUT;
        }
        else {
            maxSpeed = maxOutput;
        }

        robotDrive.setMaxOutput(maxSpeed);
    }
    
    public void stop() {
        robotDrive.stopMotor();
    }

    public void driveStraight(double speed) {
        RobotMap.leftMasterMotor.set(speed);
        RobotMap.rightMasterMotor.set(-speed);
    }

    public void turn(boolean left, double speed) {
        if (left) {
            RobotMap.leftMasterMotor.set(-speed);
            RobotMap.rightMasterMotor.set(-speed);
        } else {
            System.out.println("Turn right: " + speed);
            RobotMap.leftMasterMotor.set(speed);
            RobotMap.rightMasterMotor.set(speed);
        }
    }

    // autoturn is just a gentler version of (joystick) turn.
    public void autoturn(boolean left) {
        if (left) {
            robotDrive.arcadeDrive(0, -.55);
        } else {
            robotDrive.arcadeDrive(0, .55);
        }
    }

    public void turnToward(double target) {
        double heading = RobotMap.imu.getNormalizedHeading();
        double delta =  target - heading;
        System.out.println("target: " + target +
                           " heading: " + heading +
                           " delta: " + delta);
        SmartDashboard.putNumber("Vision Delta", delta);
        if (Math.abs(delta) < 5.0) {
            this.stop();
        } else {
            this.autoturn(delta > 0.0 /*turn left*/ );
        }
    }
}

@@ -606,516 +606,8 @@
  %7B%0A%0A
-%3C%3C%3C%3C%3C%3C%3C HEAD%0A    // Constructor for SpeedControllers: frontLeftMotor, rearLeftMotor,%0A    // frontRightMotor, rearRightMotor%0A    public static RobotDrive robotDrive =%0A            new RobotDrive(RobotMap.leftBackMotor, RobotMap.rightBackMotor);%0A    public double joystickThrottle;%0A%0A    private double lastTopSpeed=-1.;%0A    // motors%0A    public static List%3CCANTalon%3E motors =%0A            Arrays.asList(RobotMap.leftFrontMotor, RobotMap.leftBackMotor, RobotMap.rightFrontMotor, RobotMap.rightBackMotor);%0A=======%0A
     
@@ -1017,74 +1017,8 @@
 = 0;
-%0A%3E%3E%3E%3E%3E%3E%3E 81e0108... position mode with autonomous drives striaght!
 %0A%0A  
@@ -1431,20 +1431,16 @@
 );%0A    %7D
-    
 %0A%0A    pu
@@ -1739,36 +1739,32 @@
 Speed());%0A    %7D%0A
-    
 %0A    public void
@@ -1892,23 +1892,16 @@
 ;%0A    %7D%0A
-       
 %0A    /*%0A
@@ -1957,17 +1957,16 @@
 ur robot
- 
 %0A     */
@@ -2447,20 +2447,16 @@
 ;%0A    %7D%0A
-    
 %0A    pub

Fix for integration issue. (code compiles now).