Debounced Car Blinkers

/*
 Debounced car circuit test

 The circuit:
 * LED attached from pin 3 to ground
 * reed switch attached from pin 7 to ground
 * LED attached from pin 4 to ground
 * reed switch attached from pin 8 to ground
 * speaker attached from pin 11 to ground
*/

// constants won't change. They're used here to
// set pin numbers:
const int     leftBlinker  = 3;   // left blinker pin
const int     rightBlinker = 4;   // right blinker pin
const int     leftSensor   = 7;   // left sensor pin
const int     rightSensor  = 8;   // right sensor pin
const int     speakerPin   = 11;  // left blinker pin
// set other constants
const long    timerLength      = 0;  // delay before blinker/alarm in seconds
const long    blinkTimerLength = 2;  // delay before blinker/alarm in seconds
const int     theTone          = 440; // tone in hertz
const double  theDuration      = .25;  //duration in seconds


// Variables will change:
int ledState = LOW;               // the current state of the output pin
int leftSensorState;              // the current reading from the input pin
int lastLeftSensorState = HIGH;   // the previous reading from the input pin
int rightSensorState;              // the current reading from the input pin
int lastRightSensorState = HIGH;   // the previous reading from the input pin
//boolean flip = true;

int toneDelay = 0;
int toneDuration = 0;

// the following variables are long's because the time, measured in miliseconds,
// will quickly become a bigger number than can be stored in an int.
long lastDebounceTimeLeft  = 0;  // the last time the output pin was toggled
long lastDebounceTimeRight = 0;  // the last time the output pin was toggled
long debounceDelay = 50;    // the debounce time; increase if the output flickers

int     lastMode      = 1;
int     lastModeLeft  = LOW;
int     lastModeRight = LOW;
int     lastModeTemp  = 0;
boolean timer         = false;
boolean blinkTimer    = false;
long    startTime     = 0;    // when mode last changed
long    blinkStartTime= 0;    // when mode last changed


void changeState(int mode)
{
  // case 1 / default -         | : :- -: : |     <- Left LOW , Right LOW
  // case 2           -        :|:- -: :    |     <- Left HIGH, Right LOW
  // case 3           -     : :-|-: :       |     <- Left HIGH, Right HIGH
  // case 4           -         |    : :- -:|:    <- Left LOW , Right HIGH
  boolean speaker = false;

  switch(mode)
  {
    case 2:
      // case 2           -        :|:- -: :    |     <- Left HIGH, Right LOW
      //                  -   if coming from state 1, turn on left blinker
      //                  -   if coming from state 3, maintain state
      if (mode != lastMode)
      {
        if(lastMode == 1)
        {
          startTime    = millis();
          timer        = true;
          speaker      = false;
          lastModeLeft = HIGH;
          lastModeRight= LOW;
        }
        lastModeTemp = lastMode;
        lastMode     = mode;
      }
      if ((millis() - startTime) > (long)(timerLength * 1000))
      {
        digitalWrite(leftBlinker , lastModeLeft);
        digitalWrite(rightBlinker, lastModeRight);
        speaker = true;
      }

      break;
    case 3:
      // case 3           -     : :-|-: :       |     <- Left HIGH, Right HIGH
      //                  -   if coming from state 2, maintain state
      //                  -   if coming from state 4, maintain state
      /*
      if (mode != lastMode)
      {
        if(lastMode == 1)
        {
          startTime = millis();
          timer = true;
          speaker = false;
          lastModeLeft = HIGH;
          lastModeRight= LOW;
        }
        lastModeTemp = lastMode;
        lastMode     = mode;
      }
      */
      if ((millis() - startTime) > (timerLength * 1000))
      {
        digitalWrite(leftBlinker , lastModeLeft);
        digitalWrite(rightBlinker, lastModeRight);
        speaker = true;
      }

      break;
    case 4:
      // case 4           -         |    : :- -:|:    <- Left LOW , Right HIGH
      //                  -   if coming from state 1, turn on right blinker
      //                  -   if coming from state 3, maintain state
      if (mode != lastMode)
      {
        if(lastMode == 1)
        {
          startTime    = millis();
          timer        = true;
          speaker      = false;
          lastModeLeft = LOW;
          lastModeRight= HIGH;
        }
        lastModeTemp = lastMode;
        lastMode     = mode;
      }
      if ((millis() - startTime) > (timerLength * 1000))
      {
        digitalWrite(leftBlinker , lastModeLeft);
        digitalWrite(rightBlinker, lastModeRight);
        speaker = true;
      }
      break;
    default:
      // case 1 / default -         | : :- -: : |     <- Left LOW , Right LOW
      // both blinkers off (rest state)
      if (mode != lastMode)
      {
        lastMode = 1;
        timer    = false;
        blinkTimer = true;
        blinkStartTime = millis();
      }
  }
  if (blinkTimer && ((millis() - blinkStartTime) > (blinkTimerLength * 1000)))
  {
      for (int i=0; i < (2*(1000000/(4*toneDelay))); i++)
      {
        digitalWrite(speakerPin,HIGH);
        delayMicroseconds(toneDelay);
        digitalWrite(speakerPin, LOW);
        delayMicroseconds(toneDelay);
      }

      digitalWrite(rightBlinker, LOW);
      digitalWrite(leftBlinker , LOW);
      speaker  = false;
      blinkTimer = false;
  }

  //Serial.println("speaker: ");

  if(speaker)
  {
    //Serial.println("true");
    digitalWrite(speakerPin, LOW);
    delayMicroseconds(toneDelay);
    for (int i=0; i < toneDuration; i++)
    {
    //  if (flip)
        digitalWrite(speakerPin,HIGH);
      delayMicroseconds(toneDelay);
      digitalWrite(speakerPin, LOW);
      delayMicroseconds(toneDelay);
    }
    digitalWrite(speakerPin,HIGH);
    //flip = !flip;
  }
  else
  {
    //Serial.println("false");
  }
}

