"""High Level Structure for the Line Following Assignment in FYS-102-20

   Author: Thomas C. Bressoud
   Date: 6 December 2011
   
"""
from myro import *
from random import *
from HistoryWin import *

FARLEFT  =  2
LEFT     =  1
CENTER   =  0
RIGHT    = -1
FARRIGHT = -2

class GLOBAL:
    '''
    Define a container class holding all the global variables
    of the LineFollow program
    '''
    def __init__(self):
        self.history = [FARRIGHT]
        self.histlen = 10
        self.timePerCycle = 0.5
        self.hw = HistoryWin(400, 700, 30)
        self.acquireSpeed = .3
        self.acquireTurntime = .2
        self.dir = -1
        self.maxRotate = 0
        self.Kp_rotate = 0
        self.maxTranslate = 0
        self.Kp_translate = 0

# When this module is 'run', create the GLOBAL object G with all its
# initializations

G = GLOBAL()

def addSample(direction):
    """
    Insert the distance value given by direction into the history of
    distances maintained in the globals
    """
    G.history.insert(0, direction)
    if len(G.history) > G.histlen:    # Maintain the maximum length of
        G.history.pop(G.histlen - 1)  # the history

def lastSample():
    """
    Return the most recent sample added to the distance history
    """
    return G.history[0]

def lastNonCenter():
    index = 0
    while index < len(G.history) and G.history[index] == CENTER:
        index += 1
    if index == len(G.history):
        coin = int(randomNumber() * 2)
        if coin == 0:
            return LEFT
        else:
            return RIGHT
    return G.history[index]

def Protate(distance):
    """
    Calculate the rotation parameter based on the given distance
    in a proportional feedback control.
    """
    rotate = 0 # Fill this in
    return rotate

def Ptranslate(distance):
    """
    Calculate the translation parameter based on the given distance
    in a proportional feedback control.
    """
    translate = 0 #fill this in
    return max(translate,0)

def acquireLine():
    """
    Based on the assumption that the robot is initially positioned on the
    'lastSample()' side of the line, move the robot until at least one of
    the line sensors indicates that the robot is over a line.
    """    
    if lastSample() == FARRIGHT:
        turnLeft(G.acquireSpeed, G.acquireTurntime)
    else:
        turnRight(G.acquireSpeed, G.acquireTurntime)
    
    left, right = getLine()
    while left == 0 and right == 0:    
	pass  # Fill in the body of this loop
    
    stop()
    
def lineDistance(left, right):
    """Given 0/1 values for left and right, determine the 'distance'/error
    of the robot, given that centered on the line is error 0, and errors to
    the right progress through -1 and -2 and errors to the left progress
    through 1 and 2.
    """
    d = 0 # replace this assignment with the body of this function, caluclating d
    return d
        
def followLineP():
    """
    Heart of the line following program, this function loops until a mouse click in the history
    window, following the line and employing a proportional
    feedback control algorithm.
    """
    while G.hw.checkMouse() == None:

	pass  # Fill this in with the body of the loop
    
    stop()

def control():
    """
    Top level function combining both acquiring the line and then following
    the line.  The parameter duration gives the total time used by the
    algorithm.
    """

    # First, acquire the line
    
    acquireLine()

	# Follow the line.  We are assuming that a stop is included in
	# this function.
	
    followLineP()