#!/usr/bin/env python3

import ev3dev.ev3 as ev3

'''
add enum for color definitions (type should be tupel of int's).
later onwards compare the idividual readouts like this to determine the color:
RED = (>90, <40, <25)
BLUE = (<25, >75, >90)

Example:
color = (100, 30, 20)
(color[0] > 90 and (color[1], color[2]) < (40, 25))
'''

'''
Calculate brightness/to greyscale from RGB:
http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5445596
https://en.wikipedia.org/wiki/YUV
Formula:
Y=(0.299 x R) + (0.587 x G) + (0.114 x B);
'''

'''
class Color(tuple, Enum):
    RED = (65, 30, 20)
    BLUE = (20, 45, 60)
'''


class Sensor:
    def __init__(self):
        try:
            self._bumper_l = ev3.TouchSensor("in1")
            self._bumper_r = ev3.TouchSensor("in4")
            self.bumperwaspressed = False
            self._camera = ev3.ColorSensor()
            self._camera.mode = 'RGB-RAW'
            self.RGB = self._camera.bin_data("hhh")
            self._brightness = self._camera.value()
        except OSError:
            print("Error while setting up sensors. Please make sure that everything is plugged in properly.")
        self.lastcolor = None
        self.edgebrightness = None
        self.red = (40, 30, 30)
        self.blue = (40, 40, 60)

    def refresh(self):
        self.RGB = self._camera.bin_data("hhh")
        self._brightness = self._camera.value()

    def reset(self):
        self.bumperwaspressed = False
        self.lastcolor = None

    def calibrateBrightness(self):
        self.refresh()
        self.edgebrightness = self.getbrightness()

    def checkbumper(self):
        if(self._bumper_l.value() or self._bumper_r.value()):
            self.bumperwaspressed = True

    def getbrightness(self):
        return self._brightness

    def getcolor(self):
        if(self._camera.mode == 'RGB-RAW'):
            if(self.RGB[0] > self.red[0] and self.RGB[1] < self.red[1] and self.RGB[2] < self.red[2]):
                print("red: ", self.RGB)
                self.lastcolor = self.red
            if(self.RGB[0] < self.blue[0] and self.RGB[1] > self.blue[1] and self.RGB[2] > self.blue[2]):
                print("blue ", self.RGB)
                self.lastcolor = self.blue

    def calibrateRGB(self, color):
        self.refresh()
        print("Color:", self.RGB)
        if (color is self.red):
            newRED = (self.RGB[0]-20, self.RGB[1]+15, self.RGB[2]+15)
            self.red = newRED
        elif (color is self.blue):
            newBLUE = (self.RGB[0]+10, self.RGB[1]-10, self.RGB[2])
            self.blue = newBLUE