/* FioV3BluetoothSetup.ino
* This sketch sets up RN-42XV Xbee format bluetooth module to run at 9600 baud in SPP mode with PIN of 0000
* Load the sketch and then open the SerialMonitor within 10sec to monitor the results.
*
* Note: unlike the wifi board the bluetooth module does not echo the commands so the code below has been modified accordingly
*
* (c)2013 Forward Computing and Control Pty. Ltd.
* www.forward.com.au
* This code may be freely used
*/

// set these strings to suit
char *pin = "0000";

Stream *console = NULL;
Stream *bt = NULL;

void setup() {

  // put your setup code here, to run once:
  // Open serial communications
  Serial.begin(9600);  // console stream  Almost all Arduino boards with USB connections use this.
  console = &Serial;  // console it usually Serial  Almost all Arduino boards with USB connections use this.

  // this setting suits FioV3
  // but for some boards bt = Serial if there is not another serial available
  // bt stream  change this to suit your Arduino board
  Serial1.begin(9600);  // this setting suits FioV3 + RN-42XV board
  bt = &Serial1;   // set this to the serial connected to your bt as specified above in the .begin( ) i.e. Serial1 etc or SofwareSerial etc
  setupBluetooth(); // setup at 9600 baud
}

void loop() {
  // put your main code here, to run repeatedly:
  if ((console != bt) && (console != NULL)) {
    // give the SerialMonitor access to the bt board
    if (bt->available()) {
      console->write(bt->read());
    }
    if (console->available()) {
      byte in = console->read();
      console->write(in); // local echo
      bt->write(in);
    }
  }
}


boolean setupBluetooth() {
  if (bt == NULL) {
    streamPrint(console, F("bt cannot be null."), true);
  }
  if (bt == console) {
    // can not send debug
    console = NULL;
  }
  for (int i = 10; i > 0; i--) {
    delay(1000);
    streamPrint(console, F(" "), false);
    streamPrint(console, i, false);
  }
  streamPrint(console, F(""), true);
  boolean enteredCMDmode = false;
  write(F("$$$"), false, true); // no CR here, but echo to console
  enteredCMDmode = waitFor("CMD");
  if (!enteredCMDmode) {
    // did not get into CMD mode.
    write(F(" ")); // send cr to terminate last command
    dumpReply();
    streamPrint(console,
                F("Are we still in CMD mode?  Trying '---' to exit CMD mode"), true);
    write(F("---"));
    if (waitFor("END")) {
      delay(500);
      write(F("$$$"), false, true); // no CR here but echo to console
      enteredCMDmode = waitFor("CMD");
    }
    if (!enteredCMDmode) {
      streamPrint(console,
                  F("Could still not get into CMD mode. Try changing bt BAUD rate. Skipping programmed config. Use console"),
                  true);
    }
    return false;
  }
  // skip all these cmds if did not get to CMD mode
  if (enteredCMDmode) {
    write(F("SM,0")); // Slave mode, in this mode other Bluetooth devices can discover and connect to the device.
    waitForAOK();

    write(F("SA,4"));
    // Authenication, This mode is PIN code mode, which forces Bluetooth version 2.0 PIN code authentication.
    waitForAOK();

    write(F("SP,"), false, true);
    write(pin);
    //  Set the pin code
    waitForAOK();

    write(F("ST,0"));
    //  enables the hardware connection indication. GPIO-6, high = connected, low = not connected.
    waitForAOK();

    write(F("S~,0"));
    //  Suppress all the status messages.
    waitForAOK();

    write(F("SN,IR TypeK Temp meter"));
    //  Set bluetooth name.
    waitForAOK();

    write(F("D-"));
    dumpReply();


    write(F("R,1"));
    waitFor("Reboot!");

    // reboot exits CMD mode.  if not rebooting then issue exit to exit CMD mode
    //  write(F("---"));
    //  waitFor("END");
  }
  return true;
}

// write to bt only with echo to console and with terminating cr nl
void write(const __FlashStringHelper *ifsh) {
  write(ifsh, true, true);
}

// write to bt only with echo to console and with terminating cr nl
void write(char *str) {
  write(str, true, true);
}

void write(char * str,  boolean newLine, boolean echoToConsole) {
  if (echoToConsole) {
    streamPrint(console, str, newLine);
  }
  streamPrint(bt, str, newLine);
}

void write(const __FlashStringHelper *ifsh,  boolean newLine, boolean echoToConsole) {
  if (echoToConsole) {
    streamPrint(console, ifsh, newLine);
  }
  streamPrint(bt, ifsh, newLine);
}

void streamPrint(Stream *stream, const __FlashStringHelper *ifsh,   boolean newLine) {
  if (stream == NULL) {
    return; // nothing to write to
  }
  stream->print(ifsh);
  if (newLine) {
    stream->print("\r\n");
  }
}

void streamPrint(Stream *stream, int i, boolean newLine) {
  if (stream == NULL) {
    return; // nothing to write to
  }
  stream->print(i);
  if (newLine) {
    stream->print("\r\n");
  }
}

void streamPrint(Stream *stream, char *str, boolean newLine) {
  if (stream == NULL) {
    return; // nothing to write to
  }
  stream->print(str);
  if (newLine) {
    stream->print("\r\n");
  }
}

boolean waitForAOK() {
  return waitFor("AOK");
}

boolean waitFor(char* str) {
  size_t targetLen = 0;
  char* target;
  size_t index = 0;
  target = str;
  targetLen = strlen(str);
  index = 0;
  long startTime = millis();
  const long timeout = 1000;
  const long newLineTimeout = 100;
  while (millis() - startTime < timeout) {

    if (console && bt && console->available()) {
      byte in = console->read();
      //Serial.write(in); // local echo
      bt->write(in);
    }
    if (targetLen == 0) {
      return true;
    }
    // else
    if (bt->available()) {
      char c = bt->read();
      if (console) {
        console->write(c);
      }
      if (c == '?') {   // failed
        streamPrint(console, " Error: returned ? expected '", false);
        streamPrint(console, target, false);
        streamPrint(console, "' cmd unrecognized.", true);
        dumpReply();
        return false;
      }
      if (c != target[index]) {
        index = 0; // reset index if any char does not match
      } // no else here
      if (c == target[index]) {
        //Serial.print("found "); Serial.write(c); Serial.print("index now"); Serial.println(index+1);
        if (++index >= targetLen) { // return true if all chars in the target match
          targetLen = 0; // finished matching this one
          dumpReply(); // show any other response
        }
      }
    }
  }
  streamPrint(console, "Timed out waiting for '", false);
  streamPrint(console, target, false);
  streamPrint(console, "'", true);
  return false; // timed out
}

// just dump chars from bt to console until none found for 100mS
void dumpReply() {
  long startTime = millis();
  const long timeout = 500;
  while (millis() - startTime < timeout) {
    // else
    if (bt->available()) {
      char c = bt->read();
      if ((console != bt) && (console != NULL)) {
        console->write(c); // echo it
      }
      startTime = millis(); // reset start time
    }
  }
}