#!/usr/bin/env python

import os
import os.path
import yaml
import time
import random
import multiprocessing
import RPi.GPIO as GPIO
from talk import say
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
from adafruit_servokit import ServoKit

Motor1 = {'EN': 27, 'input1': 19, 'input2': 16}
Motor2 = {'EN': 22, 'input1': 26, 'input2': 20}

for x in Motor1:
    GPIO.setup(Motor1[x], GPIO.OUT)
    GPIO.setup(Motor2[x], GPIO.OUT)

EN1 = GPIO.PWM(Motor1['EN'], 100)    
EN2 = GPIO.PWM(Motor2['EN'], 100)    

EN1.start(0)                    
EN2.start(0)  


hand = ServoKit(channels=16)

ROOT_PATH = os.path.realpath(os.path.join(__file__, '..', '..'))

def readYaml():
    with open('{}/src/configuration.yaml'.format(ROOT_PATH),'r+', encoding='utf8') as conf:
        servo = yaml.load(conf, Loader=yaml.FullLoader)
    return servo


def writeYaml(s=None):
    with open('{}/src/configuration.yaml'.format(ROOT_PATH),'w', encoding='utf8') as conf:
        if s==None:
            yaml.dump(servo,conf)
        else:
            yaml.dump(s,conf)


servo = readYaml()

if servo == None:
    with open('{}/src/configurationBackUp.yaml'.format(ROOT_PATH),'r+', encoding='utf8') as conf:
        servoBackUp = yaml.load(conf, Loader=yaml.FullLoader)
    writeYaml(servoBackUp)
    servo = readYaml()
    if servo == None:
        print('close')
        exit()


Initial = servo['Initial_Position']['I2C']
Current = servo['Current_Position']['I2C']

InitialGpio = servo['Initial_Position']['Gpio']
CurrentGpio = servo['Current_Position']['Gpio']
GpioPin     = servo['Pin']['Gpio']


for i in range(0,6):
    GPIO.setup(GpioPin[i], GPIO.OUT)
Servo = []
for i in range(0,6):
    Servo.append(GPIO.PWM(GpioPin[i],50))
    Servo[i].start(0)


def changeDegree(pin,newDegree,time1=0.05,update=5):
    maxChange = 0
    pinSize = len(pin)
    for i in range(0,pinSize):
        maxChange = max(abs(Current[pin[i]]-newDegree[i]),maxChange)
    for deg in range(0,maxChange,update):
        for i in range(0,pinSize):
            if Current[pin[i]]<newDegree[i]:
                Current[pin[i]] += update
            elif Current[pin[i]]>newDegree[i]:
                Current[pin[i]] -= update

        for i in range(0,pinSize):
            hand.servo[pin[i]].angle = Current[pin[i]]
            servo['Current_Position']['I2C'][pin[i]] = Current[pin[i]]
        writeYaml()
        time.sleep(time1)
        

        
def takePosition():
    changeDegree([7,8],[180,0])
    changeDegree([0,1,2,3,4,5,6,7,8,9,10,11],[0,50,130,0,170,170,0,180,0,60,150,0])


def changeDegreeGpio(pin,degree,update,duration):
    pinSize = len(pin)
    for i in range(0,pinSize):
        p = pin[i]
        if CurrentGpio[p]>degree[i]:
            update = -update

        for deg in range(CurrentGpio[p],degree[i],update):
            duty = deg/18
            duty+=2
            Servo[p].ChangeDutyCycle(duty)
            time.sleep(duration)
        CurrentGpio[p]=degree[i]
        writeYaml()


def Run(a, b, c, d, x):
    GPIO.output(Motor1['input1'], GPIO.LOW)
    GPIO.output(Motor1['input2'], GPIO.LOW)
    GPIO.output(Motor2['input1'], GPIO.LOW)
    GPIO.output(Motor2['input2'], GPIO.LOW)

    if a==1:
        GPIO.output(Motor1['input1'], GPIO.HIGH)
    if b==1:
        GPIO.output(Motor1['input2'], GPIO.HIGH)
    if c==1:
        GPIO.output(Motor2['input1'], GPIO.HIGH)
    if d==1:
        GPIO.output(Motor2['input2'], GPIO.HIGH)

    EN2.ChangeDutyCycle(x)
    EN1.ChangeDutyCycle(x)


