from gopigo import *
import time
import pigpio

#This program scans a 180 degree map of its surroundings then turns towards its most distant measurement

#Methods to control the robot better
#!/usr/bin/python
'''
This module contains convenience functions to simplify
the coding of simple tasks.
This really needs to be moved to a GoPiGo package
e.g. from gopigo.control import *
'''

en_debug=1

## 360 rotation is ~64 encoder pulses or 5 deg/pulse
## DPR is the Deg:Pulse Ratio or the # of degrees per
##  encoder pulse.
DPR = 360.0/64
WHEEL_RAD = 3.25 # Wheels are ~6.5 cm diameter.
CHASS_WID = 11 # Chassis is ~13.5 cm wide.

def left_deg(deg=None):
    '''
    Turn chassis left by a specified number of degrees.
    DPR is the #deg/pulse (Deg:Pulse ratio)
    This function sets the encoder to the correct number
     of pulses and then invokes left().
    '''
    if deg is not None:
        pulse= int(deg/DPR)
        enc_tgt(0,1,pulse)
    left()

def right_deg(deg=None):
    '''
    Turn chassis right by a specified number of degrees.
    DPR is the #deg/pulse (Deg:Pulse ratio)
    This function sets the encoder to the correct number
     of pulses and then invokes right().
    '''
    if deg is not None:
        pulse= int(deg/DPR)
        enc_tgt(0,1,pulse)
    right()

def fwd_cm(dist=None):
    '''
    Move chassis fwd by a specified number of cm.
    This function sets the encoder to the correct number
     of pulses and then invokes fwd().
    '''
    if dist is not None:
        pulse = int(cm2pulse(dist))
        enc_tgt(1,1,pulse)
    fwd()


def bwd_cm(dist=None):
    '''
    Move chassis bwd by a specified number of cm.
    This function sets the encoder to the correct number
     of pulses and then invokes bwd().
    '''
    if dist is not None:
        pulse = int(cm2pulse(dist))
        enc_tgt(1,1,pulse)
    bwd()

def cm2pulse(dist):
    '''
    Calculate the number of pulses to move the chassis dist cm.
    pulses = dist * [pulses/revolution]/[dist/revolution]
    '''
    wheel_circ = 2*math.pi*WHEEL_RAD # [cm/rev] cm traveled per revolution of wheel
    revs = dist/wheel_circ
    PPR = 18 # [p/rev] encoder Pulses Per wheel Revolution
    pulses = PPR*revs # [p] encoder pulses required to move dist cm.
    if en_debug:
        print 'WHEEL_RAD',WHEEL_RAD
        print 'revs',revs
        print 'pulses',pulses
    return pulses
#def int sleep = .5
#def int servo_angle = 45
def scanArea():
    distance = [0, 0, 0, 0, 0]
    for i in range(0, 5):
        servo(i*36) #Moves the servo at the angles 0, 10 up to 180
        time.sleep(.2) #Gives time for the sensor to be in position before start measuring
        distance[i] = us_dist(15)
        time.sleep(.2) #So that the servo doesn't spas out
    return distance

def determineAngle(distance):
    m = max(distance) #finds max measured distance in the array
    angles = [i for i, j in enumerate(distance) if j == m]
    #might return two angles if the values are the same
    #returns indexes of largest values
    #indexes are used to determine the angles
    #enumerate creates a list [(index, value) etc]
    #the for loop loops through the list
    #the first i is the index of what it puts in the new list
    #the second i and the j get increased until the if statement is met
    #once it is met the index second i gets put in the first i
    #I don't fully understand it but this is a really smart way to do it
    return max(angles) #always picks the lowest value because it corresponds to the rightmost value


set_speed(50)

while True:
    fwd()
    distance = scanArea()
    print(distance)
    angle = determineAngle(distance) #Angles: 0 to 8 is right, 9 is front, 10 to 18 is left
    print(angle)
    #Right 0 1 2 3 4 5 6 7 8 9* 10 11 12 13 14 15 16 17 18 Left

    #this value times 10 repsents the angle to go to

    rotation = 90-(angle*36) #Right will be a positive value, left will be negative

    if angle < 2:
        #right_deg(rotation) #Rotate right by that angle
        right()
        time.sleep(1)
        fwd() #Go forward 10 cm
    elif angle > 2:
        #left_deg(-rotation) #Rotate left by that
        left()
        time.sleep(1)
        fwd()
    elif angle == 2:
        time.sleep(1)
        fwd()

    #time.sleep(3) #Depends on the time it takes it to stop