import wpilib
import wpilib.drive
import math
import ctre

class MyRobot(wpilib.TimedRobot):
    def robotInit(self):
        self.leftFront = ctre.WPI_VictorSPX(0)
        self.leftRear = ctre.WPI_VictorSPX(1)
        self.rightFront = ctre.WPI_VictorSPX(2)
        self.rightRear = ctre.WPI_VictorSPX(3)

        self.left = wpilib.MotorControllerGroup(self.leftFront, self.leftRear)
        self.right = wpilib.MotorControllerGroup(self.rightFront, self.rightRear)
        self.right.setInverted(True)

        self.drivetrain = wpilib.drive.DifferentialDrive(self.left, self.right)

        self.primary = wpilib.XboxController(0)

        self.PULSES_PER_REVOLUTION = 512
        self.WHEEL_DIAMETER = 6
        self.DISTANCE_PER_PULSE = math.pi * self.WHEEL_DIAMETER / self.PULSES_PER_REVOLUTION

        self.leftDrivetrainEncoder = wpilib.Encoder(0, 1)
        self.leftDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)
        self.rightDrivetrainEncoder = wpilib.Encoder(2, 3)
        self.rightDrivetrainEncoder.setDistancePerPulse(self.DISTANCE_PER_PULSE)

    def autonomousInit(self):
        self.leftDrivetrainEncoder.reset()
        self.rightDrivetrainEncoder.reset()

    def autonomousPeriodic(self):
        if (self.leftDrivetrainEncoder.getDistance() + self.rightDrivetrainEncoder.getDistance()) / 2 < 60:
            self.drivetrain.tankDrive(.5, .5)
        else:
            self.drivetrain.tankDrive(0, 0)

    def teleopInit(self):
        pass

    def teleopPeriodic(self):
        self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())

if __name__ == "__main__":
    wpilib.run(MyRobot)