def Stop():
    Run(0,0,0,0,0)


def Start_Slow(a, b, c, d):
    for i in range(0,100,20):
        Run(a,b,c,d,i)
        time.sleep(0.5)

    
def Stop_Slow(a,b,c,d):
    for i in range(100,0,-20):
        Run(a,b,c,d,i)
        time.sleep(0.5)


def yes(times=3):
    for i in range(0,times):
        changeDegree([0],[30])
        time.sleep(0.08)
        changeDegree([0],[0])
        time.sleep(0.08)

def no(times=3):
    for i in range(0,times):
        changeDegree([15],[70],5,0.05)
        time.sleep(0.2)
        changeDegree([15],[110],5,0.05)
        time.sleep(0.2)
    changeDegree([15],[90],5,0.05)

def move_head(times=3):
    for i in range(0,times):
        changeDegree([0],[20])
        changeDegreeGpio([0],[80],5,0.05)
        changeDegree([0],[0])
        changeDegreeGpio([0],[100],5,0.05)

    changeDegreeGpio([0],[90],10,0.01)

def random0():
    r = random.randrange(1,10000000)%3
    if(r==1):
        changeDegree([0],[20])
        changeDegree([0],[0])      
    elif(r==2):
        changeDegreeGpio([0],[120],5,0.05)
        changeDegreeGpio([0],[90],5,0.05)
    else:
        changeDegreeGpio([0],[60],5,0.05)
        changeDegreeGpio([0],[90],5,0.05)
          
def random1():
    r = random.randrange(1,3)
    if(r==1):
        changeDegree([0],[20])
        changeDegree([0],[0])
        changeDegree([3],[50])
        changeDegree([9],[100])
        changeDegree([9],[60])
        changeDegree([3],[0])   
    elif(r==2):
        changeDegree([0],[20])
        changeDegree([0],[0])
        changeDegree([4],[120])
        changeDegree([10],[140])
        changeDegree([10],[180])
        changeDegree([4],[170])
    else:
        changeDegree([3,4],[50,120])
        changeDegree([9,10],[100,140])
        changeDegree([9,10],[60,180])
        changeDegree([3,4],[0,180])

def random2():
    changeDegree([3,4],[20,150])
    pin = [7,8,9,10]
    deg = [[160,0,60,100],[180,20,100,140]]
    ok = [0,0,0,0]
    select = [1,2,0,3,1,0,3,2,1,0,2,3,1,2,3,0,3,1,2,3,1,2,3,0,3,1]
    for i in range(0,15):
        r = select[i%len(select)]%4
        print (' move ',r)
        changeDegree([pin[r]],[deg[ok[r]][r]])
        ok[r]^=1
    takePosition()

def random3():
    changeDegree([3,4],[20,150])
    pin = [7,8,9,10]
    deg = [[160,0,60,100],[180,20,100,140]]
    ok = [0,0,0,0]
    for i in range(0,15):
        r = random.randrange(1,1000000)%4
        print (' move ',r)
        changeDegree([pin[r]],[deg[ok[r]][r]])
    takePosition()


def randomCall(t):
    changeDegree([3,4,5,6,7,8,9,10],[50,110,80,70,100,80,160,20])
    pin = [5,6,7,8]
    deg = [[80,50,100,70],[110,90,110,90]]
    select = [89,93,472,347,2, 34, 134, 1937, 1983, 1739, 107, 894, 48, 28, 2048,589,689,123, 34,27,4,91,102,893,10283,53,1283,9485,1973,873,1973,0,10973]
    ok = [0,0,0,0]
    ln = len(select)
    for i in range(0,t*3):
        r = select[i%16]%4
        changeDegree([pin[r]],[deg[ok[r]][r]])
        ok[r]^=1
    takePosition()


def expression(t):
    print (' i got value of t is : ',t)
    if(t==0):
        random0()
    elif(t==1):
        random1()
    elif(t==2):
        random2()
    elif(t==3):
        random3()
    else:
        randomCall(t)

def speakOnline(t):
    expression(t)

def speakOffline(speech):
    t = int(len(speech)/15)
    print ('Offline t value is : ',t)
    p1 = multiprocessing.Process(target=expression,args=[t])
    p1.start()
    say(speech)



