<repo_name>stephenjust/arduino-gps-glasses<file_sep>/server/readgraph.py
"""
    python3 readgraph.py [ digraph-file ]

Takes a csv (comma separated values) text file containing the vertices
and edges of a street digraph and converts it into a digraph instance.

If the optional argument digraph-file is supplied, reads that, otherwise
takes input from stdin
"""
# For testing, just use a simple representation of set of vertices, set of
# edges as ordered pairs, and dctionaries that map
#   vertex to (lat,long)
#   edge to street name

def readgraph(filename):
    digraph_file = open(filename, 'r')

    V = set()
    E = set()
    V_coord = {}
    E_name = {}

    # process each line in the file
    for line in digraph_file:

        # strip all trailing whitespace
        line = line.rstrip()

        fields = line.split(",")
        type = fields[0]

        if type == 'V':
            # got a vertex record
            (i,lat,lng) = fields[1:]

            # vertex id's should be ints
            i=int(i)

            # lat and long are floats
            lat=float(lat)
            lng=float(lng)

            V.add(i)
            V_coord[i] = (lat,lng)

        elif type == 'E':
            # got an edge record
            (start,stop,name) = fields[1:]

            # vertices are ints
            start=int(start)
            stop=int(stop)
            e = (start,stop)

            # get rid of leading and trailing quote " chars around name
            name = name.strip('"')

            # consistency check, we don't want auto adding of vertices when
            # adding an edge.
            if start not in V or stop not in V:
                raise Exception("Edge {} has an endpoint that is not a vertex".format(e) )

            E.add(e)
            E_name[e] = name
        else:
            # weird input
            raise Exception("Error: weird line |{}|".format(line))

    return (V, E, V_coord, E_name)
<file_sep>/glasses-test/wiringtest.c
#include <Arduino.h>

void setup() {
    int a0 = 22;
    int a1 = 23;
    int a2 = 24;
    //Used to enable certain leds
    int v[] = {0, 0, 0};
    
    pinMode(a0, OUTPUT);
    pinMode(a1, OUTPUT);
    pinMode(a2, OUTPUT);
    while (1) {
    for (int i = 0; i <8; i++){
    
    //Make i a bitfield 
    if (i & 0x1) { v[0] = 1;} else {v[0] = 0;}
    if (i & 0x2) { v[1] = 1;} else {v[1] = 0;}
    if (i & 0x4) { v[2] = 1;} else {v[2] = 0;}
    if (v[0]) 
    digitalWrite(a0, HIGH);
    if (v[1])
        digitalWrite(a1, HIGH);
    if (v[2])
    digitalWrite(a2, HIGH);
    
    delay(2000);
    
    digitalWrite(a0, LOW);
    digitalWrite(a1, LOW);
    digitalWrite(a2, LOW);
    
    }
    }
}



void loop() {
    
    
}
<file_sep>/server/readme.txt
Python Path-Finding Server
by <NAME>
==========================

To execute the server, run the following:
  python server.py -s <serial-port> [-v] [-g <graph-file>]

There must be a file named edmonton_roads.txt in the script directory.

This server takes its input from a serial port in the format:
  a b c d
where a, b, c, and d are integers representing 100,000ths of a degree.
The starting coordinate is (a, b), and the destination is (c, d). Vertices
are in the format (latitude, longitude).

The server returns a path in the following format:
  n
  x1 y1
  x2 y2
  ...
  xn yn
where n is the number of vertices in the path, and x and y are the
coordinates of each vertex (lat, long).
<file_sep>/server/map.py
class Map:
    """

    >>> m = Map("test.map")

    >>> p = m.get_path( (0.0, 0.0), (0.0, 0.0) )
    >>> p == [(0.0, 0.0)]
    True
    """
    def __init__(self, file):
        pass

    def get_path(self, start_coord, stop_coord):
        return []

if __name__ == "__main__":
    import doctest
    doctest.testmod()
<file_sep>/client/GTPA010.cpp
#include "GTPA010.h"

#include "Sensors.h"
#include "TinyGPS.h"
#include "TimerThree.h"


bool GTPA010::newData = 0;

/* Static variable declarations */
char GTPA010::dataBuffer;
gpsData GTPA010::data;

// Inturupt values
volatile bool GTPA010::gpsValue = -1;
volatile bool GTPA010::gpsLock = 0;

gpsData* GTPA010::getData()
{
	return &data; // Returns the data pointer
}

bool GTPA010::check()
{
	return newData; // Returns the status of newData
}

void GTPA010::begin()
{
	// Start GPS - Defines in classes/Config.h
	Serial2.begin(GPS_BAUD_RATE);
	pinMode(GPS_FIX_PIN,INPUT);
	pinMode(GPS_ENABLE_PIN,OUTPUT);


	// Setup GPS Interrupt check - Defines in classes/Config.h
        Timer3.initialize(timer_ticks);
	digitalWrite(GPS_ENABLE_PIN,HIGH);
	Timer3.attachInterrupt(&gpsCheck);
}

void GTPA010::gpsCheck()
{
	bool oldGpsValue = gpsValue; // check the old value for the 2D/3D lock pin
	
	gpsValue = digitalRead(GPS_FIX_PIN); // refresh value with a digitalRead
	
	if(!(oldGpsValue ^ gpsValue)) // Does the {new,old} value both 0?
		gpsLock = 1; // Indicate a lock is present
	else
		gpsLock = 0; // Clear the lock variable
	
	sensorSecond = !sensorSecond; // Toggle the static sensor class variable for use of other sensors
}

void GTPA010::readData()
{
#if FAKE_GPS_DATA
    GTPA010::fakeData();
    return;
#endif
	if(gpsLock) // If we have a lock
	{
		while (Serial2.available()) // And while the Serial. is available
		{
			dataBuffer = Serial2.read(); // Read the available data
				
			if (gps.encode(dataBuffer)) // Did a new valid sentence come in?
				newData = true; // Notify the class that there is new data to show!
		}
		
		gps.get_position(&data.lat, &data.lon, &data.age); // Parse the position data
		gps.crack_datetime(&data.year, &data.month, &data.day, &data.hour, &data.minute, &data.second, &data.hundredths, &data.age); // Parse the time data
		
		if(data.lat == TinyGPS::GPS_INVALID_ANGLE || data.lon == TinyGPS::GPS_INVALID_ANGLE || gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES || gps.hdop() == TinyGPS::GPS_INVALID_HDOP) // If any component of this data is invalid, then cancel the new data decleration, and if enabled, print that the data is invalid across the serial
		{
			newData = false;
			
			#if SERIAL_PRINT_ENABLE
			Serial.println("GPS DATA INVALID");
			#endif
		}
			
	} else {
#ifdef DEBUG_GPS
            Serial.println("No lock!");
#endif
        }
}

#if FAKE_GPS_DATA
#import "gps_fake_data.h"

int tstart_time = 0;

/**
 * Provide some fake data in case we are demoing in a location
 * where it is not possible to get a GPS fix
 */
void GTPA010::fakeData() {
    // Set the start time of the path
    if (!tstart_time) {
        tstart_time = Sensors::getTime();
    }

    // Calculate how far into the path we are
    int index = (Sensors::getTime() - tstart_time) % fd_len;

    // Set the fake data
    data.lat = fd_lat[index];
    data.lon = fd_lon[index];

    newData = true;
}
#endif

void GTPA010::printData()
{
	// Print the data via serial
	
	Serial.print("LAT=");
		Serial.print(data.lat == TinyGPS::GPS_INVALID_ANGLE ? NAN : data.lat);
	Serial.print(" LON=");
		Serial.print(data.lon == TinyGPS::GPS_INVALID_ANGLE ? NAN : data.lon);
	Serial.print(" SAT=");
		Serial.print(gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? NAN : gps.satellites());
	Serial.print(" PREC=");
		Serial.print(gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? NAN : gps.hdop());
	  
	Serial.print(" DATE=");
	  
	if (data.age == TinyGPS::GPS_INVALID_AGE)
		Serial.println("*******    *******    ");
	else
	{
		char sz[32];
		sprintf(sz, "%02d,%02d,%02d,%02d:%02d:%02d", data.month, data.day, data.year, data.hour, data.minute, data.second);
		Serial.println(sz);
	}
}
<file_sep>/client/Sensors.h
#ifndef _SENSORS_H_
#define _SENSORS_H_

#include <Arduino.h>
#include <Wire.h>

class Sensors {
public:
	Sensors();
	static void writeTo(int destination, byte address, byte val); // Provides the functionality to write to a I2C device given its address (destiniation), the destination register (address), and the value we are writing (val)
	static void readFrom(int destination, byte address, int num, byte _buff[]); // Provides the functionality to read from a I2C device given its address (destination), the buffer we are loading said information into (_buff) from a specified register (address), a number of bytes (num)
	static int getTime(); // Returns the time since sensor ini in seconds
	static unsigned long int getLongTime(); // Returns the time since sensor ini in miliseconds
	static volatile bool sensorSecond; // Provides a method for reading/writing values every second
private:
	static unsigned long int time; // Stores the time as used in above functions
};

#endif
<file_sep>/client/serial_handling.h
#ifndef SERIAL_HANDLING_H
#define SERIAL_HANDLING_H

#include <stdint.h>

/*
    Function to read a single line from the serial buffer up to a specified
  length (length includes the null termination character that must be
  appended onto the string).  This function is blocking. The newline
  character sequence is given by CRLF, or "\r\n".

  Arguments:
  buffer: Pointer to a buffer of characters where the string will be stored.

  length: The maximum length of the string to be read.

  Preconditions:  None.

  Postconditions: Function will block until a full newline has been read, or the
    maximum length has been reached.  Afterwards the new string will be stored 
    in the buffer passed to the function.

  Returns: the number of bytes read

*/
uint16_t serial_readline(char *line, uint16_t line_size);

/*
    Function to read a portion of a string into a buffer, up to any given
  separation characters.  This will read up to the specified length of the
  character buffer if a separation character is not encountered, or until the 
  end of the string which is being copied.  A starting index of the string 
  being copied can also be specified.

  Arguments:
  str:  The string which is having a portion copied.
  str_start:  The index of the string to start at. (Less than str's length).
  buf:  The buffer to store the copied chunk of the string into.
  buf_len:  The length of the buffer.
  sep:  String containing characters that will be used as separators.

  Preconditions:  Make sure str_start does *NOT* exceed the size of str.

  Postconditions: Stores the resulting string in buf, and returns the
    position where reading was left off at.  The position returned will skip
    separation characters.

*/

