'''
Program: readOptical.py
Author: Joe Pitz
Description:

parses the valid values of the readOprical()
prints out logic and binary results 
that determine if robot turns right, left
or goes forward

'''
def main():
    # declare and initialize array
    sensor_in = 0
    sensorValue1 = 0
    sensorValue2 = 0
    action1 = 0
    action2 = 0
    
    optValues = [0xff, 0x100, 0x00, 0x100, 0xff, 0x100]
    
    for elem in optValues:
        sensor_in = elem
        print "\n"
        print "sensor _in = " + str(hex(sensor_in)) 
        print "action1 = " + str(hex(action1)) 
        print "action2 = " + str(hex(action2))
        print "sensorValue1 = " + str(hex(sensorValue1)) 
        print "sensorValue2 = " + str(hex(sensorValue2)) 
        print "\n"
        print str(hex(sensor_in)) + " & " + str(hex(0xf00))
        print str(hex(sensor_in)) + " = " + format(sensor_in, '#018b') 
        print str(hex(0xf00)) + " = "   + format(0xf00, '#018b')
        print "if " + hex(sensor_in) + " & " + hex(0xf00) + " == 0 "
        
        if (sensor_in & 0xf00 == 0):
            print "inside if sensor_in & 0xf00 == 0"
            print str(hex(sensor_in)) + " & " + str(hex(0xff))
            print "sensorValue1 = " +  format(sensor_in, '#018b')
            print "             & " + format(0xff, '#018b') 
            print "               ------------------"
            print "               " + format(sensor_in & 0xff, '#018b')
            sensorValue1 = sensor_in & 0xff
            print "sensorValue1 = " + str(hex(sensorValue1))
            print "\n"
        elif ((sensor_in & 0xf00) >> 8 == 1): 
            print "inside elif\n"
            print str(hex(sensor_in)) + " & " + str(hex(0xff)) 
            print "sensorValue2 = " + format(sensor_in, '#018b') 
            print "             & " + format(0xff, '#018b') 
            print "               ------------------"
            print "               " + format(sensor_in & 0xff, '#018b')
            sensorValue2  = sensor_in & 0xff
            print "SensorValue2 = " + str(hex(sensorValue2))
            print "\n"
        if (sensorValue1 == 0x00):
            print "inside sensorValue1 == 0x00" 
            print str(hex(action1)) + " & 0xfe"
            print "action1 = " + format(action1, '#018b')
            print "        & " + format(0xfe, '#018b') 
            print "          ------------------"
            print "          " + format(action1 & 0xfe, '#018b') 
            action1 = action1 & 0xfe
            print "action1 = " + str(hex(action1))
            print "\n"
        if (sensorValue1 == 0xff):
            print "inside of sensorValue1 == 0xff"
            print str(hex(action1))  + " | 0x01"
            print "action1 = " + format(action1, '#018b') 
            print "        | " + format(0x01, '#018b') 
            print "          ------------------"
            print "          " + format(action1 | 0x01, '#018b') 
            action1 = action1 | 0x01
            print "action1 = " + str(hex(action1))
            print "\n" 
        if (sensorValue2 == 0x00):
            print "inside of sensorValue2 == 0x00"
            print str(hex(action1)) + " | 0x02"
            print "action1 = " + format(action1, '#018b') 
            print "        | " + format(0x02, '#018b') 
            print "          ------------------"
            print "          " + format(action1 | 0x02, '#018b')
            action1 = action1 | 0x02
            print "action1 = " + str(hex(action1))
            print "\n"
        if (sensorValue2 == 0xff):
            print "inside of sensorValue2 == 0xff" 
            print str(hex(action1)) + " & 0xfd"
            print "action1 = " + format(action1, '#018b')     
            print "        & " + format(0xfd, '#018b')
            print "          ------------------"
            print "          " + format(action1 & 0xfd, '#018b')
            action1 = action1 & 0xfd
            print "action1 = " + str(hex(action1))
            print "\n"
        
        if (action1 != action2):
            if (action1 == 3 ):
                print "go forward"
            if (action1 == 1):
                print "turn left"  
            if (action1 == 2):
                print "turn right" 
            if (action1 == 0):
                print "go forward" 

        action2 = action1
        
if __name__ == "__main__":
    main()