+
+#include <SerialCommand.h>
+
 #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;
+#define BAUD             115200
+#define LIGHT_SENSOR_PIN      0
+#define RANGE_SENSOR_PIN      1
+
+#define MIDICMD_NOTEON     0x90 // MIDI command (Note On, Channel 0)
+#define MIDICMD_NOTEOFF    0x80 // MIDI command (Note On, Channel 0)
+
+#define NOTE_COUNT_MAX      100 // kinda a hack to allow static initialization in waitForNote
+
+int  gNoteCount        =   10;
+bool gMute             =   false;
+
+int gMotorStepSize    =   4;
+int gMotorStepsPerRev = 200;
+int gMotorStepDelay   =  10; // set to <= 0 to pause motor
+
+int    gLightAverage     = 0;
+double gLightSensitivity = 5;
+
+//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)
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+Adafruit_StepperMotor *myMotor = AFMS.getStepper(gMotorStepsPerRev, 2);
+
+SerialCommand sCmd;
+
 
 // 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 sampleLight(int sampleCount, int interval=2)
{
  unsigned long average = analogRead(lightSensorPin);
  for (int i = 1; i < sampleCount; i++) 
  {
    delay(interval);
    int sample = analogRead(lightSensorPin);
    average *= i;
    average += sample;
    average /= i + 1; 
    Log.Info("sample %d: %d, average - %d\n", i, sample, average);
  }
  return max(1, (int)average);
}

void setup()
{
  Log.Init(gLogLevel, gBaud);
  harpNoteDetector.setNumNotes(numberNotes);

  //For the light sensor
  analogReference(EXTERNAL);

#ifdef ADJUSTER_BUTTONS
  pinMode(buttonApin, INPUT_PULLUP);
  pinMode(buttonBpin, INPUT_PULLUP);
#endif

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

  //Get some initial values for each light string
  gAverageLightReading = sampleLight(50);
  for (int i = 0; i < numberNotes; i++)
  {
    reflectedLightValues[i] = analogRead(lightSensorPin);
    pluckedNotes[i] = false;
  }

#ifdef GINSING
  setupGinSing();
#endif
}

#ifdef GINSING
//This is the code required to get GinSing ready to go.
void setupGinSing()
{
  GS.begin(rcvPin, sndPin, ovfPin);               // start the device (required) and enter preset mode
  //For preset mode - change this when going to poly mode
  poly = GS.getPoly();
  poly->begin();                                    // enter presetmode

  poly->setWaveform(GINSING0, SAWTOOTH);
  poly->setWaveform(GINSING1, SAWTOOTH);

  setupGinsingNotes();
}

//These are the notes I want to play with GinSing.
void setupGinsingNotes()
{
  ginsingNotes[0] = C_4;
  ginsingNotes[1] = D_4;
  ginsingNotes[2] = E_4;
  ginsingNotes[3] = F_4;
  ginsingNotes[4] = G_4;
  ginsingNotes[5] = A_4;
  ginsingNotes[6] = B_4;
}
#endif

//This code is used to take the sonar reading and convert
//that into something to change the GinSing note being played.
int findMultiplier(int height)
{
  /*
    Over 170 = nothing
    140 = high           80 - 5
    125 = normal  64 - 4
    110 = low            48 - 3
    92 = very low       32 - 2
    74 = lower          16 - 1
    62 bottom out
  */
  if (height > 135) return 5;
  if (height > 119) return 4;
  if (height > 100) return 3;
  if (height > 80) return 2;
  //My speaker gets weird at the low values. So I commented that one out.
  //	if (height > 65) return 1;
  return 0;
}

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

  logMeter(Log, height, 160);

  if (height > 170)
  {
    return;
  }
  int multiplier = findMultiplier(height);
  int baseValue = 16 * multiplier;
#ifdef GINSING
  ginsingNotes[0] = (GSNote)baseValue;
  ginsingNotes[1] = (GSNote)(baseValue + 1);
  ginsingNotes[2] = (GSNote)(baseValue + 2);
  ginsingNotes[3] = (GSNote)(baseValue + 3);
  ginsingNotes[4] = (GSNote)(baseValue + 4);
  ginsingNotes[5] = (GSNote)(baseValue + 5);
  ginsingNotes[6] = (GSNote)(baseValue + 6);
#endif
}

#ifdef GINSING
//These two are the notes that Ginsing is currently playing.
int ginsingNote0 = -1;
int ginsingNote1 = -1;
#endif

//FirstNote and SecondNote are the notes we want to be playing.
void playNote(int firstNote, int secondNote)
{

  Log.Verbose("Playing notes %d and %d\n", firstNote, secondNote);

  //Pick the notes to be played and which channel to play them on
#if GINSING
  harpNoteChoice.pickNotes(ginsingNote0, ginsingNote1, firstNote, secondNote);
  if (ginsingNote0 == -1)
  {
    poly->release(GINSING0);
  }
  else
  {
    poly->setNote(GINSING0, ginsingNotes[ginsingNote0]);
    poly->trigger(GINSING0);
  }

  if (ginsingNote1 == -1)
  {
    poly->release(GINSING1);
  }
  else
  {
    poly->setNote(GINSING1, ginsingNotes[ginsingNote1]);
    poly->trigger(GINSING1);
  }
#endif
}

void SendMIDI(char cmd, char data1, char data2) 
{
  Serial.print(cmd);
  Serial.print(data1);
  Serial.print(data2);
}

//Move the motor one step. 
void stepTheMotor(int directionToStep)
{
  if (gPauseMotor == false)
    myMotor->step(stepSize, directionToStep, DOUBLE);
}

void waitForNote(int note) 
{
  static bool noteStates[numberNotes] = {0};
  bool detected = false;
  bool currentlyOn = noteStates[note];
  unsigned long duration = millis() + delayBetweenSteps;
  while (millis() < duration)
  {
    int light = analogRead(lightSensorPin);
    logMeter(Log, light, 600, LOG_LEVEL_INFOS, '.');
    
    detected = (light > (gAverageLightReading * 2));

    if (currentlyOn != detected)
    {

  noteStates[note] = detected;
}

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()
{
  while (Serial.available() > 0)
  {
    char c = Serial.read();
    Serial.print(F("Read character from serial: ")); Serial.println(c);
    switch (c)
    {
      case 'f':
        stepMotor(FORWARD);
        break;
      case 'b':
        stepMotor(BACKWARD);
        break;
      case 'p':
        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(F("Log level is ")); Serial.println(levelStrings[gLogLevel]);
      case 'm':
        gMute = !gMute;
        break;
      case 'i':
        printInfo();
        break;
      case '\n':
        break; // ignore newlines
      default:
        Serial.print(F("Ignorning unknown command: ")); Serial.println(c);        
    }
  }
}

void loop()
{

  for (int i = 1; i < gNoteCount; i++)
  {
    if (gMotorStepDelay > 0) {
      stepMotor(FORWARD);
      waitForNote(i);
    }
  }

  checkSonar();

  for (int i = gNoteCount - 2; i >= 0; i--)
  {
    if (gMotorStepDelay > 0) {
      stepMotor(BACKWARD);
      waitForNote(i);
    }
  }

  checkButtons();
  checkSonar();
}
