#!/usr/bin/python
# ========================================================
# Python script for PiBot-A: maze follower
# Version 1.0 - by Thomas Schoch - www.retas.de
# ========================================================

from __future__ import print_function
from pololu_drv8835_rpi import motors, MAX_SPEED
from time import sleep
from sys import exit, argv
import RPi.GPIO as GPIO

# Signal handler for SIGTERM
import signal
def sigterm_handler(signal, frame):
    motors.setSpeeds(0, 0)
    exit(0) 
signal.signal(signal.SIGTERM, sigterm_handler)

# GPIO pins of sensors
GPIO.setmode(GPIO.BCM)
GPIO_right  = 21
GPIO_middle = 20
GPIO_left   = 19
GPIO.setup(GPIO_right, GPIO.IN)
GPIO.setup(GPIO_middle, GPIO.IN)
GPIO.setup(GPIO_left, GPIO.IN)

# Three speed constants for different purposes
v3 = MAX_SPEED # = 480
v2 = 380
v1 = 150

# Loop period
delay = 0.001

# Read sensor input
def read_sensors():

    R = GPIO.input(GPIO_right)
    M = GPIO.input(GPIO_middle)
    L = GPIO.input(GPIO_left)
    return (L, M, R)

# Drive some distance, time to sleep is calculated from
# given value (val) and result of calibration (cal)
def drive(val):

    sec = val * cal/500
    sleep (sec)

# Drive a 180 degree turn and back and measure time needed
def calibrate():

    global moving
    global cal
    moving = "calibrating"
    (L, M, R) = read_sensors()

    # 180 degrees left turn
    turn_left = 0
    motors.setSpeeds(-v3, v3)
    while M == 1:
        (L, M, R) = read_sensors()
        turn_left += 1
        sleep (delay)
    while M == 0:
        (L, M, R) = read_sensors()
        turn_left += 1
        sleep (delay)
    motors.setSpeeds(0, 0)
    sleep (0.3)

    # 180 degrees right turn
    turn_right = 0
    motors.setSpeeds(v3, -v3)
    while M == 1:
        (L, M, R) = read_sensors()
        turn_right += 1
        sleep (delay)
    while M == 0:
        (L, M, R) = read_sensors()
        turn_right += 1
        sleep (delay)
    motors.setSpeeds(0, 0)
    sleep (0.3)

    # calulate arithmetic mean
    cal = (turn_left + turn_right) / 2
    print ("cal: ", turn_left, turn_right, " => ", cal)

# Perform 180 degree turn (at dead end)
def turn_180():

    global moving
    moving = "turn 180"

    motors.setSpeeds(-v3, v3)
    (L, M, R) = read_sensors()
    while M == 0:
        (L, M, R) = read_sensors()
        sleep (delay)

# 90 degree turn, left or right
def turn_90(dir):

    global moving
    moving = "turn 90" + dir

    if dir == "left":
        motors.setSpeeds(-v3, v3)
    else:
        motors.setSpeeds(v3, -v3)

    (L, M, R) = read_sensors()
    while M == 1:
        (L, M, R) = read_sensors()
        sleep (delay)
    while M == 0:
        (L, M, R) = read_sensors()
        sleep (delay)

# Drive until the node is under the axle
def axle_to_node():

    motors.setSpeeds(v2, v2)
    drive (0.28)

# --------------------------------------------------------
# MAIN
# --------------------------------------------------------
try:
    # After calibration start driving straight on the line
    calibrate()
    motors.setSpeeds(v3, v3)
    moving = "straight" # current moving
    sensors = "line"    # status under the sensors

    while True: # Main loop

        # Repeat this loop every delay seconds
        sleep (delay)

        # === Read sensors and determine where you are ===
        (L, M, R) = read_sensors()

        if M == 0:
            # Line is lost: dead end
            sensors = "blank"

        elif L == 1 and R == 1:
            # All sensors black: reaching next node
            sensors = "black"

        elif L == 1 and R == 0:
            # Line is a bit left: deviation to the right
            sensors = "line_left"

        elif L == 0 and R == 1:
            # Line is a bit right: deviation to the left
            sensors = "line_right"
        else:
            # Line is in the middle
            sensors = "line"

        # === Some simple decisions ===

        if sensors == "line":
            # We are on the line: go straight ahead
            motors.setSpeeds(v3, v3)
            moving = "straight"

        elif sensors == "line_left":
            # Deviation to the right: correct to the left
            motors.setSpeeds(v1, v3)
            moving = "left"

        elif sensors == "line_right":
            # Deviation to the left: correct to the right
            motors.setSpeeds(v3, v1)
            moving = "right"

        elif sensors == "blank":
            # Dead end: drive axle over the node and turn
            axle_to_node()
            turn_180()
            motors.setSpeeds(v3, v3)
            moving = "straight"

        # === End simple decisions ===
        if sensors != "black":
            continue

        # === At a node: determine type of node ===

        # Read sensor at the node
        (L1, M1, R1) = read_sensors()

        if moving == "left":
            # Reaching node after correction to the left:
            # Rotate to the right a little bit
            motors.setSpeeds(v2, -v2)
            drive(0.1)
            # Again read right sensor (R1) at this position
            (x, x, R1) = read_sensors()
            # Rotate back
            motors.setSpeeds(-v2, v2)
            drive(0.07)

        elif moving == "right":
            # Reaching node after correction to the right:
            # Rotate to the left a little bit
            motors.setSpeeds(-v2, v2)
            drive(0.1)
            # Again read left sensor (L1) at this position
            (L1, x, x) = read_sensors()
            # Rotate back
            motors.setSpeeds(v2, -v2)
            drive(0.07)

        # Status at node
        if L1 == 0 and R1 == 1:
            at_node = "line_to_right"

        elif L1 == 1 and R1 == 0:
            at_node = "line_to_left"

        elif L1 == 1 and R1 == 1:
            at_node = "line_crossing"

        # Drive until node is under the axle
        axle_to_node()

        # Read sensors behind the node
        (L2, M2, R2) = read_sensors()
        if L2 == 0 and M2 == 0 and R2 == 0:
            behind_node = "blank"
        else:
            # Black under any sensor?
            behind_node = "line"

        # Determine type of node
        if at_node == "line_to_right" and behind_node == "blank":
            node = "turn_right"

        elif at_node == "line_to_left" and behind_node == "blank":
            node = "turn_left"

        elif at_node == "line_crossing" and behind_node == "line":
            node = "crossing"

        elif at_node == "line_crossing" and behind_node == "blank":
            node = "T_straight"

        elif at_node == "line_to_right" and behind_node == "line":
            node = "T_right"

        elif at_node == "line_to_left" and behind_node == "line":
            node = "T_left"

        # Calculate movement depending on type of node
        # (FOR NOW: ONLY LEFT-HAND RULE)

        if node == "turn_right":
            turn_90 ("right")

        elif node != "T_right":
            turn_90 ("left")

        motors.setSpeeds(v3, v3)
        moving = "straight"

finally:
    # Stop motors in case of <Ctrl-C> or SIGTERM:
    motors.setSpeeds(0, 0)