uint16_t string_read_field(const char *str, uint16_t str_start, 
    char *field, uint16_t field_size, const char *sep);

/*
 Function to convert a string to an int32_t.

 Arguments:
 str:  The string to convert to an integer.

 Preconditions:  The string should probably represent an integer value.

 Postconditions: Will return the equivalent integer.  If an error occured
     the blink assert may be triggered, and sometimes zero is just returned
    from this function because EINVAL does not exist for the Arduino strtol.

 */

int32_t string_get_int(const char *str);

#endif
<file_sep>/client/joystick.cpp
#include <Arduino.h>

#include "joystick.h"

// Arduino analog input pin for the horizontal on the joystick.
const uint8_t joy_pin_x = 0;
// Arduino analog input pin for the vertical on the joystick.
const uint8_t joy_pin_y = 1;
// Digital pin for the joystick button on the Arduino.
const uint8_t joy_pin_button = 4;

// Center point of the joystick - analog reads from the Arduino.
int16_t joy_center_x = 512;
int16_t joy_center_y = 512;


// button state: 0 not pressed, 1 pressed
uint8_t prev_button_state = 0;

// time of last sampling of button state
uint32_t button_prev_time = 0;

// only after this much time has passed is the state sampled.
uint32_t button_sample_delay = 200;

void initialize_joystick() {
    // Initialize the button pin, turn on pullup resistor
    pinMode(joy_pin_button, INPUT);
    digitalWrite(joy_pin_button, HIGH);

    // Center Joystick
    joy_center_x = analogRead(joy_pin_x);
    joy_center_y = analogRead(joy_pin_y);
}


/*
  Read the joystick position, and return the x, y displacement from the zero
  position.  The joystick has to be at least 4 units away from zero before a
  non-zero displacement is returned.  This filters out the centering errors that
  occur when the joystick is released.

  Also, return 1 if the joystick button has been pushed, held for a minimum
  amount of time, and then released.  That is, a 1 is returned if a button
  select action has occurred.  

*/
uint8_t process_joystick(int16_t *dx, int16_t *dy) {
    int16_t joy_x;
    int16_t joy_y;
    uint8_t button_state;

    joy_x = -(analogRead(joy_pin_y) - joy_center_x);
    joy_y = -(analogRead(joy_pin_x) - joy_center_y);

    if (abs(joy_x) <= 4) {
        joy_x = 0;
    }

    if (abs(joy_y) <= 4) {
        joy_y = 0;
    }

    *dx = joy_x / 128;
    *dy = joy_y / 128;

    // first, don't even sample unless enough time has passed
    uint8_t have_sample = 0;
    uint32_t cur_time = millis();
    if ( cur_time < button_prev_time ) {
        // time inversion caused by wraparound, so reset
        button_prev_time = cur_time;
    }
    if ( button_prev_time == 0 || 
         button_prev_time + button_sample_delay < cur_time ) {
        // button pushed after suitable delay, so ok to return state
        button_prev_time = cur_time;
        have_sample = 1;
        button_state = LOW == digitalRead(joy_pin_button);
    }

    // if no sample, return no press result
    if ( ! have_sample ) { return 0; }

    // if prev and current state are same, no transition occurs
    if ( prev_button_state == button_state ) { return 0; }

    // are we waiting for push or release?
    if ( prev_button_state ) {
        // we got a release, return the press event
        prev_button_state = button_state;
        return 1;
    }

    // we got a press, so change state to waiting for release, but
    // don't signal the press event yet.
    prev_button_state = button_state;
    return 0;
}
<file_sep>/client/Sensors.cpp
#include "Sensors.h"

// Static allocations
unsigned long int Sensors::time;

// Prepare the inturupt element
volatile bool Sensors::sensorSecond = 0;

Sensors::Sensors()
{
	Wire.begin(); // Start the wire interface
	time = millis(); // Record the start time
}

int Sensors::getTime()
{
	return (int)((float)(0.001 * (millis() - time))); // Return the time in seconds
}

unsigned long int Sensors::getLongTime()
{
	return millis() - time; // Return the time in miliseconds
}

void Sensors::writeTo(int destination, byte address, byte val) {
	Wire.beginTransmission(destination); // start transmission to device 
	Wire.write(address);             // send register address
	Wire.write(val);                 // send value to write
	Wire.endTransmission();         // end transmission
}

// Reads num bytes starting from address register on device in to _buff array
void Sensors::readFrom(int destination, byte address, int num, byte _buff[]) {
	Wire.beginTransmission(destination); // start transmission to device 
	Wire.write(address);             // sends address to read from
	Wire.endTransmission();         // end transmission
	
	delay(6);
	
	Wire.beginTransmission(destination); // start transmission to device
	Wire.requestFrom(destination, num);    // request 6 bytes from device
	
	int i = 0;
	while(Wire.available())         // device may send less than requested (abnormal)
	{ 
		_buff[i] = Wire.read();    // receive a byte
		i++;
	}
	
	Wire.endTransmission();         // end transmission
}
<file_sep>/client/client.cpp
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_ST7735.h> 
#include <SD.h>
#include <mem_syms.h>

#include "TimerThree.h"
#include "Sensors.h"
#include "TinyGPS.h"
#include "GTPA010.h"
#include "LSM303.h"

#include "joystick.h"
#include "map.h"
#include "path.h"
#include "serial_handling.h"
#include "ledon.h"

// #define DEBUG_SCROLLING
// #define DEBUG_PATH
// #define DEBUG_MEMORY

TinyGPS gps;
LSM303 compass;

// the pins used to connect to the AdaFruit display
const uint8_t sd_cs = 5;
const uint8_t tft_cs = 6;
const uint8_t tft_dc = 7;
const uint8_t tft_rst = 8;    

// forward function declarations
void initialize_sd_card();
void initialize_screen();
void initialize_joystick();
uint8_t process_joystick(int16_t *dx, int16_t *dy);
void status_msg(char *msg);
void clear_status_msg();

// Interrupt routines for zooming in and out.
void handle_zoom_in();
void handle_zoom_out();

// global state variables

// globally accessible screen
Adafruit_ST7735 tft = Adafruit_ST7735(tft_cs, tft_dc, tft_rst);

// Map number (zoom level) currently selected.
extern uint8_t current_map_num;

// First time flag for loop, set to cause actions for the first time only
uint8_t first_time;

void setup() {
    Sensors::Sensors();
    Serial.begin(9600);
    Serial.println("Starting...");
    Serial.flush();    // There can be nasty leftover bits.

    GTPA010::begin();
    Serial.println("GPS initialized!");

    compass.init(LSM303DLH_DEVICE);
    compass.enableDefault();
    compass.setMagGain(LSM303::magGain_47);
    Serial.println("Compass initialized!");

    compass.read();
#ifdef DEBUG
    if (!compass.timeoutOccurred()) {
        Serial.print("Compass heading: ");
        Serial.println(compass.heading());
    } else {
        Serial.println("Timed out!"); 
    }
#endif

#ifdef GLASSES_DEBUG
    while (1) {
        compass.reaqd();
        map_to_glasses(compass.heading());
    }
#endif

    initialize_screen();

    initialize_sd_card();

    initialize_joystick();

    initialize_map();

    // Want to start viewing window in the center of the map
    move_window(-11353058,5352706);

    // with cursor in the middle of the window
    move_cursor_to(
                   screen_map_x + display_window_width / 2, 
                   screen_map_y + display_window_height / 2);

    // Draw the initial screen and cursor
    first_time = 1;

#ifdef DEBUG_MEMORY
    Serial.print("Available mem:");
    Serial.println(AVAIL_MEM);
#endif
}

const uint16_t screen_scroll_delta = 64;
const uint16_t screen_left_margin = 10;
const uint16_t screen_right_margin = 117;
const uint16_t screen_top_margin = 10;
const uint16_t screen_bottom_margin = 117;

// the path request, start and stop lat and lon
uint8_t request_state = 0;  // 0 - wait for start, 1 - wait for stop point
int32_t start_lat;
int32_t start_lon;
int32_t stop_lat;
int32_t stop_lon;

uint32_t g_lat;
uint32_t g_lon;

// the most recent path to display, length = 0 means no path
uint16_t path_length = 0;
coord_t *path;


char * prev_loc_msg = 0;
/**
 * Print the current GPS position to the LCD
 */
void pos_msg(char * msg) {
    if (prev_loc_msg != msg) {
        prev_loc_msg = msg;
        
        // Draw background
        tft.fillRect(0, 136, 128, 12, WHITE);
        
        // Set text options
        tft.setTextSize(1);
        tft.setTextColor(BLACK);
        tft.setCursor(0, 138);

        // Draw text
        tft.println(msg);
    }
}

/**
 * Redraw the screen
 */
void refresh_display() {
#ifdef DEBUG_SCROLLING
    Serial.println("Screen update");
    Serial.print(current_map_num);
    Serial.print(" ");
    Serial.print(cursor_lon);
    Serial.print(" ");
    Serial.print(cursor_lat);
    Serial.println();
#endif

    draw_map_screen();
    draw_cursor();

    // Need to redraw any other things that are on the screen
    if ( path_length > 0 ) {
        draw_path(path_length, path);
    }

    // force a redisplay of status message
    clear_status_msg();
    draw_compass();
    draw_gps_dot();

    char * pos_str = 0;
#if FAKE_GPS_DATA
    pos_str = "USING FAKE DATA";
#else
    if (GTPA010::gpsLock)
        pos_str = "USING GPS COORDS";
    else
        pos_str = "SEARCHING SATELLITES";
#endif
    pos_msg("");
    pos_msg(pos_str);
}

/**
 * Send a path request to the server and wait for a response
 */
