import RPi.GPIO as GPIO
from rpi_ws281x import Adafruit_NeoPixel, Color
import time
import math
import smbus


class motor ():
    def __init__(self, ain1=12, ain2=13, ena=6, bin1=20, bin2=21, enb=26):
        self.AIN1 = ain1
        self.AIN2 = ain2
        self.BIN1 = bin1
        self.BIN2 = bin2
        self.ENA = ena
        self.ENB = enb
        self.PA = 50
        self.PB = 50

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.AIN1, GPIO.OUT)
        GPIO.setup(self.AIN2, GPIO.OUT)
        GPIO.setup(self.BIN1, GPIO.OUT)
        GPIO.setup(self.BIN2, GPIO.OUT)
        GPIO.setup(self.ENA, GPIO.OUT)
        GPIO.setup(self.ENB, GPIO.OUT)
        self.PWMA = GPIO.PWM(self.ENA, 500)
        self.PWMB = GPIO.PWM(self.ENB, 500)
        self.PWMA.start(self.PA)
        self.PWMB.start(self.PB)
        self.stop()

    def forward(self, PWM=30):
        self.PWMA.ChangeDutyCycle(PWM)
        self.PWMB.ChangeDutyCycle(PWM)
        GPIO.output(self.AIN1, GPIO.LOW)
        GPIO.output(self.AIN2, GPIO.HIGH)
        GPIO.output(self.BIN1, GPIO.LOW)
        GPIO.output(self.BIN2, GPIO.HIGH)

    def stop(self):
        self.PWMA.ChangeDutyCycle(0)
        self.PWMB.ChangeDutyCycle(0)
        GPIO.output(self.AIN1, GPIO.LOW)
        GPIO.output(self.AIN2, GPIO.LOW)
        GPIO.output(self.BIN1, GPIO.LOW)
        GPIO.output(self.BIN2, GPIO.LOW)

    def backward(self, PWM=30):
        self.PWMA.ChangeDutyCycle(PWM)
        self.PWMB.ChangeDutyCycle(PWM)
        GPIO.output(self.AIN1, GPIO.HIGH)
        GPIO.output(self.AIN2, GPIO.LOW)
        GPIO.output(self.BIN1, GPIO.HIGH)
        GPIO.output(self.BIN2, GPIO.LOW)

    def left(self, PWM=30):
        self.PWMA.ChangeDutyCycle(PWM)
        self.PWMB.ChangeDutyCycle(PWM)
        GPIO.output(self.AIN1, GPIO.HIGH)
        GPIO.output(self.AIN2, GPIO.LOW)
        GPIO.output(self.BIN1, GPIO.LOW)
        GPIO.output(self.BIN2, GPIO.HIGH)

    def right(self, PWM=30):
        self.PWMA.ChangeDutyCycle(PWM)
        self.PWMB.ChangeDutyCycle(PWM)
        GPIO.output(self.AIN1, GPIO.LOW)
        GPIO.output(self.AIN2, GPIO.HIGH)
        GPIO.output(self.BIN1, GPIO.HIGH)
        GPIO.output(self.BIN2, GPIO.LOW)

    def setMotor(self, left, right):
        left = -left
        right = -right
        if((right >= 0) and (right <= 100)):
            GPIO.output(self.AIN1, GPIO.HIGH)
            GPIO.output(self.AIN2, GPIO.LOW)
            self.PWMA.ChangeDutyCycle(right)
        elif((right < 0) and (right >= -100)):
            GPIO.output(self.AIN1, GPIO.LOW)
            GPIO.output(self.AIN2, GPIO.HIGH)
            self.PWMA.ChangeDutyCycle(0 - right)
        if((left >= 0) and (left <= 100)):
            GPIO.output(self.BIN1, GPIO.HIGH)
            GPIO.output(self.BIN2, GPIO.LOW)
            self.PWMB.ChangeDutyCycle(left)
        elif((left < 0) and (left >= -100)):
            GPIO.output(self.BIN1, GPIO.LOW)
            GPIO.output(self.BIN2, GPIO.HIGH)
            self.PWMB.ChangeDutyCycle(0 - left)


class beep():
    def __init__(self):
        self.BUZ = 4
        GPIO.setup(self.BUZ, GPIO.OUT)

    def on(self):
        GPIO.output(self.BUZ, GPIO.HIGH)

    def off(self):
        GPIO.output(self.BUZ, GPIO.LOW)


