+// ############################################
+// CONFIG SECTION
+bool veryVerboseDebug = true;
+
 int pinA = 3;  // Connected to CLK on KY-040
 int pinB = 4;  // Connected to DT on KY-040
 
 int rotaryTicks = 0;
 int lastRotaryTicks = 0;
+
+const String usbFileName = "PONDS.PD";
+
+// every n writes we manually close the file and reopen to prevent data
+// loss. Lower n = slower perf, but safer
+const int USB_WRITE_MANUAL_FLUSH_RATE = 2;
+
+// END CONFIG SECT
+// ############################################
 
 int lastRotaryValue; // 1/0 read from rotary
 

int rollingAveragePotent();


// util functions for writing in CSV format
void writeCsvLabel();
void writeCsvRow(int rotaryTickCount, int potentTickCount, double x, double y);


// hacky because for some reason we can print in the setup() fxn
bool wroteCsvHeader;
 
void setup() { 
    pinMode(pinA,INPUT);
    pinMode(pinB,INPUT);
    /* Read Pin A
    Whatever state it's in will reflect the last position   
    */
    lastRotaryValue = digitalRead(pinA);
    initialPotentiometerReading = analogRead(A0);
    wroteCsvHeader = false;
    Serial.begin (9600);
    Serial.println("Now running!");
}

 void loop() { 
  // if they disconnect from reading the output stream, resend the header data on reconnec
  if (!Serial) {
    wroteCsvHeader = false;
  }

  if (!wroteCsvHeader && Serial) // if(Serial) only available on Leonardo: detects if tty connected
  {
    delay(150); // 150ms for the client to prepare for display cause clients were slow & dropping label packets
    writeCsvLabel();
    wroteCsvHeader = true;
  }
  
   int newRotaryValue = digitalRead(pinA);

  // we write the read sensor values first to preserve the raw data and then our interpreted/calculated x,y points
  // in the cartesian plane
  // so it's (r, theta, x, y)
  String header = "RotaryEncoder,Potentiometer,MillisSinceStart\n";
  if (Serial) {
    Serial.print("LABEL,");
    Serial.println(header);
  }

  write2file(header);
}

void writeCsvRow(int rotaryTickCount, int potentTickCount)
{
  // these need to be in the same order as writeCsvLabel()'s implementatio
  if (Serial) {
    Serial.print("DATA");
    Serial.print(","); Serial.print(rotaryTickCount);
    Serial.print(","); Serial.print(potentTickCount);
  }

  String dataLine = "";
  dataLine += rotaryTickCount;
  dataLine += ',';
  dataLine += potentTickCount;
  dataLine += ',';
  dataLine += millis();
  dataLine += '\n';

  if (veryVerboseDebug)
  {
    Serial.print("-----------Would have written: (");
    Serial.print(dataLine.length());
    Serial.println(" characters)");
    Serial.println(dataLine);
    Serial.println("----------------------");
  }

  write2file(dataLine);
}

void usbDeviceWriteUnbuffered(const String& data)
{
  if (veryVerboseDebug) {
    Serial.println("DEBUG: WRITING 2 USB:");
    Serial.println(data);
  }
  usbDev.write(data.c_str(), data.length());
  delay(data.length());
  usbDev.flush();
  delay(data.length());
}

void usbDeviceWrite(const String& data)
{
  usbDeviceWriteUnbuffered(data);
  dumpUsbOutput();
}

void setupUsbDevice()
{
  usbDev.begin(9600);
  dumpUsbOutput();
  usbDeviceWrite("failingspacercommand\r");
  usbDeviceWrite("IPA\r");
  usbDeviceWrite("ECS\r");
  delay(100);

  
}

void setupUsbStickForWrite()
{
  // open a file for writing
  usbOpenFile4Write(usbFileName);
  // special delimeter for mathematica to know that
  // this is a new section of the file
  write2file("########## NEW SECTION ##########\n");
  usbDeviceWrite("plzfail\r");
}


int write2fileCount = 0;

void write2file(const String& data) {
  String writeCommand = "WRF ";
  writeCommand += data.length();
  writeCommand += '\r';
  writeCommand += data;
  usbDeviceWrite(writeCommand);

  if (++write2fileCount >= USB_WRITE_MANUAL_FLUSH_RATE) {
    Serial.println("Manually closing & reopening datafile.");
    write2fileCount = 0;
    usbDeviceWrite("CLF\r");
    usbOpenFile4Write(usbFileName);
  }
}

String dumpUsbOutput()
{
    if (usbDev.available() && Serial) {
    String fromUsb = "";
    while (usbDev.available()) {
      char readC = usbDev.read();
      fromUsb += readC;
    }

    // wait 10ms to see if anything else comes
    bool delayWorked = true;
    while (delayWorked) {
      delayWorked = false;
      delay(20);
      while (usbDev.available()) {
        delayWorked = true;
        char readC = usbDev.read();
        fromUsb += readC;
      }
    }
    

    Serial.println("###########################");
    Serial.println("From USB STICK:");
    Serial.println(fromUsb);
    Serial.println("###########################");

    return fromUsb;
  }

  return "";
}

void usbOpenFile4Write(const String& filename)
{
  usbDeviceWrite("OPW " + filename + "\r");
  delay(100);
  dumpUsbOutput();
}

bool isUsbStickPluggedIn()
{
  usbDeviceWriteUnbuffered("DVL\r");
  delay(100);
  String response = dumpUsbOutput();
  return response != "No Device blalbla";
}


