import wpilib
import wpilib.drive
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.leds = wpilib.AddressableLED(0)
        self.leds.setLength(20)

        self.buffer = wpilib.AddressableLED.LEDData(20)

        self.leds.setData(self.buffer)
        self.leds.start()

        self.rainbowTimer = wpilib.Timer()
        self.rainbowTimer.start()

        self.rainbowColorSelection = 0
        self.hsvColorSelection = 0

        self.RAINBOW = [
            wpilib.Color.kRed, 
            wpilib.Color.kOrange, 
            wpilib.Color.kYellow, 
            wpilib.Color.kGreen,
            wpilib.Color.kBlue,
            wpilib.Color.kPurple]

    def robotPeriodic(self):
        if self.rainbowTimer.get() > 1:
            for i in range(10):
                self.buffer.setLED(i, self.RAINBOW[self.rainbowColorSelection % len(self.rainbowColorSelection)])
            
            self.rainbowColorSelection += 1

            self.rainbowTimer.reset()

        for i in range(10, 20):
            self.buffer.setHSV(i, self.hsvColorSelection % 180, 255, 255)

        self.hsvColorSelection += 1

        self.leds.setData(self.buffer)

    def autonomousInit(self):
        pass

    def autonomousPeriodic(self):
        pass

    def teleopInit(self):
        pass

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

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