[((11, 0, 11, 23), 'RPi.GPIO.setwarnings', 'GPIO.setwarnings', ({(11, 17, 11, 22): '(False)'}, {}), '(False)', True, 'import RPi.GPIO as GPIO\n'), ((12, 0, 12, 22), 'RPi.GPIO.setmode', 'GPIO.setmode', ({(12, 13, 12, 21): 'GPIO.BCM'}, {}), '(GPIO.BCM)', True, 'import RPi.GPIO as GPIO\n'), ((22, 6, 22, 33), 'RPi.GPIO.PWM', 'GPIO.PWM', ({(22, 15, 22, 27): "Motor1['EN']", (22, 29, 22, 32): '100'}, {}), "(Motor1['EN'], 100)", True, 'import RPi.GPIO as GPIO\n'), ((23, 6, 23, 33), 'RPi.GPIO.PWM', 'GPIO.PWM', ({(23, 15, 23, 27): "Motor2['EN']", (23, 29, 23, 32): '100'}, {}), "(Motor2['EN'], 100)", True, 'import RPi.GPIO as GPIO\n'), ((29, 7, 29, 28), 'adafruit_servokit.ServoKit', 'ServoKit', (), '', False, 'from adafruit_servokit import ServoKit\n'), ((19, 4, 19, 35), 'RPi.GPIO.setup', 'GPIO.setup', ({(19, 15, 19, 24): 'Motor1[x]', (19, 26, 19, 34): 'GPIO.OUT'}, {}), '(Motor1[x], GPIO.OUT)', True, 'import RPi.GPIO as GPIO\n'), ((20, 4, 20, 35), 'RPi.GPIO.setup', 'GPIO.setup', ({(20, 15, 20, 24): 'Motor2[x]', (20, 26, 20, 34): 'GPIO.OUT'}, {}), '(Motor2[x], GPIO.OUT)', True, 'import RPi.GPIO as GPIO\n'), ((31, 29, 31, 63), 'os.path.join', 'os.path.join', ({(31, 42, 31, 50): '__file__', (31, 52, 31, 56): '""".."""', (31, 58, 31, 62): '""".."""'}, {}), "(__file__, '..', '..')", False, 'import os\n'), ((68, 4, 68, 36), 'RPi.GPIO.setup', 'GPIO.setup', ({(68, 15, 68, 25): 'GpioPin[i]', (68, 27, 68, 35): 'GPIO.OUT'}, {}), '(GpioPin[i], GPIO.OUT)', True, 'import RPi.GPIO as GPIO\n'), ((117, 4, 117, 43), 'RPi.GPIO.output', 'GPIO.output', ({(117, 16, 117, 32): "Motor1['input1']", (117, 34, 117, 42): 'GPIO.LOW'}, {}), "(Motor1['input1'], GPIO.LOW)", True, 'import RPi.GPIO as GPIO\n'), ((118, 4, 118, 43), 'RPi.GPIO.output', 'GPIO.output', ({(118, 16, 118, 32): "Motor1['input2']", (118, 34, 118, 42): 'GPIO.LOW'}, {}), "(Motor1['input2'], GPIO.LOW)", True, 'import RPi.GPIO as GPIO\n'), ((119, 4, 119, 43), 'RPi.GPIO.output', 'GPIO.output', ({(119, 16, 119, 32): "Motor2['input1']", (119, 34, 119, 42): 'GPIO.LOW'}, {}), "(Motor2['input1'], GPIO.LOW)", True, 'import RPi.GPIO as GPIO\n'), ((120, 4, 120, 43), 'RPi.GPIO.output', 'GPIO.output', ({(120, 16, 120, 32): "Motor2['input2']", (120, 34, 120, 42): 'GPIO.LOW'}, {}), "(Motor2['input2'], GPIO.LOW)", True, 'import RPi.GPIO as GPIO\n'), ((188, 8, 188, 29), 'random.randrange', 'random.randrange', ({(188, 25, 188, 26): '1', (188, 27, 188, 28): '3'}, {}), '(1, 3)', False, 'import random\n'), ((267, 9, 267, 60), 'multiprocessing.Process', 'multiprocessing.Process', (), '', False, 'import multiprocessing\n'), ((269, 4, 269, 15), 'talk.say', 'say', ({(269, 8, 269, 14): 'speech'}, {}), '(speech)', False, 'from talk import say\n'), ((35, 16, 35, 55), 'yaml.load', 'yaml.load', (), '', False, 'import yaml\n'), ((51, 22, 51, 61), 'yaml.load', 'yaml.load', (), '', False, 'import yaml\n'), ((71, 17, 71, 40), 'RPi.GPIO.PWM', 'GPIO.PWM', ({(71, 26, 71, 36): 'GpioPin[i]', (71, 37, 71, 39): '(50)'}, {}), '(GpioPin[i], 50)', True, 'import RPi.GPIO as GPIO\n'), ((91, 8, 91, 25), 'time.sleep', 'time.sleep', ({(91, 19, 91, 24): 'time1'}, {}), '(time1)', False, 'import time\n'), ((123, 8, 123, 48), 'RPi.GPIO.output', 'GPIO.output', ({(123, 20, 123, 36): "Motor1['input1']", (123, 38, 123, 47): 'GPIO.HIGH'}, {}), "(Motor1['input1'], GPIO.HIGH)", True, 'import RPi.GPIO as GPIO\n'), ((125, 8, 125, 48), 'RPi.GPIO.output', 'GPIO.output', ({(125, 20, 125, 36): "Motor1['input2']", (125, 38, 125, 47): 'GPIO.HIGH'}, {}), "(Motor1['input2'], GPIO.HIGH)", True, 'import RPi.GPIO as GPIO\n'), ((127, 8, 127, 48), 'RPi.GPIO.output', 'GPIO.output', ({(127, 20, 127, 36): "Motor2['input1']", (127, 38, 127, 47): 'GPIO.HIGH'}, {}), "(Motor2['input1'], GPIO.HIGH)", True, 'import RPi.GPIO as GPIO\n'), ((129, 8, 129, 48), 'RPi.GPIO.output', 'GPIO.output', ({(129, 20, 129, 36): "Motor2['input2']", (129, 38, 129, 47): 'GPIO.HIGH'}, {}), "(Motor2['input2'], GPIO.HIGH)", True, 'import RPi.GPIO as GPIO\n'), ((142, 8, 142, 23), 'time.sleep', 'time.sleep', ({(142, 19, 142, 22): '(0.5)'}, {}), '(0.5)', False, 'import time\n'), ((148, 8, 148, 23), 'time.sleep', 'time.sleep', ({(148, 19, 148, 22): '(0.5)'}, {}), '(0.5)', False, 'import time\n'), ((154, 8, 154, 24), 'time.sleep', 'time.sleep', ({(154, 19, 154, 23): '(0.08)'}, {}), '(0.08)', False, 'import time\n'), ((156, 8, 156, 24), 'time.sleep', 'time.sleep', ({(156, 19, 156, 23): '(0.08)'}, {}), '(0.08)', False, 'import time\n'), ((161, 8, 161, 23), 'time.sleep', 'time.sleep', ({(161, 19, 161, 22): '(0.2)'}, {}), '(0.2)', False, 'import time\n'), ((163, 8, 163, 23), 'time.sleep', 'time.sleep', ({(163, 19, 163, 22): '(0.2)'}, {}), '(0.2)', False, 'import time\n'), ((176, 8, 176, 36), 'random.randrange', 'random.randrange', ({(176, 25, 176, 26): '(1)', (176, 27, 176, 35): '(10000000)'}, {}), '(1, 10000000)', False, 'import random\n'), ((42, 12, 42, 33), 'yaml.dump', 'yaml.dump', ({(42, 22, 42, 27): 'servo', (42, 28, 42, 32): 'conf'}, {}), '(servo, conf)', False, 'import yaml\n'), ((44, 12, 44, 29), 'yaml.dump', 'yaml.dump', ({(44, 22, 44, 23): 's', (44, 24, 44, 28): 'conf'}, {}), '(s, conf)', False, 'import yaml\n'), ((111, 12, 111, 32), 'time.sleep', 'time.sleep', ({(111, 23, 111, 31): 'duration'}, {}), '(duration)', False, 'import time\n'), ((228, 12, 228, 39), 'random.randrange', 'random.randrange', ({(228, 29, 228, 30): '(1)', (228, 31, 228, 38): '(1000000)'}, {}), '(1, 1000000)', False, 'import random\n')]