import grove_gesture_sensor
from grove_rgb_lcd import *
import time
import Adafruit_PCA9685
import grove_i2c_motor_driver

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)

m= grove_i2c_motor_driver.motor_driver()

def move(degree_1,degree_2):
    degree_1 = int(degree_1 * 5.27)
    degree_2 = int(degree_2 * 5.27)
    pwm.set_pwm(0, 0, degree_1)
    pwm.set_pwm(1, 0, degree_2)

def main():
    g=grove_gesture_sensor.gesture()
    g.init()
    setText("Newtral\n-")
    setRGB(255,255,255) #White
    while True:
        gest=g.return_gesture()
        #Match the gesture
        if gest==g.FORWARD:
            print("FORWARD")
            setText("FORWARD")
            setRGB(255,0,255) #Magenta
            move(110,40)
            m.MotorSpeedSetAB(100,100)
            m.MotorDirectionSet(0b0101)
            time.sleep(0.5)
            move(70,40)
            m.MotorSpeedSetAB(0,0)
            time.sleep(0.5)
            move(90,40)
            time.sleep(0.5)
        elif gest==g.CLOCKWISE:
            print("CLOCKWISE")
            setText("CLOCKWISE")
            setRGB(255,255,255) #White
            move(110,20)
            m.MotorSpeedSetAB(100,100)
            m.MotorDirectionSet(0b1001)
            time.sleep(0.5)
            move(90,40)
            m.MotorSpeedSetAB(100,100)
            m.MotorDirectionSet(0b0110)
            move(70,60)
            time.sleep(0.5)
            m.MotorSpeedSetAB(0,0)
            move(90,40)
            time.sleep(0.5)
        elif gest==g.WAVE:
            print("WAVE")
            setText("WAVE")
            setRGB(255,255,0) #Yellow
            move(90,20)
            m.MotorSpeedSetAB(100,100)
            m.MotorDirectionSet(0b1010)
            time.sleep(0.5)
            move(90,60)
            m.MotorSpeedSetAB(0,0)
            time.sleep(0.5)
            move(90,40)
            time.sleep(0.5)
        elif gest==0:
            print("-")
        else:
            print("Error")
            time.sleep(.1)

if __name__ == '__main__':
    main()