#########################################################################
##
##  RC Tank_no PWM.py
##
##  Author:     Jeremy Greenwood
##
##  Description:
##
##              Python script for Series 60 Symbian phone, designed for
##          Nokia E75 (3rd Edition, Feature Pack 2) using PyS60 2.0.0
##
#########################################################################

from sensor import *
import btsocket
import e32
import time
import audio
import appuifw
import sys

#Define the exit function
def shutdown():
    global terminate
    terminate = True

class read_sensor():

    def __init__(self):
        self.accelerometer = \
            AccelerometerXYZAxisData(data_filter=LowPassFilter())
        self.accelerometer.set_callback(data_callback=self.my_callback)

    def my_callback(self):
        global x,y,z
        # For stream sensor data the callback is hit 35 times per sec(On 5800).
        # The device cannot handle resource hungry operations like print in the
        # callback function for such high frequencies. A workaround is to
        # sample the data as demonstrated below.
        x = self.accelerometer.x
        y = self.accelerometer.y
        z = self.accelerometer.z
        self.counter = self.counter + 1

    def run(self):
        self.accelerometer.start_listening()
        self.counter = 0

    def acquire_reading(self):
        d.run()
        e32.ao_sleep(0.02)                                      # number of seconds d will run

if __name__ == '__main__':

    terminate = False
    appuifw.app.exit_key_handler = shutdown
    appuifw.app.title=u"Accelerometer RC"
    appuifw.app.screen='normal'    
    
    target_address = '00:06:66:04:0f:7d'
    port = 1
    s = btsocket.socket( btsocket.AF_BT, btsocket.SOCK_STREAM )
    connect_failed = False
    
    try:
        s.connect( (target_address, port) )
    except:
        audio.say(u"turn on bluetooth")
        connect_failed = True

    if not connect_failed:
        audio.say(u"start your engines")
        state = "stop"
        d = read_sensor()
        d.run()                                                 # initializes accelerometer readings which are inaccurate the first few cycles
        e32.ao_sleep(0.5)
        while not terminate:
            d.acquire_reading()

            previous_state = state
            if x < 48:
                if abs(y) > abs(z):
                    if y > 0:
                        state = "right"
                    else:
                        state = "left"
                else:
                    if z > 0:
                        state = "forward"
                    else:
                        state = "reverse"            
            else:
                state = "stop"
            if previous_state == state:
                action = "none"
            else:
                action = state
            
            if action != "none":
##                print "\nX:%s, Y:%s, Z:%s" % (x,y,z)          #this shows accelerometer data
                print "action: ",action

                if action == "forward":
                    data = '8'
                if action == "left":
                    data = '4'
                if action == "right":
                    data = '6'
                if action == "reverse":
                    data = '2'
                if action == "stop":
                    data = '5'
                s.send(data)

        
        d.accelerometer.stop_listening()
        s.send('5')
        s.close()
        audio.say(u"Mission complete.")
        print "Exiting..."