void query_path(int32_t s_lat, int32_t s_lon, int32_t e_lat, int32_t e_lon) {

    // send out the start and stop coordinates to the server
    Serial.print(s_lat);
    Serial.print(" "); 
    Serial.print(s_lon);
    Serial.print(" "); 
    Serial.print(e_lat);
    Serial.print(" "); 
    Serial.print(e_lon);
    Serial.println();

    // free any existing path
    if ( path_length > 0 ) {
        free(path);
    }

    // read the path from the serial port
    status_msg("WAITING");
    if ( read_path(&path_length, &path) ) {
#ifdef DEBUG_PATH
        uint8_t is_visible;
        for (uint16_t i=0; i < path_length; i++) {
            is_visible = is_coord_visible(path[i]);
            Serial.print(i);
            Serial.print(": ");
            Serial.print(path[i].lat);
            Serial.print(",");
            Serial.print(path[i].lon);
            Serial.print(is_visible ? "V": "");
            Serial.println();
        }
#endif
    } else {
        // should display this error on the screen
        pos_msg("Path error!");
    }
    refresh_display();
}

        int path_time = 0;

void loop() {

    // Make sure we don't update the map tile on screen when we don't need to!
    uint8_t update_display_window = 0;

    if ( first_time ) {
        first_time = 0;
        update_display_window = 1;
    }

    // Joystick displacement.
    int16_t dx = 0;
    int16_t dy = 0;
    uint8_t select_button_event = 0;

    // Update glasses heading
    if (path_length > 0) {
        //Serial.print("Compass: ");
        //Serial.println(compass.heading());
        compass.read();
        map_to_glasses((int)(target_dir - compass.heading()) % 360);
    }

    // See if the joystick has moved, in which case we want to
    // also want to move the visible cursor on the screen.

    // Process joystick input.
    select_button_event = process_joystick(&dx, &dy);

    // the joystick routine filters out small changes, so anything non-0
    // is a real movement
    if ( abs(dx) > 0 || abs(dy) > 0 ) {
        // Is the cursor getting near the edge of the screen?  If so
        // then scroll the map over by re-centering the window.

        uint16_t new_screen_map_x = screen_map_x;
        uint16_t new_screen_map_y = screen_map_y;
        uint8_t need_to_move = 0;

        uint16_t cursor_screen_x;
        uint16_t cursor_screen_y;
        if ( get_cursor_screen_x_y(&cursor_screen_x, &cursor_screen_y) ) {
            // if the cursor is visible, then adjust the display to 
            // to scroll if near the edge.

            if ( cursor_screen_x < screen_left_margin ) {
                new_screen_map_x = screen_map_x - screen_scroll_delta;
                need_to_move = 1;
            }
            else if ( cursor_screen_x > screen_right_margin ) {
                new_screen_map_x = screen_map_x + screen_scroll_delta;
                need_to_move = 1;
            }

            if ( cursor_screen_y < screen_top_margin ) {
                new_screen_map_y = screen_map_y - screen_scroll_delta;
                need_to_move = 1;
            }
            else if ( cursor_screen_y > screen_bottom_margin ) {
                new_screen_map_y = screen_map_y + screen_scroll_delta;
                need_to_move = 1;
            }

            if ( need_to_move ) {
                // move the display window, leaving cursor at same lat-lon
                move_window_to(new_screen_map_x, new_screen_map_y);
                update_display_window = 1;
            } 
            else {
                // erase old cursor, move, and draw new one, no need to 
                // redraw the underlying map tile
                erase_cursor();
                move_cursor_by(dx, dy);
                draw_cursor();
            }
        }

    }

    // at this point the screen is updated, with a new tile window and
    // cursor position if necessary

    // will only be down once, then waits for a min time before allowing
    // pres again.
    if (select_button_event) {
        // Button was pressed, we are selecting a point!
#ifdef DEBUG_PATH
        Serial.print("x ");
        Serial.print(cursor_map_x);
        Serial.print(" y ");
        Serial.print(cursor_map_y);
        Serial.println();
#endif

        // which press is this, the start or the stop selection?

        // If we are making a request to find a shortest path, we will send out
        // the request on the serial port and then wait for a response from the
        // server.  While this is happening, the client user interface is
        // suspended.

        gpsData * gdata;
        GTPA010::readData();
        gdata = GTPA010::getData();
        g_lat = gdata->lat;
        g_lon = gdata->lon;

        start_lat = g_lat;
        start_lon = g_lon;
        stop_lat = cursor_lat;
        stop_lon = cursor_lon;


        query_path(start_lat, start_lon, stop_lat, stop_lon);
        path_time = Sensors::getTime();
    } // end of select_button_event processing

    // do we have to redraw the map tile?  
    if (update_display_window) {
        refresh_display();
    }

    // Spam the server for a new path
    if (path_length > 0 && Sensors::getTime() - path_time >= 5) {
        gpsData * gData = GTPA010::getData();
        // If we've moved, request a new path
        if (abs(gData->lat - start_lat) > 5 || abs(gData->lon - start_lon) > 5) {
            start_lat = gData->lat;
            start_lon = gData->lon;

            query_path(start_lat, start_lon, stop_lat, stop_lon);
        }
        path_time = Sensors::getTime();
    }

    // Refresh compass display
    draw_compass();
    // Refresh gps dot
    draw_gps_dot();

    // always update the status message area if message changes
    // Indicate which point we are waiting for
    if ( request_state == 0 ) {
        status_msg("DESTINATION?");
    }
}

char* prev_status_msg = 0;

void clear_status_msg() {
    status_msg("");
}

void status_msg(char *msg) {
    // messages are strings, so we assume constant, and if they are the
    // same pointer then the contents are the same.  You can force by
    // setting prev_status_msg = 0

    if ( prev_status_msg != msg ) {
        prev_status_msg = msg;
        tft.fillRect(0, 148, 128, 12, GREEN);

        tft.setTextSize(1);
        tft.setTextColor(MAGENTA);
        tft.setCursor(0, 150);
        tft.setTextSize(1);

        tft.println(msg);
    }
}

void initialize_screen() {

    tft.initR(INITR_REDTAB);

    tft.setRotation(0);

    tft.setCursor(0, 0);
    tft.setTextColor(0x0000);
    tft.setTextSize(1);
    tft.fillScreen(BLUE);    
}

void initialize_sd_card() {
    if (!SD.begin(sd_cs)) {
#ifdef DEBUG_SERIAL
        Serial.println("Initialization has failed. Things to check:");
        Serial.println("* Is a card inserted?");
        Serial.println("* Is your wiring correct?");
        Serial.println("* Is the chipSelect pin the one for your shield or module?");

        Serial.println("SD card could not be initialized");
#endif

        while (1) {};    // Just wait, stuff exploded.
    }
    else {
#ifdef DEBUG_SERIAL

        Serial.println("Wiring is correct and a card is present.");

#endif
    }
}


<file_sep>/README.md
arduino-gps-glasses
===================

Arduino-powered GPS with a python map-finding server

This was a group project for CMPUT 297 (now called CMPUT 275) at the University of Alberta.

The project featured a set of plastic glasses frames with eight LED's affixed to the frame around one eye. The LED's were wired such that they could be selected using a 3-to-8 multiplexer. A switch was connected to one of the multiplexer's enable pins to turn the lights on or off.

The multiplexer was to connect to an Arduino Mega 2560, which also had a GPS receiver and a tilt-compensated compass connected to it. Through this, the Arduino could gather its position and orientation on a map of Edmonton, which was loaded on to an SD card.

The Arduino communicated with a Raspberry Pi, which ran a pathfinding program. This would generate a path for a person to follow. Based on the GPS and compass readings, this would allow the user to turn on the glasses to see which direction they should be moving.

The system could successfully retrieve a path and direct a user, however the digital compass was not effective on the university campus. As such, its accuracy was limited where it was tested.

The hardware assembled to run this project has since been decomissioned and disassembled.
<file_sep>/server/server.py
"""
Python route-finding server

Call this program with the map data file as a parameter
$ python server.py roads-digraph.txt

CMPUT297 Assignment #3
<NAME>

This assignment is a solo effort, although discussion about the assignment
occurred with several students: <NAME>, <NAME>

"""

import argparse
import digraph
import pqueue
import readgraph
import serial
import sys
import time

global debug
debug = False

def send(serial_port, message):
    """
    Sends a message back to the client device.
    """
    full_message = ''.join((message, "\n"))

    (debug and
        print("server:" + full_message + ":") )

    reencoded = bytes(full_message, encoding='ascii')
    serial_port.write(reencoded)


def receive(serial_port, timeout=None):
    """
    Listen for a message. Attempt to timeout after a certain number of
    milliseconds.
    """
    raw_message = serial_port.readline()

    debug and print("client:", raw_message, ":")

    message = raw_message.decode('ascii')

    return message.rstrip("\n\r")



def parse_args():
    """
    Parses arguments for this program.
    Returns an object with the following members:
        args.
             serialport -- str
             verbose    -- bool
             graphname  -- str
    """

    parser = argparse.ArgumentParser(
        description='Assignment 1: Map directions.',
        epilog = 'If SERIALPORT is not specified, stdin/stdout are used.')
    parser.add_argument('-s', '--serial',
                        help='path to serial port',
                        dest='serialport',
                        default=None)
    parser.add_argument('-v', dest='verbose',
                        help='verbose',
                        action='store_true')
    parser.add_argument('-g', '--graph',
                        help='path to graph (DEFAULT = " edmonton_roads.txt")',
                        dest='graphname',
                        default='edmonton_roads.txt')

    return parser.parse_args()

def distance(a, b):
    """
    Calculate the distance between two coordinates
    using a standard distance equation
    ( (x1-x2)^2 + (y1-y2)^2 )^0.5

    >>> distance( (0, 0), (3, 4) ) == 5
    True
    >>> round(distance( (-1.6, 2.4), (3.45, -5) )) == 9
    True
    """
    if len(a) != 2 or len(b) != 2:
        raise ValueError

    dx = float(a[0]) - float(b[0])
    dy = float(a[1]) - float(b[1])

    return ( dx**2 + dy**2 )**0.5

def nearest_vertex(coord):
    """
    Find the nearest vertex to a coordinate

    This cannot be unit tested generically here because overriding
    V_coord here in a doctest does not seem to override V_coord
    used in the function, and spec says we have load the Edmonton roads
    graph
    """
    # Make sure a coordinate was passed in
    if len(coord) != 2:
        raise ValueError

    # Make sure any vertex has a shorter distance
    shortest_dist = float('inf')
    vertex = -1
    
    # Loop through all vertices and find closest
    for v in V_coord:
        dist = distance(coord, V_coord[v])
        if dist < shortest_dist:
            shortest_dist = dist
            vertex = v

    return vertex

