If you ever run out of space, you may get a little more flash by uploading via SPI or another arduino, this will grant you another ~2k of flash. If you are still pushing for more flash, then under worst case scenarios you can always upgrade to an ATmega1284P with will have more flash then you know what to do with (although I am not sure if your timer functions will work or not).
Another thing you may want to try is removing all occurances of digitalWrite, digitalRead, and pinMode and replacing them with "True C" equivalents. It saves not only time, but some flash.
Extra: http://forum.arduino.cc/index.php?topic=91079.0
// --- PIN MODE: OUTPUT ---
DDRD |= _BV (0); // pinMode (0, OUTPUT);
DDRD |= _BV (1); // pinMode (1, OUTPUT);
DDRD |= _BV (2); // pinMode (2, OUTPUT);
DDRD |= _BV (3); // pinMode (3, OUTPUT);
DDRD |= _BV (4); // pinMode (4, OUTPUT);
DDRD |= _BV (5); // pinMode (5, OUTPUT);
DDRD |= _BV (6); // pinMode (6, OUTPUT);
DDRD |= _BV (7); // pinMode (7, OUTPUT);
DDRB |= _BV (0); // pinMode (8, OUTPUT);
DDRB |= _BV (1); // pinMode (9, OUTPUT);
DDRB |= _BV (2); // pinMode (10, OUTPUT);
DDRB |= _BV (3); // pinMode (11, OUTPUT);
DDRB |= _BV (4); // pinMode (12, OUTPUT);
DDRB |= _BV (5); // pinMode (13, OUTPUT);
DDRC |= _BV (0); // pinMode (A0, OUTPUT);
DDRC |= _BV (1); // pinMode (A1, OUTPUT);
DDRC |= _BV (2); // pinMode (A2, OUTPUT);
DDRC |= _BV (3); // pinMode (A3, OUTPUT);
DDRC |= _BV (4); // pinMode (A4, OUTPUT);
DDRC |= _BV (5); // pinMode (A5, OUTPUT);
// --- PIN MODE: INPUT ---
DDRD &= ~_BV (0); // pinMode (0, INPUT);
DDRD &= ~_BV (1); // pinMode (1, INPUT);
DDRD &= ~_BV (2); // pinMode (2, INPUT);
DDRD &= ~_BV (3); // pinMode (3, INPUT);
DDRD &= ~_BV (4); // pinMode (4, INPUT);
DDRD &= ~_BV (5); // pinMode (5, INPUT);
DDRD &= ~_BV (6); // pinMode (6, INPUT);
DDRD &= ~_BV (7); // pinMode (7, INPUT);
DDRB &= ~_BV (0); // pinMode (8, INPUT);
DDRB &= ~_BV (1); // pinMode (9, INPUT);
DDRB &= ~_BV (2); // pinMode (10, INPUT);
DDRB &= ~_BV (3); // pinMode (11, INPUT);
DDRB &= ~_BV (4); // pinMode (12, INPUT);
DDRB &= ~_BV (5); // pinMode (13, INPUT);
DDRC &= ~_BV (0); // pinMode (A0, INPUT);
DDRC &= ~_BV (1); // pinMode (A1, INPUT);
DDRC &= ~_BV (2); // pinMode (A2, INPUT);
DDRC &= ~_BV (3); // pinMode (A3, INPUT);
DDRC &= ~_BV (4); // pinMode (A4, INPUT);
DDRC &= ~_BV (5); // pinMode (A5, INPUT);
// --- DIGITAL WRITE: HIGH ---
PORTD |= _BV (0); // digitalWrite (0, HIGH);
PORTD |= _BV (1); // digitalWrite (1, HIGH);
PORTD |= _BV (2); // digitalWrite (2, HIGH);
PORTD |= _BV (3); // digitalWrite (3, HIGH);
PORTD |= _BV (4); // digitalWrite (4, HIGH);
PORTD |= _BV (5); // digitalWrite (5, HIGH);
PORTD |= _BV (6); // digitalWrite (6, HIGH);
PORTD |= _BV (7); // digitalWrite (7, HIGH);
PORTB |= _BV (0); // digitalWrite (8, HIGH);
PORTB |= _BV (1); // digitalWrite (9, HIGH);
PORTB |= _BV (2); // digitalWrite (10, HIGH);
PORTB |= _BV (3); // digitalWrite (11, HIGH);
PORTB |= _BV (4); // digitalWrite (12, HIGH);
PORTB |= _BV (5); // digitalWrite (13, HIGH);
PORTC |= _BV (0); // digitalWrite (A0, HIGH);
PORTC |= _BV (1); // digitalWrite (A1, HIGH);
PORTC |= _BV (2); // digitalWrite (A2, HIGH);
PORTC |= _BV (3); // digitalWrite (A3, HIGH);
PORTC |= _BV (4); // digitalWrite (A4, HIGH);
PORTC |= _BV (5); // digitalWrite (A5, HIGH);
// --- DIGITAL WRITE: LOW ---
PORTD &= ~_BV (0); // digitalWrite (0, LOW);
PORTD &= ~_BV (1); // digitalWrite (1, LOW);
PORTD &= ~_BV (2); // digitalWrite (2, LOW);
PORTD &= ~_BV (3); // digitalWrite (3, LOW);
PORTD &= ~_BV (4); // digitalWrite (4, LOW);
PORTD &= ~_BV (5); // digitalWrite (5, LOW);
PORTD &= ~_BV (6); // digitalWrite (6, LOW);
PORTD &= ~_BV (7); // digitalWrite (7, LOW);
PORTB &= ~_BV (0); // digitalWrite (8, LOW);
PORTB &= ~_BV (1); // digitalWrite (9, LOW);
PORTB &= ~_BV (2); // digitalWrite (10, LOW);
PORTB &= ~_BV (3); // digitalWrite (11, LOW);
PORTB &= ~_BV (4); // digitalWrite (12, LOW);
PORTB &= ~_BV (5); // digitalWrite (13, LOW);
PORTC &= ~_BV (0); // digitalWrite (A0, LOW);
PORTC &= ~_BV (1); // digitalWrite (A1, LOW);
PORTC &= ~_BV (2); // digitalWrite (A2, LOW);
PORTC &= ~_BV (3); // digitalWrite (A3, LOW);
PORTC &= ~_BV (4); // digitalWrite (A4, LOW);
PORTC &= ~_BV (5); // digitalWrite (A5, LOW);
// --- DIGITAL READ ---
x = (PIND & _BV (0)) == 0; // digitalRead (0);
x = (PIND & _BV (1)) == 0; // digitalRead (1);
x = (PIND & _BV (2)) == 0; // digitalRead (2);
x = (PIND & _BV (3)) == 0; // digitalRead (3);
x = (PIND & _BV (4)) == 0; // digitalRead (4);
x = (PIND & _BV (5)) == 0; // digitalRead (5);
x = (PIND & _BV (6)) == 0; // digitalRead (6);
x = (PIND & _BV (7)) == 0; // digitalRead (7);
x = (PINB & _BV (0)) == 0; // digitalRead (8);
x = (PINB & _BV (1)) == 0; // digitalRead (9);
x = (PINB & _BV (2)) == 0; // digitalRead (10);
x = (PINB & _BV (3)) == 0; // digitalRead (11);
x = (PINB & _BV (4)) == 0; // digitalRead (12);
x = (PINB & _BV (5)) == 0; // digitalRead (13);
x = (PINC & _BV (0)) == 0; // digitalRead (A0);
x = (PINC & _BV (1)) == 0; // digitalRead (A1);
x = (PINC & _BV (2)) == 0; // digitalRead (A2);
x = (PINC & _BV (3)) == 0; // digitalRead (A3);
x = (PINC & _BV (4)) == 0; // digitalRead (A4);
x = (PINC & _BV (5)) == 0; // digitalRead (A5);