#! /usr/bin/python

import RPi.GPIO as GPIO
from time import sleep
import threading

def readadc(adcnum, clockpin, mosipin, misopin, cspin):
    if adcnum > 7 or adcnum < 0:
        return -1
    GPIO.output(cspin, GPIO.HIGH)
    GPIO.output(clockpin, GPIO.LOW)
    GPIO.output(cspin, GPIO.LOW)

    commandout = adcnum
    commandout |= 0x18
    commandout <<= 3
    for i in range(5):
        if commandout & 0x80:
            GPIO.output(mosipin, GPIO.HIGH)
        else:
            GPIO.output(mosipin, GPIO.LOW)
        commandout <<= 1
        GPIO.output(clockpin, GPIO.HIGH)
        GPIO.output(clockpin, GPIO.LOW)
    adcout = 0
    for i in range(13):
        GPIO.output(clockpin, GPIO.HIGH)
        GPIO.output(clockpin, GPIO.LOW)
        adcout <<= 1
        if i>0 and GPIO.input(misopin)==GPIO.HIGH:
            adcout |= 0x1
    GPIO.output(cspin, GPIO.HIGH)
    return adcout

GPIO.setmode(GPIO.BCM)

SPICLK = 11
SPIMOSI = 10
SPIMISO = 9
SPICS = 8
GPIO.setup(SPICLK, GPIO.OUT)
GPIO.setup(SPIMOSI, GPIO.OUT)
GPIO.setup(SPIMISO, GPIO.IN)
GPIO.setup(SPICS, GPIO.OUT)

GPIO.setup(13,GPIO.OUT)  #F1 v1
GPIO.setup(6,GPIO.OUT)  #F1 v2
GPIO.setup(22,GPIO.OUT) #F2 v3
GPIO.setup(5,GPIO.OUT)  #F2 v4

GPIO.setup(12,GPIO.OUT)  #P1
GPIO.setup(20,GPIO.OUT)  #P4

GPIO.setup(15,GPIO.IN)  #ss1
GPIO.setup(18,GPIO.IN)  #ss2
GPIO.setup(23,GPIO.IN)  #ss4
GPIO.setup(24,GPIO.IN)  #ss5
GPIO.setup(19,GPIO.IN)  #ss3
GPIO.setup(26,GPIO.IN)  #ss6

GPIO.setup(7,GPIO.IN)   #Swich 2
GPIO.setup(25,GPIO.IN)  #Swich 1

v1 = GPIO.PWM(13, 1000)
v2 = GPIO.PWM(6, 1000)
v3 = GPIO.PWM(22, 1000)
v4 = GPIO.PWM(5, 1000)

GPIO.output(12,GPIO.HIGH)
GPIO.output(20,GPIO.HIGH)
sleep(0.02)
GPIO.output(12,GPIO.LOW)
GPIO.output(20,GPIO.LOW)

v1.start(0)
v2.start(0)
v3.start(0)
v4.start(0)

def direction1(densha):
    GPIO.output(12,GPIO.LOW)
    GPIO.output(20,GPIO.LOW)
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(4, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v2.ChangeDutyCycle(0)
        v1.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(23) == GPIO.LOW:
        alarm()
    while readadc(6, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v1.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v1.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(23) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(23) == GPIO.HIGH:
            sleep(0.2)
    return

def direction2(densha):
    GPIO.output(12,GPIO.LOW)
    GPIO.output(20,GPIO.LOW)
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(6, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v1.ChangeDutyCycle(0)
        v2.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(18) == GPIO.LOW:
        alarm()
    while readadc(4, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v2.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v2.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(18) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(18) == GPIO.HIGH:
            sleep(0.2)
    return

def direction3(densha):
    GPIO.output(12,GPIO.HIGH)
    GPIO.output(20,GPIO.HIGH)
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(4, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v2.ChangeDutyCycle(0)
        v1.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(24) == GPIO.LOW:
        alarm()
    while readadc(6, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v1.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v1.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(24) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(24) == GPIO.HIGH:
            sleep(0.2)
    return

def direction4(densha):
    GPIO.output(12,GPIO.HIGH)
    GPIO.output(20,GPIO.HIGH)
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(6, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v1.ChangeDutyCycle(0)
        v2.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(15) == GPIO.LOW:
        alarm()
    while readadc(4, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v2.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v2.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(15) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(15) == GPIO.HIGH:
            sleep(0.2)
    return

def direction5(densha):
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(7, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v4.ChangeDutyCycle(0)
        v3.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(26) == GPIO.LOW:
        alarm()
    while readadc(5, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v3.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v3.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(26) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(26) == GPIO.HIGH:
            sleep(0.2)
    return

def direction6(densha):
    if readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000:
        sleep(0.2)
        return
    while readadc(5, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v3.ChangeDutyCycle(0)
        v4.ChangeDutyCycle(duty*2/3)
        sleep(0.2)
    sleep(0.2)
    if  GPIO.input(19) == GPIO.LOW:
        alarm()
    while readadc(7, SPICLK, SPIMOSI, SPIMISO, SPICS) < 1000 :
        duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
        v4.ChangeDutyCycle(duty)
        sleep(0.2)
    duty = (readadc(densha, SPICLK, SPIMOSI, SPIMISO, SPICS)-2200)/20
    v4.ChangeDutyCycle(duty*2/3)
    sleep(0.2)
    while GPIO.input(19) == GPIO.HIGH:
        sleep(0.2)
        while GPIO.input(19) == GPIO.HIGH:
            sleep(0.2)
    return

def alarm() :
    v1.ChangeDutyCycle(0)
    v2.ChangeDutyCycle(0)
    while GPIO.input(25) == GPIO.LOW:
        sleep(0.2)
    return

if __name__== '__main__':
    while True:
        d1 = threading.Thread(target=direction5,args=(2,))
        d1.start()
        direction1(0)
        sleep(2)
        d2 = threading.Thread(target=direction6,args=(2,))
        d2.start()
        direction2(0)
        sleep(2)

# except KeyboardInterrupt:
#    pass
v1.stop()
v2.stop()
GPIO.cleanup()