def cost_distance(e):
    """
    Get the distance between two vertices

    Like above we can't test it here as is because I can't
    override V_coord.
    """
    # Make sure we have a proper edge with two vertices
    if len(e) != 2:
        raise ValueError

    a = V_coord[e[0]]
    b = V_coord[e[1]]

    # Return the distance between two points
    return distance(a, b)

def least_cost_path(G, start, dest, cost):
    """
    path = least_cost_path(G, start, dest, cost)

    least_cost_path returns a least cost path in the digraph G from vertex
    start to vertex dest, where costs are defined by the cost function.
    cost should be a function that takes a single edge argument and returns
    a real-valued cost.

    if there is no path, then returns None

    the path from start to start is [start]

    >>> g = digraph.Digraph( [(1, 2), (4, 2), (4, 6), (6, 7), (1, 7), (2, 4)] )
    >>> least_cost_path(g, 1, 7, lambda e: abs(2*e[0]-e[1]))
    [1, 7]
    >>> least_cost_path(g, 7, 2, lambda e: 1) is None
    True
    """

    # Create a priority queue
    todo = pqueue.PQueue()
    todo.update(start, 0);

    # v in visited when the vertex v's least cost from start has been determined
    visited = set()

    # parent[v] is the vertex that just precedes v in the path from start to v
    parent = {}

    while todo and (dest not in visited):

        # priority queue operation
        # remove smallest estimated cost vertex from todo list
        (cur, c) = todo.pop_smallest()

        # it is now visited, and will never have a smaller cost
        visited.add(cur)

        for n in G.adj_to(cur):
            if n in visited: continue
            if todo.update(n, c+cost((cur,n))):
                parent[n] = cur

    # now, if there is a path, extract it.  The graph may be disconnected
    # so in that case return None
    if dest not in visited:
        return None

    path = [dest]
    cur = dest
    while start not in path:
        cur = parent[cur]
        path.append(cur)

    path.reverse()
    return path

def main():
    """
    Run the server
    """
    
    time1 = time.time()
    print("Initializing..")

    # Grab program arguments
    args = parse_args()

    # Load data file
    try:
        global V_coord
        (V, E, V_coord, E_name) = readgraph.readgraph(args.graphname)
    except FileNotFoundError:
        print("Data file {} not found!".format(args.graphname), file=sys.stderr)
        exit()

    # Initialize serial port
    try:
        if args.serialport:
            print("Opening serial port: {}".format(args.serialport))
            serial_out = serial_in = serial.Serial(args.serialport, 9600)
        else:
            print("No serial port. Supply one with the -s port option.")
            exit()
    except serial.SerialException:
        print("Could not open serial port: {}".format(args.serialport),
              file=sys.stderr)
        exit()

    # Set debug mode
    if args.verbose:
        debug = True
    else:
        debug = False

    # Generate a graph from the map and grab the needed values
    G = digraph.Digraph(E)
    # Add all orphan vertices. Not really useful, and in fact detrimental
    # in all cases, but do it for the sake of completeness
    for v in V:
        G.add_vertex(v)

    # Print some debug output
    if debug:
        print("Graph loaded with {} vertices and {} edges.".format(G.num_vertices(), G.num_edges()))

    time2 = time.time()
    delta_t = repr(time2 - time1)
    print("Done initializing, took " + delta_t + " seconds")

    # Parse input
    while True:
        
        msg = receive(serial_in)
        debug and print("GOT:{}:".format(msg), file=sys.stderr)

        fields = msg.split(" ")

        # Ignore malformed messages
        if len(fields) != 4:
            debug and print("Ignoring message: {}".format(msg))
            continue
        time1 = time.time()
        print("Processing..")
        # Get start and end vertices
        start_v = (int(fields[0])/10**5, int(fields[1])/10**5)
        end_v = (int(fields[2])/10**5, int(fields[3])/10**5)
        start = nearest_vertex( start_v )
        end = nearest_vertex( end_v )

        debug and print("Routing path from vertex {} to {}".format(start, end))

        path = least_cost_path(G, start, end, cost_distance)
        if path is None:
            send(serial_out, "0")
            debug and print("No path found!", file=sys.stderr)
        else:
            send(serial_out, str(len(path)))
            for v in path:
                send(serial_out, "{} {}".format(int(V_coord[v][0] * 10**5),
                                     int(V_coord[v][1] * 10**5)))
            print("Send path of length {}".format(len(path)))
        time2 = time.time()
        delta_t = repr(time2 - time1)
        print("Done processing, took " + delta_t + "seconds")
        

# Execute the main function
if __name__ == "__main__":
    main()
<file_sep>/server/pqueue.py
"""
Priority queue implementation as a binary heap

pop_smallest()
    remove the element with the lowest priority
    return tuple (key, priority)

update(key, priority)
    lower the priority of an element
    if priority is not lower do nothing,
    if key is not in priority queue, add it
    return True if priority of a key has changed or is added, otherwise False

is_empty()
    return True if nothing is in the priority queue, otherwise False

>>> q = PQueue()
>>> q.is_empty()
True
>>> q.update("thing", 1)
True
>>> q.is_empty()
False
>>> q.update("another thing", 5)
True
>>> q.pop_smallest()
('thing', 1)
>>> q.is_empty()
False
>>> q.update("thing", 3)
True
>>> q.update("another thing", 1)
True
>>> len(q)
2
>>> 'thing' in q
True
>>> 'nothing' in q
False
>>> q.pop_smallest()
('another thing', 1)
>>> q.pop_smallest()
('thing', 3)
>>> q.is_empty()
True

"""

def _parent(i):
    """
    >>> _parent(3)
    1
    >>> _parent(27)
    13
    """
    return (i-1)//2

def _children(i):
    """
    >>> _children(5)
    [11, 12]
    """
    return [ 2*i + 1, 2*i + 2 ]

class PQueue:
    def __init__(self):
        self._heap = []
        self._keyindex = {}

    def __len__(self):
        return len(self._heap)

    def __contains__(self, key):
        return key in self._keyindex

    def update(self, key, priority):
        if key in self._keyindex:
            i = self._keyindex[key]
            # Update existing entry, possibly lower its priority
            if priority > self._priority(i):
                return False
            else:
                self._heap[i] = (key, priority)
                self._heapify_up(i)
                return True
        else:
            # Create a new heap entry
            self._heap.append( (key, priority) )
            self._keyindex[key] = len(self._heap) - 1;
            self._heapify_up(len(self._heap) - 1)
            return True

    def pop_smallest(self):
        self._swap(0, len(self._heap) - 1)
        rv = self._heap.pop()
        del self._keyindex[rv[0]]
        self._heapify_down(0)
        return rv

    def is_empty(self):
        return len(self._heap) == 0

    def _swap(self, i, j):
        (self._heap[i], self._heap[j]) = (self._heap[j], self._heap[i])
        self._keyindex[self._heap[i][0]] = i
        self._keyindex[self._heap[j][0]] = j

    def _priority(self, i):
        return self._heap[i][1]

    def _heapify_down(self, i):
        children = [ c for c in _children(i) if c < len(self._heap) ]
        if not children: return

        # Find child with smallest priority
        minchild = min(children, key = self._priority)

        if self._priority(i) > self._priority(minchild):
            # Swap element i with its smallest child
            self._swap(i, minchild)

            self._heapify_down(minchild)

    def _heapify_up(self, i):
        if i == 0: return

        # compare i with its parent
        if self._priority(i) < self._priority(_parent(i)):
            self._swap(i, _parent(i))
            self._heapify_up(_parent(i)) 

if __name__ == "__main__":
    import doctest
    doctest.testmod()
<file_sep>/client/mem_syms.h
/*
  
  If you have the libc 1.6.4 libraries then free space at the end of the
  heap is NOT released.

  With the libc 1.7.1 libraries with the fixed malloc, then free space 
  at the end of the heap is released so that the space is available 
  to the stack or the heap.
  
  AVAIL_MEM is a macro to tell you how much available free memory is left
  between the top of stack and end of heap.  

  STACK_TOP is a macro to tell you the current address of the top of
  the stack.  There is data in this location ??

  STACK_BOTTOM is a macro to tell you the address of the bottom of the
  stack.  It should normally be constant.

  STACK_SIZE is a macro to tell you the current size of the stack

  HEAP_START is a macro to provide the address of the first location in
  the heap.

  HEAP_END is macro to provide the address of the current last location in
  in the heap.  This will increase as memory is allocated, and decrease if
  the end of the heap consists of free memory and can be shrunk.

  HEAP_SIZE is a macro to tell you the current size of the heap, but it
  includes chunks that are free.

  There should be a routine to detect the presence of the old or new malloc.

 */

#ifndef mem_syms_h
#define mem_syms_h

#include "avr/io.h" 

/* these symbols get us to the details of the stack and heap */

// the address of the first byte of the stack, the last byte of onchip SRAM
#define STACK_BOTTOM ( (char*)RAMEND )

// the first free location at the top of the stack
#define STACK_TOP ((char *)AVR_STACK_POINTER_REG)
  
// the amount of stack used.  i.e. from bottom to top-1
#define STACK_SIZE (STACK_BOTTOM - STACK_TOP)

#define AVAIL_MEM (STACK_TOP - HEAP_END)

// the address of the first location of the heap
#define HEAP_START __malloc_heap_start

// the address of the next free location after the end of the heap
// __brkval is 0 until the first malloc
#define HEAP_END (__brkval ? __brkval : __malloc_heap_start) 

// the amount of space taken up by the heap, including free chunks
// inside the heap
#define HEAP_SIZE (HEAP_END - HEAP_START)

extern char*__brkval;

#endif
<file_sep>/client/readme.txt
Arduino Pathfinder Client
CMPUT 297

by <NAME>

This assignment is a solo effort, although discussion took place with
<NAME>, <NAME>
==========================================================================

This map client is designed to be run on the Arduino Mega 2550 with an
Adafruit ST7735-based display.

Use the joystick to move around the map. Click the joystick button to
select paths. Use the two zoom buttons to zoom in and out.

Limitations:
When moving the cursor, you can erase a path if you move the cursor over
it. The path will be redrawn after the window moves.

Any time the window moves, the cursor will jump to the middle of the
window to prevent getting stuck at the map edges or being put in an
unpredictable location when zooming the map.
<file_sep>/client/serial_handling.cpp
#include "serial_handling.h"

