# ShakerRacer
# v1.5 - 19.1.2007

# Developed by:
# Andreas Jakl (andreas.jakl@fh-hagenberg.at)
# Department of Mobile Computing
# University of Applied Sciences - Hagenberg, Austria

# ==============================================================================
# License:
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
# ==============================================================================


# Offset to be added to (or substracted from) the acceleration value - calibration
acc_offset = 0
# Debug mode: -1 = deactivated, 1 = activated
debug = -1
# Use Bluetooth or don't send data: -1 = no data, 1 = bluetooth
use_bluetooth = 1
# Graphics mode: -1 = deactivated, 1 = activated
draw_graphics = 1
# Speed limit: -1 = deactivated, 1 = activated
speed_limit = 1
max_speed_fw = 35
max_speed_bw = -30
# Size of the point that displays the acceleration value
pointsize = 10
# Landscape mode: -1 = portrait, 1 = landscape
landscape = 1
# Inverted acceleration: -1 = off, 1 = on
inverted_acceleration = -1


import appuifw  # s60 user interface
import socket   # bluetooth
import sysinfo
import e32
import audio    # text to speech
import miso     # for keeping the backlight on
import struct   # formatting output to bytes
from key_codes import *
from graphics import *

# Screen size: large shows softkey labels, full is the whole screen
appuifw.app.screen='full'
# 1 = Use official Nokia Sensor API (5500, N95 8GB, ...), -1 = Use aXYZ (N95 < FW20, N93i)
use_sensor_api = 1
if use_sensor_api == 1:
    import sensor
    ignore_max = 4  # Don't process every value -> would be too much for Bluetooth
else:
    import axyz
    ignore_max = 3      

