/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [c]2020 - D. Ottensmeyer ------------------------*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~
 ~ Target:    Arduino NANO with RoboDuck (created by the "Fakultät für Physik
 ~            der Universität Regensburg", Germany, in 2019).
 ~            Link: http://www.physik.uni-regensburg.de/rsl/roboduck.html
 ~            Attached HC-05 or HC-06 Bluetooth module. HowTo:
 ~            https://forum.arduino.cc/index.php?topic=544399.0
 ~            Android smartphone with Bluetooth Control app. Download:
 ~            https://play.google.com/store/apps/details?id=com.broxcode.arduinobluetoothfree
 ~ Example:   RoboDuck_BT-Demo_01
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Description:
 ~ In this example we show a first Bluetooth test for the RoboDuck.
 ~ In the Bluetooth Control app please define the characters to send as
 ~ "Arrow Keys" as follows:
 ~              UP    -> 1
 ~              Down  -> 2
 ~              Right -> 3
 ~              Left  -> 4
 ~ Additional characters for other keys:
 ~              Quack -> 5
 ~              LED 1 -> 6
 ~              LED 2 -> 7
 ~              LED 3 -> 8
 ~              LEDs off -> 0
 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Defines:
//#define SERVO_CENTER
//#define SERVO_DEFAULT
#define DEMO_RUN

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Includes:
#include <RoboDuck.h>
#include <RoboDuck_BT.h>

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Constants:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Variables:
char controlKey;
uint8_t controlKeyCnt;

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void setup() {

  // Initialize serial:
  Serial.begin(9600); 
  Serial.println("Arduino NANO RoboDuck BT Test 1!");
  Serial.println("");
  Serial.println("PLEASE ENSURE THAT ROBODUCK CAN MOVE FREELY!!!");
  Serial.println("");


// Assign Arduino Nano ports to the LEDs:
  pinMode(LED_BUILTIN, OUTPUT);     // built-in LED pin
  pinMode(LED_R_GREEN_PIN, OUTPUT); // pin 14 (A0, PC0)
  pinMode(LED_L_RED_PIN, OUTPUT);   // pin 15 (A1, PC1)

// Assign an Arduino Nano port to the Beeper:
  pinMode(BEEPER_PIN, OUTPUT);      // pin 5 (PD5)

#ifdef SERVO_CENTER
  Servo_Init();
  AllServo_Center();
  while(1);
#endif

#ifdef SERVO_DEFAULT
  Servo_Init();  
  AllServo_Default();
  while(1);
#endif

#ifdef DEMO_RUN 
  Servo_Init();
  AllServo_Default();
  delay(2000);
  // Initialize BT serial:
  BT_Begin(9600);
  delay(50);
#endif
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Functions:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void loop() {
  if(BT_Control(&controlKey, &controlKeyCnt))
  {
    LEDs(controlKey);
    Serial.print("<");
    Serial.print(controlKey);
    Serial.print("> ");
    Serial.println(controlKeyCnt);
  }
  // No BT commands: do other things ...
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
