import wpilib
import wpilib.drive
import wpimath.controller
import ctre
import Limelight

def clamp(x, low, high):
    if x < low:
        return low
    elif x > high:
        return high
    else:
        return x

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.limelight = Limelight.Limelight()

        self.PID_P = .1
        self.PID_I = 0
        self.PID_D = .01
        self.PID_TOLERANCE = 1

        self.pidController = wpimath.controller.PIDController(
            self.PID_P,
            self.PID_I,
            self.PID_D
        )
        self.pidController.setTolerance(self.PID_TOLERANCE)

        self.useLimelightTargeting = False


    def autonomousInit(self):
        pass

    def autonomousPeriodic(self):
        pass

    def teleopInit(self):
        pass

    def teleopPeriodic(self):
        if self.primary.getAButtonPressed():
            self.useLimelightTargeting = True
            self.pidController.reset()
        elif self.primary.getAButtonReleased():
            self.useLimelightTargeting = False

        if self.useLimelightTargeting:
            self.getOnTarget(.5)
        else:
            self.drivetrain.arcadeDrive(self.primary.getLeftY(), self.primary.getLeftX())

    def getOnTarget(self, maxSpeed):
        if self.limelight.hasTargets():
            outputSpeed = clamp(
                self.pidController.calculate(
                    self.limelight.getHorizontalOffset(), 0),
                -maxSpeed,
                maxSpeed
            )

            self.drivetrain.tankDrive(outputSpeed, -outputSpeed)
        else:
            self.drivetrain.tankDrive(maxSpeed, -maxSpeed)

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