class joystick ():
    def __init__(self):
        self.CTR = 7
        self.A = 8
        self.B = 9
        self.C = 10
        self.D = 11

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.CTR, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.A, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.B, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.C, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.D, GPIO.IN, GPIO.PUD_UP)

    def check_ctr(self):
        return (GPIO.input(self.CTR) == 0)

    def check_a(self):
        return (GPIO.input(self.A) == 0)

    def check_b(self):
        return (GPIO.input(self.B) == 0)

    def check_c(self):
        return (GPIO.input(self.C) == 0)

    def check_d(self):
        return (GPIO.input(self.D) == 0)


class ir_controll ():
    def __init__(self):
        self.IR = 17

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.IR, GPIO.IN)

    def getkey(self):
        if GPIO.input(self.IR) == 0:
            count = 0
            while GPIO.input(self.IR) == 0 and count < 200:  # 9ms
                count += 1
                time.sleep(0.00006)
            if(count < 10):
                return
            count = 0
            while GPIO.input(self.IR) == 1 and count < 80:  # 4.5ms
                count += 1
                time.sleep(0.00006)

            idx = 0
            cnt = 0
            data = [0, 0, 0, 0]
            for i in range(0, 32):
                count = 0
                while GPIO.input(self.IR) == 0 and count < 15:  # 0.56ms
                    count += 1
                    time.sleep(0.00006)

                count = 0
                while GPIO.input(self.IR) == 1 and count < 40:  # 0: 0.56mx
                    count += 1  # 1: 1.69ms
                    time.sleep(0.00006)

                if count > 7:
                    data[idx] |= 1 << cnt
                if cnt == 7:
                    cnt = 0
                    idx += 1
                else:
                    cnt += 1
                #		print data
            if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:  # check
                return data[2]
            else:
                return "repeat"


class ir_bumper ():
    def __init__(self):
        self.DR = 16
        self.DL = 19

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.DR, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.DL, GPIO.IN, GPIO.PUD_UP)

    def check_r(self):
        return (GPIO.input(self.DR) == 0)

    def check_l(self):
        return (GPIO.input(self.DL) == 0)

# Ultrasonic distance measurement


class us_sensor ():
    def __init__(self):
        self.TRIG = 22
        self.ECHO = 27

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.TRIG, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(self.ECHO, GPIO.IN)

    def dist(self):
        GPIO.output(self.TRIG, GPIO.HIGH)
        time.sleep(0.000015)
        GPIO.output(self.TRIG, GPIO.LOW)
        while not GPIO.input(self.ECHO):
            pass
        t1 = time.time()
        while GPIO.input(self.ECHO):
            pass
        t2 = time.time()
        return (t2-t1)*34000/2


COLOR_ENUM = {
    "Aqua":      [000, 255, 255],
    "Black":     [000, 000, 000],
    "Blue":      [000, 000, 255],
    "Purple":    [255, 000, 255],
    "Green":     [000, 255, 000],
    "Red":       [255, 000, 000],
    "White":     [255, 255, 255],
    "Yellow":    [255, 255, 000]
}


class rgb_led():

    def __init__(self):
        self.LED_COUNT = 4           # Number of LED pixels.
        self.LED_PIN = 18
        self.LED_FREQ_HZ = 800000
        self.LED_DMA = 5
        self.LED_BRIGHTNESS = 255    # Set to 0 for darkest and 255 for brightest
        self.LED_INVERT = False
        self.LED_CHANNEL = 0

        self.strip = Adafruit_NeoPixel(
            self.LED_COUNT, self.LED_PIN, self.LED_FREQ_HZ,
            self.LED_DMA, self.LED_INVERT, self.LED_BRIGHTNESS)
        self.strip.begin()

    def color(self, N, R, G, B):
        if (N < 4 and N > -1):
            self.strip.setPixelColor(N, Color(R, G, B))
            self.strip.show()

    def color(self, N, C):
        if (N < 4 and N > -1):
            self.strip.setPixelColor(N, Color(COLOR_ENUM[C][0],
                                              COLOR_ENUM[C][1], COLOR_ENUM[C][2]))
            self.strip.show()


