package org.usfirst.frc.team2509.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Timer;


public class Robot extends IterativeRobot {
	Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(0);
    Victor leftFDrive = new Victor(1);
    Victor rightFDrive = new Victor(2);
    Victor leftRDrive = new Victor(3);
    Victor rightRDrive = new Victor(4);
    RobotDrive chassis = new RobotDrive(leftFDrive, leftRDrive, rightFDrive, rightRDrive);;
    float rightRDriveSpeed = 0.75f;
    float rightFDriveSpeed = 0.75f;
    float leftRDriveSpeed = 0.75f;
    float leftFDriveSpeed = 0.75f;
    float chassisSpeed = 0.5f;
	
    public void robotInit() {
    	chassis.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        chassis.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        chassis.setSafetyEnabled(true);
        
}

    public void autonomousPeriodic() {
    	chassis.setSafetyEnabled(false);
        chassis.drive(chassisSpeed, 0.0);
        Timer.delay(2.0);
        chassis.drive(0.0, 0.0);
    }

    public void teleopPeriodic() {
    	chassis.tankDrive(leftStick.getAxis(Joystick.AxisType.kX), leftStick.getAxis(Joystick.AxisType.kY));
        if(rightStick.getRawButton(5)){
            rightRDrive.set(rightRDriveSpeed);
            rightFDrive.set(rightFDriveSpeed);
            leftRDrive.set(-leftRDriveSpeed);
            leftFDrive.set(-leftFDriveSpeed);
        }else if(rightStick.getRawButton(4)){
            rightRDrive.set(-rightRDriveSpeed);
            rightFDrive.set(-rightFDriveSpeed);
            leftRDrive.set(leftRDriveSpeed);
            leftFDrive.set(leftFDriveSpeed);
        }else{
            rightRDrive.set(0.0);
            rightFDrive.set(0.0);
            leftRDrive.set(0.0);
            leftFDrive.set(0.0);
        }
    }

    public void testPeriodic() {
    
    }
    
}