import RPi.GPIO as gpio
import time
from time import sleep
import os
from sense_hat import SenseHat
sense = SenseHat()



def init():
    gpio.setmode(gpio.BOARD)
    gpio.setup(33,gpio.OUT)
    gpio.setup(35,gpio.OUT)
    gpio.setup(37,gpio.OUT)
    gpio.setup(38,gpio.OUT)

def forward(tf):
    init()
    gpio.output(33,False)
    gpio.output(35,True)
    gpio.output(37,False)
    gpio.output(38,True)
    time.sleep(tf)
    gpio.cleanup()

def back(tf):
    init()
    gpio.output(33,True)
    gpio.output(35,False)
    gpio.output(37,True)
    gpio.output(38,False)
    time.sleep(tf)
    gpio.cleanup()

def right(tf):
    init()
    gpio.output(33,False)
    gpio.output(35,True)
    gpio.output(37,True)
    gpio.output(38,False)
    time.sleep(tf)
    gpio.cleanup()

def left(tf):
    init()
    gpio.output(33,True)
    gpio.output(35,False)
    gpio.output(37,False)
    gpio.output(38,True)
    time.sleep(tf)
    gpio.cleanup()

def sensors():
    t = sense.get_temperature()
    p = sense.get_pressure()
    h = sense.get_humidity()
    f = ((t * 9)/5)+32

    t = round(t, 1)
    p = round(p, 1)
    h = round(h, 1)
    f= round(f, 1)


    print "Temperature Celsius = %r" % (t)
    print "Temperature Fahrenheit = %r" % (f)
    print "Pressure = %r Millibars" % (p)
    print "Humidity = %r Percent" % (h)

    orientation = sense.get_orientation_degrees()
    print("p: {pitch}, r: {roll}, y: {yaw}".format(**orientation))

def control():
    valid = False
    light = 0
    while not valid:
        os.system('clear')
        print "Robot Control"
        print ""
        print "-=SENSORS=-"
        sensors()
        print ""
        print "-=MOTOR CONTROLS=-"
        print "Drive Forward Left: Q"
        print "Drive Forward Right: E"
        print "Drive Forward: W"
        print "Drive Back: S"
        print "Turn Left: A"
        print "Turn Right: D"
        print ""
        print"Toggle Light: L"
        print "Exit: Z"
        input = raw_input("Command>")
        if input == "w":
            s = float(raw_input("Seconds?>"))
            forward(s)
        elif input =="s":
            s = float(raw_input("Seconds?>"))
            back(s)
        elif input =="a":
            s = float(raw_input("Seconds?>"))
            left(s)
        elif input =="d":
            s = float(raw_input("Seconds?>"))
            right(s)
        elif input == "q":
            left(0.5)
            forward(0.5)
            left(0.5)
            forward(0.5)
        elif input == "e":
            right(0.5)
            forward(0.5)
            right(0.5)
            forward(0.5)
        elif input == "z":
            quit()
        elif input =="l":
            if light == 0:
                light = 1
                O = [255, 255, 255]
                grid = [
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O,
                O, O, O, O, O, O, O, O
                ]
                sense.set_pixels(grid)
            else:
                light = 0
                sense.clear()

control()