import wpilib
import math

class MyRobot(wpilib.TimedRobot):
    def robotInit(self):
        self.width = 84
        self.height = 56

        self.buffer = wpilib.AddressableLED.LEDData(self.width * self.height)

        self.leds = wpilib.AddressableLED(0)
        self.leds.setLength(self.width * self.height)
        self.leds.start()

        self.hsvShift = 0

        self.plasma = [[0] * self.width] * self.height

        for i in range(self.width):
            for j in range(self.height):
                self.plasma[i][j] = int((128.0 + (128.0 * math.sin(i / 4.0)) + 128.0 + (128.0 * math.sin(j / 4.0))) / 2)

        self.palette = [0] * 256

        for i in range(len(self.palette)):
            self.palette[i] = self.__hsvToColor(self.__map(i, 0, 255, 0, 359), 1, 1)

    def robotPeriodic(self):
        for i in range(self.width):
            for j in range(self.height):
                position = (self.plasma[i][j] + self.hsvShift) % 256

                self.buffer.setLED((j * self.width) + i, self.palette[position])

        self.hsvShift += 1
        self.leds.setData(self.buffer)

    def autonomousInit(self):
        pass

    def autonomousPeriodic(self):
        pass

    def teleopInit(self):
        pass

    def teleopPeriodic(self):
        pass

    def __map(self, x, inMin, inMax, outMin, outMax):
        return int((x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin)

    def __hsvToColor(self, h, s, v):
        if s == 0:
            return wpilib.Color(wpilib.Color8Bit(int(v * 255.0), int(v * 255.0), int(v * 255.0)))

        htemp = h / 60.0
        i = int(math.floor(htemp))

        f = htemp - i
        p = v * (1.0 - s)
        q = v * (1.0 - s * f)
        t = v * (1.0 - s * (1.0 - f))

        if i == 0:
            return wpilib.Color(wpilib.Color8Bit(int(v * 255.0), int(t * 255.0), int(p * 255.0)))
        elif i == 1:
            return wpilib.Color(wpilib.Color8Bit(int(q * 255.0), int(v * 255.0), int(p * 255.0)))
        elif i == 2:
            return wpilib.Color(wpilib.Color8Bit(int(p * 255.0), int(v * 255.0), int(t * 255.0)))
        elif i == 3:
            return wpilib.Color(wpilib.Color8Bit(int(p * 255.0), int(q * 255.0), int(v * 255.0)))
        elif i == 4:
            return wpilib.Color(wpilib.Color8Bit(int(t * 255.0), int(p * 255.0), int(v * 255.0)))
        elif i == 5:
            return wpilib.Color(wpilib.Color8Bit(int(v * 255.0), int(p * 255.0), int(q * 255.0)))
        else:
            return wpilib.Color(0, 0, 0)

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