#!/usr/bin/python2.7
# -*- coding: latin-1 -*-

from KranClass import Kran
from evdev import *
from select import select
from multiprocessing import Process
import time
#  
# 
# gpio:	 minus plus greifer handgelenk ellenbogen armgelenk basis licht
#	
# gpio:	    3    5     7         8          11        10      12     16
# gpio:	   40   38    37        36          35        33      32     31
#
# key r1:	     o l       p ö	   ü ä       + #     . -      ,   
# key r2:            q a       w s         e d       r f     x c      y
#
# rel  1: in1  in2   in4       in3         in5       in7     in6    in8
# rel  2: in1  in2   in4       in3         in5       in7     in6    in8 

robot1 = Kran( 3,  5,  7,  8, 11, 10, 12, 16)
robot2 = Kran(40, 38, 37, 36, 35, 33, 32, 31)

dev = InputDevice('/dev/input/event1')

def robot1func(event1):

# handgelenk on
# taste p 

    if event1.code == 25:
        robot1.move("H", 1)
        return 1

# handgelenk off
# taste ö

    elif event1.code == 39:
        robot1.move("H", 2)
        return 1

# ellenbogen on
# taste ü

    elif event1.code == 26:
        robot1.move("E", 1)
        return 1

# ellenbogen off
# taste ä

    elif event1.code == 40:
        robot1.move("E", 2)
        return 1

# armgelenk on
# taste +

    elif event1.code == 27:
        robot1.move("A", 1)
        return 1

# armgelenk off
# taste #

    elif event1.code == 43:
        robot1.move("A", 2)
        return 1

# basis off 
# taste .

    elif event1.code == 52:
        robot1.move("B", 2)
        return 1

# basis on
# taste -

    elif event1.code == 53:
        robot1.move("B", 1)
        return 1

# greifer off
# taste o

    elif event1.code == 24:
        robot1.move("G", 2)
        return 1

# greifer on
# taste l

    elif event1.code == 38:
        robot1.move("G", 1)
        return 1

# licht on
# taste ,

    elif event1.code == 51:
        robot1.move("L", 1)
        return 1

# taste m

    elif event1.code == 50:
        robot1.cleanup()
        robot2.cleanup()
        return 1


def robot2func(event2):

# handgelenk on
# taste w 

    if event2.code == 17:
        robot2.move("H", 1)
        return 1

# handgelenk off
# taste s

    if event2.code == 31:
	robot2.move("H", 2)
	return 1

# ellenbogen on
# taste e

    elif event2.code == 18:
        robot2.move("E", 1)
        return 1

# ellenbogen off
# taste d
    elif event2.code == 32:
        robot2.move("E", 2)
        return 1

# arm on
# taste r

    elif event2.code == 19:
        robot2.move("A", 1)
        return 1

# arm off
# taste f 

    elif event2.code == 33:
        robot2.move("A", 2)
        return 1

# basis off
# taste x

    elif event2.code == 45:
        robot2.move("B", 2)
        return 1

# basis on
# taste c

    elif event2.code == 46:
        robot2.move("B", 1)
        return 1

# greifer off
# taste q 

    elif event2.code == 16:
        robot2.move("G", 2)
        return 1

# greifer on
# taste a

    elif event2.code == 30:
        robot2.move("G", 1)
        return 1

# licht on
# taste y

    elif event2.code == 44:
        robot2.move("L", 1)
        return 1

# taste v

    elif event2.code == 47:
        robot2.cleanup()
        return 1


def runInParallel(*fns):
    proc = []
    for fn in fns:
        p = Process(target=fn)
        p.start()
        proc.append(p)
    for p in proc:
        p.join()


def manuell():
    flag1 = 0
    flag2 = 0
    for event in dev.read_loop():
        if event.type == ecodes.EV_KEY:
            if event.value == 1:
                if flag1 == 0:
                    flag1 = robot1func(event)
                    if not flag1:
                        flag1 = 0

                if flag2 == 0:
                    flag2 = robot2func(event)
                    if not flag2:
                        flag2 = 0

            if event.value == 0:
                if flag1 == 1:
                    if robot1func(event) == 1:
                        robot1.reset()
                        flag1 = 0

                if flag2 == 1:
                    if robot2func(event) == 1:
                        robot2.reset()
                        flag2 = 0


#def automatic1():
    robot1.move("G", 1)
    time.sleep(1.5)
    robot1.reset()
    robot1.move("E", 1)
    time.sleep(4)
    robot1.reset()
    robot1.move("B", 2)
    time.sleep(3)
    robot1.reset()

    automatic2()
    robot2.move("B", 2)
    time.sleep(4)
    robot2.move("E", 2)
    time.sleep(4)
    robot2.reset()
    robot1.move("B", 1)
    time.sleep(3)
    robot1.reset()
    robot1.move("E", 2)
    time.sleep(3.5)
    robot1.reset()
    robot2.move("E", 1)
    time.sleep(3)
    robot2.reset()
    robot2.move("B", 1)
    time.sleep(4)
    robot2.reset()
    robot2.move("E", 2)
    time.sleep(1)
    robot2.reset()
    robot2.move("G", 2)
    time.sleep(1)
    robot2.reset()


#def automatic2():
    robot2.move("E", 2)
    time.sleep(2)
    robot2.reset()
    robot2.move("G", 1)
    time.sleep(1.5)
    robot2.reset()
    robot1.move("G", 2)
    time.sleep(1)
    robot1.reset()
    robot2.move("E", 1)
    time.sleep(4)
    robot2.reset()


manuell()
#automatic1()