# -------------------------------------------------------------------------------------------------
# Handle acceleration data
# Will be called in regular (quite frequent) intervals
def handle_sensor_data(x,y,z):
    global ignore_data, ignore_max

    # Only send every nth message, otherwise it'd get too much for
    # the Bluetooth connection to handle (resulting in exceptions)
    ignore_data += 1
    if ignore_data < ignore_max:
        return
    ignore_data = 0

    global sock
    global debug, emergency_stop, draw_graphics, landscape, inverted_acceleration, speed_limit, max_speed_fw, max_speed_bw
    global last_acceleration, last_steering, cur_acc_mode, acc_offset

    # -------------------------------------------------------------------------------------------------
    # Keyboard Handler
    
    if keyboard.is_down(EScancodeRightSoftkey): # Exit
        exit_key_handler()
    elif keyboard.pressed(EScancodeStar):       # (De)activate debug display
        debug *= -1  
    elif keyboard.pressed(EScancodeHash):       # (De)activate landscape mode
        landscape *= -1
    elif keyboard.pressed(EScancode0):          # (De)activate inverted acceleration
        inverted_acceleration *= -1
    elif keyboard.pressed(EScancode3):          # (De)activate the speed limit
        speed_limit *= -1
    elif keyboard.pressed(EScancodeSelect):     # # (De)activate the emergency stop
        emergency_stop *= -1

    raw_x = x
    raw_y = y
    raw_z = z

    # -------------------------------------------------------------------------------------------------
    # Process acceleration and steering
    
    # Emergency brake
    if emergency_stop == 1:
        x = 0
        y = 0
        z = 0

    # Control mode
    if landscape == -1:     #portrait
        steering = -x
        acceleration = -y
    else:                   #landscape
        steering = -y
        acceleration = z
    
    # Inverted acceleration
    if inverted_acceleration == 1:
        acceleration = -acceleration
    
    # Use the full range from -127 .. 127
    steering *= 3
    acceleration *= 2
    
    if (cur_acc_mode > -1) and (acceleration > 0):      # Received backward acceleration and not yet in backward mode
        acceleration = 0                                # Keep acceleration at 0 for now
        cur_acc_mode -= 1                               # Decrement acceleration mode until it reaches -1
    elif (cur_acc_mode < 0) and (acceleration <= 0):    # Received forward acceleration and not yet in forward mode
        cur_acc_mode = 1                                # Instantly switch to forward
    
    # Maximum values check (multiple g)
    steering = crop_value(steering, -127, 127)
    acceleration = crop_value(acceleration, -127, 127)
    
    # -------------------------------------------------------------------------------------------------
    # Send out new messages to the car
    
    # Build steering message
    # Only send it if the new value is different to the old one
    if last_steering != steering:
        last_steering = steering
        # Build steering message
        checksum = (255+3+1+1+steering) % 256    # checksum
        #print "st: %i  ck: %i"%(steering,checksum)
        msg = ubytecmd(255, 3, 1, 1)
        msg += bytecmd(steering)
        msg += ubytecmd(checksum)
        if use_bluetooth == 1:
            sock.send(msg)

    # Build acceleration message
    # Only send it if the new value is different to the old one *or* if the car is currently
    # switching from forward to backward mode
    if (last_acceleration != acceleration) or ((cur_acc_mode != 1) and (cur_acc_mode != -1)):
        last_acceleration = acceleration
        send_acceleration = acceleration
        # Normalize the acceleration to the restricted values if the speed limit is activated
        if speed_limit == 1:
            if send_acceleration > 0:
                # Backwards
                send_acceleration = normalize_value(send_acceleration, 0, 64, 0, max_speed_bw)
            else:
                # Forward acceleration
                send_acceleration = normalize_value(send_acceleration, -64, 0, max_speed_fw, 0)
        # Add the acceleration offset to the sent value and make sure it doesn't exceed its limits
        send_acceleration = crop_value(send_acceleration + acc_offset, -127, 127)
        checksum = (255+4+1+1+send_acceleration) % 256    # checksum
        msg = ubytecmd(255, 4, 1, 1)
        msg += bytecmd(send_acceleration)
        msg += ubytecmd(checksum)
        if use_bluetooth == 1:
            sock.send(msg)

    # -------------------------------------------------------------------------------------------------
    # Drawing
    
    # clear the image
    img.clear(0x000000)

    global res_x, res_y

    # Define the title text and measure it in order to be able to center it on the screen
    title_text = u"ShakerRacer"
    text_rect = img.measure_text(title_text,font='title')[0]
    top_offset_y = -text_rect[1]

    # Draw the background grid
    # left to right
    img.line((0, (res_y - top_offset_y) / 2 + top_offset_y, res_x, (res_y - top_offset_y) / 2 + top_offset_y),0x7f0000)
    # top to bottom
    img.line((res_x / 2, top_offset_y, res_x / 2, res_y),0x7f0000)

    # Define a point and add it to the image, define its: current x,y coordinate, and its colour and size
    if landscape == -1:     #portrait
        img.point((normalize_value(steering, -127, 127, 0 + pointsize / 2, res_x - pointsize / 2), normalize_value(acceleration, -127, 127, top_offset_y + pointsize / 2, res_y - pointsize / 2)), 0xff0000,width=pointsize)
    else:                   #landscape
        img.point((normalize_value(acceleration, -127, 127, 0 + pointsize / 2, res_x - pointsize / 2), normalize_value(steering, -127, 127, top_offset_y + pointsize / 2, res_y - pointsize / 2)), 0xff0000,width=pointsize)

    # Draw the title text
    #text_info=u"rect: %i,%i,%i,%i"%(text_rect[0],text_rect[1],text_rect[2],text_rect[3])
    #img.text((50,100),text_info,0x00ff00)
    img.text((res_x / 2 - text_rect[2] / 2, -text_rect[1]),title_text,0xffff00,font="title")

    # Debug: display numeric data
    if debug == 1:
        img.text((5, 50),u"Raw x: %d"%raw_x,0x0060ff)
        img.text((5, 65),u"Raw y: %d"%raw_y,0x0060ff)
        img.text((5, 80),u"Raw z: %d"%raw_z,0x0060ff)
        img.text((5, 95),u"Acceleration: %d"%acceleration,0x0000ff)
        img.text((5, 110),u"Send. Acc: %d"%send_acceleration,0x0000ff)
        img.text((5, 125),u"Steering: %d"%steering,0x0000ff)
        img.text((5, 140),u"Acc.-Offset: %d"%acc_offset,0x0000ff)
        img.text((5, 155),u"Landscape: %d"%landscape,0x6000ff)
        img.text((5, 170),u"Inv. Acc: %d"%inverted_acceleration,0x6000ff)
        img.text((5, 185),u"Speed limit: %d"%speed_limit,0x6000ff)

    # redraw the image
    if draw_graphics == 1:
        handle_redraw(())

    # -------------------------------------------------------------------------------------------------
    # Diverse processing
    
    # keep backlight on
    miso.reset_inactivity_time()

    # vibration - bad influence on acceleration sensor!
    #vibrate_intensity = float(acceleration - 127) * 0.787
    #if vibrate_intensity < 0:
    #    vibrate_intensity = -vibrate_intensity
    #miso.vibrate(20,vibrate_intensity)

    #print "x: %i y: %i z: %i"%(x,y,z)

    # this is needed to start a short scheduler to allow checking for key events
    e32.ao_yield()