class line_sensor():
    def __init__(self):
        self.CS = 5
        self.Clock = 25
        self.Address = 24
        self.DataOut = 23
        self.Button = 7

        self.numSensors = 5
        self.calibratedMin = [0] * self.numSensors
        self.calibratedMax = [1023] * self.numSensors
        self.last_value = 0
        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.Clock, GPIO.OUT)
        GPIO.setup(self.Address, GPIO.OUT)
        GPIO.setup(self.CS, GPIO.OUT)
        GPIO.setup(self.DataOut, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(self.Button, GPIO.IN, GPIO.PUD_UP)

    def AnalogRead(self):
        value = [0]*(self.numSensors+1)
        # Read Channel0~channel6 AD value
        for j in range(0, self.numSensors+1):
            GPIO.output(self.CS, GPIO.LOW)
            for i in range(0, 4):

                # sent 4-bit self.Address
                if(((j) >> (3 - i)) & 0x01):
                    GPIO.output(self.Address, GPIO.HIGH)
                else:
                    GPIO.output(self.Address, GPIO.LOW)

                # read MSB 4-bit data
                value[j] <<= 1
                if(GPIO.input(self.DataOut)):
                    value[j] |= 0x01

                GPIO.output(self.Clock, GPIO.HIGH)
                GPIO.output(self.Clock, GPIO.LOW)

            for i in range(0, 6):

                # read LSB 8-bit data
                value[j] <<= 1
                if(GPIO.input(self.DataOut)):
                    value[j] |= 0x01

                GPIO.output(self.Clock, GPIO.HIGH)
                GPIO.output(self.Clock, GPIO.LOW)

            time.sleep(0.0001)
            GPIO.output(self.CS, GPIO.HIGH)
        return value[1:]

class CameraAngle:

    # Registers/etc.
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09
    __ALLLED_ON_L = 0xFA
    __ALLLED_ON_H = 0xFB
    __ALLLED_OFF_L = 0xFC
    __ALLLED_OFF_H = 0xFD

    def __init__(self, address=0x40, debug=False):
        self.bus = smbus.SMBus(1)
        self.address = address
        self.debug = debug
        if (self.debug):
            print("Reseting PCA9685")
        self.write(self.__MODE1, 0x00)
        self.setPWMFreq(50)

    def write(self, reg, value):
        "Writes an 8-bit value to the specified register/address"
        self.bus.write_byte_data(self.address, reg, value)
        if (self.debug):
            print("I2C: Write 0x%02X to register 0x%02X" % (value, reg))

    def read(self, reg):
        "Read an unsigned byte from the I2C device"
        result = self.bus.read_byte_data(self.address, reg)
        if (self.debug):
            print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
                  (self.address, result & 0xFF, reg))
        return result

    def setPWMFreq(self, freq):
        "Sets the PWM frequency"
        prescaleval = 25000000.0    # 25MHz
        prescaleval /= 4096.0       # 12-bit
        prescaleval /= float(freq)
        prescaleval -= 1.0
        if (self.debug):
            print("Setting PWM frequency to %d Hz" % freq)
            print("Estimated pre-scale: %d" % prescaleval)
        prescale = math.floor(prescaleval + 0.5)
        if (self.debug):
            print("Final pre-scale: %d" % prescale)

        oldmode = self.read(self.__MODE1)
        newmode = (oldmode & 0x7F) | 0x10        # sleep
        self.write(self.__MODE1, newmode)        # go to sleep
        self.write(self.__PRESCALE, int(math.floor(prescale)))
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)

    def setPWM(self, channel, on, off):
        "Sets a single PWM channel"
        self.write(self.__LED0_ON_L+4*channel, on & 0xFF)
        self.write(self.__LED0_ON_H+4*channel, on >> 8)
        self.write(self.__LED0_OFF_L+4*channel, off & 0xFF)
        self.write(self.__LED0_OFF_H+4*channel, off >> 8)
        if (self.debug):
            print("channel: %d  LED_ON: %d LED_OFF: %d" % (channel, on, off))

    def setServoPulse(self, channel, pulse):
        "Sets the Servo Pulse,The PWM frequency must be 50HZ"
        # PWM frequency is 50HZ,the period is 20000us
        pulse = int(pulse*4096/20000)
        self.setPWM(channel, 0, pulse)

    def setCameraAngle(self, verticalAngle, horizontalAngle):
        if (verticalAngle > 180):
            verticalAngle = 180
        if (verticalAngle < 0):
            verticalAngle = 0
        if (horizontalAngle > 180):
            horizontalAngle = 180
        if (horizontalAngle < 0):
            horizontalAngle = 0

        va = verticalAngle * 2000 / 180 + 500
        vh = horizontalAngle * 2000 / 180 + 500

        self.setServoPulse(0, va)
        self.setServoPulse(1, vh)


if __name__ == '__main__':

    l = line_sensor()
    while True:
        print(l.AnalogRead())
        time.sleep(0.2)