#include <Arduino.h>
#include <errno.h>
#include <assert13.h>

uint16_t serial_readline(char *line, uint16_t line_size) {
    int bytes_read = 0;    // Number of bytes read from the serial port.

    // Read until we hit the maximum length, or a newline.
    // One less than the maximum length because we want to add a null terminator.
    while (bytes_read < line_size - 1) {
        while (Serial.available() == 0) {
            // There is no data to be read from the serial port.
            // Wait until data is available.
        }

        line[bytes_read] = (char) Serial.read();

        // A newline is given by \r or \n, or some combination of both
        // or the read may have failed and returned 0
        if ( line[bytes_read] == '\r' || line[bytes_read] == '\n' ||
             line[bytes_read] == 0 ) {
                // We ran into a newline character!  Overwrite it with \0
                break;    // Break out of this - we are done reading a line.
        } else {
            bytes_read++;
        }
    }

    // Add null termination to the end of our string.
    line[bytes_read] = '\0';
    return bytes_read;
}

uint16_t string_read_field(const char *str, uint16_t str_start, 
    char *field, uint16_t field_size, const char *sep) {

    // Want to read from the string until we encounter the separator.

    // Character that we are reading from the string.
    uint16_t str_index = str_start;    

    while (1) {
        if ( str[str_index] == '\0') {
            str_index++;  // signal off end of str
            break;
            }

        if ( field_size <= 1 ) break;

        if (strchr(sep, str[str_index])) {
            // field finished, skip over the separator character.
            str_index++;    
            break;
            }

        // Copy the string character into buffer and move over to next
        *field = str[str_index];    
        field++;
        field_size--;
        // Move on to the next character.
        str_index++;    
        }

    // Make sure to add NULL termination to our new string.
    *field = '\0';

    // Return the index of where the next token begins.
    return str_index;    
}

int32_t string_get_int(const char *str) {
    // Attempt to convert the string to an integer using strtol...
    int32_t val = strtol(str, NULL, 10);

    if (val == 0) {
        // Must check errno for possible error.
        if (errno == ERANGE) {
            Serial.print("string_get_int failed: "); Serial.println(str);
            assert13(0, errno);
        }
    }

    return val;
}
<file_sep>/client/assert13.cpp
#include <Arduino.h>
#include "assert13.h"
// create a busy-loop delay, since timers are off
int hard_loop(unsigned long int n) {
  // this introduction of y and the Serial.print
  // are to mess with gcc's optimization of the loop
  int y = 0;
  while (n > 0) {
    y = y + n;
    n--;
  }
  Serial.print(""); 
  return y;
}

#define DELAY_COUNT 1000000L

/* assertion checker
   assert13(invariant, code);
if invariant is false (i.e. 0) then fail and enter
a hard loop with interrupts disabled and repeatedly
sending code to the serial monitor, while blinking 
the LED on pin 13.

There is a small window in which an interrupt could
occur, and in which a failure could call assert13.
How would we guard against this?
*/

void assert13(int invariant, int code) {
  unsigned long int count;
  if ( invariant ) { return; }
  Serial.println("Assertion failure");
  noInterrupts();
  pinMode(13, OUTPUT);
  while ( 1 ) {
    Serial.println(code);
    digitalWrite(13, LOW);
    // Serial.println("LOW");
    hard_loop( DELAY_COUNT );
    digitalWrite(13, HIGH);
    // Serial.println("HIGH");
    hard_loop( DELAY_COUNT );    
  }
}
<file_sep>/client/joystick.h
#ifndef _JOY_H_
#define _JOY_H_

extern int16_t joy_center_x;
extern int16_t joy_center_y;

void initialize_joystick();
uint8_t process_joystick(int16_t *dx, int16_t *dy);

#endif
<file_sep>/client/ledon.cpp
#include <Arduino.h>

#include "ledon.h"

void map_to_glasses(int heading) {
    
    // Replace these with accessors to a global pin variables.
    int a0 = 22;
    int a1 = 23;
    int a2 = 24;
    //Used to enable certain leds
    int v[] = {0, 0, 0};
    
    //Put these in a global initialization file?
    pinMode(a0, OUTPUT);
    pinMode(a1, OUTPUT);
    pinMode(a2, OUTPUT);
    
    
    int i = (heading/45) % 8; //i is the led number to enable.
    
    //Make i a bitfield 
    v[0] = (i & 0x1);
    v[1] = (i & 0x2);
    v[2] = (i & 0x4);
    if (v[0]) 
        digitalWrite(a0, HIGH);
    else
        digitalWrite(a0, LOW);

    if (v[1])
        digitalWrite(a1, HIGH);
    else
        digitalWrite(a1, LOW);

    if (v[2])
        digitalWrite(a2, HIGH);
    else
        digitalWrite(a2, LOW);    
    
}

 
<file_sep>/client/LSM303.cpp

#include <LSM303.h>
#include <Wire.h>
#include <math.h>

// Defines ////////////////////////////////////////////////////////////////

// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define MAG_ADDRESS            (0x3C >> 1)
#define ACC_ADDRESS_SA0_A_LOW  (0x30 >> 1)
#define ACC_ADDRESS_SA0_A_HIGH (0x32 >> 1)

int past_headings[3] = {0, 0, 0};
int past_heading_i = 0;

// Constructors ////////////////////////////////////////////////////////////////

LSM303::LSM303(void)
{
  // These are just some values for a particular unit; it is recommended that
  // a calibration be done for your particular unit.
    m_max.x = +1; m_max.y = +2; m_max.z = 0;
  m_min.x = -1; m_min.y = -2; m_min.z = -140;

  _device = LSM303_DEVICE_AUTO;
  acc_address = ACC_ADDRESS_SA0_A_LOW;

  io_timeout = 0;  // 0 = no timeout
  did_timeout = false;
}

// Public Methods //////////////////////////////////////////////////////////////

bool LSM303::timeoutOccurred()
{
  return did_timeout;
}

void LSM303::setTimeout(unsigned int timeout)
{
  io_timeout = timeout;
}

unsigned int LSM303::getTimeout()
{
  return io_timeout;
}

void LSM303::init(byte device, byte sa0_a)
{
  _device = device;
  switch (_device)
  {
    case LSM303DLH_DEVICE:
    case LSM303DLM_DEVICE:
      if (sa0_a == LSM303_SA0_A_LOW)
        acc_address = ACC_ADDRESS_SA0_A_LOW;
      else if (sa0_a == LSM303_SA0_A_HIGH)
        acc_address = ACC_ADDRESS_SA0_A_HIGH;
      else
        acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW;
      break;

    case LSM303DLHC_DEVICE:
      acc_address = ACC_ADDRESS_SA0_A_HIGH;
      break;

    default:
      // try to auto-detect device
      if (detectSA0_A() == LSM303_SA0_A_HIGH)
      {
        // if device responds on 0011001b (SA0_A is high), assume DLHC
        acc_address = ACC_ADDRESS_SA0_A_HIGH;
        _device = LSM303DLHC_DEVICE;
      }
      else
      {
        // otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two
        acc_address = ACC_ADDRESS_SA0_A_LOW;
        _device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE;
      }
  }
}

// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void LSM303::enableDefault(void)
{
  // Enable Accelerometer
  // 0x27 = 0b00100111
  // Normal power mode, all axes enabled
  writeAccReg(LSM303_CTRL_REG1_A, 0x27);

  if (_device == LSM303DLHC_DEVICE)
    writeAccReg(LSM303_CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode

  // Enable Magnetometer
  // 0x00 = 0b00000000
  // Continuous conversion mode
  writeMagReg(LSM303_MR_REG_M, 0x00);
  writeMagReg(LSM303_CRA_REG_M, 0x14);  // 0x14 = mag 30Hz output rate
}

// Writes an accelerometer register
void LSM303::writeAccReg(byte reg, byte value)
{
  Wire.beginTransmission(acc_address);
  Wire.write(reg);
  Wire.write(value);
  last_status = Wire.endTransmission();
}

// Reads an accelerometer register
byte LSM303::readAccReg(byte reg)
{
  byte value;

  Wire.beginTransmission(acc_address);
  Wire.write(reg);
  last_status = Wire.endTransmission();
  Wire.requestFrom(acc_address, (byte)1);
  value = Wire.read();
  Wire.endTransmission();

  return value;
}

// Writes a magnetometer register
void LSM303::writeMagReg(byte reg, byte value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  last_status = Wire.endTransmission();
}

// Reads a magnetometer register
byte LSM303::readMagReg(int reg)
{
  byte value;

  // if dummy register address (magnetometer Y/Z), use device type to determine actual address
  if (reg < 0)
  {
    switch (reg)
    {
      case LSM303_OUT_Y_H_M:
        reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M;
        break;
      case LSM303_OUT_Y_L_M:
        reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M;
        break;
      case LSM303_OUT_Z_H_M:
        reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M;
        break;
      case LSM303_OUT_Z_L_M:
        reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M;
        break;
    }
  }

  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  last_status = Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 1);
  value = Wire.read();
  Wire.endTransmission();

  return value;
}

void LSM303::setMagGain(magGain value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303_CRB_REG_M);
  Wire.write((byte) value);
  Wire.endTransmission();
}

// Reads the 3 accelerometer channels and stores them in vector a
void LSM303::readAcc(void)
{
  Wire.beginTransmission(acc_address);
  // assert the MSB of the address to get the accelerometer
  // to do slave-transmit subaddress updating.
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  last_status = Wire.endTransmission();
  Wire.requestFrom(acc_address, (byte)6);

  unsigned int millis_start = millis();
  did_timeout = false;
  while (Wire.available() < 6) {
    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
      did_timeout = true;      return;
    }
  }
  
  /* Old Wire.Reads(). Going to swap all y and z here */
  
  byte xla = Wire.read();
  byte xha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();
   
  
  //New wire reads. Don't really work 
  /*
  byte xla = Wire.read();
  byte xha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  */
  
  // combine high and low bytes, then shift right to discard lowest 4 bits (which are meaningless)
  // GCC performs an arithmetic right shift for signed negative numbers, but this code will not work
  // if you port it to a compiler that does a logical right shift instead.
  a.x = ((int16_t)(xha << 8 | xla)) >> 4;
  a.y = ((int16_t)(yha << 8 | yla)) >> 4;
  a.z = ((int16_t)(zha << 8 | zla)) >> 4;
}