class Keyboard(object):
    def __init__(self,onevent=lambda:None):
        self._keyboard_state={}
        self._downs={}
        self._onevent=onevent
    def handle_event(self,event):
        if event['type'] == appuifw.EEventKeyDown:
            code=event['scancode']
            if not self.is_down(code):
                self._downs[code]=self._downs.get(code,0)+1
            self._keyboard_state[code]=1
        elif event['type'] == appuifw.EEventKeyUp:
            self._keyboard_state[event['scancode']]=0
        self._onevent()
    def is_down(self,scancode):
        return self._keyboard_state.get(scancode,0)
    def pressed(self,scancode):
        if self._downs.get(scancode,0):
            self._downs[scancode]-=1
            return True
        return False

# function that handles the bluetooth connection:
def bt_connect():
    global sock
    # create a bluetooth socket
    sock=socket.socket(socket.AF_BT,socket.SOCK_STREAM)
    target=''# here you can give the bt address of the other mobile if you know it
    if not target:
        # scan for bluetooth devices
        address,services=socket.bt_discover()
        print "Discovered: %s, %s"%(address,services)
        if len(services)>1:
            choices=services.keys()
            choices.sort()
            # bring up a popup menu and show the available bt devices for selection
            choice=appuifw.popup_menu([unicode(services[x])+": "+x
                                        for x in choices],u'Choose port:')
            target=(address,services[choices[choice]])
        else:
            target=(address,services.values()[0])
    print "Connecting to "+str(target)
    # connect to the serial port of the PC
    sock.connect(target)
    if debug == -1:
        audio.say(u"Start your engines.")

# define a function that redraws the screen, in this case the image shall be
# drawn again and again (use the .blit function for this)
def handle_redraw(rect):
    global img
    canvas.blit(img)

def exit_key_handler():
    global sock,use_bluetooth,use_sensor_api
    
    # Unregister to stop sensor data acquisition callbacks.
    if use_sensor_api == 1:
        global acc_sensor
        acc_sensor.disconnect()
    else:
        axyz.disconnect()
        
    if use_bluetooth == 1:
        sock.close()
    app_lock.signal()
    appuifw.app.set_exit()

# Normalize the value to fit into the specified range
# Requires to specify the previous maximum range
def normalize_value(val,min_val,max_val,min_new,max_new):
    # Make sure the value doesn't exceed its the official max or minimum
    val = crop_value(val, min_val, max_val)
    # Normalize it so that the value is restricted to the new range
    # and uses the full new range in case it used the full old range.
    return int(((float(val) - min_val) / (max_val - min_val)) * (max_new - min_new) + min_new)