int getMode (int leftState, int rightState)
{
  // case 1 / default -         | : :- -: : |     <- Left LOW , Right LOW
  // case 2           -        :|:- -: :    |     <- Left HIGH, Right LOW
  // case 3           -     : :-|-: :       |     <- Left HIGH, Right HIGH
  // case 4           -         |    : :- -:|:    <- Left LOW , Right HIGH
  //Serial.println("left:");
  //Serial.println(leftState);
  //Serial.println("right:");
  //Serial.println(rightState);


  if(leftState == HIGH)
  {
    if(rightState == HIGH)
      return 1;
    else
      return 4;
  }
  else
  {
    if(rightState == HIGH)
      return 2;
    else
      return 3;
  }
}

void setup() {
  pinMode(leftSensor, INPUT);
  digitalWrite(leftSensor, HIGH); //activate pullup
  pinMode(leftBlinker, OUTPUT);
  pinMode(rightSensor, INPUT);
  digitalWrite(rightSensor, HIGH); //activate pullup
  pinMode(rightBlinker, OUTPUT);
  pinMode(speakerPin, OUTPUT);
  //Serial.begin(9600);

  toneDelay    = 1000000 / theTone / 2;
  toneDuration = theDuration * theTone;
}

void loop() {
  // read the state of the switch into a local variable:
  int leftReading  = digitalRead(leftSensor);
  int rightReading = digitalRead(rightSensor);

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH),  and you've waited
  // long enough since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (leftReading != lastLeftSensorState) {
    // reset the debouncing timer
    lastDebounceTimeLeft = millis();
  }
  if (rightReading != lastRightSensorState) {
    // reset the debouncing timer
    lastDebounceTimeRight = millis();
  }

  if ((millis() - lastDebounceTimeLeft) > debounceDelay) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:
    leftSensorState = leftReading;
  }
  if ((millis() - lastDebounceTimeRight) > debounceDelay) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:
    rightSensorState = rightReading;
  }
  //Serial.println(" Left Reading: ");
  //Serial.println(leftReading);
  //Serial.println(" Right Reading: ");
  //Serial.println(rightReading);
  int state=getMode(leftSensorState, rightSensorState);
  //Serial.println(" State: ");
  //Serial.println(state);
  changeState(state);

  lastLeftSensorState = leftReading;
  lastRightSensorState = rightReading;
}