   noteStates[note] = detected;
 }
 
-void checkNotes(int reflectedLightValues[], boolean pluckedNotes[])
-{
-  for (int i = 0; i < numberNotes; i++)
-  {
-    Log.Verbose("%d=%d - ", i, reflectedLightValues[i]);
-  }
-  Log.Verbose("\n");
-
-  //So ... are any light readings different from any others?
-  //To find the anomaly, see how different each string is to every other string.
-  //One or two strings should stand out. Those are the plucked strings.
-  harpNoteDetector.checkNotes(reflectedLightValues, pluckedNotes);
-
-  //So what strings are plucked? Note that if more than two are plucked,
-  //that counts as an error ... if that's the case, just keep playing the current notes
-  //and hope the player gets his act straight.
-  int firstNote = -1;
-  int secondNote = -1;
-  if (harpNoteDetector.getNotes(firstNote, secondNote, pluckedNotes))
-  {
-    if (firstNote >= 0)
-    {
-      Log.Verbose("==============PLUCKED 1: %d\n", firstNote);
-    }
-    if (secondNote >= 0)
-    {
-      Log.Verbose("==============PLUCKED 2: %d\n", secondNote);
-    }
-    playNote(firstNote, secondNote);
-  }
-  else
-  {
-    Log.Verbose("getNotes returned false - more than three notes were counted as plucked.\n");
-  }
-  return;
-}
-
-//Is a button pressed? If so move the motor a bit. This lets the user adjust the laser fan so it's pointing upwards properly.
+void printInfo()
+{
+  Serial.print(F("BAUD                ")); Serial.println(BAUD);
+  Serial.print(F("LIGHT_SENSOR_PIN    ")); Serial.println(LIGHT_SENSOR_PIN);
+  Serial.print(F("RANGE_SENSOR_PIN    ")); Serial.println(RANGE_SENSOR_PIN);
+
+  Serial.print(F("MIDICMD_NOTEON      ")); Serial.println(MIDICMD_NOTEON);
+  Serial.print(F("MIDICMD_NOTEOFF     ")); Serial.println(MIDICMD_NOTEOFF);
+
+  Serial.print(F("NOTE_COUNT_MAX      ")); Serial.println(NOTE_COUNT_MAX);
+
+  Serial.print(F("gNoteCount        = ")); Serial.println(gNoteCount);
+  Serial.print(F("gMute             = ")); Serial.println(gMute ? F("true") : F("false"));
+  
+  Serial.print(F("gMotorStepSize    = ")); Serial.println(gMotorStepSize);
+  Serial.print(F("gMotorStepsPerRev = ")); Serial.println(gMotorStepsPerRev);
+  Serial.print(F("gMotorStepDelay   = ")); Serial.println(gMotorStepDelay);
+
+  Serial.print(F("gLightAverage     = ")); Serial.println(gLightAverage);
+  Serial.print(F("gLightSensitivity = ")); Serial.println(gLightSensitivity);
+
+  Serial.print(F("gLogLevel         = ")); Serial.println(gLogLevel);
+}
+
 void checkButtons()
 {
-#ifdef ADJUSTER_BUTTONS
-  if (digitalRead(buttonApin) == LOW)
-  {
-    myMotor->step(stepSize, BACKWARD, DOUBLE);
-  }
-
-  if (digitalRead(buttonBpin) == LOW)
-  {
-    myMotor->step(stepSize, FORWARD, DOUBLE);
-  }
-#else
   while (Serial.available() > 0)
   {
     char c = Serial.read();
+    Serial.print(F("Read character from serial: ")); Serial.println(c);
     switch (c)
     {
       case 'f':
-        myMotor->step(stepSize, FORWARD, DOUBLE);
+        stepMotor(FORWARD);
         break;
       case 'b':
-        myMotor->step(stepSize, BACKWARD, DOUBLE);
+        stepMotor(BACKWARD);
         break;
       case 'p':
-        gPauseMotor = !gPauseMotor;
+        gMotorStepDelay *= -1;
+        break;
+      case 'c':
+        Serial.println("Entering sCmd.readSerial()");
+        sCmd.readSerial();
+        Serial.println("Exited sCmd.readSerial()");
         break;
       case 'l':
         gLogLevel = (gLogLevel + 1) % (LOG_LEVEL_VERBOSE + 1);
         //Log.Init(9600, gLogLevel);
         static const  char* levelStrings[]  = {"DISABLED","VERBOSE", "DEBUG", "INFO", "ERROR"};
-        Serial.print("Log level is "); Serial.println(levelStrings[gLogLevel]);
-      case 'n':
-        SendMIDI(MIDICMD_NOTEON, 0x4a, 0x49);
-        break;
+        Serial.print(F("Log level is ")); Serial.println(levelStrings[gLogLevel]);
       case 'm':
-        SendMIDI(MIDICMD_NOTEOFF, 0x4a, 0x49);
-        break;
+        gMute = !gMute;
+        break;
+      case 'i':
+        printInfo();
+        break;
+      case '\n':
+        break; // ignore newlines
       default:
-        Log.Error("Ignorning unknown command: %c\n", c);
-        
-    }
-  }
-#endif
+        Serial.print(F("Ignorning unknown command: ")); Serial.println(c);        
+    }
+  }
 }
 
 void loop()
 {
 
-  //There must be at least a handful notes for the code below to work right.
-  if (numberNotes < 5)
-  {
-    return;
-  }
-
-  //Run the laser forward, read all values, and see what is there. Note that this pretty much uses one
-  //less note than requested - but the START position counts as a spot. So moving it numberNotes makes that many
-  //strings plus the start string.
-
-  //It's already read the zero item. So read array items 1 through 7.
-  for (int i = 1; i < numberNotes; i++)
-  {
-    stepTheMotor(FORWARD);
-    waitForNote(i);
+  for (int i = 1; i < gNoteCount; i++)
+  {
+    if (gMotorStepDelay > 0) {
+      stepMotor(FORWARD);
+      waitForNote(i);
+    }
   }
 
   checkSonar();
 
-  //It just read item 7. So going backwards, read items 6 through zero.
-  for (int i = numberNotes - 2; i >= 0; i--)
-  {
-    stepTheMotor(BACKWARD);
-    waitForNote(i);
+  for (int i = gNoteCount - 2; i >= 0; i--)
+  {
+    if (gMotorStepDelay > 0) {
+      stepMotor(BACKWARD);
+      waitForNote(i);
+    }
   }
 
   checkButtons();
   checkSonar();
-
-}
+}

#include <Logging.h>

#ifdef GINSING
#include <GinSing.h>
#include <GinSingDefs.h>
#include <GinSingMaster.h>
#include <GinSingPoly.h>
#include <GinSingPreset.h>
#include <GinSingSerial.h>
#include <GinSingSerialMacros.h>
#include <GinSingSynth.h>
#include <GinSingVoice.h>

#include <Wire.h>
#include <GinSing.h>
#endif

#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"

#include "HarpNoteChoice.h"
#include "HarpNoteDetection.h"

//When debugging I wanted more information. So ... set this boolean to true
//to get more stuff printed to the console. When it's true the console dumps LOTS of
//great stuff - but the code CRAWLS and the laser harp isn't great.
const boolean debug = false;

boolean gPauseMotor = false;

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);