// Reads the 3 magnetometer channels and stores them in vector m
void LSM303::readMag(void)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303_OUT_X_H_M);
  last_status = Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 6);

  unsigned int millis_start = millis();
  did_timeout = false;
  while (Wire.available() < 6) {
    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
      did_timeout = true;
      return;
    }
  }

  uint16_t xhm = Wire.read();
  uint16_t xlm = Wire.read();

  uint16_t yhm, ylm, zhm, zlm;

  if (_device == LSM303DLH_DEVICE)
  {
    // DLH: register address for Y comes before Z
    yhm = Wire.read();
    ylm = Wire.read();
    zhm = Wire.read();
    zlm = Wire.read();
  }
  else
  {
    // DLM, DLHC: register address for Z comes before Y
    zhm = Wire.read();
    zlm = Wire.read();
    yhm = Wire.read();
    ylm = Wire.read();

  }


  // combine high and low bytes
  m.x = (int16_t)(xhm << 8 | xlm);
  m.y = (int16_t)(yhm << 8 | ylm);
  m.z = (int16_t)(zhm << 8 | zlm);

#ifdef DEBUG_COMPASS
  Serial.println(m.x);
  Serial.println(m.y);
  Serial.println(m.z);
  Serial.println();
  delay(1000);
#endif
}

// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303::read(void)
{
  readAcc();
  readMag();
}

// Returns a heading in degrees, compensated for pitch and roll
int LSM303::heading(void)
{
    //Need to normalize the vector so that trig isn't broken
    vector b = a;
    vector_normalize(&b);
    
    float pitch = asin(-b.x);
    float roll = asin(b.y/cos(pitch));

    
    float xh = (m.x+75+30) * cos(pitch) + m.z * sin(pitch);
    float yh = (m.x+105) * sin(roll) * sin(pitch)
        + (m.y-115) * cos(roll) - m.z * sin(roll) * cos(pitch);
    float zh = -(m.x+105) * cos(roll) * sin(pitch) + (m.y-115) * sin(roll)
        + m.z * cos(roll) * cos(pitch);

    int heading = (int)(180 * atan2(yh, xh)/PI) % 360;
    
    // Populate some fields for average heading calculation
    if (!past_headings[0]) past_headings[0] = heading;
    if (!past_headings[1]) past_headings[1] = heading;
    if (!past_headings[2]) past_headings[2] = heading;
    past_headings[past_heading_i] = heading;
    past_heading_i = (past_heading_i + 1) % 3;

    /* Debugging output. */
#ifdef DEBUG_COMPASS
    Serial.print("b.x");
    Serial.print(b.x);
    Serial.print("b.y");
    Serial.print(b.y);
    Serial.println();
    Serial.print("Pitch: ");
    Serial.print(pitch);
    Serial.print("Roll:");
    Serial.print(roll);
    Serial.print("Xh: ");
    Serial.print(xh);
    Serial.print("Yh: ");
    Serial.print(yh);
    Serial.print("Zh: ");
    Serial.print(zh);
    
    Serial.print("Heading: ");
    Serial.println(heading);
#endif

    // Return average of most three recent headings
    return ((int)(past_headings[0] + past_headings[1] + past_headings[2])/3) % 360;
}

void LSM303::vector_cross(const vector *a,const vector *b, vector *out)
{
  out->x = a->y*b->z - a->z*b->y;
  out->y = a->z*b->x - a->x*b->z;
  out->z = a->x*b->y - a->y*b->x;
}

float LSM303::vector_dot(const vector *a,const vector *b)
{
  return a->x*b->x+a->y*b->y+a->z*b->z;
}