def bytecmd(cmd,*args): # Arguments are signed bytes
    return struct.pack("b%db" % len(args), cmd, *args)

def ubytecmd(cmd,*args): # Arguments are unsigned bytes
    return struct.pack("B%dB" % len(args), cmd, *args)

# Crop the value to the minimum and maximum
def crop_value(val,min_val,max_val):    
    if (val < min_val):
        val = min_val
    elif (val > max_val):
        val = max_val
    return val


def menu_about():
    appuifw.note(u"Andreas Jakl, 2007\nsymbianresources.com", "info")

def menu_exit():
    exit_key_handler()

def get_sensor_data(status):
    global value_cache,cache_pos
    # Calculate a sliding average for the sensor data of the official sensor API
    # Needed very badly for the 5500, doesn't seem to be so important for the N95.
    # Either the N95 sensor is better (but it should be the same chip), or the
    # N95 does some internal pre-filtering. Doing it ourselves doesn't hurt in any
    # case.
    value_cache[cache_pos][0] = status['data_1']
    value_cache[cache_pos][1] = status['data_2']
    value_cache[cache_pos][2] = status['data_3']
    cache_pos += 1
    if cache_pos == 5:
        cache_pos = 0
    cur_data = [0,0,0]
    for i in range(5):
        cur_data[0] += value_cache[i][0]
        cur_data[1] += value_cache[i][1]
        cur_data[2] += value_cache[i][2]
    for i in range(3):
        cur_data[i] /= 5
    #print "x: %i y: %i z: %i"%(status['data_1'],status['data_2'],status['data_3'])
    
    # 1g for aXYZ is around 64. For the sensor API on the 5500 it's about 360
    # So normalize the 5500 sensor data to the axyz data - the car can not
    # handle such fine steps anyway.
    #
    # Axis conversion from 5500 to N95:
    # axyz <= Nokia Sensor
    # x    <= -y (data_2 = array 1)
    # y    <= -x (data_1 = array 0)
    # z    <= -z (data_3 = array 2)
    handle_sensor_data(-cur_data[1]/5,-cur_data[0]/5,-cur_data[2]/5)

# -------------------------------------------------------------------------------------------------
# Initialize the application and start the sensor
    
appuifw.app.title = u"ShakerRacer"
appuifw.app.exit_key_handler = exit_key_handler
appuifw.app.menu = [(u"About", menu_about),(u"Exit", menu_exit)]


keyboard=Keyboard()

if draw_graphics == 1:
    # Use the graphical visualization
    canvas=appuifw.Canvas(event_callback=keyboard.handle_event, redraw_callback=handle_redraw)
    # set the application body as canvas
    appuifw.app.body=canvas
else:
    # Just set the keyboard handler and don't draw anything so that
    # print messages can be seen on the console
    canvas=appuifw.Canvas(event_callback=keyboard.handle_event)

# create an empty image (appears as white image)
res_x, res_y = sysinfo.display_pixels()
img=Image.new((res_x,res_y))

emergency_stop = -1     # Emergency stop is deactivated
last_acceleration = 0
last_steering = 0
ignore_data = 0
cur_acc_mode = 1        # 1... forward, -1... backward

# call the function that handles the bluetooth connection
if use_bluetooth == 1:
    bt_connect()

# sliding average
value_cache = [[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]]
cache_pos = 0

# connect to the sensor
if use_sensor_api == 1:
    # Use official Nokia Sensor API
    sensor_type = sensor.sensors()['AccSensor']
    acc_sensor = sensor.Sensor(sensor_type['id'],sensor_type['category'])
    acc_sensor.connect(get_sensor_data)
else:
    # Use AXYZ module
    axyz.connect(handle_sensor_data)

app_lock = e32.Ao_lock()
app_lock.wait()