// Connect a stepper motor with 200 steps per revolution (1.8 degree)
// to motor port #2 (M3 and M4)
Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 2);

#ifdef GINSING
GinSing  GS;
#define rcvPin  4  // this is the pin used for receiving    ( can be either 4 or 12 )
#define sndPin  3  // this is the pin used for transmitting ( can be either 3 or 11 ) 
#define ovfPin  2  // this is the pin used for overflow control ( can be either 2 or 10 )
GinSingPoly * poly;

#define GINSING0 0
#define GINSING1 1
#endif

const int lightSensorPin = 0;
const int sonarPin = 1;

const int stepSize = 1;
const int delayBetweenSteps = 15;
const int numberNotes = 7;

#ifdef GINSING
GSNote ginsingNotes[numberNotes + 1];
#endif

#ifdef ADJUSTER_BUTTONS
const int buttonApin = 9;
const int buttonBpin = 8;
#endif

HarpNoteChoice harpNoteChoice;
HarpNoteDetection harpNoteDetector;

boolean pluckedNotes[numberNotes];
int reflectedLightValues[numberNotes];


int gLogLevel = LOG_LEVEL_NOOUTPUT;
//int gLogLevel = LOG_LEVEL_ERRORS;
//int gLogLevel = LOG_LEVEL_INFOS;
//int gLogLevel = LOG_LEVEL_DEBUG;
//int gLogLevel = LOG_LEVEL_VERBOSE;

int gAverageLightReading = 0;
const unsigned long gBaud = 115200;

#define MIDICMD_NOTEON  0x90 // MIDI command (Note On, Channel 0)
#define MIDICMD_NOTEOFF 0x80 // MIDI command (Note On, Channel 0)

// buf must be an array with length > segmentCount. This function can write to buf[segmentCount]
void drawMeter(int value, int maxValue, char * buf, char segmentSymbol = '=', size_t totalSegments = 80)

  }
}

int runningAverage(int average, int sample, int index) {

  unsigned long sum = average * index;
  sum += sample;
  return (int)(sum /= (index + 1));
  
}