void LSM303::vector_normalize(vector *a)
{
  float mag = sqrt(vector_dot(a,a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}

// Private Methods //////////////////////////////////////////////////////////////

byte LSM303::detectSA0_A(void)
{
  Wire.beginTransmission(ACC_ADDRESS_SA0_A_LOW);
  Wire.write(LSM303_CTRL_REG1_A);
  last_status = Wire.endTransmission();
  Wire.requestFrom(ACC_ADDRESS_SA0_A_LOW, 1);
  if (Wire.available())
  {
    Wire.read();
    return LSM303_SA0_A_LOW;
  }
  else
    return LSM303_SA0_A_HIGH;
}
<file_sep>/server/dumb_server.py
import sys
import serial
import argparse

global debug
debug = False

def main():
    args = parse_args()

    # Initialize some stuff...
    if args.serialport:
        print("Opening serial port: %s" % args.serialport)
        serial_out = serial_in =  serial.Serial(args.serialport, 9600)
    else:
        print("No serial port.  Supply one with the -s port option")
        exit()

    if args.verbose:
        debug = True
    else:
        debug = False

    idx = 0
    while True:
        msg = receive(serial_in)

        debug and print("GOT:" + msg + ":", file=sys.stderr)

        fields = msg.split(" ");

        if len(fields) == 4:
            send(serial_out, "2")
            send(serial_out, fields[0]+" "+fields[1])
            send(serial_out, fields[2]+" "+fields[3])
        idx += 1

def send(serial_port, message):
    """
    Sends a message back to the client device.
    """
    full_message = ''.join((message, "\n"))

    (debug and
        print("server:" + full_message + ":") )

    reencoded = bytes(full_message, encoding='ascii')
    serial_port.write(reencoded)


def receive(serial_port, timeout=None):
    """
    Listen for a message. Attempt to timeout after a certain number of
    milliseconds.
    """
    raw_message = serial_port.readline()

    debug and print("client:", raw_message, ":")

    message = raw_message.decode('ascii')

    return message.rstrip("\n\r")



def parse_args():
    """
    Parses arguments for this program.
    Returns an object with the following members:
        args.
             serialport -- str
             verbose    -- bool
             graphname  -- str
    """

    parser = argparse.ArgumentParser(
        description='Assignment 1: Map directions.',
        epilog = 'If SERIALPORT is not specified, stdin/stdout are used.')
    parser.add_argument('-s', '--serial',
                        help='path to serial port',
                        dest='serialport',
                        default=None)
    parser.add_argument('-v', dest='verbose',
                        help='verbose',
                        action='store_true')
    parser.add_argument('-g', '--graph',
                        help='path to graph (DEFAULT = " edmonton-roads-2.0.1.txt")',
                        dest='graphname',
                        default=' edmonton-roads-2.0.1.txt')

    return parser.parse_args()

if __name__ == '__main__':
    main()
<file_sep>/client/GTPA010.h
#ifndef _GTPA010_H_
#define _GTPA010_H_

#include "Sensors.h"
#include "TinyGPS.h"
#include "TimerThree.h"


// GPS Setup
//#define DEBUG_GPS

#define GPS_FIX_PIN 11
#define GPS_ENABLE_PIN 12
#define GPS_BAUD_RATE 4800

#define FAKE_GPS_DATA 1

typedef struct gpsData
{
	long int lat; // Stores the latitude as a long int as specified by the TinyGPS librairy
	long int lon; // Stores the longitude as a long int as specified by the TinyGPS librairy
	unsigned long int age; // Checksum information
	
	// Date/Time information
	int year;
	byte month;
	byte day;
	byte hour;
	byte minute;
	byte second;
	byte hundredths;
};

class GTPA010 : Sensors
{
public:
	static void printData(); // Designed to print the data onto serial
	static void readData(); // Executes the reading procedures from the sensor
	static gpsData* getData(); // Gets the address of the data struct
	
	#if FAKE_GPS_DATA
	static void fakeData(); // Used in testing, designed to replicate GPS data for the purpose of validating other functions, not included in production class, defined in Config
	#endif
	
	static void begin(); // Begin sensor initilization routines
	static bool check(); // A validity check for the sensor, if true, its reporting a valid response, if false, then response is invalid
	
	static void gpsCheck(); // A function called by an inturput service to check GPS lock status

	static volatile bool gpsLock; // The indicator for a valid lock against a 2D/3D lock
private:
	static gpsData data; // Used to store the data in the gpsData struct
	static bool newData; // A private variable determining if the data has been refreshed
	static const long int timer_ticks = 1000000; // The length of ticks inbetween checks of the GPS, dependant on the devices clock speed
	static char dataBuffer; // Char buffer for the inputed data

	static volatile bool gpsValue; // The value read from the GPS 2D/3D fix pin
};

#endif
<file_sep>/client/assert13.h
#ifndef _assert13_h
#define _assert13_h
void assert13(int invariant, int code);
#endif
<file_sep>/client/map.h
/*
 Definition of functions and structures for dealing with the map.
 Longitude and latitude to screen coordinate conversions, for instance.
 */

#ifndef MAP_H
#define MAP_H

#include <stdint.h>
#include <image_handling.h>
#include <SD.h>
#include <SPI.h>

// the number of the current map being displayed
extern uint8_t current_map_num;

// the current association of the map with the screen window
extern uint16_t screen_map_x;
extern uint16_t screen_map_y;

// radius of cursor dot.
extern const uint16_t dot_radius;

// the size of the map display window
extern const uint16_t display_window_width;
extern const uint16_t display_window_height;

// the current position of the cursor on the map
extern uint16_t cursor_map_x;
extern uint16_t cursor_map_y;

// the current approximate lat and lon of the cursor
extern int32_t cursor_lon;
extern int32_t cursor_lat;

extern const uint8_t num_maps;

extern uint16_t map_x_limit[6];
extern uint16_t map_y_limit[6];

typedef struct {
    int32_t N;  // lattitude of NW corner
    int32_t W;  // longitude of NW corner
    int32_t S;  // lattitude of SE corner
    int32_t E;  // longitude of SE corner
} map_box_t;

// a coordinate point for a path
typedef struct {
    int32_t lat;
    int32_t lon;
} coord_t;

extern map_box_t map_box[];

// conversion routines between lat and long and map pixel coordinates
int32_t x_to_longitude(char map_num, int32_t map_x);
int32_t y_to_latitude(char map_num, int32_t map_y);
int32_t longitude_to_x(char map_num, int32_t map_longitude);
int32_t latitude_to_y(char map_num, int32_t map_lattitude);

uint8_t zoom_in();
uint8_t zoom_out();
uint8_t set_zoom();

uint8_t is_gps_visible();

void initialize_map();
void draw_map_screen();
uint8_t get_cursor_screen_x_y(uint16_t *cursor_screen_x,uint16_t *cursor_screen_y);
void draw_cursor();
void erase_cursor();

void draw_gps_dot();
void erase_gps();

void draw_compass();
void draw_gps_dot();
void move_window_to(int16_t x, int16_t y);
void move_window(int32_t lon, int32_t lat);
void move_cursor_to(int16_t x, int16_t y);
void move_cursor_by(int16_t dx, int16_t dy);

extern volatile uint8_t shared_new_map_num;

#endif
<file_sep>/client/path.h
#ifndef PATH_H
#define PATH_H

extern int16_t path_errno;
extern int16_t target_dir;
extern int8_t has_path;

uint8_t read_path(uint16_t *length_p, coord_t *path_p[]);
void draw_path(uint16_t length, coord_t path[]);
coord_t * get_prev_destination();
uint8_t is_coord_visible(coord_t point);

#endif
<file_sep>/client/path.cpp
#include <Arduino.h>
#include <Adafruit_ST7735.h>
#include <SD.h>
#include <mem_syms.h>
#include <math.h>

#include "map.h"
#include "serial_handling.h"
#include "ledon.h"
#include "LSM303.h"

// #define DEBUG_PATH

/* path routine error code
   0 no error
   1 
*/
int16_t path_errno;

int16_t target_dir = 0;
int8_t has_path = 0;

extern Adafruit_ST7735 tft;

// Keep track of the last requested path
uint16_t * last_path_len = 0;
coord_t ** last_path_p = 0;

// read a path from the serial port and return the length of the
// path and a pointer to the array of coordinates.  That array should
// be freed later.

// Returns 1 if the call was successful, 0 if not.

uint8_t read_path(uint16_t *length_p, coord_t *path_p[]) {
    // the line to be read, and it size
    const uint8_t line_size = 40;
    char line[line_size];
    uint16_t bytes_read;

    // the field extracted
    const uint8_t field_size = 20;
    char field[field_size];
    uint16_t field_index;
    int32_t field_value;

    *length_p = 0;
    *path_p = 0;

    // reset the error code
    path_errno = 0;

    while ( ! Serial.available() )  { };

    uint16_t max_path_size = (AVAIL_MEM - 256) / sizeof(coord_t);

    #ifdef DEBUG_PATH
        Serial.print("Max path length ");
        Serial.println(max_path_size);
    #endif

    bytes_read = serial_readline(line, line_size);

    // read the number of points, first field
    field_index = 0;   
    field_index = 
        string_read_field(line, field_index, field, field_size, " ");
    field_value = string_get_int(field);

    #ifdef DEBUG_PATH
        Serial.print("Path length ");
        Serial.print(field_value);
        Serial.println();
    #endif

    // do a consistency check
    if ( field_value < 0  || max_path_size < field_value ) {
        path_errno = 1;
        return 0;
        }
    uint8_t tmp_length = field_value;
    *length_p = tmp_length;

    // allocate the storage, see if we got it.
    coord_t *tmp_path = (coord_t *) malloc( tmp_length * sizeof(coord_t));
    if ( !tmp_path ) { 
        path_errno = 2;
        has_path = 0;
        return 0; 
        }

    *path_p = tmp_path;

    while ( tmp_length > 0 ) {
        bytes_read = serial_readline(line, line_size);

        // read the number of points, first field
        field_index = 0;   

        field_index = 
            string_read_field(line, field_index, field, field_size, " ");
        tmp_path->lat = string_get_int(field);

        field_index = 
            string_read_field(line, field_index, field, field_size, " ");
        tmp_path->lon = string_get_int(field);

        tmp_length--;
        tmp_path++;
        }

    // Give direction to follow from first to second node
    if (*length_p >= 2) {
        coord_t s_coord = (*path_p)[0];
        coord_t e_coord = (*path_p)[1];
        target_dir = (int)(atan2(e_coord.lon-s_coord.lon,
                                 e_coord.lat-s_coord.lat)*180/PI - 180) % 360;
#ifdef DEBUG_GLASSES
        map_to_glasses(target_dir);
#endif
    }
    last_path_p = path_p;
    last_path_len = length_p;

    has_path = 1;
    return 1;
    }

uint8_t is_coord_visible(coord_t point) {
    // figure out the x and y positions on the current map of the 
    // given point
    uint16_t point_map_y = latitude_to_y(current_map_num, point.lat);
    uint16_t point_map_x = longitude_to_x(current_map_num, point.lon);

    uint8_t r = 
        screen_map_x < point_map_x &&
        point_map_x < screen_map_x + display_window_width &&
        screen_map_y < point_map_y &&
        point_map_y < screen_map_y + display_window_height; 

    return r;
    }

coord_t * get_prev_destination() {
    if (!last_path_len || !*last_path_len)
        return 0;

    return &(*last_path_p[*last_path_len-1]);
}

void draw_path(uint16_t length, coord_t path[]) {
#ifdef DEBUG_PATH
    Serial.println("Drawing path!");
#endif

    for (int i = 0; i < (length - 1); i++) {
#ifdef DEBUG_PATH
        Serial.println("Drawing line");
#endif
        
        int16_t startx = longitude_to_x(current_map_num, path[i].lon)
            - screen_map_x;
        int16_t starty = latitude_to_y(current_map_num, path[i].lat)
            - screen_map_y;
        int16_t endx = longitude_to_x(current_map_num, path[i+1].lon)
            - screen_map_x;
        int16_t endy = latitude_to_y(current_map_num, path[i+1].lat)
            - screen_map_y;
        
        tft.drawLine(startx, starty, endx, endy, BLUE);
    }
}
<file_sep>/server/testserver.py
import doctest
import server

doctest.testmod(server)
<file_sep>/client/ledon.h
#ifndef LEDON_H
#define LEDON_H

#undef DEBUG_GLASSES

void map_to_glasses(int heading);


#endif
<file_sep>/client/map.cpp
#include <Arduino.h>
#include <Adafruit_GFX.h>        // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library
#include <math.h>
#include "lcd_image.h"
#include "map.h"
#include "LSM303.h"
#include "GTPA010.h"
#include "ledon.h"
#include "path.h"
// #define DEBUG

/*
    Module to handle the display of map, cursor, and path information
    on the screen.

    It assumes that all of the map information has been placed on the
    SD card as .lcd 16 bit rgb files, and that the tile dimensions
    and lat-long positions match that of the data structures below.

    All coordinates are in usual graphics corrdinates, that is, the
    top left is (0,0) and the bottom right is (SCREEN_WIDTH-1,SCREEN_HEIGHT-1)

    At any given time, the map image shown on the display is a window onto
    a bigger map tile.  The top-left position of the display corrsponds
    to position (screen_map_x, screen_map_y) in the map tile.

    The cursor can be anywhere on the map, so it may not be visible in the
    current display window.  It is visible if
        screen_map_x < 
*/

extern Adafruit_ST7735 tft;
extern LSM303 compass;

// the number of the current map being displayed
uint8_t current_map_num;

// the size of the map display window
const uint16_t display_window_width = 128;
const uint16_t display_window_height = 160;

// the current association of the map with the display window
uint16_t screen_map_x;
uint16_t screen_map_y;

// radius of cursor dot.
const uint16_t dot_radius = 2;

// the current position of the cursor on the map
uint16_t cursor_map_x;
uint16_t cursor_map_y;

// the current approximate lat and lon of the cursor
int32_t cursor_lon;
int32_t cursor_lat;

// the current position of the gps dot on map
uint16_t gps_map_y;
uint16_t gps_map_x;

// the approximate lat and lon of gps dot
int32_t gps_lon = 0;
int32_t gps_lat = 0;

const uint8_t num_maps = 6;

/* 
    Map limits: a pixel position on map i must lie in the interval
        0 <= x <= map_x_limit[i]
        0 <= y <= map_y_limit[i]
*/

uint16_t map_x_limit[6] = { 511, 1023, 2047, 4095, 8191, 16383};
uint16_t map_y_limit[6] = { 511, 1023, 2047, 4095, 8191, 16383};

lcd_image_t map_tiles[] = {
    { "yeg-1.lcd",  512, 512, },
    { "yeg-2.lcd",  1024, 1024, },
    { "yeg-3.lcd",  2048, 2048, },
    { "yeg-4.lcd",  4096, 4096, },
    { "yeg-5.lcd",  8192, 8192, },
    { "yeg-6.lcd",  16384, 16384, },
    };

map_box_t map_box[] = {
    { // map 0 zoom 11
        (int32_t)     5364463,    //   53.6446378248565 
        (int32_t)   -11373047,    // -113.73046875 
        (int32_t)     5343572,    //   53.4357192066942
        (int32_t)   -11337891,    // -113.37890625
    },
    { // map 1 zoom 12
        (int32_t)     5364464,    //   53.6446378248565 
        (int32_t)   -11373047,    // -113.73046875
        (int32_t)     5343572,    //   53.4357192066942
        (int32_t)   -11337891,    // -113.37890625
    },
    { // map 2 zoom 13
        (int32_t)     5361858,    //   53.6185793648952
        (int32_t)   -11368652,    // -113.6865234375
        (int32_t)     5340953,    //   53.4095318530864 S
        (int32_t)   -11333496,    // -113.3349609375 E
    },
    { // map 3 zoom 14
        (int32_t)     5360554,    //   53.605544099238
        (int32_t)   -11368652,    // -113.6865234375
        (int32_t)     5339643,    //   53.396432127096
        (int32_t)   -11333496,    // -113.3349609375 
    },
    { // map 4 zoom 15
        (int32_t)     5360554,    //   53.605544099238
        (int32_t)   -11367554,    // -113.675537109375
        (int32_t)     5339643,    //   53.396432127096
        (int32_t)   -11332397,    // -113.323974609375
    },
    { // map 5 zoom 16
        (int32_t)     5360228,    //   53.6022846540113
        (int32_t)   -11367554,    // -113.675537109375
        (int32_t)     5339316,    //   53.3931565653804
        (int32_t)   -11332397,    // -113.323974609375
    },
};

int compass_x = 20;
int compass_y = 20;
int compass_r = 8;

int compass_old = 0;
int compass_drawn = 0;

// conversion routines between lat and long and map pixel coordinates
int32_t x_to_longitude(char map_num, int32_t map_x) {
    return map(map_x, 
                 0, map_x_limit[map_num], 
                 map_box[map_num].W, map_box[map_num].E);
}

int32_t y_to_latitude(char map_num, int32_t map_y) {
    return map(map_y, 
                 0, map_y_limit[map_num], 
                 map_box[map_num].N, map_box[map_num].S);
}

int32_t longitude_to_x(char map_num, int32_t map_longitude) {
    return map(map_longitude, 
                 map_box[map_num].W, map_box[map_num].E,
                 0, map_x_limit[map_num]);
}

int32_t latitude_to_y(char map_num, int32_t map_latitude) {
    return map(map_latitude, 
                 map_box[map_num].N, map_box[map_num].S,
                 0, map_y_limit[map_num]);
}

// zoom in and out routines that are used in interrupt handlers to
// change the shared version of the map_num.  In order to maintain consistent
// updates, the shared value should only be sampled at well-defined points.

volatile uint8_t shared_new_map_num = 2;

void initialize_map() {
    cursor_lon = 0;
    cursor_lat = 0;
    cursor_map_x = 0;
    cursor_map_y = 0;
    screen_map_x = 0;
    screen_map_y = 0;

    // set the actual map number from the shared version
    // this should be atomic and thus can be done outside a critical section
    current_map_num = shared_new_map_num;
    }

uint8_t zoom_in() {
    if ( shared_new_map_num < num_maps-1) { 
        shared_new_map_num++; 
    }
    return shared_new_map_num;
}

uint8_t zoom_out() {
    if ( shared_new_map_num > 0 ) { 
        shared_new_map_num--; 
    }
    return shared_new_map_num;
}

uint8_t set_zoom() {
    // set the actual map number from the shared version
    // this should be atomic and thus can be done outside a critical section
    current_map_num = shared_new_map_num;

    // At this point all of the cursor and map information is invalidated
    // except for the cursor lat and long, which is the only thing that
    // survives zooming.  So set the map cursor position on the new map to
    // roughly correspond to lat and lon position that was determined by the
    // previous map.

    cursor_map_x = longitude_to_x(current_map_num, cursor_lon);
    cursor_map_y = latitude_to_y(current_map_num, cursor_lat);

    return current_map_num;
    }
        

void draw_compass() {
  compass.read();
  int compass_dir = compass.heading() + 90;

  // Avoid updating 
  if (abs(compass_dir - compass_old) < 5 && compass_drawn == 1)
      return;
  compass_old = compass_dir;
  compass_drawn = 1;

  tft.fillCircle(compass_x, compass_y, compass_r, BLUE);

  int tip_x = compass_x + compass_r * cos(compass_dir*PI/180);
  int tail_x = compass_x - compass_r * cos(compass_dir*PI/180);
  int tip_y = compass_y + compass_r * sin(compass_dir*PI/180);
  int tail_y = compass_y - compass_r * sin(compass_dir*PI/180);

  int a1_x = compass_x + (compass_r - 5) * cos((compass_dir-90)*PI/180);
  int a1_y = compass_y + (compass_r - 5) * sin((compass_dir-90)*PI/180);

  int a2_x = compass_x + (compass_r - 5) * cos((compass_dir+90)*PI/180);
  int a2_y = compass_y + (compass_r - 5) * sin((compass_dir+90)*PI/180);

  // Arrow body
  tft.drawLine(tip_x, tip_y, tail_x, tail_y, RED);
  // Arrow head
  tft.drawLine(tip_x, tip_y, a1_x, a1_y, RED);
  tft.drawLine(tip_x, tip_y, a2_x, a2_y, RED);
}


/**
 * Draw the GPS location on the map
 *
 * Display a warning message if a GPS error occurs
 */
void draw_gps_dot() {
    
    //Get the GPS data
    gpsData* gdata;
    GTPA010::readData();
    gdata = GTPA010::getData();

    // Erase old dot if position changed
    if ((gdata->lat != gps_lat || gdata->lon != gps_lon) && is_gps_visible()) {
        erase_gps();
    }

    if (gdata->lat != gps_lat || gdata->lon != gps_lon) {
        coord_t * dest = get_prev_destination();
    }

    gps_lat = gdata->lat;
    gps_lon = gdata->lon;
    
    //Take the lat, lon, map to pixels
    gps_map_x = longitude_to_x(current_map_num, gps_lon);
    gps_map_y = latitude_to_y(current_map_num, gps_lat);

#ifdef DEBUG_GPS
    GTPA010::printData();
#endif

    if (is_gps_visible())
        tft.fillCircle(gps_map_x - screen_map_x,
                       gps_map_y - screen_map_y, dot_radius, BLUE);

}


void draw_map_screen() {
    #ifdef DEBUG
        // Want to display a small message saying that we are redrawing the map!
        tft.fillRect(78, 148, 50, 12, GREEN);

        tft.setTextSize(1);
        tft.setTextColor(MAGENTA);
        tft.setCursor(80, 150);
        tft.setTextSize(1);

        tft.println("DRAWING...");
    #endif

    lcd_image_draw(&map_tiles[current_map_num], &tft,
                    screen_map_x, screen_map_y,
                    0, 0, 128, 160);

    compass_drawn = 0;
    
}

uint8_t is_gps_visible() {
    uint8_t r = 
        screen_map_x < gps_map_x &&
        gps_map_x < screen_map_x + display_window_width &&
        screen_map_y < gps_map_y &&
        gps_map_y < screen_map_y + display_window_height; 

    return r;
    }

uint8_t is_cursor_visible() {
    uint8_t r = 
        screen_map_x < cursor_map_x &&
        cursor_map_x < screen_map_x + display_window_width &&
        screen_map_y < cursor_map_y &&
        cursor_map_y < screen_map_y + display_window_height; 

#ifdef IGNORE
    if ( !r ) {
        Serial.print("cv: ");
        Serial.print(screen_map_x);
        Serial.print(" ");
        Serial.print(screen_map_y);
        Serial.print(" ");
        Serial.print(cursor_map_x);
        Serial.print(" ");
        Serial.print(cursor_map_y);
        Serial.println();
        }
#endif

    return r;
    }

uint8_t get_gps_screen_x_y(
    uint16_t *gps_screen_x,uint16_t *gps_screen_y) {
    if ( is_gps_visible ) {
        *gps_screen_x = gps_map_x - display_window_width;
        *gps_screen_y = gps_map_y - display_window_height;
        return 1;
        }

    // not visible
    return 0;
    }

uint8_t get_cursor_screen_x_y(
    uint16_t *cursor_screen_x,uint16_t *cursor_screen_y) {
    if ( is_cursor_visible ) {
        *cursor_screen_x = cursor_map_x - screen_map_x;
        *cursor_screen_y = cursor_map_y - screen_map_y;
        return 1;
        }

    // not visible
    return 0;
    }

void draw_cursor() {
    // the current position of the cursor on the screen, if visible
    uint16_t cursor_screen_x;
    uint16_t cursor_screen_y;
    if ( get_cursor_screen_x_y(&cursor_screen_x, &cursor_screen_y) ) {
        cursor_screen_x = cursor_map_x - screen_map_x;
        cursor_screen_y = cursor_map_y - screen_map_y;
        tft.fillCircle(cursor_screen_x, cursor_screen_y, dot_radius, RED);
        }
    }

void erase_gps() {
    uint16_t gps_screen_x;
    uint16_t gps_screen_y;
    if ( get_gps_screen_x_y(&gps_screen_x, &gps_screen_y) ) {
        // Redraw the map on top of the current cursor position
        lcd_image_draw(&map_tiles[current_map_num], &tft,
            gps_map_x - dot_radius,
            gps_map_y - dot_radius,
            gps_screen_x - dot_radius,
            gps_screen_y - dot_radius,
            2 * dot_radius + 1,
            2 * dot_radius + 1);
        }
    }

void erase_cursor() {
    uint16_t cursor_screen_x;
    uint16_t cursor_screen_y;
    if ( get_cursor_screen_x_y(&cursor_screen_x, &cursor_screen_y) ) {
        // Redraw the map on top of the current cursor position
        lcd_image_draw(&map_tiles[current_map_num], &tft,
            cursor_map_x - dot_radius,
            cursor_map_y - dot_radius,
            cursor_screen_x - dot_radius,
            cursor_screen_y - dot_radius,
            2 * dot_radius + 1,
            2 * dot_radius + 1);
        }
    }

void move_to_gps() {
    gpsData * gData = GTPA010::getData();
    move_window(gData->lon - display_window_width/2,
                gData->lat - display_window_height/2);
}

void move_window(int32_t lon, int32_t lat) {
    // Shift the current window to have lon and lat in top left corner

    #ifdef DEBUG
    Serial.println("move_window!");
    #endif

    screen_map_x = longitude_to_x(current_map_num, lon);
    screen_map_y = latitude_to_y(current_map_num, lat);

    move_window_to(screen_map_x, screen_map_y);
    }

void move_window_to(int16_t x, int16_t y) {
    // Shift the current window to have x y in top left corner
    // cursor does not move

    #ifdef DEBUG
    Serial.println("move_window_by!");
    #endif

    // constrain move
    screen_map_x = constrain(x, 0, 
        map_x_limit[current_map_num] - display_window_width);
    screen_map_y = constrain(y, 0, 
        map_y_limit[current_map_num] - display_window_height);

    // Force cursor to center of window
    move_cursor_to(screen_map_x + display_window_width/2,
		   screen_map_y + display_window_height/2);
}

void move_cursor_by(int16_t dx, int16_t dy) {
    move_cursor_to(cursor_map_x + dx, cursor_map_y + dy);
    }

void move_cursor_to(int16_t x, int16_t y) {
    #ifdef DEBUG
    Serial.print("Moving cursor to: ");
    Serial.print(x);
    Serial.print(" ");
    Serial.println(y);
    #endif

    // move cursor on map
    cursor_map_x = constrain(x, 0, map_x_limit[current_map_num]);

    cursor_map_y = constrain(y, 0, map_y_limit[current_map_num]);

    // then update cursor lat and long - we need this because that is the
    // only thing that will survive zooming.

    cursor_lon = x_to_longitude(current_map_num, cursor_map_x);
    cursor_lat = y_to_latitude(current_map_num, cursor_map_y);
}

