#!/usr/bin/python
# ========================================================
# Python script for PiBot-A: line 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
import wiringpi2 as wp2
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.05

# Maximum loop cycles when track is lost
# before emergency stop
max_ingap = 10

# Read sensor input and print some diagnostics
def read_sensors():
    R = GPIO.input(GPIO_right)
    M = GPIO.input(GPIO_middle)
    L = GPIO.input(GPIO_left)
    LC = "#" if L else " "
    MC = "#" if M else " "
    RC = "#" if R else " "
    print ("%-6s %2d/%d   |%c%c%c|" % 
        (moving, ingap, black_cntr, LC, MC, RC))
    return (L, M, R)

# 180 degrees turn on black disc
def turn_180():
    # speed-up and slow-down for smooth movement
    sleep (0.2)
    motors.setSpeeds(-v1, v1)
    sleep (0.1)
    motors.setSpeeds(-v3, v3)
    sleep (0.5)
    motors.setSpeeds(-v1, v1)
    # After ca. 165 degrees: snap the track
    (L, M, R) = read_sensors()
    while M == 0:
        (L, M, R) = read_sensors()
        sleep (delay)

# MAIN
try:
    # Start moving forward
    motors.setSpeeds(v2, v2)

    # Assumption: starting out of track
    moving = "search" # current movement (fwd, left ...)
    ingap = 1         # in gap (else: on track)
    black_cntr = 0    # counter for all sensors black
    in_course = 0     # in course (else: still searching)

    while True: # Main loop

        # Repeat this loop every delay seconds
        sleep (delay)
        (L, M, R) = read_sensors()

        # Found track first time? Then we are in_course
        if M == 1:
            in_course = 1
        elif in_course == 0:
            continue

        # In gap? Increment counter and stop if necessary
        if L == 0 and M == 0 and R == 0:
            if moving == "gap":
                ingap += 1
                if ingap > max_ingap:
                    print ("STOP WHITE")
                    exit (0)
        else: # reset counter
            ingap = 0

        # All sensors black? Increment counter
        # and turn on black disc.
        if L == 1 and M == 1 and R == 1:
            motors.setSpeeds(v2, v2)
            black_cntr += 1
            if black_cntr > 2:
                turn_180()
            continue
        else:
            black_cntr = 0 # reset counter

        if M == 0:
            # Departure from left curve: narrow radius
            if moving[0:4] == "left":
                motors.setSpeeds(-v1, v2)
                moving = "left)"

            # Departure from right curve: narrow radius
            elif moving[0:5] == "right":
                motors.setSpeeds(v2, -v1)
                moving = "right)"

            # Got into gap
            else:
                motors.setSpeeds(v2, v2)
                moving = "gap"

        # Swang to the right: turn left
        elif L == 1:
            motors.setSpeeds(v1, v2)
            moving = "left"

        # Swang to the left: turn right
        elif R == 1:
            motors.setSpeeds(v2, v1)
            moving = "right"

        # Else: go forward
        else:
            motors.setSpeeds(v2, v2)
            moving = "fwd"

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