int sampleLight(int sampleCount, int interval=2)
{
  int average = 0;
  for (int i = 0; i < sampleCount; i++) 
  {
    delay(interval);
    int sample = analogRead(LIGHT_SENSOR_PIN);
    average = runningAverage(average, sample, i);
    Log.Info("sample %d: %d, average - %d\n", i, sample, average);
  }
  return max(1, (int)average);
}

void stepMotor(int directionToStep=FORWARD, int steps = -1)
{
  if (steps == -1) steps = gMotorStepSize;
  
  myMotor->step(steps, directionToStep, DOUBLE);
}

void setupMotor() {
  AFMS.begin();  // create with the default frequency 1.6KHz
  myMotor->setSpeed(250);

  int maxLight = 0;
  int maxLightStep = 0;
  for (int i = 0; i < gMotorStepsPerRev; i++) {
    stepMotor(FORWARD, 1);
    int sample = sampleLight(10);
    logMeter(Log, sample, 600, LOG_LEVEL_INFOS);
    if (sample > maxLight) 
    {
      maxLight = sample;
      maxLightStep = i;
    }
  }
  Log.Error("Max light value (%d) at motor step %d\n", maxLight, maxLightStep);
  stepMotor(FORWARD, maxLightStep + (gMotorStepsPerRev / 8));
}

void sampleLightCmd()
{
  Serial.println(F("Running sampleLightCmd"));
  Serial.print(F("Before: ")); Serial.println(gLightAverage);
  gLightAverage = sampleLight(50);
  Serial.print(F("After:  ")); Serial.println(gLightAverage);
}

void setLightSensitivityCmd() 
{
  Serial.println(F("Running setLightSensitivityCmd"));
  char * str = sCmd.next();
  if (str && *str)
  {
    double val = atof(str);
    if (val > 1)
    {
      Serial.print(F("Before: ")); Serial.println(gLightSensitivity);
      gLightSensitivity = val;
      Serial.print(F("After:  ")); Serial.println(gLightSensitivity);
    }
    else 
    {
      Serial.print(F("Invalid value: "));
      Serial.print(val);
      Serial.println(F(". Must be > 1."));
    }
  }
  else
  {
    Serial.print(F("Invalid input: \""));
    Serial.print(str);
    Serial.println(F("\""));
  }
}

void setNoteCountCmd() 
{
  Serial.println(F("Running setNoteCountCmd"));
  char * str = sCmd.next();
  if (str && *str)
  {
    int val = atoi(str);
    if (val > 1)
    {
      Serial.print(F("Before: ")); Serial.println(gNoteCount);
      gNoteCount = val;
      Serial.print(F("After:  ")); Serial.println(gNoteCount);
    }
    else 
    {
      Serial.print(F("Invalid value: "));
      Serial.print(val);
      Serial.println(F(". Must be > 1."));
    }
  }
  else
  {
    Serial.print(F("Invalid input: \""));
    Serial.print(str);
    Serial.println(F("\""));
  }
}
void unrecognizedSerialCmd(const char * cmd) 
{
  Serial.print(F("Unrecognized serial command: "));
  Serial.println(cmd);  
}

void setupSerialCommands()
{
  sCmd.addCommand("SampleLight", sampleLightCmd);
  sCmd.addCommand("SetLightSensitivity", setLightSensitivityCmd);
  sCmd.addCommand("SetNoteCount", setNoteCountCmd);
  //sCmd.addCommand("PlayNote")
  sCmd.setDefaultHandler(unrecognizedSerialCmd);  
}

void setup()
{
  Log.Init(gLogLevel, BAUD);

  //For the light sensor
  analogReference(EXTERNAL);

  setupMotor();  

  //Get some initial values for each light string
  gLightAverage = sampleLight(50);

  setupSerialCommands();
}

//Read the sonar unit and figure out if the
//notes should move up or down
void checkSonar()
{
  int height = analogRead(RANGE_SENSOR_PIN);

  logMeter(Log, height, 160);

}


void SendMIDI(char cmd, char data1, char data2) 
{
  if (gMute == false)
  {
    Serial.print(cmd);
    Serial.print(data1);
    Serial.print(data2);
  }
}


void waitForNote(int note) 
{
  static bool noteStates[NOTE_COUNT_MAX] = {0};
  bool detected = false;
  bool currentlyOn = noteStates[note];
  unsigned long duration = millis() + gMotorStepDelay;
  while (millis() < duration)
  {
    int light = analogRead(LIGHT_SENSOR_PIN);
    logMeter(Log, light, 600, LOG_LEVEL_INFOS, '.');
    
    detected = (light > (gLightAverage * gLightSensitivity));

    if (currentlyOn != detected)
    {
